@@ -95,16 +95,6 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels(
95
95
distance_threshold, map, voxel)) {
96
96
return true ;
97
97
}
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
- }
108
98
if (is_in_voxel (
109
99
pcl::PointXYZ (point.x , point.y + distance_threshold, point.z - distance_threshold_z), point,
110
100
distance_threshold, map, voxel)) {
@@ -235,16 +225,17 @@ bool VoxelGridMapLoader::is_in_voxel(
235
225
const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point,
236
226
const double distance_threshold, const PointCloudPtr & map, VoxelGridPointXYZ & voxel) const
237
227
{
228
+ const double z_distance_threshold = distance_threshold * downsize_ratio_z_axis_;
238
229
int voxel_index =
239
230
voxel.getCentroidIndexAt (voxel.getGridCoordinates (src_point.x , src_point.y , src_point.z ));
240
231
if (voxel_index != -1 ) { // not empty voxel
241
232
const double dist_x = map->points .at (voxel_index).x - target_point.x ;
242
233
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 ;
244
235
// check if the point is inside the distance threshold voxel
245
236
if (
246
237
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 ) {
248
239
return true ;
249
240
}
250
241
}
0 commit comments