Merge branch 'main' into luca-new-visualizer

PRBonn · Aug 8, 2024 · 71122f6 · 71122f6
2 parents 0c8c77e + 624e46e
commit 71122f6
Show file tree

Hide file tree

Showing 5 changed files with 69 additions and 63 deletions.
diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp
@@ -55,41 +55,6 @@ void TransformPoints(const Sophus::SE3d &T, std::vector<Eigen::Vector3d> &points
                    [&](const auto &point) { return T * point; });
 }
 
-using Voxel = kiss_icp::Voxel;
-std::vector<Voxel> GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1) {
-    std::vector<Voxel> voxel_neighborhood;
-    for (int i = voxel.x() - adjacent_voxels; i < voxel.x() + adjacent_voxels + 1; ++i) {
-        for (int j = voxel.y() - adjacent_voxels; j < voxel.y() + adjacent_voxels + 1; ++j) {
-            for (int k = voxel.z() - adjacent_voxels; k < voxel.z() + adjacent_voxels + 1; ++k) {
-                voxel_neighborhood.emplace_back(i, j, k);
-            }
-        }
-    }
-    return voxel_neighborhood;
-}
-
-std::tuple<Eigen::Vector3d, double> GetClosestNeighbor(const Eigen::Vector3d &point,
-                                                       const kiss_icp::VoxelHashMap &voxel_map) {
-    // Convert the point to voxel coordinates
-    const auto &voxel = kiss_icp::PointToVoxel(point, voxel_map.voxel_size_);
-    // Get nearby voxels on the map
-    const auto &query_voxels = GetAdjacentVoxels(voxel);
-    // Extract the points contained within the neighborhood voxels
-    const auto &neighbors = voxel_map.GetPoints(query_voxels);
-
-    // Find the nearest neighbor
-    Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero();
-    double closest_distance = std::numeric_limits<double>::max();
-    std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) {
-        double distance = (neighbor - point).norm();
-        if (distance < closest_distance) {
-            closest_neighbor = neighbor;
-            closest_distance = distance;
-        }
-    });
-    return std::make_tuple(closest_neighbor, closest_distance);
-}
-
 Correspondences DataAssociation(const std::vector<Eigen::Vector3d> &points,
                                 const kiss_icp::VoxelHashMap &voxel_map,
                                 const double max_correspondance_distance) {
@@ -105,7 +70,7 @@ Correspondences DataAssociation(const std::vector<Eigen::Vector3d> &points,
         [&](const tbb::blocked_range<points_iterator> &r, Correspondences res) -> Correspondences {
             res.reserve(r.size());
             std::for_each(r.begin(), r.end(), [&](const auto &point) {
-                const auto &[closest_neighbor, distance] = GetClosestNeighbor(point, voxel_map);
+                const auto &[closest_neighbor, distance] = voxel_map.GetClosestNeighbor(point);
                 if (distance < max_correspondance_distance) {
                     res.emplace_back(point, closest_neighbor);
                 }

diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp
@@ -29,27 +29,66 @@
 
 #include "VoxelUtils.hpp"
 
-namespace kiss_icp {
+namespace {
+using kiss_icp::Voxel;
 
-std::vector<Eigen::Vector3d> VoxelHashMap::GetPoints(const std::vector<Voxel> &query_voxels) const {
+std::vector<Voxel> GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1) {
+    std::vector<Voxel> voxel_neighborhood;
+    for (int i = voxel.x() - adjacent_voxels; i < voxel.x() + adjacent_voxels + 1; ++i) {
+        for (int j = voxel.y() - adjacent_voxels; j < voxel.y() + adjacent_voxels + 1; ++j) {
+            for (int k = voxel.z() - adjacent_voxels; k < voxel.z() + adjacent_voxels + 1; ++k) {
+                voxel_neighborhood.emplace_back(i, j, k);
+            }
+        }
+    }
+    return voxel_neighborhood;
+}
+std::vector<Eigen::Vector3d> GetPoints(const std::vector<Voxel> &query_voxels,
+                                       const kiss_icp::VoxelHashMap &voxel_map) {
     std::vector<Eigen::Vector3d> points;
-    points.reserve(query_voxels.size() * static_cast<size_t>(max_points_per_voxel_));
+    points.reserve(query_voxels.size() * static_cast<size_t>(voxel_map.max_points_per_voxel_));
     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;
+        auto search = voxel_map.map_.find(query);
+        if (search != voxel_map.map_.end()) {
+            const auto &voxel_points = search.value();
             points.insert(points.end(), voxel_points.cbegin(), voxel_points.cend());
         }
     });
     points.shrink_to_fit();
     return points;
 }
 
+}  // namespace
+
+namespace kiss_icp {
+
+std::tuple<Eigen::Vector3d, double> VoxelHashMap::GetClosestNeighbor(
+    const Eigen::Vector3d &point) const {
+    // Convert the point to voxel coordinates
+    const auto &voxel = PointToVoxel(point, voxel_size_);
+    // Get nearby voxels on the map
+    const auto &query_voxels = GetAdjacentVoxels(voxel);
+    // Extract the points contained within the neighborhood voxels
+    const auto &neighbors = GetPoints(query_voxels, *this);
+
+    // Find the nearest neighbor
+    Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero();
+    double closest_distance = std::numeric_limits<double>::max();
+    std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) {
+        double distance = (neighbor - point).norm();
+        if (distance < closest_distance) {
+            closest_neighbor = neighbor;
+            closest_distance = distance;
+        }
+    });
+    return std::make_tuple(closest_neighbor, closest_distance);
+}
+
 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();
@@ -79,23 +118,34 @@ void VoxelHashMap::Update(const std::vector<Eigen::Vector3d> &points, const Soph
 }
 
 void VoxelHashMap::AddPoints(const std::vector<Eigen::Vector3d> &points) {
+    const double map_resolution = std::sqrt(voxel_size_ * voxel_size_ / max_points_per_voxel_);
     std::for_each(points.cbegin(), points.cend(), [&](const auto &point) {
         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_ ||
+                std::any_of(voxel_points.cbegin(), voxel_points.cend(),
+                            [&](const auto &voxel_point) {
+                                return (voxel_point - point).norm() < map_resolution;
+                            })) {
+                return;
+            }
+            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) {}
@@ -57,12 +48,12 @@ struct VoxelHashMap {
     void AddPoints(const std::vector<Eigen::Vector3d> &points);
     void RemovePointsFarFromLocation(const Eigen::Vector3d &origin);
     std::vector<Eigen::Vector3d> Pointcloud() const;
-    std::vector<Eigen::Vector3d> GetPoints(const std::vector<Voxel> &query_voxels) const;
     std::vector<Voxel> GetVoxels() const;
+    std::tuple<Eigen::Vector3d, double> GetClosestNeighbor(const Eigen::Vector3d &point) const;
 
     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
diff --git a/python/kiss_icp/tools/visualizer.py b/python/kiss_icp/tools/visualizer.py
@@ -437,4 +437,4 @@ def _main_gui_callback(self):
         self._gui.SameLine()
         self._center_viewpoint_callback()
         self._gui.Separator()
-        self._quit_callback()
+        self._quit_callback()
diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt
@@ -25,7 +25,7 @@ project(kiss_icp VERSION 1.0.0 LANGUAGES CXX)
 
 set(CMAKE_BUILD_TYPE Release)
 
-if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kiss_icp/)
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kiss_icp/ AND NOT IS_SYMLINK ${CMAKE_CURRENT_SOURCE_DIR})
   add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kiss_icp ${CMAKE_CURRENT_BINARY_DIR}/kiss_icp)
 else()
   cmake_minimum_required(VERSION 3.18)