diff --git a/aruco_opencv/include/aruco_opencv/parameters.hpp b/aruco_opencv/include/aruco_opencv/parameters.hpp index 504e9ba..7ba30d7 100644 --- a/aruco_opencv/include/aruco_opencv/parameters.hpp +++ b/aruco_opencv/include/aruco_opencv/parameters.hpp @@ -147,8 +147,17 @@ void retrieve_aruco_parameters( "aruco.minOtsuStdDev", detector_parameters->minOtsuStdDev); node.get_parameter( "aruco.errorCorrectionRate", detector_parameters->errorCorrectionRate); + + #if CV_VERSION_MAJOR > 4 || CV_VERSION_MAJOR == 4 && CV_VERSION_MINOR >= 7 + int refine_method = 0; + node.get_parameter("aruco.cornerRefinementMethod", refine_method); + detector_parameters->cornerRefinementMethod = + static_cast(refine_method); + #else node.get_parameter( "aruco.cornerRefinementMethod", detector_parameters->cornerRefinementMethod); + #endif + node.get_parameter( "aruco.cornerRefinementWinSize", detector_parameters->cornerRefinementWinSize); node.get_parameter( diff --git a/aruco_opencv/include/aruco_opencv/utils.hpp b/aruco_opencv/include/aruco_opencv/utils.hpp index 8dafd67..bf057e4 100644 --- a/aruco_opencv/include/aruco_opencv/utils.hpp +++ b/aruco_opencv/include/aruco_opencv/utils.hpp @@ -34,6 +34,12 @@ namespace aruco_opencv geometry_msgs::msg::Pose convert_rvec_tvec(const cv::Vec3d & rvec, const cv::Vec3d & tvec); -extern const std::unordered_map ARUCO_DICT_MAP; +#if CV_VERSION_MAJOR > 4 || CV_VERSION_MAJOR == 4 && CV_VERSION_MINOR >= 7 +using ArucoDictType = cv::aruco::PredefinedDictionaryType; +#else +using ArucoDictType = cv::aruco::PREDEFINED_DICTIONARY_NAME; +#endif + +extern const std::unordered_map ARUCO_DICT_MAP; } // namespace aruco_opencv diff --git a/aruco_opencv/src/aruco_tracker.cpp b/aruco_opencv/src/aruco_tracker.cpp index 9ea9f2f..5ba6049 100644 --- a/aruco_opencv/src/aruco_tracker.cpp +++ b/aruco_opencv/src/aruco_tracker.cpp @@ -108,7 +108,11 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode { RCLCPP_INFO(get_logger(), "Configuring"); + #if CV_VERSION_MAJOR > 4 || CV_VERSION_MAJOR == 4 && CV_VERSION_MINOR >= 7 + detector_parameters_ = cv::makePtr(); + #else detector_parameters_ = cv::aruco::DetectorParameters::create(); + #endif retrieve_parameters(); @@ -117,7 +121,12 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode return LifecycleNodeInterface::CallbackReturn::FAILURE; } + #if CV_VERSION_MAJOR > 4 || CV_VERSION_MAJOR == 4 && CV_VERSION_MINOR >= 7 + dictionary_ = cv::makePtr(cv::aruco::getPredefinedDictionary( + ARUCO_DICT_MAP.at(marker_dict_))); + #else dictionary_ = cv::aruco::getPredefinedDictionary(ARUCO_DICT_MAP.at(marker_dict_)); + #endif if (!board_descriptions_path_.empty()) { load_boards(); @@ -230,6 +239,7 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode tf_listener_.reset(); tf_buffer_.reset(); tf_broadcaster_.reset(); + dictionary_.reset(); detector_parameters_.reset(); detection_pub_.reset(); debug_pub_.reset(); @@ -367,20 +377,42 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode const int markers_y = desc["markers_y"].as(); const double marker_size = desc["marker_size"].as(); const double separation = desc["separation"].as(); - - auto board = cv::aruco::GridBoard::create( - markers_x, markers_y, marker_size, separation, - dictionary_, desc["first_id"].as()); + const int first_id = desc["first_id"].as(); + + #if CV_VERSION_MAJOR > 4 || CV_VERSION_MAJOR == 4 && CV_VERSION_MINOR >= 7 + std::vector ids(markers_x * markers_y); + std::iota(ids.begin(), ids.end(), first_id); + cv::Ptr board = cv::makePtr( + cv::Size(markers_x, markers_y), marker_size, separation, *dictionary_, ids); + #else + cv::Ptr board = cv::aruco::GridBoard::create( + markers_x, markers_y, marker_size, separation, dictionary_, first_id); + #endif if (frame_at_center) { double offset_x = (markers_x * (marker_size + separation) - separation) / 2.0; double offset_y = (markers_y * (marker_size + separation) - separation) / 2.0; - for (auto & obj : board->objPoints) { + + #if CV_VERSION_MAJOR > 4 || CV_VERSION_MAJOR == 4 && CV_VERSION_MINOR >= 7 + std::vector> obj_points(board->getObjPoints()); + #else + std::vector> obj_points(board->objPoints); + #endif + + for (auto & obj : obj_points) { for (auto & point : obj) { point.x -= offset_x; point.y -= offset_y; } } + + // Create a new board with all the object point offsetted so that point (0,0) + // is at the center of the board + #if CV_VERSION_MAJOR > 4 || CV_VERSION_MAJOR == 4 && CV_VERSION_MINOR >= 7 + board = cv::makePtr(obj_points, *dictionary_, ids); + #else + board = cv::aruco::Board::create(obj_points, dictionary_, board->ids); + #endif } boards_.push_back(std::make_pair(name, board)); @@ -388,7 +420,7 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode RCLCPP_ERROR_STREAM(get_logger(), "Failed to load board '" << name << "': " << e.what()); continue; } - RCLCPP_ERROR_STREAM( + RCLCPP_INFO_STREAM( get_logger(), "Successfully loaded configuration for board '" << name << "'"); } } diff --git a/aruco_opencv/src/utils.cpp b/aruco_opencv/src/utils.cpp index 9de6e07..cb5d6b5 100644 --- a/aruco_opencv/src/utils.cpp +++ b/aruco_opencv/src/utils.cpp @@ -26,8 +26,6 @@ namespace aruco_opencv { -using cv::aruco::PREDEFINED_DICTIONARY_NAME; - geometry_msgs::msg::Pose convert_rvec_tvec(const cv::Vec3d & rvec, const cv::Vec3d & tvec) { geometry_msgs::msg::Pose pose_out; @@ -49,28 +47,28 @@ geometry_msgs::msg::Pose convert_rvec_tvec(const cv::Vec3d & rvec, const cv::Vec return pose_out; } -const std::unordered_map ARUCO_DICT_MAP = { - {"4X4_50", PREDEFINED_DICTIONARY_NAME::DICT_4X4_50}, - {"4X4_100", PREDEFINED_DICTIONARY_NAME::DICT_4X4_100}, - {"4X4_250", PREDEFINED_DICTIONARY_NAME::DICT_4X4_250}, - {"4X4_1000", PREDEFINED_DICTIONARY_NAME::DICT_4X4_1000}, - {"5X5_50", PREDEFINED_DICTIONARY_NAME::DICT_5X5_50}, - {"5X5_100", PREDEFINED_DICTIONARY_NAME::DICT_5X5_100}, - {"5X5_250", PREDEFINED_DICTIONARY_NAME::DICT_5X5_250}, - {"5X5_1000", PREDEFINED_DICTIONARY_NAME::DICT_5X5_1000}, - {"6X6_50", PREDEFINED_DICTIONARY_NAME::DICT_6X6_50}, - {"6X6_100", PREDEFINED_DICTIONARY_NAME::DICT_6X6_100}, - {"6X6_250", PREDEFINED_DICTIONARY_NAME::DICT_6X6_250}, - {"6X6_1000", PREDEFINED_DICTIONARY_NAME::DICT_6X6_1000}, - {"7X7_50", PREDEFINED_DICTIONARY_NAME::DICT_7X7_50}, - {"7X7_100", PREDEFINED_DICTIONARY_NAME::DICT_7X7_100}, - {"7X7_250", PREDEFINED_DICTIONARY_NAME::DICT_7X7_250}, - {"7X7_1000", PREDEFINED_DICTIONARY_NAME::DICT_7X7_1000}, - {"ARUCO_ORIGINAL", PREDEFINED_DICTIONARY_NAME::DICT_ARUCO_ORIGINAL}, - {"APRILTAG_16h5", PREDEFINED_DICTIONARY_NAME::DICT_APRILTAG_16h5}, - {"APRILTAG_25h9", PREDEFINED_DICTIONARY_NAME::DICT_APRILTAG_25h9}, - {"APRILTAG_36h10", PREDEFINED_DICTIONARY_NAME::DICT_APRILTAG_36h10}, - {"APRILTAG_36h11", PREDEFINED_DICTIONARY_NAME::DICT_APRILTAG_36h11}, +const std::unordered_map ARUCO_DICT_MAP = { + {"4X4_50", ArucoDictType::DICT_4X4_50}, + {"4X4_100", ArucoDictType::DICT_4X4_100}, + {"4X4_250", ArucoDictType::DICT_4X4_250}, + {"4X4_1000", ArucoDictType::DICT_4X4_1000}, + {"5X5_50", ArucoDictType::DICT_5X5_50}, + {"5X5_100", ArucoDictType::DICT_5X5_100}, + {"5X5_250", ArucoDictType::DICT_5X5_250}, + {"5X5_1000", ArucoDictType::DICT_5X5_1000}, + {"6X6_50", ArucoDictType::DICT_6X6_50}, + {"6X6_100", ArucoDictType::DICT_6X6_100}, + {"6X6_250", ArucoDictType::DICT_6X6_250}, + {"6X6_1000", ArucoDictType::DICT_6X6_1000}, + {"7X7_50", ArucoDictType::DICT_7X7_50}, + {"7X7_100", ArucoDictType::DICT_7X7_100}, + {"7X7_250", ArucoDictType::DICT_7X7_250}, + {"7X7_1000", ArucoDictType::DICT_7X7_1000}, + {"ARUCO_ORIGINAL", ArucoDictType::DICT_ARUCO_ORIGINAL}, + {"APRILTAG_16h5", ArucoDictType::DICT_APRILTAG_16h5}, + {"APRILTAG_25h9", ArucoDictType::DICT_APRILTAG_25h9}, + {"APRILTAG_36h10", ArucoDictType::DICT_APRILTAG_36h10}, + {"APRILTAG_36h11", ArucoDictType::DICT_APRILTAG_36h11}, }; } // namespace aruco_opencv