Tiziano/Remove voxel block (#381)

* Add inner voxel resolution * Thank you to exist stl library * Rename variable for consistency * Shrink code * Try to fix windows build * Try to make windows happy * atomic change
PRBonn · Aug 1, 2024 · 05d596e · 05d596e
1 parent e50972b
commit 05d596e
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 19 deletions.
diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp
@@ -37,7 +37,7 @@ std::vector<Eigen::Vector3d> VoxelHashMap::GetPoints(const std::vector<Voxel> &q
     std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query) {
         auto search = map_.find(query);
         if (search != map_.end()) {
-            const auto &voxel_points = search->second.points;
+            const auto &voxel_points = search.value();
             points.insert(points.end(), voxel_points.cbegin(), voxel_points.cend());
         }
     });
@@ -49,7 +49,7 @@ std::vector<Eigen::Vector3d> VoxelHashMap::Pointcloud() const {
     std::vector<Eigen::Vector3d> points;
     points.reserve(map_.size() * static_cast<size_t>(max_points_per_voxel_));
     std::for_each(map_.cbegin(), map_.cend(), [&](const auto &map_element) {
-        const auto &voxel_points = map_element.second.points;
+        const auto &voxel_points = map_element.second;
         points.insert(points.end(), voxel_points.cbegin(), voxel_points.cend());
     });
     points.shrink_to_fit();
@@ -75,19 +75,24 @@ void VoxelHashMap::AddPoints(const std::vector<Eigen::Vector3d> &points) {
         auto voxel = PointToVoxel(point, voxel_size_);
         auto search = map_.find(voxel);
         if (search != map_.end()) {
-            auto &voxel_block = search.value();
-            voxel_block.AddPoint(point);
+            auto &voxel_points = search.value();
+            if (voxel_points.size() < max_points_per_voxel_) {
+                voxel_points.emplace_back(point);
+            }
         } else {
-            map_.insert({voxel, VoxelBlock{{point}, max_points_per_voxel_}});
+            std::vector<Eigen::Vector3d> voxel_points;
+            voxel_points.reserve(max_points_per_voxel_);
+            voxel_points.emplace_back(point);
+            map_.insert({voxel, std::move(voxel_points)});
         }
     });
 }
 
 void VoxelHashMap::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) {
     const auto max_distance2 = max_distance_ * max_distance_;
     for (auto it = map_.begin(); it != map_.end();) {
-        const auto &[voxel, voxel_block] = *it;
-        const auto &pt = voxel_block.points.front();
+        const auto &[voxel, voxel_points] = *it;
+        const auto &pt = voxel_points.front();
         if ((pt - origin).squaredNorm() >= (max_distance2)) {
             it = map_.erase(it);
         } else {

diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp
@@ -36,16 +36,7 @@
 
 namespace kiss_icp {
 struct VoxelHashMap {
-    struct VoxelBlock {
-        // buffer of points with a max limit of n_points
-        std::vector<Eigen::Vector3d> points;
-        int num_points_;
-        inline void AddPoint(const Eigen::Vector3d &point) {
-            if (points.size() < static_cast<size_t>(num_points_)) points.push_back(point);
-        }
-    };
-
-    explicit VoxelHashMap(double voxel_size, double max_distance, int max_points_per_voxel)
+    explicit VoxelHashMap(double voxel_size, double max_distance, unsigned int max_points_per_voxel)
         : voxel_size_(voxel_size),
           max_distance_(max_distance),
           max_points_per_voxel_(max_points_per_voxel) {}
@@ -61,7 +52,7 @@ struct VoxelHashMap {
 
     double voxel_size_;
     double max_distance_;
-    int max_points_per_voxel_;
-    tsl::robin_map<Voxel, VoxelBlock> map_;
+    unsigned int max_points_per_voxel_;
+    tsl::robin_map<Voxel, std::vector<Eigen::Vector3d>> map_;
 };
 }  // namespace kiss_icp