From 2cd334c474e3ed04ef65ca1ba7f08ab535a3dc6d Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 29 Jan 2020 05:17:39 +0300 Subject: [PATCH] aruco_pose: dynamic reconfiguration of aruco detector (#180) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * aruco_detect: dynamic reconfiguration * aruco_pose: Depend on dynamic_reconfigure * aruco_pose: Use c++11 features instead of Boost * aruco_pose: Rearrange parameters, reset to OpenCV defaults * Update constrains for some parameters * aruco_pose: don’t hard-cord defaults for dynamic reconfigure * aruco_pose: add missing parameters * aruco_pose: fix tests * aruco_pose: typo * aruco_pose: fix * aruco_pose: fix test * aruco_pose: hardcode some new dynamic reconfigure parameters Co-authored-by: Alexey Rogachevskiy Co-authored-by: Arthur Golubtsov --- aruco_pose/CMakeLists.txt | 10 +-- aruco_pose/cfg/DetectorParams.cfg | 107 ++++++++++++++++++++++++++++++ aruco_pose/package.xml | 1 + aruco_pose/src/aruco_detect.cpp | 42 ++++++++++++ aruco_pose/test/basic.test | 1 + 5 files changed, 156 insertions(+), 5 deletions(-) create mode 100755 aruco_pose/cfg/DetectorParams.cfg diff --git a/aruco_pose/CMakeLists.txt b/aruco_pose/CMakeLists.txt index 30061a9b..cdd55fd4 100644 --- a/aruco_pose/CMakeLists.txt +++ b/aruco_pose/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(catkin REQUIRED COMPONENTS tf2_geometry_msgs sensor_msgs message_generation + dynamic_reconfigure ) find_package(OpenCV 3 REQUIRED COMPONENTS core imgproc calib3d) @@ -111,10 +112,9 @@ generate_messages( ## and list every .cfg file to be processed ## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) +generate_dynamic_reconfigure_options( + cfg/DetectorParams.cfg +) ################################### ## catkin specific configuration ## @@ -151,7 +151,7 @@ add_library(aruco_pose src/draw.cpp ) -add_dependencies(${PROJECT_NAME} aruco_pose_generate_messages_cpp) +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp ${PROJECT_NAME}_gencfg) ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context diff --git a/aruco_pose/cfg/DetectorParams.cfg b/aruco_pose/cfg/DetectorParams.cfg new file mode 100755 index 00000000..c96c942b --- /dev/null +++ b/aruco_pose/cfg/DetectorParams.cfg @@ -0,0 +1,107 @@ +#!/usr/bin/env python +PACKAGE = "aruco_pose" + +from dynamic_reconfigure.parameter_generator_catkin import * +import cv2.aruco + +p = cv2.aruco.DetectorParameters_create() + +gen = ParameterGenerator() + +gen.add("adaptiveThreshConstant", double_t, 0, + "Constant for adaptive thresholding before finding contours", + p.adaptiveThreshConstant, 0, 100) + +gen.add("adaptiveThreshWinSizeMin", int_t, 0, + "Minimum window size for adaptive thresholding before finding contours", + p.adaptiveThreshWinSizeMin, 1, 100) + +gen.add("adaptiveThreshWinSizeMax", int_t, 0, + "Maximum window size for adaptive thresholding before finding contours", + p.adaptiveThreshWinSizeMax, 1, 100) + +gen.add("adaptiveThreshWinSizeStep", int_t, 0, + "Increments from adaptiveThreshWinSizeMin to adaptiveThreshWinSizeMax during the thresholding", + p.adaptiveThreshWinSizeStep, 1, 100) + +gen.add("cornerRefinementMaxIterations", int_t, 0, + "Maximum number of iterations for stop criteria of the corner refinement process", + p.cornerRefinementMaxIterations, 1, 1000) + +corner_refine_enum = gen.enum([ gen.const("CORNER_REFINE_NONE", int_t, 0, "No refinement"), + gen.const("CORNER_REFINE_SUBPIX", int_t, 1, "Do subpixel refinement"), + gen.const("CORNER_REFINE_CONTOUR", int_t, 2, "Use contour-Points"), + gen.const("CORNER_REFINE_APRILTAG", int_t, 3, "Use the AprilTag2 approach")], + "An enum to set corner refinement method") + +gen.add("cornerRefinementMethod", int_t, 0, "Corner refinement method", 0, 0, 3, edit_method=corner_refine_enum) + +gen.add("cornerRefinementMinAccuracy", double_t, 0, + "Minimum error for the stop criteria of the corner refinement process", + p.cornerRefinementMinAccuracy, 0, 1) + +gen.add("cornerRefinementWinSize", int_t, 0, + "Window size for the corner refinement process (in pixels)", + p.cornerRefinementWinSize, 1, 100) + +gen.add("detectInvertedMarker", bool_t, 0, + "check if there is a white marker. In order to generate a 'white' marker just invert a normal marker by using a tilde", + False) + +gen.add("errorCorrectionRate", double_t, 0, + "Error correction rate respect to the maximum error correction capability for each dictionary", + p.errorCorrectionRate, 0, 1) + +gen.add("minCornerDistanceRate", double_t, 0, + "Minimum distance between corners for detected markers relative to its perimeter", + p.minCornerDistanceRate, 0, 0.25) + +gen.add("markerBorderBits", int_t, 0, + "Number of bits of the marker border, i.e. marker border width", + p.markerBorderBits, 1, 10) + +gen.add("maxErroneousBitsInBorderRate", double_t, 0, + "Maximum number of accepted erroneous bits in the border (i.e. number of allowed white bits in the border)", + p.maxErroneousBitsInBorderRate, 0, 1) + +gen.add("minDistanceToBorder", int_t, 0, + "Minimum distance of any corner to the image border for detected markers (in pixels)", + p.minDistanceToBorder, 0, 1000) + +gen.add("minMarkerDistanceRate", double_t, 0, + "minimum mean distance beetween two marker corners to be considered similar, so that the smaller one is removed. The rate is relative to the smaller perimeter of the two markers", + p.minMarkerDistanceRate, 0, 1) + +gen.add("minMarkerPerimeterRate", double_t, 0, + "Determine minimum perimeter for marker contour to be detected. This is defined as a rate respect to the maximum dimension of the input image", + p.minMarkerPerimeterRate, 0, 4) + +gen.add("maxMarkerPerimeterRate", double_t, 0, + "Determine maximum perimeter for marker contour to be detected. This is defined as a rate respect to the maximum dimension of the input image", + p.maxMarkerPerimeterRate, 0, 4) + +gen.add("minOtsuStdDev", double_t, 0, + "Minimun standard deviation in pixels values during the decodification step to apply Otsu thresholding (otherwise, all the bits are set to 0 or 1 depending on mean higher than 128 or not)", + p.minOtsuStdDev, 0, 100) + +gen.add("perspectiveRemoveIgnoredMarginPerCell", double_t, 0, + "Width of the margin of pixels on each cell not considered for the determination of the cell bit. Represents the rate respect to the total size of the cell, i.e. perpectiveRemovePixelPerCell", + p.perspectiveRemoveIgnoredMarginPerCell, 0, 1) + +gen.add("perspectiveRemovePixelPerCell", int_t, 0, + "Number of bits (per dimension) for each cell of the marker when removing the perspective", + p.perspectiveRemovePixelPerCell, 1, 100) + +gen.add("polygonalApproxAccuracyRate", double_t, 0, + "Minimum accuracy during the polygonal approximation process to determine which contours are squares", + p.polygonalApproxAccuracyRate, 0, 1) + +gen.add("aprilTagQuadDecimate", double_t, 0, + "Detection of quads can be done on a lower-resolution image, improving speed at a cost of pose accuracy and a slight decrease in detection rate. Decoding the binary payload is still done at full resolution", + 0, 0, 1000) + +gen.add("aprilTagQuadSigma", double_t, 0, + "What Gaussian blur should be applied to the segmented image (used for quad detection?) Parameter is the standard deviation in pixels. Very noisy images benefit from non-zero values", + 0, 0, 1000) + +exit(gen.generate(PACKAGE, "aruco_pose", "Detector")) diff --git a/aruco_pose/package.xml b/aruco_pose/package.xml index c47f784e..a525bc4d 100644 --- a/aruco_pose/package.xml +++ b/aruco_pose/package.xml @@ -27,6 +27,7 @@ visualization_msgs sensor_msgs rostest + dynamic_reconfigure image_publisher ros_pytest diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp index cf15bc04..65953e53 100644 --- a/aruco_pose/src/aruco_detect.cpp +++ b/aruco_pose/src/aruco_detect.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -46,8 +47,11 @@ #include #include +#include #include "utils.h" +#include +#include using std::vector; using cv::Mat; @@ -58,6 +62,7 @@ private: tf2_ros::TransformBroadcaster br_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_{tf_buffer_}; + std::shared_ptr> dyn_srv_; cv::Ptr dictionary_; cv::Ptr parameters_; image_transport::Publisher debug_pub_; @@ -110,6 +115,12 @@ public: vis_markers_pub_ = nh_priv_.advertise("visualization", 1); img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, this); + dyn_srv_ = std::make_shared>(nh_priv_); + dynamic_reconfigure::Server::CallbackType cb; + + cb = std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2); + dyn_srv_->setCallback(cb); + NODELET_INFO("ready"); } @@ -341,6 +352,37 @@ private: map_markers_ids_.insert(marker.id); } } + + void paramCallback(aruco_pose::DetectorConfig &config, uint32_t level) + { + parameters_->adaptiveThreshConstant = config.adaptiveThreshConstant; + parameters_->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin; + parameters_->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax; + parameters_->adaptiveThreshWinSizeStep = config.adaptiveThreshWinSizeStep; + parameters_->cornerRefinementMaxIterations = config.cornerRefinementMaxIterations; + parameters_->cornerRefinementMethod = config.cornerRefinementMethod; + parameters_->cornerRefinementMinAccuracy = config.cornerRefinementMinAccuracy; + parameters_->cornerRefinementWinSize = config.cornerRefinementWinSize; +#if ((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR >= 4) && (CV_VERSION_REVISION >= 7)) || (CV_VERSION_MAJOR > 3) + parameters_->detectInvertedMarker = config.detectInvertedMarker; +#endif + parameters_->errorCorrectionRate = config.errorCorrectionRate; + parameters_->minCornerDistanceRate = config.minCornerDistanceRate; + parameters_->markerBorderBits = config.markerBorderBits; + parameters_->maxErroneousBitsInBorderRate = config.maxErroneousBitsInBorderRate; + parameters_->minDistanceToBorder = config.minDistanceToBorder; + parameters_->minMarkerDistanceRate = config.minMarkerDistanceRate; + parameters_->minMarkerPerimeterRate = config.minMarkerPerimeterRate; + parameters_->maxMarkerPerimeterRate = config.maxMarkerPerimeterRate; + parameters_->minOtsuStdDev = config.minOtsuStdDev; + parameters_->perspectiveRemoveIgnoredMarginPerCell = config.perspectiveRemoveIgnoredMarginPerCell; + parameters_->perspectiveRemovePixelPerCell = config.perspectiveRemovePixelPerCell; + parameters_->polygonalApproxAccuracyRate = config.polygonalApproxAccuracyRate; +#if ((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR >= 4) && (CV_VERSION_REVISION >= 2)) || (CV_VERSION_MAJOR > 3) + parameters_->aprilTagQuadDecimate = config.aprilTagQuadDecimate; + parameters_->aprilTagQuadSigma = config.aprilTagQuadSigma; +#endif + } }; PLUGINLIB_EXPORT_CLASS(ArucoDetect, nodelet::Nodelet) diff --git a/aruco_pose/test/basic.test b/aruco_pose/test/basic.test index ac9099ca..c7d90178 100644 --- a/aruco_pose/test/basic.test +++ b/aruco_pose/test/basic.test @@ -14,6 +14,7 @@ +