@@ -135,7 +135,8 @@ void RadiusSearch2dFilter::filter(
135
135
const int min_points_threshold = std::min (
136
136
std::max (static_cast <int >(min_points_and_distance_ratio_ / distance + 0 .5f ), min_points_),
137
137
max_points_);
138
- const int points_num = kd_tree_->nearestKSearch (i, min_points_threshold, k_indices, k_distances);
138
+ const int points_num =
139
+ kd_tree_->nearestKSearch (i, min_points_threshold, k_indices, k_distances);
139
140
140
141
if (min_points_threshold <= points_num) {
141
142
output.points .push_back (xyz_cloud.points .at (i));
@@ -169,20 +170,25 @@ void RadiusSearch2dFilter::filter(
169
170
std::vector<float > k_distances_low (low_conf_xy_cloud->points .size ());
170
171
kd_tree_->setInputCloud (low_conf_xy_cloud);
171
172
for (size_t i = 0 ; i < low_conf_input.points .size (); ++i) {
172
- const float distance = std::hypot (low_conf_xy_cloud->points [i].x - pose.position .x , low_conf_xy_cloud->points [i].y - pose.position .y );
173
+ const float distance = std::hypot (
174
+ low_conf_xy_cloud->points [i].x - pose.position .x ,
175
+ low_conf_xy_cloud->points [i].y - pose.position .y );
173
176
const int min_points_threshold = std::min (
174
177
std::max (static_cast <int >(min_points_and_distance_ratio_ / distance + 0 .5f ), min_points_),
175
178
max_points_);
176
- const int points_num = kd_tree_->nearestKSearch (i, min_points_threshold, k_indices_low, k_distances_low);
179
+ const int points_num =
180
+ kd_tree_->nearestKSearch (i, min_points_threshold, k_indices_low, k_distances_low);
177
181
if (min_points_threshold <= points_num) {
178
182
output.points .push_back (low_conf_input.points .at (i));
179
183
} else {
180
184
outlier.points .push_back (low_conf_input.points .at (i));
181
185
}
182
186
}
183
187
184
- // High conf cloud check
185
- if (outlier.points .size () == 0 ){return ;}
188
+ // High conf cloud check
189
+ if (outlier.points .size () == 0 ) {
190
+ return ;
191
+ }
186
192
187
193
pcl::PointCloud<pcl::PointXY>::Ptr high_conf_xy_cloud (new pcl::PointCloud<pcl::PointXY>);
188
194
high_conf_xy_cloud->points .resize (high_conf_input.points .size ());
@@ -195,11 +201,14 @@ void RadiusSearch2dFilter::filter(
195
201
std::vector<float > k_distances_high (high_conf_xy_cloud->points .size ());
196
202
kd_tree_->setInputCloud (high_conf_xy_cloud);
197
203
for (size_t i = 0 ; i < outlier.points .size (); ++i) {
198
- const float distance = std::hypot (high_conf_xy_cloud->points [i].x - pose.position .x , high_conf_xy_cloud->points [i].y - pose.position .y );
204
+ const float distance = std::hypot (
205
+ high_conf_xy_cloud->points [i].x - pose.position .x ,
206
+ high_conf_xy_cloud->points [i].y - pose.position .y );
199
207
const int min_points_threshold = std::min (
200
208
std::max (static_cast <int >(min_points_and_distance_ratio_ / distance + 0 .5f ), min_points_),
201
209
max_points_);
202
- const int points_num = kd_tree_->nearestKSearch (i, min_points_threshold, k_indices_high, k_distances_high);
210
+ const int points_num =
211
+ kd_tree_->nearestKSearch (i, min_points_threshold, k_indices_high, k_distances_high);
203
212
204
213
if (min_points_threshold <= points_num) {
205
214
output.points .push_back (outlier.points .at (i));
@@ -259,7 +268,7 @@ void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack(
259
268
{
260
269
size_t front_count = 0 ;
261
270
size_t behind_count = 0 ;
262
-
271
+
263
272
for (sensor_msgs::PointCloud2ConstIterator<float > x (*input_pc, " x" ); x != x.end (); ++x) {
264
273
if (*x < 0.0 ) {
265
274
behind_count++;
@@ -270,15 +279,16 @@ void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack(
270
279
271
280
sensor_msgs::PointCloud2Modifier front_pc_modfier (front_pc);
272
281
sensor_msgs::PointCloud2Modifier behind_pc_modfier (behind_pc);
273
- front_pc_modfier.setPointCloud2FieldsByString (1 ," xyz" );
274
- behind_pc_modfier.setPointCloud2FieldsByString (1 ," xyz" );
282
+ front_pc_modfier.setPointCloud2FieldsByString (1 , " xyz" );
283
+ behind_pc_modfier.setPointCloud2FieldsByString (1 , " xyz" );
275
284
front_pc_modfier.resize (front_count);
276
285
behind_pc_modfier.resize (behind_count);
277
286
278
287
sensor_msgs::PointCloud2Iterator<float > fr_iter (front_pc, " x" );
279
288
sensor_msgs::PointCloud2Iterator<float > be_iter (behind_pc, " x" );
280
289
281
- for (sensor_msgs::PointCloud2ConstIterator<float > in_iter (*input_pc, " x" ); in_iter != in_iter.end (); ++in_iter) {
290
+ for (sensor_msgs::PointCloud2ConstIterator<float > in_iter (*input_pc, " x" );
291
+ in_iter != in_iter.end (); ++in_iter) {
282
292
if (*in_iter < 0.0 ) {
283
293
*be_iter = in_iter[0 ];
284
294
be_iter[1 ] = in_iter[1 ];
0 commit comments