@@ -120,25 +120,23 @@ void RadiusSearch2dFilter::filter(
120
120
{
121
121
const auto & xyz_cloud = input;
122
122
pcl::PointCloud<pcl::PointXY>::Ptr xy_cloud (new pcl::PointCloud<pcl::PointXY>);
123
- auto xyz_cloud_points_size = xyz_cloud.points .size ();
124
- xy_cloud->points .resize (xyz_cloud_points_size);
125
- for (size_t i = 0 ; i < xyz_cloud_points_size; ++i) {
123
+ xy_cloud->points .resize (xyz_cloud.points .size ());
124
+ for (size_t i = 0 ; i < xyz_cloud.points .size (); ++i) {
126
125
xy_cloud->points [i].x = xyz_cloud.points [i].x ;
127
126
xy_cloud->points [i].y = xyz_cloud.points [i].y ;
128
127
}
129
128
130
- auto xy_cloud_points_size = xy_cloud->points .size ();
131
- std::vector<int > k_indices (xy_cloud_points_size);
132
- std::vector<float > k_distances (xy_cloud_points_size);
129
+ std::vector<int > k_indices (xy_cloud->points .size ());
130
+ std::vector<float > k_distances (xy_cloud->points .size ());
133
131
kd_tree_->setInputCloud (xy_cloud);
134
- for (size_t i = 0 ; i < xy_cloud_points_size ; ++i) {
132
+ for (size_t i = 0 ; i < xy_cloud-> points . size () ; ++i) {
135
133
const float distance =
136
134
std::hypot (xy_cloud->points [i].x - pose.position .x , xy_cloud->points [i].y - pose.position .y );
137
135
const int min_points_threshold = std::min (
138
136
std::max (static_cast <int >(min_points_and_distance_ratio_ / distance + 0 .5f ), min_points_),
139
137
max_points_);
140
138
const int points_num =
141
- kd_tree_->nearestKSearch (i, min_points_threshold , k_indices, k_distances);
139
+ kd_tree_->radiusSearch (i, search_radius_ , k_indices, k_distances, min_points_threshold );
142
140
143
141
if (min_points_threshold <= points_num) {
144
142
output.points .push_back (xyz_cloud.points .at (i));
@@ -152,75 +150,45 @@ void RadiusSearch2dFilter::filter(
152
150
const PclPointCloud & high_conf_input, const PclPointCloud & low_conf_input, const Pose & pose,
153
151
PclPointCloud & output, PclPointCloud & outlier)
154
152
{
155
- auto low_conf_input_points_size = low_conf_input.points .size ();
153
+ const auto & high_conf_xyz_cloud = high_conf_input;
154
+ const auto & low_conf_xyz_cloud = low_conf_input;
156
155
// check the limit points number
157
- if (low_conf_input_points_size > max_filter_points_nb_) {
156
+ if (low_conf_xyz_cloud. points . size () > max_filter_points_nb_) {
158
157
RCLCPP_WARN (
159
158
rclcpp::get_logger (" OccupancyGridMapOutlierFilterComponent" ),
160
159
" Skip outlier filter since too much low_confidence pointcloud!" );
161
160
return ;
162
161
}
163
162
164
- pcl::PointCloud<pcl::PointXY>::Ptr low_conf_xy_cloud (new pcl::PointCloud<pcl::PointXY>);
165
- low_conf_xy_cloud->points .resize (low_conf_input_points_size);
166
- for (size_t i = 0 ; i < low_conf_input_points_size; ++i) {
167
- low_conf_xy_cloud->points [i].x = low_conf_input.points [i].x ;
168
- low_conf_xy_cloud->points [i].y = low_conf_input.points [i].y ;
169
- }
170
-
171
- // Low conf cloud check
172
- std::vector<int > k_indices_low (low_conf_xy_cloud->points .size ());
173
- std::vector<float > k_distances_low (low_conf_xy_cloud->points .size ());
174
- kd_tree_->setInputCloud (low_conf_xy_cloud);
175
- for (size_t i = 0 ; i < low_conf_input_points_size; ++i) {
176
- const float distance = std::hypot (
177
- low_conf_xy_cloud->points [i].x - pose.position .x ,
178
- low_conf_xy_cloud->points [i].y - pose.position .y );
179
- const int min_points_threshold = std::min (
180
- std::max (static_cast <int >(min_points_and_distance_ratio_ / distance + 0 .5f ), min_points_),
181
- max_points_);
182
- const int points_num =
183
- kd_tree_->nearestKSearch (i, min_points_threshold, k_indices_low, k_distances_low);
184
- if (min_points_threshold <= points_num) {
185
- output.points .push_back (low_conf_input.points .at (i));
186
- } else {
187
- outlier.points .push_back (low_conf_input.points .at (i));
188
- }
189
- }
190
- auto outlier_points_size = outlier.points .size ();
191
- // High conf cloud check
192
- if (outlier_points_size == 0 ) {
193
- return ;
163
+ pcl::PointCloud<pcl::PointXY>::Ptr xy_cloud (new pcl::PointCloud<pcl::PointXY>);
164
+ xy_cloud->points .resize (low_conf_xyz_cloud.points .size () + high_conf_xyz_cloud.points .size ());
165
+ for (size_t i = 0 ; i < low_conf_xyz_cloud.points .size (); ++i) {
166
+ xy_cloud->points [i].x = low_conf_xyz_cloud.points [i].x ;
167
+ xy_cloud->points [i].y = low_conf_xyz_cloud.points [i].y ;
194
168
}
195
-
196
- pcl::PointCloud<pcl::PointXY>::Ptr high_conf_xy_cloud (new pcl::PointCloud<pcl::PointXY>);
197
- high_conf_xy_cloud->points .resize (high_conf_input.points .size ());
198
- for (size_t i = 0 ; i < high_conf_input.points .size (); ++i) {
199
- high_conf_xy_cloud->points [i].x = high_conf_input.points [i].x ;
200
- high_conf_xy_cloud->points [i].y = high_conf_input.points [i].y ;
169
+ for (size_t i = low_conf_xyz_cloud.points .size (); i < xy_cloud->points .size (); ++i) {
170
+ xy_cloud->points [i].x = high_conf_xyz_cloud.points [i - low_conf_xyz_cloud.points .size ()].x ;
171
+ xy_cloud->points [i].y = high_conf_xyz_cloud.points [i - low_conf_xyz_cloud.points .size ()].y ;
201
172
}
202
173
203
- std::vector<int > k_indices_high (high_conf_xy_cloud->points .size ());
204
- std::vector<float > k_distances_high (high_conf_xy_cloud->points .size ());
205
- kd_tree_->setInputCloud (high_conf_xy_cloud);
206
- for (size_t i = 0 ; i < outlier_points_size; ++i) {
207
- const float distance = std::hypot (
208
- high_conf_xy_cloud->points [i].x - pose.position .x ,
209
- high_conf_xy_cloud->points [i].y - pose.position .y );
174
+ std::vector<int > k_indices (xy_cloud->points .size ());
175
+ std::vector<float > k_distances (xy_cloud->points .size ());
176
+ kd_tree_->setInputCloud (xy_cloud);
177
+ for (size_t i = 0 ; i < low_conf_xyz_cloud.points .size (); ++i) {
178
+ const float distance =
179
+ std::hypot (xy_cloud->points [i].x - pose.position .x , xy_cloud->points [i].y - pose.position .y );
210
180
const int min_points_threshold = std::min (
211
181
std::max (static_cast <int >(min_points_and_distance_ratio_ / distance + 0 .5f ), min_points_),
212
182
max_points_);
213
183
const int points_num =
214
- kd_tree_->nearestKSearch (i, min_points_threshold, k_indices_high, k_distances_high );
184
+ kd_tree_->radiusSearch (i, search_radius_, k_indices, k_distances, min_points_threshold );
215
185
216
186
if (min_points_threshold <= points_num) {
217
- output.points .push_back (outlier .points .at (i));
187
+ output.points .push_back (low_conf_xyz_cloud .points .at (i));
218
188
} else {
219
- outlier.points .push_back (outlier .points .at (i));
189
+ outlier.points .push_back (low_conf_xyz_cloud .points .at (i));
220
190
}
221
191
}
222
-
223
- outlier.points .erase (outlier.points .begin (), outlier.points .begin () + outlier_points_size);
224
192
}
225
193
226
194
OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent (
0 commit comments