Skip to content

Commit 68b16b6

Browse files
committed
change filter
Signed-off-by: Taiga Takano <ttttghghnb554z@outlook.jp>
1 parent 2bfc2b3 commit 68b16b6

File tree

1 file changed

+26
-58
lines changed

1 file changed

+26
-58
lines changed

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

+26-58
Original file line numberDiff line numberDiff line change
@@ -120,25 +120,23 @@ void RadiusSearch2dFilter::filter(
120120
{
121121
const auto & xyz_cloud = input;
122122
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) {
126125
xy_cloud->points[i].x = xyz_cloud.points[i].x;
127126
xy_cloud->points[i].y = xyz_cloud.points[i].y;
128127
}
129128

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());
133131
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) {
135133
const float distance =
136134
std::hypot(xy_cloud->points[i].x - pose.position.x, xy_cloud->points[i].y - pose.position.y);
137135
const int min_points_threshold = std::min(
138136
std::max(static_cast<int>(min_points_and_distance_ratio_ / distance + 0.5f), min_points_),
139137
max_points_);
140138
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);
142140

143141
if (min_points_threshold <= points_num) {
144142
output.points.push_back(xyz_cloud.points.at(i));
@@ -152,75 +150,45 @@ void RadiusSearch2dFilter::filter(
152150
const PclPointCloud & high_conf_input, const PclPointCloud & low_conf_input, const Pose & pose,
153151
PclPointCloud & output, PclPointCloud & outlier)
154152
{
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;
156155
// 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_) {
158157
RCLCPP_WARN(
159158
rclcpp::get_logger("OccupancyGridMapOutlierFilterComponent"),
160159
"Skip outlier filter since too much low_confidence pointcloud!");
161160
return;
162161
}
163162

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;
194168
}
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;
201172
}
202173

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);
210180
const int min_points_threshold = std::min(
211181
std::max(static_cast<int>(min_points_and_distance_ratio_ / distance + 0.5f), min_points_),
212182
max_points_);
213183
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);
215185

216186
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));
218188
} else {
219-
outlier.points.push_back(outlier.points.at(i));
189+
outlier.points.push_back(low_conf_xyz_cloud.points.at(i));
220190
}
221191
}
222-
223-
outlier.points.erase(outlier.points.begin(), outlier.points.begin() + outlier_points_size);
224192
}
225193

226194
OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent(

0 commit comments

Comments
 (0)