Skip to content

Commit

Permalink
support melodic
Browse files Browse the repository at this point in the history
  • Loading branch information
@knorth55
knorth55 committed Dec 9, 2024
1 parent b464e79 commit 887ad0a
Show file tree
Hide file tree
Showing 2 changed files with 91 additions and 95 deletions.
16 changes: 10 additions & 6 deletions jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h
View file
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 @@ -63,13 +67,13 @@ namespace jsk_pcl_ros

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

virtual void filter(const sensor_msgs::PointCloud2::ConstPtr& msg);
virtual void filterCloud(const pcl::PCLPointCloud2::Ptr pcl_cloud,
pcl::PointIndices::Ptr pcl_indices_filtered,
bool keep_organized);
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
170 changes: 81 additions & 89 deletions jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,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 @@ -80,107 +80,102 @@ namespace jsk_pcl_ros
std_mul_ = config.stddev;
}

void OrganizedStatisticalOutlierRemoval::filterCloud(
void OrganizedStatisticalOutlierRemoval::filter(
const pcl::PCLPointCloud2::Ptr pcl_cloud,
pcl::PointIndices::Ptr pcl_indices_filtered,
bool keep_organized)
pcl::PointIndices::Ptr pcl_indices_filtered)
{
#if PCL_VERSION_COMPARE (<, 1, 9, 0)
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(*pcl_cloud, *cloud);
if (keep_organized) {
// 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> indices_filtered;
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)
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))
{
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++;
distances[cp] = 0;
continue;
}

// 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)
if (tree->nearestKSearch (indices[cp], mean_k_, nn_indices, nn_dists) == 0)
{
sum += distances[i];
sq_sum += distances[i] * distances[i];
distances[cp] = 0;
continue;
}
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);
// 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++;
}

double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier
// 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)
// Build a new cloud by neglecting outliers
for (int cp = 0; cp < static_cast<int> (indices.size ()); ++cp)
{
bool remove_point = false;
if (negative_)
{
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]);
}
remove_point = (distances[cp] <= distance_threshold);
}
else
{
remove_point = (distances[cp] > distance_threshold);
}
if (!remove_point)
{
indices_filtered.push_back(indices[cp]);
}
pcl_indices_filtered->indices = indices_filtered;
}
else
{
pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> sor;
sor.setInputCloud(pcl_cloud);
sor.setMeanK(mean_k_);
sor.setStddevMulThresh(std_mul_);
sor.setNegative(negative_);
sor.filter(pcl_indices_filtered->indices);
}
#else
pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> sor;
sor.setInputCloud(pcl_cloud);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud);
sor.setMeanK(mean_k_);
sor.setStddevMulThresh(std_mul_);
sor.setNegative(negative_);
sor.setKeepOrganized (keep_organized);
sor.filter(pcl_indices_filtered->indices);
sor.setKeepOrganized (true);
sor.filter(indices_filtered);
#endif
return indices_filtered;
}

void OrganizedStatisticalOutlierRemoval::filter(const sensor_msgs::PointCloud2::ConstPtr& msg)
void OrganizedStatisticalOutlierRemoval::filterCloud(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
vital_checker_->poke();
Expand All @@ -195,10 +190,7 @@ namespace jsk_pcl_ros

// filter
pcl::PointIndices::Ptr pcl_indices_filtered (new pcl::PointIndices());
pcl_indices_filtered->indices.resize(pcl_cloud->data.size());
OrganizedStatisticalOutlierRemoval::filterCloud(pcl_cloud,
pcl_indices_filtered,
keep_organized);
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);
Expand All @@ -213,13 +205,13 @@ namespace jsk_pcl_ros
output.is_bigendian = msg->is_bigendian;
output.fields = msg->fields;
output.point_step = msg->point_step;
output.data.resize (msg->data.size ());
output.data.resize(msg->data.size());
output.width = msg->width;
output.height = msg->height;
output.row_step = msg->point_step * msg->width;
}
#endif
output.header = msg->header;
output.row_step = output.point_step * output.width;
output.is_dense = !keep_organized;
pub_.publish(output);
diagnostic_updater_->update();
Expand Down

0 comments on commit 887ad0a

Please sign in to comment.