aruco_pose: dynamic reconfiguration of aruco detector (#180)

* 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 <sfalexrog@gmail.com>
Co-authored-by: Arthur Golubtsov <goldartt@gmail.com>

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

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"))

aruco_pose/package.xml

@@ -27,6 +27,7 @@
   <depend>visualization_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>rostest</depend>
+  <depend>dynamic_reconfigure</depend>
 
   <test_depend>image_publisher</test_depend>
   <test_depend>ros_pytest</test_depend>

aruco_pose/src/aruco_detect.cpp

@@ -30,6 +30,7 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <image_transport/image_transport.h>
 #include <cv_bridge/cv_bridge.h>
+#include <dynamic_reconfigure/server.h>
 #include <geometry_msgs/Vector3.h>
 #include <geometry_msgs/Pose.h>
 #include <geometry_msgs/PoseStamped.h>
@@ -46,8 +47,11 @@
 
 #include <aruco_pose/Marker.h>
 #include <aruco_pose/MarkerArray.h>
+#include <aruco_pose/DetectorConfig.h>
 
 #include "utils.h"
+#include <memory>
+#include <functional>
 
 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<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>> dyn_srv_;
 	cv::Ptr<cv::aruco::Dictionary> dictionary_;
 	cv::Ptr<cv::aruco::DetectorParameters> parameters_;
 	image_transport::Publisher debug_pub_;
@@ -110,6 +115,12 @@ public:
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
 		img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, this);
 
+		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>>(nh_priv_);
+		dynamic_reconfigure::Server<aruco_pose::DetectorConfig>::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)

aruco_pose/test/basic.test

@@ -14,6 +14,7 @@
         <param name="length_override/3" value="0.1"/>
         <param name="estimate_poses" value="true"/>
         <param name="send_tf" value="true"/>
+        <param name="cornerRefinementMethod" value="1"/>
     </node>
 
     <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="true">