From 0eb6d8d083bec6d8e711caba83927ac08f093d49 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Wed, 10 Apr 2024 15:23:51 +0900 Subject: [PATCH] fix(compare_map_segmentation): adjust compare map filter for lv4 Signed-off-by: badai-nguyen --- .../src/voxel_grid_map_loader.cpp | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp index 913cabe5a15af..9b5db1372872d 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp @@ -95,16 +95,6 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( distance_threshold, map, voxel)) { return true; } - if (is_in_voxel( - pcl::PointXYZ(point.x, point.y, point.z - distance_threshold_z), point, distance_threshold, - map, voxel)) { - return true; - } - if (is_in_voxel( - pcl::PointXYZ(point.x, point.y, point.z + distance_threshold_z), point, distance_threshold, - map, voxel)) { - return true; - } if (is_in_voxel( pcl::PointXYZ(point.x, point.y + distance_threshold, point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { @@ -235,16 +225,17 @@ bool VoxelGridMapLoader::is_in_voxel( const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point, const double distance_threshold, const PointCloudPtr & map, VoxelGridPointXYZ & voxel) const { + const double z_distance_threshold = distance_threshold * downsize_ratio_z_axis_; int voxel_index = voxel.getCentroidIndexAt(voxel.getGridCoordinates(src_point.x, src_point.y, src_point.z)); if (voxel_index != -1) { // not empty voxel const double dist_x = map->points.at(voxel_index).x - target_point.x; const double dist_y = map->points.at(voxel_index).y - target_point.y; - const double dist_z = map->points.at(voxel_index).z - target_point.z - 0.1; + const double dist_z = map->points.at(voxel_index).z - target_point.z - z_distance_threshold; // check if the point is inside the distance threshold voxel if ( std::abs(dist_x) < distance_threshold && std::abs(dist_y) < distance_threshold && - std::abs(dist_z) < distance_threshold * downsize_ratio_z_axis_) { + std::abs(dist_z) < z_distance_threshold) { return true; } }