graduated non-convexity

Author: koide3

CMakeLists.txt

@@ -144,15 +144,17 @@ add_library(gtsam_points SHARED
   src/gtsam_points/features/normal_estimation.cpp
   src/gtsam_points/features/fpfh_estimation.cpp
   # registration
-  src/gtsam_points/registration/ransac.cpp
   src/gtsam_points/registration/alignment.cpp
+  src/gtsam_points/registration/ransac.cpp
+  src/gtsam_points/registration/graduated_non_convexity.cpp
   # ann
   src/gtsam_points/ann/kdtree.cpp
   src/gtsam_points/ann/kdtreex.cpp
   src/gtsam_points/ann/intensity_kdtree.cpp
   src/gtsam_points/ann/ivox.cpp
   src/gtsam_points/ann/incremental_covariance_container.cpp
   src/gtsam_points/ann/incremental_covariance_voxelmap.cpp
+  src/gtsam_points/ann/fast_occupancy_grid.cpp
   # types
   src/gtsam_points/types/point_cloud.cpp
   src/gtsam_points/types/point_cloud_cpu.cpp
@@ -257,14 +259,15 @@ endif()
 
 #Demo
 if(BUILD_DEMO)
+  find_package(spdlog REQUIRED)
   find_package(Iridescence REQUIRED)
 
   file(GLOB demo_sources "src/demo/*.cpp")
   foreach(demo_src IN LISTS demo_sources)
     get_filename_component(demo_name ${demo_src} NAME_WE)
 
     add_executable(${demo_name} ${demo_src})
-    target_link_libraries(${demo_name} gtsam_points Iridescence::Iridescence)
+    target_link_libraries(${demo_name} gtsam_points spdlog::spdlog Iridescence::Iridescence)
   endforeach()
 endif()
 

include/gtsam_points/ann/fast_occupancy_grid.hpp

@@ -0,0 +1,115 @@
+// SPDX-License-Identifier: MIT
+// Copyright (c) 2025  Kenji Koide ([email protected])
+#pragma once
+
+#include <vector>
+#include <bitset>
+#include <numeric>
+#include <gtsam_points/types/point_cloud.hpp>
+
+namespace gtsam_points {
+
+/// @brief Fast occupancy grid block.
+struct FastOccupancyBlock {
+public:
+  FastOccupancyBlock() : cells(0) {}
+
+  static constexpr int stride = 8;                            ///< Size of the block in each dimension
+  static constexpr int num_cells = stride * stride * stride;  ///< Number of cells in the block
+
+  /// @brief Calculate the index of the cell in the block.
+  /// @param coord Coordinate of the cell.
+  int cell_index(const Eigen::Vector3i& coord) const { return coord.x() + coord.y() * stride + coord.z() * stride * stride; }
+
+  /// @brief Check if the cell is occupied.
+  /// @param coord Coordinate of the cell.
+  /// @return True if the cell is occupied.
+  bool occupied(const Eigen::Vector3i& coord) const { return cells[cell_index(coord)]; }
+
+  /// @brief Check if the cell is free.
+  /// @param coord Coordinate of the cell.
+  /// @return True if the cell is free.
+  bool free(const Eigen::Vector3i& coord) const { return !occupied(coord); }
+
+  /// @brief Set the cell as occupied.
+  /// @param coord Coordinate of the cell.
+  void set_occupied(const Eigen::Vector3i& coord) { cells.set(cell_index(coord)); }
+
+  /// @brief Set the cell as free.
+  /// @param coord Coordinate of the cell.
+  void set_free(const Eigen::Vector3i& coord) { cells.reset(cell_index(coord)); }
+
+  /// @brief  Count the number of occupied cells.
+  /// @return Number of occupied cells.
+  int count() const { return cells.count(); }
+
+public:
+  std::bitset<num_cells> cells;  ///< Occupancy status of each cell in the block.
+};
+
+/// @brief Fast occupancy grid with occupancy blocks and flat hashing for efficient point cloud overlap evaluation.
+class FastOccupancyGrid {
+public:
+  using Ptr = std::shared_ptr<FastOccupancyGrid>;
+  using ConstPtr = std::shared_ptr<const FastOccupancyGrid>;
+
+  /// @brief Constructor.
+  /// @param resolution Voxel resolution.
+  FastOccupancyGrid(double resolution);
+  ~FastOccupancyGrid();
+
+  /// @brief Insert points into the grid.
+  /// @param points Point cloud.
+  /// @param pose   Pose of the points.
+  template <typename PointCloud>
+  void insert(const PointCloud& points, const Eigen::Isometry3d& pose = Eigen::Isometry3d::Identity());
+
+  /// @brief Calculate the number of points overlapping with the grid.
+  /// @param points Point cloud.
+  /// @param pose   Pose of the points.
+  /// @return       Overlap ratio.
+  template <typename PointCloud>
+  int calc_overlap(const PointCloud& points, const Eigen::Isometry3d& pose = Eigen::Isometry3d::Identity()) const;
+
+  /// @brief Calculate the overlap ratio of the points with the grid.
+  /// @param points Point cloud.
+  /// @param pose   Pose of the points.
+  /// @return       Overlap ratio.
+  template <typename PointCloud>
+  double calc_overlap_rate(const PointCloud& points, const Eigen::Isometry3d& pose = Eigen::Isometry3d::Identity()) const;
+
+  /// @brief Get the overlap status of each point in the point cloud.
+  /// @param points Point cloud.
+  /// @param pose   Pose of the points.
+  /// @return       Overlap status of each point (0=free, 1=occupied).
+  template <typename PointCloud>
+  std::vector<unsigned char> get_overlaps(const PointCloud& points, const Eigen::Isometry3d& pose = Eigen::Isometry3d::Identity()) const;
+
+  /// @brief Get the number of occupied cells in the grid.
+  int num_occupied_cells() const;
+
+private:
+  std::uint64_t calc_index(const Eigen::Vector4i& coord) const;
+
+  Eigen::Vector3i calc_coord(std::uint64_t index) const;
+
+  std::uint64_t calc_hash(std::uint64_t index) const;
+
+  std::uint64_t find_block(std::uint64_t block_index) const;
+
+  std::uint64_t find_or_insert_block(std::uint64_t block_index);
+
+  void rehash(size_t hash_size);
+
+private:
+  static constexpr std::uint64_t INVALID_INDEX = std::numeric_limits<std::uint64_t>::max();
+  static constexpr int coord_bit_size = 21;                          ///< Bits to represent each voxel coordinate (pack 21x3=63bits in 64bit int)
+  static constexpr std::uint64_t coord_bit_mask = (1ull << 21) - 1;  ///< Bit mask
+  static constexpr std::uint64_t coord_offset = 1ull << (coord_bit_size - 1);  ///< Coordinate offset to make values positive
+
+  double inv_resolution;                                             ///< Inverse of the voxel resolution
+  int max_seek_count;                                                ///< Maximum number of seek attempts
+  std::vector<std::pair<std::uint64_t, FastOccupancyBlock>> blocks;  ///< Hash table of occupancy blocks
+};
+
+}  // namespace gtsam_points

include/gtsam_points/ann/impl/fast_occupancy_grid_impl.hpp

@@ -0,0 +1,76 @@
+// SPDX-License-Identifier: MIT
+// Copyright (c) 2025  Kenji Koide ([email protected])
+#pragma once
+
+#include <gtsam_points/ann/fast_occupancy_grid.hpp>
+
+#include <gtsam_points/types/frame_traits.hpp>
+#include <gtsam_points/util/fast_floor.hpp>
+#include <gtsam_points/util/vector3i_hash.hpp>
+#include <gtsam_points/types/point_cloud_cpu.hpp>
+
+namespace gtsam_points {
+
+template <typename PointCloud>
+void FastOccupancyGrid::insert(const PointCloud& points, const Eigen::Isometry3d& pose) {
+  for (int i = 0; i < frame::size(points); i++) {
+    const auto& pt = frame::point(points, i);
+    const Eigen::Array4i global_coord = fast_floor((pose * pt) * inv_resolution) + coord_offset;
+    const Eigen::Array4i block_coord = global_coord / FastOccupancyBlock::stride;
+    const Eigen::Array4i cell_coord = global_coord - block_coord * FastOccupancyBlock::stride;
+
+    const std::uint64_t block_index = calc_index(block_coord);
+    const std::uint64_t block_loc = find_or_insert_block(block_index);
+    blocks[block_loc].second.set_occupied(cell_coord.head<3>());
+  }
+}
+
+template <typename PointCloud>
+int FastOccupancyGrid::calc_overlap(const PointCloud& points, const Eigen::Isometry3d& pose) const {
+  int num_overlap = 0;
+  for (int i = 0; i < frame::size(points); i++) {
+    const auto& pt = frame::point(points, i);
+    const Eigen::Array4i global_coord = fast_floor((pose * pt) * inv_resolution) + coord_offset;
+    const Eigen::Array4i block_coord = global_coord / FastOccupancyBlock::stride;
+
+    const std::uint64_t block_index = calc_index(block_coord);
+    const std::uint64_t block_loc = find_block(block_index);
+    if (block_loc == INVALID_INDEX) {
+      continue;
+    }
+
+    const Eigen::Array4i cell_coord = global_coord - block_coord * FastOccupancyBlock::stride;
+    num_overlap += blocks[block_loc].second.occupied(cell_coord.head<3>());
+  }
+
+  return num_overlap;
+}
+
+template <typename PointCloud>
+double FastOccupancyGrid::calc_overlap_rate(const PointCloud& points, const Eigen::Isometry3d& pose) const {
+  return calc_overlap(points, pose) / static_cast<double>(frame::size(points));
+}
+
+template <typename PointCloud>
+std::vector<unsigned char> FastOccupancyGrid::get_overlaps(const PointCloud& points, const Eigen::Isometry3d& pose) const {
+  std::vector<unsigned char> overlaps(frame::size(points), 0);
+
+  for (int i = 0; i < frame::size(points); i++) {
+    const auto& pt = frame::point(points, i);
+    const Eigen::Array4i global_coord = fast_floor((pose * pt) * inv_resolution) + coord_offset;
+    const Eigen::Array4i block_coord = global_coord / FastOccupancyBlock::stride;
+
+    const std::uint64_t block_index = calc_index(block_coord);
+    const std::uint64_t block_loc = find_block(block_index);
+    if (block_loc == INVALID_INDEX) {
+      continue;
+    }
+
+    const Eigen::Array4i cell_coord = global_coord - block_coord * FastOccupancyBlock::stride;
+    overlaps[i] = blocks[block_loc].second.occupied(cell_coord.head<3>());
+  }
+
+  return overlaps;
+}
+
+}  // namespace gtsam_points