diff --git a/image_pipeline/doc/changelog.rst b/image_pipeline/doc/changelog.rst index b69fb088..c96ed210 100644 --- 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 index 5ac76213..8fe3ac66 100644 --- 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 index 8c2c81cb..d5834368 100644 --- 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 index 97a32f7a..a5f9c2d6 100644 --- 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> 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> 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) {