Skip to content

Commit 7726ae6

Browse files
pre-commit-ci[bot]takanotaiga
authored andcommitted
style(pre-commit): autofix
Signed-off-by: Taiga Takano <ttttghghnb554z@outlook.jp>
1 parent dbf7f93 commit 7726ae6

File tree

1 file changed

+21
-11
lines changed

1 file changed

+21
-11
lines changed

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

+21-11
Original file line numberDiff line numberDiff line change
@@ -135,7 +135,8 @@ void RadiusSearch2dFilter::filter(
135135
const int min_points_threshold = std::min(
136136
std::max(static_cast<int>(min_points_and_distance_ratio_ / distance + 0.5f), min_points_),
137137
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);
139140

140141
if (min_points_threshold <= points_num) {
141142
output.points.push_back(xyz_cloud.points.at(i));
@@ -169,20 +170,25 @@ void RadiusSearch2dFilter::filter(
169170
std::vector<float> k_distances_low(low_conf_xy_cloud->points.size());
170171
kd_tree_->setInputCloud(low_conf_xy_cloud);
171172
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);
173176
const int min_points_threshold = std::min(
174177
std::max(static_cast<int>(min_points_and_distance_ratio_ / distance + 0.5f), min_points_),
175178
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);
177181
if (min_points_threshold <= points_num) {
178182
output.points.push_back(low_conf_input.points.at(i));
179183
} else {
180184
outlier.points.push_back(low_conf_input.points.at(i));
181185
}
182186
}
183187

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+
}
186192

187193
pcl::PointCloud<pcl::PointXY>::Ptr high_conf_xy_cloud(new pcl::PointCloud<pcl::PointXY>);
188194
high_conf_xy_cloud->points.resize(high_conf_input.points.size());
@@ -195,11 +201,14 @@ void RadiusSearch2dFilter::filter(
195201
std::vector<float> k_distances_high(high_conf_xy_cloud->points.size());
196202
kd_tree_->setInputCloud(high_conf_xy_cloud);
197203
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);
199207
const int min_points_threshold = std::min(
200208
std::max(static_cast<int>(min_points_and_distance_ratio_ / distance + 0.5f), min_points_),
201209
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);
203212

204213
if (min_points_threshold <= points_num) {
205214
output.points.push_back(outlier.points.at(i));
@@ -259,7 +268,7 @@ void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack(
259268
{
260269
size_t front_count = 0;
261270
size_t behind_count = 0;
262-
271+
263272
for (sensor_msgs::PointCloud2ConstIterator<float> x(*input_pc, "x"); x != x.end(); ++x) {
264273
if (*x < 0.0) {
265274
behind_count++;
@@ -270,15 +279,16 @@ void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack(
270279

271280
sensor_msgs::PointCloud2Modifier front_pc_modfier(front_pc);
272281
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");
275284
front_pc_modfier.resize(front_count);
276285
behind_pc_modfier.resize(behind_count);
277286

278287
sensor_msgs::PointCloud2Iterator<float> fr_iter(front_pc, "x");
279288
sensor_msgs::PointCloud2Iterator<float> be_iter(behind_pc, "x");
280289

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) {
282292
if (*in_iter < 0.0) {
283293
*be_iter = in_iter[0];
284294
be_iter[1] = in_iter[1];

0 commit comments

Comments
 (0)