diff --git a/stereo_image_proc/cfg/Disparity.cfg b/stereo_image_proc/cfg/Disparity.cfg deleted file mode 100755 index 5da257c41..000000000 --- a/stereo_image_proc/cfg/Disparity.cfg +++ /dev/null @@ -1,39 +0,0 @@ -#! /usr/bin/env python - -# Declare parameters that control stereo processing - -PACKAGE='stereo_image_proc' - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -stereo_algo_enum = gen.enum([gen.const("StereoBM", int_t, 0, "Block Matching"), - gen.const("StereoSGBM", int_t, 1, "SemiGlobal Block Matching")], - "stereo algorithm") -gen.add("stereo_algorithm", int_t, 0, "stereo algorithm", 0, 0, 1, - edit_method = stereo_algo_enum) -# disparity block matching pre-filtering parameters -gen.add("prefilter_size", int_t, 0, "Normalization window size, pixels", 9, 5, 255) -gen.add("prefilter_cap", int_t, 0, "Bound on normalized pixel values", 31, 1, 63) - -# disparity block matching correlation parameters -gen.add("correlation_window_size", int_t, 0, "SAD correlation window width, pixels", 15, 5, 255) -gen.add("min_disparity", int_t, 0, "Disparity to begin search at, pixels (may be negative)", 0, -128, 128) -gen.add("disparity_range", int_t, 0, "Number of disparities to search, pixels", 64, 32, 256) -# TODO What about trySmallerWindows? - -# disparity block matching post-filtering parameters -# NOTE: Making uniqueness_ratio int_t instead of double_t to work around dynamic_reconfigure gui issue -gen.add("uniqueness_ratio", double_t, 0, "Filter out if best match does not sufficiently exceed the next-best match", 15, 0, 100) -gen.add("texture_threshold", int_t, 0, "Filter out if SAD window response does not exceed texture threshold", 10, 0, 10000) -gen.add("speckle_size", int_t, 0, "Reject regions smaller than this size, pixels", 100, 0, 1000) -gen.add("speckle_range", int_t, 0, "Max allowed difference between detected disparities", 4, 0, 31) -gen.add("fullDP", bool_t, 0, "Run the full variant of the algorithm, only available in SGBM", False) -gen.add("P1", double_t, 0, "The first parameter controlling the disparity smoothness, only available in SGBM", 200, 0, 4000) -gen.add("P2", double_t, 0, "The second parameter controlling the disparity smoothness., only available in SGBM", 400, 0, 4000) -gen.add("disp12MaxDiff", int_t, 0, "Maximum allowed difference (in integer pixel units) in the left-right disparity check, only available in SGBM", 0, 0, 128) -# First string value is node name, used only for generating documentation -# Second string value ("Disparity") is name of class and generated -# .h file, with "Config" added, so class DisparityConfig -exit(gen.generate(PACKAGE, "stereo_image_proc", "Disparity")) 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 3f4f1822e..1427b91d9 100644 --- a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp +++ b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp @@ -47,7 +47,12 @@ #include #include +#include #include +#include +#include +#include +#include namespace stereo_image_proc { @@ -58,6 +63,12 @@ class DisparityNode : public rclcpp::Node explicit DisparityNode(const rclcpp::NodeOptions & options); private: + enum StereoAlgorithm + { + BLOCK_MATCHING = 0, + SEMI_GLOBAL_BLOCK_MATCHING + }; + // Subscriptions image_transport::SubscriberFilter sub_l_image_, sub_r_image_; message_filters::Subscriber sub_l_info_, sub_r_info_; @@ -91,9 +102,49 @@ class DisparityNode : public rclcpp::Node const sensor_msgs::msg::Image::ConstSharedPtr & r_image_msg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr & r_info_msg); - // void configCb(Config &config, uint32_t level); + rcl_interfaces::msg::SetParametersResult parameterSetCb( + const std::vector & parameters); }; +// Some helper functions for adding a parameter to a collection +static void add_param_to_map( + std::map> & parameters, + const std::string & name, + const std::string & description, + const int default_value, + const int from_value, + const int to_value, + const int step) +{ + rcl_interfaces::msg::IntegerRange integer_range; + integer_range.from_value = from_value; + integer_range.to_value = to_value; + integer_range.step = step; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = description; + descriptor.integer_range = {integer_range}; + parameters[name] = std::make_pair(default_value, descriptor); +} + +static void add_param_to_map( + std::map> & parameters, + const std::string & name, + const std::string & description, + const double default_value, + const double from_value, + const double to_value, + const double step) +{ + rcl_interfaces::msg::FloatingPointRange floating_point_range; + floating_point_range.from_value = from_value; + floating_point_range.to_value = to_value; + floating_point_range.step = step; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = description; + descriptor.floating_point_range = {floating_point_range}; + parameters[name] = std::make_pair(default_value, descriptor); +} + DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) : rclcpp::Node("disparity_node", options) { @@ -120,8 +171,92 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) std::bind(&DisparityNode::imageCb, this, _1, _2, _3, _4)); } - // TODO(jacobperron): Setup equivalent of dynamic reconfigure with ROS 2 parameters - // See configCb + // Register a callback for when parameters are set + this->set_on_parameters_set_callback(std::bind(&DisparityNode::parameterSetCb, this, _1)); + + // Describe int parameters + std::map> int_params; + add_param_to_map( + int_params, + "stereo_algorithm", + "Stereo algorithm: Block Matching (0) or Semi-Global Block Matching (1)", + 0, 0, 1, 1); // default, from, to, step + add_param_to_map( + int_params, + "prefilter_size", + "Normalization window size in pixels (must be odd)", + 9, 5, 255, 2); + add_param_to_map( + int_params, + "prefilter_cap", + "Bound on normalized pixel values", + 31, 1, 63, 1); + add_param_to_map( + int_params, + "correlation_window_size", + "SAD correlation window width in pixels (must be odd)", + 15, 5, 255, 2); + add_param_to_map( + int_params, + "min_disparity", + "Disparity to begin search at in pixels", + 0, -128, 128, 1); + add_param_to_map( + int_params, + "disparity_range", + "Number of disparities to search in pixels (must be a multiple of 16)", + 64, 32, 256, 16); + add_param_to_map( + int_params, + "texture_ratio", + "Filter out if SAD window response does not exceed texture threshold", + 10, 0, 10000, 1); + add_param_to_map( + int_params, + "speckle_size", + "Reject regions smaller than this size in pixels", + 100, 0, 1000, 1); + add_param_to_map( + int_params, + "speckle_range", + "Maximum allowed difference between detected disparities", + 4, 0, 31, 1); + add_param_to_map( + int_params, + "disp12_max_diff", + "Maximum allowed difference in the left-right disparity check in pixels" + " (Semi-Global Block Matching only)", + 0, 0, 128, 1); + + // Describe double parameters + std::map> double_params; + add_param_to_map( + double_params, + "uniqueness_ratio", + "Filter out if best match does not sufficiently exceed the next-best match", + 15.0, 0.0, 100.0, 0.0); + add_param_to_map( + double_params, + "P1", + "The first parameter ccontrolling the disparity smoothness (Semi-Global Block Matching only)", + 200.0, 0.0, 4000.0, 0.0); + add_param_to_map( + double_params, + "P2", + "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); pub_disparity_ = create_publisher("disparity", 1); @@ -190,36 +325,55 @@ void DisparityNode::imageCb( pub_disparity_->publish(*disp_msg); } -// void DisparityNode::configCb(Config &config, uint32_t level) -// { -// // Tweak all settings to be valid -// config.prefilter_size |= 0x1; // must be odd -// config.correlation_window_size |= 0x1; // must be odd -// config.disparity_range = (config.disparity_range / 16) * 16; // must be multiple of 16 -// -// // check stereo method -// // Note: With single-threaded NodeHandle, configCb and imageCb can't be called -// // concurrently, so this is thread-safe. -// block_matcher_.setPreFilterCap(config.prefilter_cap); -// block_matcher_.setCorrelationWindowSize(config.correlation_window_size); -// block_matcher_.setMinDisparity(config.min_disparity); -// block_matcher_.setDisparityRange(config.disparity_range); -// block_matcher_.setUniquenessRatio(config.uniqueness_ratio); -// block_matcher_.setSpeckleSize(config.speckle_size); -// block_matcher_.setSpeckleRange(config.speckle_range); -// if (config.stereo_algorithm == stereo_image_proc::Disparity_StereoBM) { // StereoBM -// block_matcher_.setStereoType(StereoProcessor::BM); -// block_matcher_.setPreFilterSize(config.prefilter_size); -// block_matcher_.setTextureThreshold(config.texture_threshold); -// } -// else if (config.stereo_algorithm == stereo_image_proc::Disparity_StereoSGBM) { // StereoSGBM -// block_matcher_.setStereoType(StereoProcessor::SGBM); -// block_matcher_.setSgbmMode(config.fullDP); -// block_matcher_.setP1(config.P1); -// block_matcher_.setP2(config.P2); -// block_matcher_.setDisp12MaxDiff(config.disp12MaxDiff); -// } -// } +rcl_interfaces::msg::SetParametersResult DisparityNode::parameterSetCb( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & param : parameters) { + const std::string param_name = param.get_name(); + if ("stereo_algorithm" == param_name) { + const int stereo_algorithm_value = param.as_int(); + if (BLOCK_MATCHING == stereo_algorithm_value) { + block_matcher_.setStereoType(StereoProcessor::BM); + } else if (SEMI_GLOBAL_BLOCK_MATCHING == stereo_algorithm_value) { + block_matcher_.setStereoType(StereoProcessor::SGBM); + } else { + result.successful = false; + std::ostringstream oss; + oss << "Unknown stereo algorithm type '" << stereo_algorithm_value << "'"; + result.reason = oss.str(); + } + } else if ("prefilter_size" == param_name) { + block_matcher_.setPreFilterSize(param.as_int()); + } else if ("prefilter_cap" == param_name) { + block_matcher_.setPreFilterCap(param.as_int()); + } else if ("correlation_window_size" == param_name) { + block_matcher_.setCorrelationWindowSize(param.as_int()); + } else if ("min_disparity" == param_name) { + block_matcher_.setMinDisparity(param.as_int()); + } else if ("disparity_range" == param_name) { + block_matcher_.setDisparityRange(param.as_int()); + } else if ("uniqueness_ratio" == param_name) { + block_matcher_.setUniquenessRatio(param.as_double()); + } else if ("texture_threshold" == param_name) { + block_matcher_.setTextureThreshold(param.as_int()); + } else if ("speckle_size" == param_name) { + 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 ("P1" == param_name) { + block_matcher_.setP1(param.as_double()); + } else if ("P2" == param_name) { + block_matcher_.setP2(param.as_double()); + } else if ("disp12_max_diff" == param_name) { + block_matcher_.setDisp12MaxDiff(param.as_int()); + } + } + return result; +} } // namespace stereo_image_proc