Documentation of detailed behaviors (#82)

* detailed documentation (cpp)

* doc for invalid normal estimation results

* docs of detailed algorithm behavior for py

include/small_gicp/ann/flat_container.hpp

@@ -14,6 +14,7 @@ namespace small_gicp {
 /// @brief Point container with a flat vector.
 /// @note  IncrementalVoxelMap combined with FlastContainer is mostly the same as linear iVox.
 ///        Bai et al., "Faster-LIO: Lightweight Tightly Coupled Lidar-Inertial Odometry Using Parallel Sparse Incremental Voxels", IEEE RA-L, 2022
+/// @note  This container stores only up to max_num_points_in_cell points and avoids insertings points that are too close to existing points (min_sq_dist_in_cell).
 /// @tparam HasNormals  If true, store normals.
 /// @tparam HasCovs     If true, store covariances.
 template <bool HasNormals = false, bool HasCovs = false>
@@ -32,6 +33,7 @@ struct FlatContainer {
   size_t size() const { return points.size(); }
 
   /// @brief Add a point to the container.
+  ///        If there is a point that is too close to the input point, or there are too many points in the cell, the input point will not be ignored.
   /// @param setting         Setting
   /// @param transformed_pt  Transformed point (== T * points[i])
   /// @param points          Point cloud
@@ -77,7 +79,7 @@ struct FlatContainer {
   /// @param pt           Query point
   /// @param k            Number of neighbors
   /// @param k_index      Indices of nearest neighbors
-  /// @param k_sq_dist    Squared distances to nearest neighbors
+  /// @param k_sq_dist    Squared distances to nearest neighbors (sorted in ascending order)
   /// @return             Number of found points
   size_t knn_search(const Eigen::Vector4d& pt, int k, size_t* k_indices, double* k_sq_dists) const {
     if (points.empty()) {
@@ -93,7 +95,7 @@ struct FlatContainer {
   /// @param pt           Query point
   /// @param k            Number of neighbors
   /// @param k_index      Indices of nearest neighbors
-  /// @param k_sq_dist    Squared distances to nearest neighbors
+  /// @param k_sq_dist    Squared distances to nearest neighbors (sorted in ascending order)
   /// @return             Number of found points
   template <typename Result>
   void knn_search(const Eigen::Vector4d& pt, Result& result) const {

include/small_gicp/ann/incremental_voxelmap.hpp

@@ -31,8 +31,9 @@ struct VoxelInfo {
 };
 
 /// @brief Incremental voxelmap.
-///        This class supports incremental point cloud insertion and LRU-based voxel deletion.
+///        This class supports incremental point cloud insertion and LRU-based voxel deletion that removes voxels that are not recently referenced.
 /// @note  This class can be used as a point cloud as well as a neighbor search structure.
+/// @note  This class can handle arbitrary number of voxels and arbitrary range of voxel coordinates (in 32-bit int range).
 template <typename VoxelContents>
 struct IncrementalVoxelMap {
 public:
@@ -120,7 +121,7 @@ struct IncrementalVoxelMap {
   /// @param pt          Query point
   /// @param k           Number of neighbors
   /// @param k_indices   Indices of nearest neighbors
-  /// @param k_sq_dists  Squared distances to nearest neighbors
+  /// @param k_sq_dists  Squared distances to nearest neighbors (sorted in ascending order)
   /// @return            Number of found points
   size_t knn_search(const Eigen::Vector4d& pt, size_t k, size_t* k_indices, double* k_sq_dists) const {
     const Eigen::Vector3i center = fast_floor(pt * inv_leaf_size).template head<3>();
@@ -151,6 +152,7 @@ struct IncrementalVoxelMap {
   inline size_t point_id(const size_t i) const { return i & ((1ull << point_id_bits) - 1); }  ///< Extract the voxel ID from a global index.
 
   /// @brief Set the pattern of the search offsets. (Must be 1, 7, or 27)
+  /// @note  1: center only, 7: center + 6 adjacent neighbors (+- 1X/1Y/1Z), 27: center + 26 neighbors (3 x 3 x 3 cube)
   void set_search_offsets(int num_offsets) {
     switch (num_offsets) {
       default:

include/small_gicp/ann/kdtree.hpp

@@ -154,8 +154,8 @@ struct UnsafeKdTree {
 
   /// @brief Find the nearest neighbor.
   /// @param query        Query point
-  /// @param k_indices    Index of the nearest neighbor
-  /// @param k_sq_dists   Squared distance to the nearest neighbor
+  /// @param k_indices    Index of the nearest neighbor (uninitialized if not found)
+  /// @param k_sq_dists   Squared distance to the nearest neighbor (uninitialized if not found)
   /// @param setting      KNN search setting
   /// @return             Number of found neighbors (0 or 1)
   size_t nearest_neighbor_search(const Eigen::Vector4d& query, size_t* k_indices, double* k_sq_dists, const KnnSetting& setting = KnnSetting()) const {
@@ -166,7 +166,7 @@ struct UnsafeKdTree {
   /// @param  query       Query point
   /// @param  k           Number of neighbors
   /// @param  k_indices   Indices of neighbors
-  /// @param  k_sq_dists  Squared distances to neighbors
+  /// @param  k_sq_dists  Squared distances to neighbors (sorted in ascending order)
   /// @param  setting     KNN search setting
   /// @return             Number of found neighbors
   size_t knn_search(const Eigen::Vector4d& query, int k, size_t* k_indices, double* k_sq_dists, const KnnSetting& setting = KnnSetting()) const {
@@ -178,7 +178,7 @@ struct UnsafeKdTree {
   /// @brief Find k-nearest neighbors. This method uses fixed and static memory allocation. Might be faster for small k.
   /// @param query       Query point
   /// @param k_indices   Indices of neighbors
-  /// @param k_sq_dists  Squared distances to neighbors
+  /// @param k_sq_dists  Squared distances to neighbors (sorted in ascending order)
   /// @param setting     KNN search setting
   /// @return            Number of found neighbors
   template <int N>
@@ -255,7 +255,7 @@ struct KdTree {
   /// @param  query       Query point
   /// @param  k           Number of neighbors
   /// @param  k_indices   Indices of neighbors
-  /// @param  k_sq_dists  Squared distances to neighbors
+  /// @param  k_sq_dists  Squared distances to neighbors (sorted in ascending order)
   /// @param  setting     KNN search setting
   /// @return             Number of found neighbors
   size_t nearest_neighbor_search(const Eigen::Vector4d& query, size_t* k_indices, double* k_sq_dists, const KnnSetting& setting = KnnSetting()) const {
@@ -266,7 +266,7 @@ struct KdTree {
   /// @param  query       Query point
   /// @param  k           Number of neighbors
   /// @param  k_indices   Indices of neighbors
-  /// @param  k_sq_dists  Squared distances to neighbors
+  /// @param  k_sq_dists  Squared distances to neighbors (sorted in ascending order)
   /// @param  setting     KNN search setting
   /// @return             Number of found neighbors
   size_t knn_search(const Eigen::Vector4d& query, size_t k, size_t* k_indices, double* k_sq_dists, const KnnSetting& setting = KnnSetting()) const {

include/small_gicp/ann/knn_result.hpp

@@ -76,6 +76,7 @@ struct KnnResult {
   double worst_distance() const { return distances[buffer_size() - 1]; }
 
   /// @brief  Push a pair of point index and distance to the result.
+  /// @note   The result is sorted by distance in ascending order.
   void push(size_t index, double distance) {
     if (distance >= worst_distance()) {
       return;

include/small_gicp/ann/projection.hpp

@@ -14,6 +14,7 @@ struct ProjectionSetting {
 };
 
 /// @brief Conventional axis-aligned projection (i.e., selecting any of XYZ axes with the largest variance).
+/// @note  Up to max_scan_count samples are used to estimate the variance.
 struct AxisAlignedProjection {
 public:
   /// @brief Project the point to the selected axis.
@@ -53,6 +54,7 @@ struct AxisAlignedProjection {
 };
 
 /// @brief Normal projection (i.e., selecting the 3D direction with the largest variance of the points).
+/// @note  Up to max_scan_count samples are used to estimate the variance along the axis.
 struct NormalProjection {
 public:
   /// @brief Project the point to the normal direction.

include/small_gicp/registration/registration_helper.hpp

@@ -9,7 +9,9 @@
 
 namespace small_gicp {
 
-/// @brief Preprocess point cloud (downsampling, kdtree creation, and normal and covariance estimation)
+/// @brief Preprocess point cloud (downsampling, kdtree creation, and normal and covariance estimation).
+/// @note  When num_threads >= 2, this function has minor run-by-run non-determinism due to the parallel downsampling.
+/// @see   small_gicp::voxelgrid_sampling_omp, small_gicp::estimate_normals_covariances_omp
 /// @param points                Input points
 /// @param downsampling_resolution Downsample resolution
 /// @param num_neighbors         Number of neighbors for normal/covariance estimation
@@ -19,11 +21,14 @@ preprocess_points(const PointCloud& points, double downsampling_resolution, int
 
 /// @brief Preprocess point cloud (downsampling, kdtree creation, and normal and covariance estimation)
 /// @note  This function only accepts Eigen::Vector(3|4)(f|d) as input
+/// @note  When num_threads >= 2, this function has minor run-by-run non-determinism due to the parallel downsampling.
+/// @see   small_gicp::voxelgrid_sampling_omp, small_gicp::estimate_normals_covariances_omp
 template <typename T, int D>
 std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>>
 preprocess_points(const std::vector<Eigen::Matrix<T, D, 1>>& points, double downsampling_resolution, int num_neighbors = 10, int num_threads = 4);
 
-/// @brief Create Gaussian voxel map
+/// @brief Create an incremental Gaussian voxel map.
+/// @see   small_gicp::IncrementalVoxelMap
 /// @param points            Input points
 /// @param voxel_resolution  Voxel resolution
 GaussianVoxelMap::Ptr create_gaussian_voxelmap(const PointCloud& points, double voxel_resolution);
@@ -45,6 +50,7 @@ struct RegistrationSetting {
 
 /// @brief Align point clouds
 /// @note This function only accepts Eigen::Vector(3|4)(f|d) as input
+/// @see  small_gicp::voxelgrid_sampling_omp, small_gicp::estimate_normals_covariances_omp
 /// @param target     Target points
 /// @param source     Source points
 /// @param init_T     Initial guess of T_target_source
@@ -72,9 +78,8 @@ RegistrationResult align(
   const RegistrationSetting& setting = RegistrationSetting());
 
 /// @brief Align preprocessed point clouds with VGICP
-/// @param target       Target point cloud
+/// @param target       Target Gaussian voxelmap
 /// @param source       Source point cloud
-/// @param target_tree  Nearest neighbor search for the target point cloud
 /// @param init_T       Initial guess of T_target_source
 /// @param setting      Registration setting
 /// @return             Registration result

include/small_gicp/registration/rejector.hpp

@@ -24,7 +24,7 @@ struct DistanceRejector {
     return sq_dist > max_dist_sq;
   }
 
-  double max_dist_sq;
+  double max_dist_sq;  ///< Maximum squared distance between corresponding points
 };
 
 }  // namespace small_gicp

include/small_gicp/util/downsampling.hpp

@@ -12,8 +12,10 @@
 
 namespace small_gicp {
 
-/// @brief Voxelgrid downsampling.
+/// @brief Voxelgrid downsampling. This function computes exact average of points in each voxel, and each voxel can contain arbitrary number of points.
 /// @note  Discretized voxel coords must be in 21bit range [-1048576, 1048575].
+///        For example, if the downsampling resolution is 0.01 m, point coordinates must be in [-10485.76, 10485.75] m.
+///        Points outside the valid range will be ignored.
 /// @param points     Input points
 /// @param leaf_size  Downsampling resolution
 /// @return           Downsampled points
@@ -25,16 +27,17 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling(const InputPointCloud& poin
 
   const double inv_leaf_size = 1.0 / leaf_size;
 
-  const int coord_bit_size = 21;                       // Bits to represent each voxel coordinate (pack 21x3=63bits in 64bit int)
-  const size_t coord_bit_mask = (1 << 21) - 1;         // Bit mask
-  const int coord_offset = 1 << (coord_bit_size - 1);  // Coordinate offset to make values positive
+  constexpr std::uint64_t invalid_coord = std::numeric_limits<std::uint64_t>::max();
+  constexpr int coord_bit_size = 21;                       // Bits to represent each voxel coordinate (pack 21x3=63bits in 64bit int)
+  constexpr size_t coord_bit_mask = (1 << 21) - 1;         // Bit mask
+  constexpr int coord_offset = 1 << (coord_bit_size - 1);  // Coordinate offset to make values positive
 
   std::vector<std::pair<std::uint64_t, size_t>> coord_pt(traits::size(points));
   for (size_t i = 0; i < traits::size(points); i++) {
     const Eigen::Array4i coord = fast_floor(traits::point(points, i) * inv_leaf_size) + coord_offset;
     if ((coord < 0).any() || (coord > coord_bit_mask).any()) {
       std::cerr << "warning: voxel coord is out of range!!" << std::endl;
-      coord_pt[i] = {0, i};
+      coord_pt[i] = {invalid_coord, i};
       continue;
     }
 
@@ -56,6 +59,10 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling(const InputPointCloud& poin
   size_t num_points = 0;
   Eigen::Vector4d sum_pt = traits::point(points, coord_pt.front().second);
   for (size_t i = 1; i < traits::size(points); i++) {
+    if (coord_pt[i].first == invalid_coord) {
+      continue;
+    }
+
     if (coord_pt[i - 1].first != coord_pt[i].first) {
       traits::set_point(*downsampled, num_points++, sum_pt / sum_pt.w());
       sum_pt.setZero();

include/small_gicp/util/downsampling_omp.hpp

@@ -14,7 +14,11 @@
 namespace small_gicp {
 
 /// @brief Voxel grid downsampling with OpenMP backend.
+/// @note  This function has minor run-by-run non-deterministic behavior due to parallel data collection that results
+///        in a deviation of the number of points in the downsampling results (up to 10% increase from the single-thread version).
 /// @note  Discretized voxel coords must be in 21bit range [-1048576, 1048575].
+///        For example, if the downsampling resolution is 0.01 m, point coordinates must be in [-10485.76, 10485.75] m.
+///        Points outside the valid range will be ignored.
 /// @param points     Input points
 /// @param leaf_size  Downsampling resolution
 /// @return           Downsampled points
@@ -25,17 +29,19 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling_omp(const InputPointCloud&
   }
 
   const double inv_leaf_size = 1.0 / leaf_size;
-  const int coord_bit_size = 21;                       // Bits to represent each voxel coordinate (pack 21x3 = 63bits in 64bit int)
-  const size_t coord_bit_mask = (1 << 21) - 1;         // Bit mask
-  const int coord_offset = 1 << (coord_bit_size - 1);  // Coordinate offset to make values positive
+
+  constexpr std::uint64_t invalid_coord = std::numeric_limits<std::uint64_t>::max();
+  constexpr int coord_bit_size = 21;                       // Bits to represent each voxel coordinate (pack 21x3 = 63bits in 64bit int)
+  constexpr size_t coord_bit_mask = (1 << 21) - 1;         // Bit mask
+  constexpr int coord_offset = 1 << (coord_bit_size - 1);  // Coordinate offset to make values positive
 
   std::vector<std::pair<std::uint64_t, size_t>> coord_pt(traits::size(points));
 #pragma omp parallel for num_threads(num_threads) schedule(guided, 32)
   for (std::int64_t i = 0; i < traits::size(points); i++) {
     const Eigen::Array4i coord = fast_floor(traits::point(points, i) * inv_leaf_size) + coord_offset;
     if ((coord < 0).any() || (coord > coord_bit_mask).any()) {
       std::cerr << "warning: voxel coord is out of range!!" << std::endl;
-      coord_pt[i] = {0, i};
+      coord_pt[i] = {invalid_coord, i};
       continue;
     }
     // Compute voxel coord bits (0|1bit, z|21bit, y|21bit, x|21bit)
@@ -65,6 +71,10 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling_omp(const InputPointCloud&
 
     Eigen::Vector4d sum_pt = traits::point(points, coord_pt[block_begin].second);
     for (size_t i = block_begin + 1; i != block_end; i++) {
+      if (coord_pt[i].first == invalid_coord) {
+        continue;
+      }
+
       if (coord_pt[i - 1].first != coord_pt[i].first) {
         sub_points.emplace_back(sum_pt / sum_pt.w());
         sum_pt.setZero();

include/small_gicp/util/downsampling_tbb.hpp

@@ -14,7 +14,11 @@
 namespace small_gicp {
 
 /// @brief Voxel grid downsampling with TBB backend.
+/// @note  This function has minor run-by-run non-deterministic behavior due to parallel data collection that results
+///        in a deviation of the number of points in the downsampling results (up to 10% increase from the single-thread version).
 /// @note  Discretized voxel coords must be in 21bit range [-1048576, 1048575].
+///        For example, if the downsampling resolution is 0.01 m, point coordinates must be in [-10485.76, 10485.75] m.
+///        Points outside the valid range will be ignored.
 /// @param points     Input points
 /// @param leaf_size  Downsampling resolution
 /// @return           Downsampled points
@@ -25,22 +29,24 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling_tbb(const InputPointCloud&
   }
 
   const double inv_leaf_size = 1.0 / leaf_size;
-  const int coord_bit_size = 21;                       // Bits to represent each voxel coordinate (pack 21x3 = 63bits in 64bit int)
-  const size_t coord_bit_mask = (1 << 21) - 1;         // Bit mask
-  const int coord_offset = 1 << (coord_bit_size - 1);  // Coordinate offset to make values positive
+
+  constexpr std::uint64_t invalid_coord = std::numeric_limits<std::uint64_t>::max();
+  constexpr int coord_bit_size = 21;                       // Bits to represent each voxel coordinate (pack 21x3 = 63bits in 64bit int)
+  constexpr size_t coord_bit_mask = (1 << 21) - 1;         // Bit mask
+  constexpr int coord_offset = 1 << (coord_bit_size - 1);  // Coordinate offset to make values positive
 
   std::vector<std::pair<std::uint64_t, size_t>> coord_pt(traits::size(points));
   tbb::parallel_for(tbb::blocked_range<size_t>(0, traits::size(points), 64), [&](const tbb::blocked_range<size_t>& range) {
     for (size_t i = range.begin(); i != range.end(); i++) {
       const Eigen::Array4i coord = fast_floor(traits::point(points, i) * inv_leaf_size) + coord_offset;
       if ((coord < 0).any() || (coord > coord_bit_mask).any()) {
         std::cerr << "warning: voxel coord is out of range!!" << std::endl;
-        coord_pt[i] = {0, i};
+        coord_pt[i] = {invalid_coord, i};
         continue;
       }
 
       // Compute voxel coord bits (0|1bit, z|21bit, y|21bit, x|21bit)
-      const std::uint64_t bits =                                 //
+      const std::uint64_t bits =                                                           //
         (static_cast<std::uint64_t>(coord[0] & coord_bit_mask) << (coord_bit_size * 0)) |  //
         (static_cast<std::uint64_t>(coord[1] & coord_bit_mask) << (coord_bit_size * 1)) |  //
         (static_cast<std::uint64_t>(coord[2] & coord_bit_mask) << (coord_bit_size * 2));
@@ -63,6 +69,10 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling_tbb(const InputPointCloud&
 
     Eigen::Vector4d sum_pt = traits::point(points, coord_pt[range.begin()].second);
     for (size_t i = range.begin() + 1; i != range.end(); i++) {
+      if (coord_pt[i].first == invalid_coord) {
+        continue;
+      }
+
       if (coord_pt[i - 1].first != coord_pt[i].first) {
         sub_points.emplace_back(sum_pt / sum_pt.w());
         sum_pt.setZero();