Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[jsk_pcl/OrganizedStatisticalOutlierRemoval] support several pointcloud type #2846

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

[jsk_pcl/OrganizedStatisticalOutlierRemoval] support several pointcloud type #2846

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4beafbc
support various point cloud type
knorth55 May 11, 2024
da7f974
support melodic
knorth55 Dec 5, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 10 additions & 4 deletions jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,10 @@
#include <jsk_topic_tools/diagnostic_nodelet.h>
#include <jsk_topic_tools/counter.h>
#include <dynamic_reconfigure/server.h>

#include "sensor_msgs/PointCloud2.h"
#include "jsk_recognition_msgs/ClusterPointIndices.h"

#include "jsk_pcl_ros/OrganizedStatisticalOutlierRemovalConfig.h"

namespace jsk_pcl_ros
Expand All @@ -50,7 +54,6 @@ namespace jsk_pcl_ros
{
public:
typedef jsk_pcl_ros::OrganizedStatisticalOutlierRemovalConfig Config;
typedef pcl::PointXYZRGB PointT;
OrganizedStatisticalOutlierRemoval();
protected:
////////////////////////////////////////////////////////
Expand All @@ -64,10 +67,13 @@ namespace jsk_pcl_ros

virtual void configCallback (Config &config, uint32_t level);

virtual void filter(const sensor_msgs::PointCloud2::ConstPtr& msg);
void filter(const pcl::PCLPointCloud2::Ptr pcl_cloud,
pcl::PointIndices::Ptr pcl_indices_filtered);
std::vector<int> getFilteredIndices(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
void filterCloud(const sensor_msgs::PointCloud2::ConstPtr& msg);


virtual void updateDiagnostic(
diagnostic_updater::DiagnosticStatusWrapper &stat);
void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat);

////////////////////////////////////////////////////////
// ROS variables
Expand Down
231 changes: 115 additions & 116 deletions jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@
#include <jsk_topic_tools/diagnostic_utils.h>
#include <pcl/pcl_base.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl_conversions/pcl_conversions.h>
#include <jsk_recognition_utils/pcl_util.h>

namespace jsk_pcl_ros
Expand All @@ -62,7 +63,7 @@ namespace jsk_pcl_ros

void OrganizedStatisticalOutlierRemoval::subscribe()
{
sub_ = pnh_->subscribe("input", 1, &OrganizedStatisticalOutlierRemoval::filter, this);
sub_ = pnh_->subscribe("input", 1, &OrganizedStatisticalOutlierRemoval::filterCloud, this);
}

void OrganizedStatisticalOutlierRemoval::unsubscribe()
Expand All @@ -84,7 +85,102 @@ namespace jsk_pcl_ros
#define pcl_isfinite(x) std::isfinite(x)
#endif

void OrganizedStatisticalOutlierRemoval::filter(const sensor_msgs::PointCloud2::ConstPtr& msg)
void OrganizedStatisticalOutlierRemoval::filter(
const pcl::PCLPointCloud2::Ptr pcl_cloud,
pcl::PointIndices::Ptr pcl_indices_filtered)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(*pcl_cloud, *cloud);
pcl_indices_filtered->indices = OrganizedStatisticalOutlierRemoval::getFilteredIndices(cloud);
}

std::vector<int> OrganizedStatisticalOutlierRemoval::getFilteredIndices(
const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
std::vector<int> indices_filtered;
indices_filtered.resize(cloud->size());
#if PCL_VERSION_COMPARE (<, 1, 9, 0)
// Initialize the spatial locator
pcl::search::Search<pcl::PointXYZ>::Ptr tree;
tree.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ());
tree->setInputCloud (cloud);

// Allocate enough space to hold the results
std::vector<int> indices (cloud->size());
std::vector<int> nn_indices (mean_k_);
std::vector<float> nn_dists (mean_k_);
std::vector<float> distances (indices.size ());
int valid_distances = 0;

for (size_t i = 0; i < indices.size (); ++i) indices[i] = i;

// Go over all the points and calculate the mean or smallest distance
for (size_t cp = 0; cp < indices.size (); ++cp)
{
if (!pcl_isfinite (cloud->points[indices[cp]].x) ||
!pcl_isfinite (cloud->points[indices[cp]].y) ||
!pcl_isfinite (cloud->points[indices[cp]].z))
{
distances[cp] = 0;
continue;
}
if (tree->nearestKSearch (indices[cp], mean_k_, nn_indices, nn_dists) == 0)
{
distances[cp] = 0;
continue;
}
// Minimum distance (if mean_k_ == 2) or mean distance
double dist_sum = 0;
for (int j = 1; j < mean_k_; ++j)
dist_sum += sqrt (nn_dists[j]);
distances[cp] = static_cast<float> (dist_sum / (mean_k_ - 1));
valid_distances++;
}

// Estimate the mean and the standard deviation of the distance vector
double sum = 0, sq_sum = 0;
for (size_t i = 0; i < distances.size (); ++i)
{
sum += distances[i];
sq_sum += distances[i] * distances[i];
}
double mean = sum / static_cast<double>(valid_distances);
double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(valid_distances) - 1);
double stddev = sqrt (variance);
//getMeanStd (distances, mean, stddev);

double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier

// Build a new cloud by neglecting outliers
for (int cp = 0; cp < static_cast<int> (indices.size ()); ++cp)
{
bool remove_point = false;
if (negative_)
{
remove_point = (distances[cp] <= distance_threshold);
}
else
{
remove_point = (distances[cp] > distance_threshold);
}
if (!remove_point)
{
indices_filtered.push_back(indices[cp]);
}
}
#else
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud);
sor.setMeanK(mean_k_);
sor.setStddevMulThresh(std_mul_);
sor.setNegative(negative_);
sor.setKeepOrganized (true);
sor.filter(indices_filtered);
#endif
return indices_filtered;
}

void OrganizedStatisticalOutlierRemoval::filterCloud(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
vital_checker_->poke();
Expand All @@ -94,132 +190,35 @@ namespace jsk_pcl_ros
NODELET_ERROR("keep_organized parameter is true, but input pointcloud is not organized.");
}
bool keep_organized = keep_organized_ && !msg->is_dense;

pcl::PCLPointCloud2::Ptr pcl_cloud (new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*msg, *pcl_cloud);

// filter
pcl::PointIndices::Ptr pcl_indices_filtered (new pcl::PointIndices());
OrganizedStatisticalOutlierRemoval::filter(pcl_cloud, pcl_indices_filtered);
pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2);
pcl::ExtractIndices<pcl::PCLPointCloud2> ex;
ex.setInputCloud(pcl_cloud);
ex.setKeepOrganized(keep_organized);
ex.setNegative(false);
ex.setIndices(pcl_indices_filtered);
ex.filter(*pcl_cloud_filtered);
pcl_conversions::fromPCL(*pcl_cloud_filtered, output);
#if PCL_VERSION_COMPARE (<, 1, 9, 0)
if (keep_organized) {
// Send the input dataset to the spatial locator
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*msg, *cloud);

// Initialize the spatial locator
pcl::search::Search<pcl::PointXYZ>::Ptr tree;
tree.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ());
tree->setInputCloud (cloud);

// Allocate enough space to hold the results
std::vector<int> indices (cloud->size ());
std::vector<int> nn_indices (mean_k_);
std::vector<float> nn_dists (mean_k_);
std::vector<float> distances (indices.size ());
int valid_distances = 0;

for (size_t i = 0; i < indices.size (); ++i) indices[i] = i;

// Go over all the points and calculate the mean or smallest distance
for (size_t cp = 0; cp < indices.size (); ++cp)
{
if (!pcl_isfinite (cloud->points[indices[cp]].x) ||
!pcl_isfinite (cloud->points[indices[cp]].y) ||
!pcl_isfinite (cloud->points[indices[cp]].z))
{
distances[cp] = 0;
continue;
}
if (tree->nearestKSearch (indices[cp], mean_k_, nn_indices, nn_dists) == 0)
{
distances[cp] = 0;
continue;
}
// Minimum distance (if mean_k_ == 2) or mean distance
double dist_sum = 0;
for (int j = 1; j < mean_k_; ++j)
dist_sum += sqrt (nn_dists[j]);
distances[cp] = static_cast<float> (dist_sum / (mean_k_ - 1));
valid_distances++;
}

// Estimate the mean and the standard deviation of the distance vector
double sum = 0, sq_sum = 0;
for (size_t i = 0; i < distances.size (); ++i)
{
sum += distances[i];
sq_sum += distances[i] * distances[i];
}
double mean = sum / static_cast<double>(valid_distances);
double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(valid_distances) - 1);
double stddev = sqrt (variance);
//getMeanStd (distances, mean, stddev);

double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier

// Copy the common fields
output.is_bigendian = msg->is_bigendian;
output.fields = msg->fields;
output.point_step = msg->point_step;
output.data.resize (msg->data.size ());

// Build a new cloud by neglecting outliers
int nr_p = 0;
int nr_removed_p = 0;
bool remove_point = false;
for (int cp = 0; cp < static_cast<int> (indices.size ()); ++cp)
{
if (negative_)
remove_point = (distances[cp] <= distance_threshold);
else
remove_point = (distances[cp] > distance_threshold);
if (remove_point)
{
/* Set the current point to NaN. */
*(reinterpret_cast<float*>(&output.data[nr_p * output.point_step])+0) = std::numeric_limits<float>::quiet_NaN();
*(reinterpret_cast<float*>(&output.data[nr_p * output.point_step])+1) = std::numeric_limits<float>::quiet_NaN();
*(reinterpret_cast<float*>(&output.data[nr_p * output.point_step])+2) = std::numeric_limits<float>::quiet_NaN();
nr_p++;
}
else
{
memcpy (&output.data[nr_p * output.point_step],
&msg->data[indices[cp] * output.point_step],
output.point_step);
nr_p++;
}
}
output.data.resize(msg->data.size());
output.width = msg->width;
output.height = msg->height;
output.row_step = output.point_step * output.width;
}
else
{
pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
pcl::fromROSMsg(*msg, *cloud);
pcl::StatisticalOutlierRemoval<PointT> sor;
sor.setInputCloud(cloud);
sor.setMeanK (mean_k_);
sor.setStddevMulThresh (std_mul_);
sor.setNegative (negative_);
sor.filter (*cloud_filtered);
pcl::toROSMsg(*cloud_filtered, output);
}
output.header = msg->header;
output.is_dense = !keep_organized;
pub_.publish(output);
#else
pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
pcl::fromROSMsg(*msg, *cloud);
pcl::StatisticalOutlierRemoval<PointT> sor;
sor.setInputCloud(cloud);
sor.setMeanK (mean_k_);
sor.setStddevMulThresh (std_mul_);
sor.setNegative (negative_);
sor.setKeepOrganized (keep_organized);
sor.filter (*cloud_filtered);
pcl::toROSMsg(*cloud_filtered, output);
#endif
output.header = msg->header;
output.row_step = output.point_step * output.width;
output.is_dense = !keep_organized;
pub_.publish(output);
#endif
diagnostic_updater_->update();
}

Expand Down
Loading