DisparityNode: replace full_dp parameter with sgbm_mode (#945)

Previously, only the SGBM and HH modes were allowed
ros-perception · Feb 19, 2024 · 1ac3711 · 1ac3711
1 parent 14f604a
commit 1ac3711
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 13 deletions.
diff --git a/image_pipeline/doc/changelog.rst b/image_pipeline/doc/changelog.rst
@@ -38,3 +38,7 @@ There are several major change between ``Iron`` and ``Jazzy``:
    ``rgb/image_rect_color`` to ``rgb/image_raw`` to make clear that the
    unrectified camera projection matrix is used, and for consistency with
    other radial nodes.
+ * The boolen parameter ``full_dp`` from the DisparityNode has been deleted
+   and a new integer parameter ``sgbm_mode`` added to enable all the 
+   variations of the stereo matching algorithm SGBM available from the
+   OpenCV library.
diff --git a/stereo_image_proc/doc/components.rst b/stereo_image_proc/doc/components.rst
@@ -30,6 +30,14 @@ Published Topics
 Parameters
 ^^^^^^^^^^
 
+*Disparity algorithm variant*
+ * **sgbm_mode** (int, default: 0): Stereo matching algorithm variation:
+
+   * SGBM (0)
+   * HH (1)
+   * SGBM_3WAY (2)
+   * HH4 (3)
+
 *Disparity pre-filtering* 
 
  * **prefilter_size** (int, default: 9): Normalization window size, pixels.

diff --git a/stereo_image_proc/launch/stereo_image_proc.launch.py b/stereo_image_proc/launch/stereo_image_proc.launch.py
@@ -69,7 +69,7 @@ def generate_launch_description():
                 'uniqueness_ratio': LaunchConfiguration('uniqueness_ratio'),
                 'P1': LaunchConfiguration('P1'),
                 'P2': LaunchConfiguration('P2'),
-                'full_dp': LaunchConfiguration('full_dp'),
+                'sgbm_mode': LaunchConfiguration('sgbm_mode'),
             }],
             remappings=[
                 ('left/image_rect', [LaunchConfiguration('left_namespace'), '/image_rect']),
@@ -199,8 +199,8 @@ def generate_launch_description():
                         '(Semi-Global Block Matching only)'
         ),
         DeclareLaunchArgument(
-            name='full_dp', default_value='False',
-            description='Run the full variant of the algorithm (Semi-Global Block Matching only)'
+            name='sgbm_mode', default_value='0',
+            description='The mode of the SGBM matcher to be used'
         ),
         ComposableNodeContainer(
             condition=LaunchConfigurationEquals('container', ''),

diff --git a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp
@@ -258,6 +258,12 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options)
     "Maximum allowed difference in the left-right disparity check in pixels"
     " (Semi-Global Block Matching only)",
     0, 0, 128, 1);
+  add_param_to_map(
+    int_params,
+    "sgbm_mode",
+    "Mode of the SGBM stereo matcher."
+    "",
+    0, 0, 3, 1);
 
   // Describe double parameters
   std::map<std::string, std::pair<double, rcl_interfaces::msg::ParameterDescriptor>> double_params;
@@ -277,17 +283,9 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options)
     "The second parameter ccontrolling the disparity smoothness (Semi-Global Block Matching only)",
     400.0, 0.0, 4000.0, 0.0);
 
-  // Describe bool parameters
-  std::map<std::string, std::pair<bool, rcl_interfaces::msg::ParameterDescriptor>> bool_params;
-  rcl_interfaces::msg::ParameterDescriptor full_dp_descriptor;
-  full_dp_descriptor.description =
-    "Run the full variant of the algorithm (Semi-Global Block Matching only)";
-  bool_params["full_dp"] = std::make_pair(false, full_dp_descriptor);
-
   // Declaring parameters triggers the previously registered callback
   this->declare_parameters("", int_params);
   this->declare_parameters("", double_params);
-  this->declare_parameters("", bool_params);
 
   // Publisher options to allow reconfigurable qos settings and connect callback
   rclcpp::PublisherOptions pub_opts;
@@ -424,8 +422,8 @@ rcl_interfaces::msg::SetParametersResult DisparityNode::parameterSetCb(
       block_matcher_.setSpeckleSize(param.as_int());
     } else if ("speckle_range" == param_name) {
       block_matcher_.setSpeckleRange(param.as_int());
-    } else if ("full_dp" == param_name) {
-      block_matcher_.setSgbmMode(param.as_bool());
+    } else if ("sgbm_mode" == param_name) {
+      block_matcher_.setSgbmMode(param.as_int());
     } else if ("P1" == param_name) {
       block_matcher_.setP1(param.as_double());
     } else if ("P2" == param_name) {