Skip to content

Commit 33ccf4c

Browse files
knorth55knorth55
committed
support melodic
1 parent 799ff8e commit 33ccf4c

File tree

2 files changed

+91
-95
lines changed

2 files changed

+91
-95
lines changed

‎jsk_pcl_ros/include/jsk_pcl_ros/organized_statistical_outlier_removal.h

Lines changed: 10 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,10 @@
4242
#include <jsk_topic_tools/diagnostic_nodelet.h>
4343
#include <jsk_topic_tools/counter.h>
4444
#include <dynamic_reconfigure/server.h>
45+
46+
#include "sensor_msgs/PointCloud2.h"
47+
#include "jsk_recognition_msgs/ClusterPointIndices.h"
48+
4549
#include "jsk_pcl_ros/OrganizedStatisticalOutlierRemovalConfig.h"
4650

4751
namespace jsk_pcl_ros
@@ -63,13 +67,13 @@ namespace jsk_pcl_ros
6367

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

66-
virtual void filter(const sensor_msgs::PointCloud2::ConstPtr& msg);
67-
virtual void filterCloud(const pcl::PCLPointCloud2::Ptr pcl_cloud,
68-
pcl::PointIndices::Ptr pcl_indices_filtered,
69-
bool keep_organized);
70+
void filter(const pcl::PCLPointCloud2::Ptr pcl_cloud,
71+
pcl::PointIndices::Ptr pcl_indices_filtered);
72+
std::vector<int> getFilteredIndices(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
73+
void filterCloud(const sensor_msgs::PointCloud2::ConstPtr& msg);
74+
7075

71-
virtual void updateDiagnostic(
72-
diagnostic_updater::DiagnosticStatusWrapper &stat);
76+
void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat);
7377

7478
////////////////////////////////////////////////////////
7579
// ROS variables

‎jsk_pcl_ros/src/organized_statistical_outlier_removal_nodelet.cpp

Lines changed: 81 additions & 89 deletions
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,7 @@ namespace jsk_pcl_ros
6363

6464
void OrganizedStatisticalOutlierRemoval::subscribe()
6565
{
66-
sub_ = pnh_->subscribe("input", 1, &OrganizedStatisticalOutlierRemoval::filter, this);
66+
sub_ = pnh_->subscribe("input", 1, &OrganizedStatisticalOutlierRemoval::filterCloud, this);
6767
}
6868

6969
void OrganizedStatisticalOutlierRemoval::unsubscribe()
@@ -80,107 +80,102 @@ namespace jsk_pcl_ros
8080
std_mul_ = config.stddev;
8181
}
8282

83-
void OrganizedStatisticalOutlierRemoval::filterCloud(
83+
void OrganizedStatisticalOutlierRemoval::filter(
8484
const pcl::PCLPointCloud2::Ptr pcl_cloud,
85-
pcl::PointIndices::Ptr pcl_indices_filtered,
86-
bool keep_organized)
85+
pcl::PointIndices::Ptr pcl_indices_filtered)
8786
{
88-
#if PCL_VERSION_COMPARE (<, 1, 9, 0)
89-
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
87+
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
9088
pcl::fromPCLPointCloud2(*pcl_cloud, *cloud);
91-
if (keep_organized) {
92-
// Initialize the spatial locator
93-
pcl::search::Search<pcl::PointXYZ>::Ptr tree;
94-
tree.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ());
95-
tree->setInputCloud (cloud);
96-
97-
// Allocate enough space to hold the results
98-
std::vector<int> indices (cloud->size());
99-
std::vector<int> indices_filtered;
100-
std::vector<int> nn_indices (mean_k_);
101-
std::vector<float> nn_dists (mean_k_);
102-
std::vector<float> distances (indices.size ());
103-
int valid_distances = 0;
104-
105-
for (size_t i = 0; i < indices.size (); ++i) indices[i] = i;
106-
107-
// Go over all the points and calculate the mean or smallest distance
108-
for (size_t cp = 0; cp < indices.size (); ++cp)
89+
pcl_indices_filtered->indices = OrganizedStatisticalOutlierRemoval::getFilteredIndices(cloud);
90+
}
91+
92+
std::vector<int> OrganizedStatisticalOutlierRemoval::getFilteredIndices(
93+
const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
94+
{
95+
std::vector<int> indices_filtered;
96+
indices_filtered.resize(cloud->size());
97+
#if PCL_VERSION_COMPARE (<, 1, 9, 0)
98+
// Initialize the spatial locator
99+
pcl::search::Search<pcl::PointXYZ>::Ptr tree;
100+
tree.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ());
101+
tree->setInputCloud (cloud);
102+
103+
// Allocate enough space to hold the results
104+
std::vector<int> indices (cloud->size());
105+
std::vector<int> nn_indices (mean_k_);
106+
std::vector<float> nn_dists (mean_k_);
107+
std::vector<float> distances (indices.size ());
108+
int valid_distances = 0;
109+
110+
for (size_t i = 0; i < indices.size (); ++i) indices[i] = i;
111+
112+
// Go over all the points and calculate the mean or smallest distance
113+
for (size_t cp = 0; cp < indices.size (); ++cp)
114+
{
115+
if (!pcl_isfinite (cloud->points[indices[cp]].x) ||
116+
!pcl_isfinite (cloud->points[indices[cp]].y) ||
117+
!pcl_isfinite (cloud->points[indices[cp]].z))
109118
{
110-
if (!pcl_isfinite (cloud->points[indices[cp]].x) ||
111-
!pcl_isfinite (cloud->points[indices[cp]].y) ||
112-
!pcl_isfinite (cloud->points[indices[cp]].z))
113-
{
114-
distances[cp] = 0;
115-
continue;
116-
}
117-
if (tree->nearestKSearch (indices[cp], mean_k_, nn_indices, nn_dists) == 0)
118-
{
119-
distances[cp] = 0;
120-
continue;
121-
}
122-
// Minimum distance (if mean_k_ == 2) or mean distance
123-
double dist_sum = 0;
124-
for (int j = 1; j < mean_k_; ++j)
125-
dist_sum += sqrt (nn_dists[j]);
126-
distances[cp] = static_cast<float> (dist_sum / (mean_k_ - 1));
127-
valid_distances++;
119+
distances[cp] = 0;
120+
continue;
128121
}
129-
130-
// Estimate the mean and the standard deviation of the distance vector
131-
double sum = 0, sq_sum = 0;
132-
for (size_t i = 0; i < distances.size (); ++i)
122+
if (tree->nearestKSearch (indices[cp], mean_k_, nn_indices, nn_dists) == 0)
133123
{
134-
sum += distances[i];
135-
sq_sum += distances[i] * distances[i];
124+
distances[cp] = 0;
125+
continue;
136126
}
137-
double mean = sum / static_cast<double>(valid_distances);
138-
double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(valid_distances) - 1);
139-
double stddev = sqrt (variance);
140-
//getMeanStd (distances, mean, stddev);
127+
// Minimum distance (if mean_k_ == 2) or mean distance
128+
double dist_sum = 0;
129+
for (int j = 1; j < mean_k_; ++j)
130+
dist_sum += sqrt (nn_dists[j]);
131+
distances[cp] = static_cast<float> (dist_sum / (mean_k_ - 1));
132+
valid_distances++;
133+
}
141134

142-
double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier
135+
// Estimate the mean and the standard deviation of the distance vector
136+
double sum = 0, sq_sum = 0;
137+
for (size_t i = 0; i < distances.size (); ++i)
138+
{
139+
sum += distances[i];
140+
sq_sum += distances[i] * distances[i];
141+
}
142+
double mean = sum / static_cast<double>(valid_distances);
143+
double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(valid_distances) - 1);
144+
double stddev = sqrt (variance);
145+
//getMeanStd (distances, mean, stddev);
146+
147+
double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier
143148

144-
// Build a new cloud by neglecting outliers
145-
for (int cp = 0; cp < static_cast<int> (indices.size ()); ++cp)
149+
// Build a new cloud by neglecting outliers
150+
for (int cp = 0; cp < static_cast<int> (indices.size ()); ++cp)
151+
{
152+
bool remove_point = false;
153+
if (negative_)
146154
{
147-
bool remove_point = false;
148-
if (negative_)
149-
{
150-
remove_point = (distances[cp] <= distance_threshold);
151-
}
152-
else
153-
{
154-
remove_point = (distances[cp] > distance_threshold);
155-
}
156-
if (!remove_point)
157-
{
158-
indices_filtered.push_back(indices[cp]);
159-
}
155+
remove_point = (distances[cp] <= distance_threshold);
156+
}
157+
else
158+
{
159+
remove_point = (distances[cp] > distance_threshold);
160+
}
161+
if (!remove_point)
162+
{
163+
indices_filtered.push_back(indices[cp]);
160164
}
161-
pcl_indices_filtered->indices = indices_filtered;
162-
}
163-
else
164-
{
165-
pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> sor;
166-
sor.setInputCloud(pcl_cloud);
167-
sor.setMeanK(mean_k_);
168-
sor.setStddevMulThresh(std_mul_);
169-
sor.setNegative(negative_);
170-
sor.filter(pcl_indices_filtered->indices);
171165
}
172166
#else
173-
pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> sor;
174-
sor.setInputCloud(pcl_cloud);
167+
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
168+
sor.setInputCloud(cloud);
175169
sor.setMeanK(mean_k_);
176170
sor.setStddevMulThresh(std_mul_);
177171
sor.setNegative(negative_);
178-
sor.setKeepOrganized (keep_organized);
179-
sor.filter(pcl_indices_filtered->indices);
172+
sor.setKeepOrganized (true);
173+
sor.filter(indices_filtered);
180174
#endif
175+
return indices_filtered;
181176
}
182177

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

196191
// filter
197192
pcl::PointIndices::Ptr pcl_indices_filtered (new pcl::PointIndices());
198-
pcl_indices_filtered->indices.resize(pcl_cloud->data.size());
199-
OrganizedStatisticalOutlierRemoval::filterCloud(pcl_cloud,
200-
pcl_indices_filtered,
201-
keep_organized);
193+
OrganizedStatisticalOutlierRemoval::filter(pcl_cloud, pcl_indices_filtered);
202194
pcl::PCLPointCloud2::Ptr pcl_cloud_filtered (new pcl::PCLPointCloud2);
203195
pcl::ExtractIndices<pcl::PCLPointCloud2> ex;
204196
ex.setInputCloud(pcl_cloud);
@@ -213,13 +205,13 @@ namespace jsk_pcl_ros
213205
output.is_bigendian = msg->is_bigendian;
214206
output.fields = msg->fields;
215207
output.point_step = msg->point_step;
216-
output.data.resize (msg->data.size ());
208+
output.data.resize(msg->data.size());
217209
output.width = msg->width;
218210
output.height = msg->height;
219-
output.row_step = msg->point_step * msg->width;
220211
}
221212
#endif
222213
output.header = msg->header;
214+
output.row_step = output.point_step * output.width;
223215
output.is_dense = !keep_organized;
224216
pub_.publish(output);
225217
diagnostic_updater_->update();

0 commit comments

Comments
 (0)