apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add the StatisticalOutlierRemoval filter. (#400)

Browse Source
This commit is contained in:
Andrew Symington 2023-02-21 10:35:21 -08:00 committed by GitHub
parent 628aaec1dc
commit de15639154
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 117 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
pcl_ros/CMakeLists.txt
View File

@ -87,7 +87,7 @@ add_library(pcl_ros_filters SHARED
src/pcl_ros/filters/passthrough.cpp
src/pcl_ros/filters/project_inliers.cpp
src/pcl_ros/filters/radius_outlier_removal.cpp
# src/pcl_ros/filters/statistical_outlier_removal.cpp
src/pcl_ros/filters/statistical_outlier_removal.cpp
# src/pcl_ros/filters/voxel_grid.cpp
# src/pcl_ros/filters/crop_box.cpp
)
@ -109,6 +109,10 @@ rclcpp_components_register_node(pcl_ros_filters
PLUGIN "pcl_ros::RadiusOutlierRemoval"
EXECUTABLE filter_radius_outlier_removal_node
)
rclcpp_components_register_node(pcl_ros_filters
PLUGIN "pcl_ros::StatisticalOutlierRemoval"
EXECUTABLE filter_statistical_outlier_removal_node
)
class_loader_hide_library_symbols(pcl_ros_filters)
#
### Declare the pcl_ros_segmentation library

29
pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp
View File

@ -40,11 +40,9 @@
// PCL includes
#include <pcl/filters/statistical_outlier_removal.h>
#include <vector>
#include "pcl_ros/filters/filter.hpp"
// Dynamic reconfigure
#include "pcl_ros/StatisticalOutlierRemovalConfig.hpp"
namespace pcl_ros
{
/** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more
@ -61,9 +59,6 @@ namespace pcl_ros
class StatisticalOutlierRemoval : public Filter
{
protected:
/** \brief Pointer to a dynamic reconfigure service. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>> srv_;
/** \brief Call the actual filter.
* \param input the input point cloud dataset
* \param indices the input set of indices to use from \a input
@ -71,10 +66,10 @@ protected:
*/
inline void
filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output)
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
PointCloud2 & output) override
{
boost::mutex::scoped_lock lock(mutex_);
std::lock_guard<std::mutex> lock(mutex_);
pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*(input), *(pcl_input));
impl_.setInputCloud(pcl_input);
@ -84,17 +79,13 @@ protected:
pcl_conversions::moveFromPCL(pcl_output, output);
}
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
/** \brief Parameter callback
* \param params parameter values to set
*/
bool child_init(ros::NodeHandle & nh, bool & has_service);
rcl_interfaces::msg::SetParametersResult
config_callback(const std::vector<rclcpp::Parameter> & params);
/** \brief Dynamic reconfigure callback
* \param config the config object
* \param level the dynamic reconfigure level
*/
void config_callback(pcl_ros::StatisticalOutlierRemovalConfig & config, uint32_t level);
OnSetParametersCallbackHandle::SharedPtr callback_handle_;
private:
/** \brief The PCL filter implementation used. */
@ -102,6 +93,8 @@ private:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
explicit StatisticalOutlierRemoval(const rclcpp::NodeOptions & options);
};
} // namespace pcl_ros

126
pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp
View File

@ -35,53 +35,105 @@
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/filters/statistical_outlier_removal.hpp"
//////////////////////////////////////////////////////////////////////////////////////////////
bool
pcl_ros::StatisticalOutlierRemoval::child_init(ros::NodeHandle & nh, bool & has_service)
pcl_ros::StatisticalOutlierRemoval::StatisticalOutlierRemoval(const rclcpp::NodeOptions & options)
: Filter("StatisticalOutlierRemovalNode", options)
{
// Enable the dynamic reconfigure service
has_service = true;
srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>>(
nh);
dynamic_reconfigure::Server<pcl_ros::StatisticalOutlierRemovalConfig>::CallbackType f =
boost::bind(&StatisticalOutlierRemoval::config_callback, this, _1, _2);
srv_->setCallback(f);
rcl_interfaces::msg::ParameterDescriptor mean_k_desc;
mean_k_desc.name = "mean_k";
mean_k_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
mean_k_desc.description =
"The number of points (k) to use for mean distance estimation.";
{
rcl_interfaces::msg::IntegerRange int_range;
int_range.from_value = 2;
int_range.to_value = 100;
mean_k_desc.integer_range.push_back(int_range);
}
declare_parameter(mean_k_desc.name, rclcpp::ParameterValue(2), mean_k_desc);
return true;
rcl_interfaces::msg::ParameterDescriptor stddev_desc;
stddev_desc.name = "stddev";
stddev_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
stddev_desc.description =
"The standard deviation multiplier threshold."
"All points outside the mean +- sigma * std_mul will be considered outliers.";
{
rcl_interfaces::msg::FloatingPointRange float_range;
float_range.from_value = 0.0;
float_range.to_value = 5.0;
stddev_desc.floating_point_range.push_back(float_range);
}
declare_parameter(stddev_desc.name, rclcpp::ParameterValue(0.0), stddev_desc);
rcl_interfaces::msg::ParameterDescriptor negative_desc;
negative_desc.name = "negative";
negative_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
negative_desc.description =
"Set whether the inliers should be returned (true) or the outliers (false).";
declare_parameter(negative_desc.name, rclcpp::ParameterValue(false), negative_desc);
const std::vector<std::string> param_names {
mean_k_desc.name,
stddev_desc.name,
negative_desc.name,
};
callback_handle_ =
add_on_set_parameters_callback(
std::bind(
&StatisticalOutlierRemoval::config_callback, this,
std::placeholders::_1));
config_callback(get_parameters(param_names));
// TODO(daisukes): lazy subscription after rclcpp#2060
subscribe();
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::StatisticalOutlierRemoval::config_callback(
pcl_ros::StatisticalOutlierRemovalConfig & config, uint32_t level)
rcl_interfaces::msg::SetParametersResult
pcl_ros::StatisticalOutlierRemoval::config_callback(const std::vector<rclcpp::Parameter> & params)
{
boost::mutex::scoped_lock lock(mutex_);
std::lock_guard<std::mutex> lock(mutex_);
if (impl_.getMeanK() != config.mean_k) {
impl_.setMeanK(config.mean_k);
NODELET_DEBUG(
"[%s::config_callback] Setting the number of points (k) to use for mean "
"distance estimation to: %d.",
getName().c_str(), config.mean_k);
for (const rclcpp::Parameter & param : params) {
if (param.get_name() == "mean_k") {
if (impl_.getMeanK() != param.as_int()) {
RCLCPP_DEBUG(
get_logger(),
"Setting the number of points (k) to use for mean distance estimation to: %ld.",
param.as_int());
impl_.setMeanK(param.as_int());
}
}
if (param.get_name() == "stddev") {
if (impl_.getStddevMulThresh() != param.as_double()) {
RCLCPP_DEBUG(
get_logger(),
"Setting the standard deviation multiplier threshold to: %f.",
param.as_double());
impl_.setStddevMulThresh(param.as_double());
}
}
if (param.get_name() == "negative") {
if (impl_.getNegative() != param.as_bool()) {
RCLCPP_DEBUG(
get_logger(),
"Returning only inliers: %s.",
(param.as_bool() ? "false" : "true"));
impl_.setNegative(param.as_bool());
}
}
}
if (impl_.getStddevMulThresh() != config.stddev) {
impl_.setStddevMulThresh(config.stddev);
NODELET_DEBUG(
"[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.",
getName().c_str(), config.stddev);
}
if (impl_.getNegative() != config.negative) {
impl_.setNegative(config.negative);
NODELET_DEBUG(
"[%s::config_callback] Returning only inliers: %s.",
getName().c_str(), config.negative ? "false" : "true");
}
// Range constraints are enforced by rclcpp::Parameter.
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
return result;
}
typedef pcl_ros::StatisticalOutlierRemoval StatisticalOutlierRemoval;
PLUGINLIB_EXPORT_CLASS(StatisticalOutlierRemoval, nodelet::Nodelet);
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::StatisticalOutlierRemoval)

12
pcl_ros/tests/filters/CMakeLists.txt
View File

@ -47,6 +47,12 @@ ament_add_pytest_test(test_pcl_ros::RadiusOutlierRemoval
FILTER_PLUGIN=pcl_ros::RadiusOutlierRemoval
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
)
ament_add_pytest_test(test_pcl_ros::StatisticalOutlierRemoval
test_filter_component.py
ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
FILTER_PLUGIN=pcl_ros::StatisticalOutlierRemoval
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
)
# test executables
ament_add_pytest_test(test_filter_extract_indices_node
@ -74,3 +80,9 @@ ament_add_pytest_test(test_filter_radius_outlier_removal_node
FILTER_EXECUTABLE=filter_radius_outlier_removal_node
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
)
ament_add_pytest_test(test_filter_statistical_outlier_removal_node
test_filter_executable.py
ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
FILTER_EXECUTABLE=filter_statistical_outlier_removal_node
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
)