Skip to content

Commit 0eb6d8d

Browse files
committed
fix(compare_map_segmentation): adjust compare map filter for lv4
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
1 parent 7503f42 commit 0eb6d8d

File tree

1 file changed

+3
-12
lines changed

1 file changed

+3
-12
lines changed

perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp

+3-12
Original file line numberDiff line numberDiff line change
@@ -95,16 +95,6 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels(
9595
distance_threshold, map, voxel)) {
9696
return true;
9797
}
98-
if (is_in_voxel(
99-
pcl::PointXYZ(point.x, point.y, point.z - distance_threshold_z), point, distance_threshold,
100-
map, voxel)) {
101-
return true;
102-
}
103-
if (is_in_voxel(
104-
pcl::PointXYZ(point.x, point.y, point.z + distance_threshold_z), point, distance_threshold,
105-
map, voxel)) {
106-
return true;
107-
}
10898
if (is_in_voxel(
10999
pcl::PointXYZ(point.x, point.y + distance_threshold, point.z - distance_threshold_z), point,
110100
distance_threshold, map, voxel)) {
@@ -235,16 +225,17 @@ bool VoxelGridMapLoader::is_in_voxel(
235225
const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point,
236226
const double distance_threshold, const PointCloudPtr & map, VoxelGridPointXYZ & voxel) const
237227
{
228+
const double z_distance_threshold = distance_threshold * downsize_ratio_z_axis_;
238229
int voxel_index =
239230
voxel.getCentroidIndexAt(voxel.getGridCoordinates(src_point.x, src_point.y, src_point.z));
240231
if (voxel_index != -1) { // not empty voxel
241232
const double dist_x = map->points.at(voxel_index).x - target_point.x;
242233
const double dist_y = map->points.at(voxel_index).y - target_point.y;
243-
const double dist_z = map->points.at(voxel_index).z - target_point.z - 0.1;
234+
const double dist_z = map->points.at(voxel_index).z - target_point.z - z_distance_threshold;
244235
// check if the point is inside the distance threshold voxel
245236
if (
246237
std::abs(dist_x) < distance_threshold && std::abs(dist_y) < distance_threshold &&
247-
std::abs(dist_z) < distance_threshold * downsize_ratio_z_axis_) {
238+
std::abs(dist_z) < z_distance_threshold) {
248239
return true;
249240
}
250241
}

0 commit comments

Comments
 (0)