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 4, 2024
1 parent 799ff8e commit f088f31
Show file tree
Hide file tree
Showing 2 changed files with 53 additions and 27 deletions.
5 changes: 4 additions & 1 deletion 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 @@ -65,8 +65,11 @@ namespace jsk_pcl_ros

virtual void filter(const sensor_msgs::PointCloud2::ConstPtr& msg);
virtual void filterCloud(const pcl::PCLPointCloud2::Ptr pcl_cloud,
pcl::PointIndices::Ptr pcl_indices_filtered,
pcl::PCLPointCloud2::Ptr pcl_cloud_filtered,
bool keep_organized);
virtual void filterIndices(const pcl::PCLPointCloud2::Ptr pcl_cloud,
pcl::PointIndices::Ptr pcl_indices_filtered,
bool keep_organized);

virtual void updateDiagnostic(
diagnostic_updater::DiagnosticStatusWrapper &stat);
Expand Down
75 changes: 49 additions & 26 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 @@ -82,21 +82,19 @@ namespace jsk_pcl_ros

void OrganizedStatisticalOutlierRemoval::filterCloud(
const pcl::PCLPointCloud2::Ptr pcl_cloud,
pcl::PointIndices::Ptr pcl_indices_filtered,
pcl::PCLPointCloud2::Ptr pcl_cloud_filtered,
bool keep_organized)
{
#if PCL_VERSION_COMPARE (<, 1, 9, 0)
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(*pcl_cloud, *cloud);
if (keep_organized) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(*pcl_cloud, *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> indices_filtered;
std::vector<int> nn_indices (mean_k_);
std::vector<float> nn_dists (mean_k_);
std::vector<float> distances (indices.size ());
Expand Down Expand Up @@ -142,6 +140,9 @@ namespace jsk_pcl_ros
double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier

// 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)
{
bool remove_point = false;
Expand All @@ -153,12 +154,22 @@ namespace jsk_pcl_ros
{
remove_point = (distances[cp] > distance_threshold);
}
if (!remove_point)

if (remove_point)
{
indices_filtered.push_back(indices[cp]);
/* Set the current point to NaN. */
*(reinterpret_cast<float*>(&pcl_cloud_filtered->data[nr_p * pcl_cloud_filtered->point_step])+0) = std::numeric_limits<float>::quiet_NaN();
*(reinterpret_cast<float*>(&pcl_cloud_filtered->data[nr_p * pcl_cloud_filtered->point_step])+1) = std::numeric_limits<float>::quiet_NaN();
*(reinterpret_cast<float*>(&pcl_cloud_filtered->data[nr_p * pcl_cloud_filtered->point_step])+2) = std::numeric_limits<float>::quiet_NaN();
}
else
{
memcpy (&pcl_cloud_filtered->data[nr_p * pcl_cloud_filtered->point_step],
&pcl_cloud->data[indices[cp] * pcl_cloud_filtered->point_step],
pcl_cloud_filtered->point_step);
}
nr_p++;
}
pcl_indices_filtered->indices = indices_filtered;
}
else
{
Expand All @@ -167,9 +178,16 @@ namespace jsk_pcl_ros
sor.setMeanK(mean_k_);
sor.setStddevMulThresh(std_mul_);
sor.setNegative(negative_);
sor.filter(pcl_indices_filtered->indices);
sor.filter(*pcl_cloud_filtered);
}
#else
}

void OrganizedStatisticalOutlierRemoval::filterIndices(
const pcl::PCLPointCloud2::Ptr pcl_cloud,
pcl::PointIndices::Ptr pcl_indices_filtered,
bool keep_organized)
{
#if PCL_VERSION_COMPARE (>=, 1, 9, 0)
pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> sor;
sor.setInputCloud(pcl_cloud);
sor.setMeanK(mean_k_);
Expand All @@ -190,15 +208,15 @@ 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;
// filter
#if PCL_VERSION_COMPARE (>=, 1, 9, 0)
pcl::PCLPointCloud2::Ptr pcl_cloud (new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*msg, *pcl_cloud);

// 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::filterIndices(pcl_cloud,
pcl_indices_filtered,
keep_organized);
pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2);
pcl::ExtractIndices<pcl::PCLPointCloud2> ex;
ex.setInputCloud(pcl_cloud);
Expand All @@ -207,17 +225,22 @@ namespace jsk_pcl_ros
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) {
// 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 ());
output.width = msg->width;
output.height = msg->height;
output.row_step = msg->point_step * msg->width;
}
#else
pcl::PCLPointCloud2::Ptr pcl_cloud (new pcl::PCLPointCloud2);
pcl_conversions::toPCL(*msg, *pcl_cloud);
pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2);
// Copy the common fields
pcl_cloud_filtered->is_bigendian = msg->is_bigendian;
pcl_cloud_filtered->fields = msg->fields;
pcl_cloud_filtered->point_step = msg->point_step;
pcl_cloud_filtered->data.resize (msg->data.size ());
pcl_cloud_filtered->width = msg->width;
pcl_cloud_filtered->height = msg->height;
pcl_cloud_filtered->row_step = msg->point_step * msg->width;
OrganizedStatisticalOutlierRemoval::filterCloud(pcl_cloud,
pcl_cloud_filtered,
keep_organized);
pcl_conversions::fromPCL(*pcl_cloud_filtered, output);
#endif
output.header = msg->header;
output.is_dense = !keep_organized;
Expand Down

0 comments on commit f088f31

Please sign in to comment.