VoxelHashMap ereasing of the voxels (#310)

* Fix for invalid iterators manipulation This could potentially lead to undefined behaviour or segmentation faults. * Remove max_distance_2_ Leave it for other PR * Avoid corner case --------- Co-authored-by: Ignacio Vizzo <[email protected]>
PRBonn · Apr 3, 2024 · e808b27 · e808b27
1 parent 11c7d97
commit e808b27
Showing 1 changed file with 6 additions and 3 deletions.
diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp
@@ -85,11 +85,14 @@ void VoxelHashMap::AddPoints(const std::vector<Eigen::Vector3d> &points) {
 }
 
 void VoxelHashMap::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) {
-    for (const auto &[voxel, voxel_block] : map_) {
+    for (auto it = map_.begin(); it != map_.end();) {
+        const auto &[voxel, voxel_block] = *it;
         const auto &pt = voxel_block.points.front();
         const auto max_distance2 = max_distance_ * max_distance_;
-        if ((pt - origin).squaredNorm() > (max_distance2)) {
-            map_.erase(voxel);
+        if ((pt - origin).squaredNorm() >= (max_distance2)) {
+            it = map_.erase(it);
+        } else {
+            ++it;
         }
     }
 }