From 0a2617d035f2500869ef6ca2a16384bab1adddd3 Mon Sep 17 00:00:00 2001 From: koide3 <31344317+koide3@users.noreply.github.com> Date: Tue, 6 Aug 2024 10:56:32 +0900 Subject: [PATCH] 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 | 6 +- .../small_gicp/ann/incremental_voxelmap.hpp | 6 +- include/small_gicp/ann/kdtree.hpp | 12 ++-- include/small_gicp/ann/knn_result.hpp | 1 + include/small_gicp/ann/projection.hpp | 2 + .../registration/registration_helper.hpp | 13 +++-- include/small_gicp/registration/rejector.hpp | 2 +- include/small_gicp/util/downsampling.hpp | 17 ++++-- include/small_gicp/util/downsampling_omp.hpp | 18 ++++-- include/small_gicp/util/downsampling_tbb.hpp | 20 +++++-- include/small_gicp/util/normal_estimation.hpp | 15 +++++ .../small_gicp/util/normal_estimation_omp.hpp | 12 ++++ .../small_gicp/util/normal_estimation_tbb.hpp | 12 ++++ src/python/align.cpp | 32 +++++++---- src/python/factors.cpp | 51 ++++++++++++----- src/python/kdtree.cpp | 20 +++---- src/python/misc.cpp | 2 +- src/python/pointcloud.cpp | 2 +- src/python/preprocess.cpp | 55 +++++++++++++------ src/python/voxelmap.cpp | 19 +++++-- 20 files changed, 230 insertions(+), 87 deletions(-) diff --git a/include/small_gicp/ann/flat_container.hpp b/include/small_gicp/ann/flat_container.hpp index 0d48d180..cf6f83d4 100644 --- a/include/small_gicp/ann/flat_container.hpp +++ b/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 @@ -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 void knn_search(const Eigen::Vector4d& pt, Result& result) const { diff --git a/include/small_gicp/ann/incremental_voxelmap.hpp b/include/small_gicp/ann/incremental_voxelmap.hpp index f297ac2f..faa74f04 100644 --- a/include/small_gicp/ann/incremental_voxelmap.hpp +++ b/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 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: diff --git a/include/small_gicp/ann/kdtree.hpp b/include/small_gicp/ann/kdtree.hpp index 2141f708..b0d7b34f 100644 --- a/include/small_gicp/ann/kdtree.hpp +++ b/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 @@ -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 { diff --git a/include/small_gicp/ann/knn_result.hpp b/include/small_gicp/ann/knn_result.hpp index edaa4197..dcfc59bd 100644 --- a/include/small_gicp/ann/knn_result.hpp +++ b/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; diff --git a/include/small_gicp/ann/projection.hpp b/include/small_gicp/ann/projection.hpp index 9d584579..3a9af369 100644 --- a/include/small_gicp/ann/projection.hpp +++ b/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. diff --git a/include/small_gicp/registration/registration_helper.hpp b/include/small_gicp/registration/registration_helper.hpp index 8cb9ec6d..8e2390b9 100644 --- a/include/small_gicp/registration/registration_helper.hpp +++ b/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 std::pair>> preprocess_points(const std::vector>& 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 diff --git a/include/small_gicp/registration/rejector.hpp b/include/small_gicp/registration/rejector.hpp index 46481d41..1b307a41 100644 --- a/include/small_gicp/registration/rejector.hpp +++ b/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 diff --git a/include/small_gicp/util/downsampling.hpp b/include/small_gicp/util/downsampling.hpp index 7d6ff0de..97aab239 100644 --- a/include/small_gicp/util/downsampling.hpp +++ b/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 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::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> 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 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(); diff --git a/include/small_gicp/util/downsampling_omp.hpp b/include/small_gicp/util/downsampling_omp.hpp index 91975525..b616fe7b 100644 --- a/include/small_gicp/util/downsampling_omp.hpp +++ b/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,9 +29,11 @@ std::shared_ptr 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::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> coord_pt(traits::size(points)); #pragma omp parallel for num_threads(num_threads) schedule(guided, 32) @@ -35,7 +41,7 @@ std::shared_ptr voxelgrid_sampling_omp(const InputPointCloud& 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 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(); diff --git a/include/small_gicp/util/downsampling_tbb.hpp b/include/small_gicp/util/downsampling_tbb.hpp index 74047a39..8d5746e9 100644 --- a/include/small_gicp/util/downsampling_tbb.hpp +++ b/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,9 +29,11 @@ std::shared_ptr 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::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> coord_pt(traits::size(points)); tbb::parallel_for(tbb::blocked_range(0, traits::size(points), 64), [&](const tbb::blocked_range& range) { @@ -35,12 +41,12 @@ std::shared_ptr voxelgrid_sampling_tbb(const InputPointCloud& 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(coord[0] & coord_bit_mask) << (coord_bit_size * 0)) | // (static_cast(coord[1] & coord_bit_mask) << (coord_bit_size * 1)) | // (static_cast(coord[2] & coord_bit_mask) << (coord_bit_size * 2)); @@ -63,6 +69,10 @@ std::shared_ptr 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(); diff --git a/include/small_gicp/util/normal_estimation.hpp b/include/small_gicp/util/normal_estimation.hpp index d9cc661c..8fabf85b 100644 --- a/include/small_gicp/util/normal_estimation.hpp +++ b/include/small_gicp/util/normal_estimation.hpp @@ -8,6 +8,7 @@ namespace small_gicp { /// @brief Computes point normals from eigenvectors and sets them to the point cloud. +/// @note If a sufficient number of neighbor points is not found, a zero vector is set to the point. template struct NormalSetter { /// @brief Handle invalid case (too few points). @@ -25,6 +26,7 @@ struct NormalSetter { }; /// @brief Computes point covariances from eigenvectors and sets them to the point cloud. +/// @note If a sufficient number of neighbor points is not found, an identity matrix is set to the point. template struct CovarianceSetter { /// @brief Handle invalid case (too few points). @@ -44,6 +46,7 @@ struct CovarianceSetter { }; /// @brief Computes point normals and covariances from eigenvectors and sets them to the point cloud. +/// @note If a sufficient number of neighbor points is not found, a zero vector and an identity matrix are set to the point. template struct NormalCovarianceSetter { /// @brief Handle invalid case (too few points). @@ -107,6 +110,8 @@ void estimate_local_features(PointCloud& cloud, int num_neighbors) { } /// @brief Estimate point normals +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param num_neighbors Number of neighbors used for attribute estimation template @@ -115,6 +120,8 @@ void estimate_normals(PointCloud& cloud, int num_neighbors = 20) { } /// @brief Estimate point normals +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param kdtree Nearest neighbor search /// @param num_neighbors Number of neighbors used for attribute estimation @@ -124,6 +131,8 @@ void estimate_normals(PointCloud& cloud, KdTree& kdtree, int num_neighbors = 20) } /// @brief Estimate point covariances +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param num_neighbors Number of neighbors used for attribute estimation template @@ -132,6 +141,8 @@ void estimate_covariances(PointCloud& cloud, int num_neighbors = 20) { } /// @brief Estimate point covariances +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param kdtree Nearest neighbor search /// @param num_neighbors Number of neighbors used for attribute estimation @@ -141,6 +152,8 @@ void estimate_covariances(PointCloud& cloud, KdTree& kdtree, int num_neighbors = } /// @brief Estimate point normals and covariances +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param num_neighbors Number of neighbors used for attribute estimation template @@ -149,6 +162,8 @@ void estimate_normals_covariances(PointCloud& cloud, int num_neighbors = 20) { } /// @brief Estimate point normals and covariances +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param kdtree Nearest neighbor search /// @param num_neighbors Number of neighbors used for attribute estimation diff --git a/include/small_gicp/util/normal_estimation_omp.hpp b/include/small_gicp/util/normal_estimation_omp.hpp index cfd6e8b6..0a09bd73 100644 --- a/include/small_gicp/util/normal_estimation_omp.hpp +++ b/include/small_gicp/util/normal_estimation_omp.hpp @@ -26,6 +26,8 @@ void estimate_local_features_omp(PointCloud& cloud, KdTree& kdtree, int num_neig } /// @brief Estimate point normals with OpenMP +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param num_neighbors Number of neighbors used for attribute estimation /// @param num_threads Number of threads @@ -35,6 +37,8 @@ void estimate_normals_omp(PointCloud& cloud, int num_neighbors = 20, int num_thr } /// @brief Estimate point normals with OpenMP +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param kdtree Nearest neighbor search /// @param num_neighbors Number of neighbors used for attribute estimation @@ -45,6 +49,8 @@ void estimate_normals_omp(PointCloud& cloud, KdTree& kdtree, int num_neighbors = } /// @brief Estimate point covariances with OpenMP +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param num_neighbors Number of neighbors used for attribute estimation /// @param num_threads Number of threads @@ -54,6 +60,8 @@ void estimate_covariances_omp(PointCloud& cloud, int num_neighbors = 20, int num } /// @brief Estimate point covariances with OpenMP +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param kdtree Nearest neighbor search /// @param num_neighbors Number of neighbors used for attribute estimation @@ -64,6 +72,8 @@ void estimate_covariances_omp(PointCloud& cloud, KdTree& kdtree, int num_neighbo } /// @brief Estimate point normals and covariances with OpenMP +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param num_neighbors Number of neighbors used for attribute estimation /// @param num_threads Number of threads @@ -73,6 +83,8 @@ void estimate_normals_covariances_omp(PointCloud& cloud, int num_neighbors = 20, } /// @brief Estimate point normals and covariances with OpenMP +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param kdtree Nearest neighbor search /// @param num_neighbors Number of neighbors used for attribute estimation diff --git a/include/small_gicp/util/normal_estimation_tbb.hpp b/include/small_gicp/util/normal_estimation_tbb.hpp index e3089e42..65004587 100644 --- a/include/small_gicp/util/normal_estimation_tbb.hpp +++ b/include/small_gicp/util/normal_estimation_tbb.hpp @@ -30,6 +30,8 @@ void estimate_local_features_tbb(PointCloud& cloud, int num_neighbors) { } /// @brief Estimate point normals with TBB +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param num_neighbors Number of neighbors used for attribute estimation template @@ -38,6 +40,8 @@ void estimate_normals_tbb(PointCloud& cloud, int num_neighbors = 20) { } /// @brief Estimate point normals with TBB +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param kdtree Nearest neighbor search /// @param num_neighbors Number of neighbors used for attribute estimation @@ -47,6 +51,8 @@ void estimate_normals_tbb(PointCloud& cloud, KdTree& kdtree, int num_neighbors = } /// @brief Estimate point covariances with TBB +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param num_neighbors Number of neighbors used for attribute estimation template @@ -55,6 +61,8 @@ void estimate_covariances_tbb(PointCloud& cloud, int num_neighbors = 20) { } /// @brief Estimate point covariances with TBB +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param kdtree Nearest neighbor search /// @param num_neighbors Number of neighbors used for attribute estimation @@ -64,6 +72,8 @@ void estimate_covariances_tbb(PointCloud& cloud, KdTree& kdtree, int num_neighbo } /// @brief Estimate point normals and covariances with TBB +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param num_neighbors Number of neighbors used for attribute estimation template @@ -72,6 +82,8 @@ void estimate_normals_covariances_tbb(PointCloud& cloud, int num_neighbors = 20) } /// @brief Estimate point normals and covariances with TBB +/// @note If a sufficient number of neighbor points for normal/covariance estimation (5 points) is not found, +/// an invalid normal/covariance is set to the point (normal=zero vector, covariance=identity matrix). /// @param cloud [in/out] Point cloud /// @param kdtree Nearest neighbor search /// @param num_neighbors Number of neighbors used for attribute estimation diff --git a/src/python/align.cpp b/src/python/align.cpp index eecbeb07..d1935f1a 100644 --- a/src/python/align.cpp +++ b/src/python/align.cpp @@ -100,10 +100,13 @@ void define_align(py::module& m) { py::arg("verbose") = false, R"pbdoc( Align two point clouds using various ICP-like algorithms. + This function first performs preprocessing (downsampling, normal estimation, KdTree construction) and then estimates the transformation. + + See also: :class:`voxelgrid_sampling` :class:`estimate_normals` :class:`preprocess_points` Parameters ---------- - target_points : numpy.ndarray[np.float64] + target_points : :class:`numpy.ndarray[np.float64]` Nx3 or Nx4 matrix representing the target point cloud. source_points : numpy.ndarray[np.float64] Nx3 or Nx4 matrix representing the source point cloud. @@ -115,6 +118,7 @@ void define_align(py::module& m) { Resolution of voxels used for correspondence search (used only in VGICP). downsampling_resolution : float = 0.25 Resolution for downsampling the point clouds. + Input points out of the 21bit range after discretization will be ignored (See also: :class:`voxelgrid_sampling`). max_correspondence_distance : float = 1.0 Maximum distance for matching points between point clouds. num_threads : int = 1 @@ -126,7 +130,7 @@ void define_align(py::module& m) { Returns ------- - result : RegistrationResult + result : :class:`RegistrationResult` Object containing the final transformation matrix and convergence status. )pbdoc"); @@ -175,15 +179,17 @@ void define_align(py::module& m) { py::arg("verbose") = false, R"pbdoc( Align two point clouds using specified ICP-like algorithms, utilizing point cloud and KD-tree inputs. + Input point clouds are assumed to be preprocessed (downsampled, normals estimated, KD-tree built). + See also: :class:`voxelgrid_sampling` :class:`estimate_normals` :class:`preprocess_points` Parameters ---------- - target : PointCloud::ConstPtr - Pointer to the target point cloud. - source : PointCloud::ConstPtr - Pointer to the source point cloud. - target_tree : KdTree::ConstPtr, optional - Pointer to the KD-tree of the target for nearest neighbor search. If nullptr, a new tree is built. + target : :class:`PointCloud` + Target point cloud. + source : :class:`PointCloud` + Source point cloud. + target_tree : :class:`KdTree`, optional + KdTree for the target point cloud. If not given, a new KdTree is built. init_T_target_source : numpy.ndarray[np.float64] 4x4 matrix representing the initial transformation from target to source. registration_type : str = 'GICP' @@ -199,7 +205,7 @@ void define_align(py::module& m) { Returns ------- - result : RegistrationResult + result : :class:`RegistrationResult` Object containing the final transformation matrix and convergence status. )pbdoc"); @@ -231,12 +237,14 @@ void define_align(py::module& m) { py::arg("verbose") = false, R"pbdoc( Align two point clouds using voxel-based GICP algorithm, utilizing a Gaussian Voxel Map. + Input source point cloud is assumed to be preprocessed (downsampled, normals estimated, KD-tree built). + See also: :class:`voxelgrid_sampling` :class:`estimate_normals` :class:`preprocess_points` Parameters ---------- - target_voxelmap : GaussianVoxelMap + target_voxelmap : :class:`GaussianVoxelMap` Voxel map constructed from the target point cloud. - source : PointCloud + source : :class:`PointCloud` Source point cloud to align to the target. init_T_target_source : numpy.ndarray[np.float64] 4x4 matrix representing the initial transformation from target to source. @@ -251,7 +259,7 @@ void define_align(py::module& m) { Returns ------- - result : RegistrationResult + result : :class:`RegistrationResult` Object containing the final transformation matrix and convergence status. )pbdoc"); } diff --git a/src/python/factors.cpp b/src/python/factors.cpp index 5f581ab5..608d5c6e 100644 --- a/src/python/factors.cpp +++ b/src/python/factors.cpp @@ -20,7 +20,12 @@ using namespace small_gicp; void define_factors(py::module& m) { // DistanceRejector - py::class_(m, "DistanceRejector", "Correspondence rejection based on the distance between points.") + py::class_( + m, + "DistanceRejector", + R"pbdoc( + Correspondence rejection based on the distance between points. + )pbdoc") .def(py::init<>()) .def( "set_max_distance", @@ -36,7 +41,13 @@ void define_factors(py::module& m) { )pbdoc"); // ICPFactor - py::class_(m, "ICPFactor", "ICP per-point factor") + py::class_(m, "ICPFactor", R"pbdoc( + ICP per-point factor based on the conventional point-to-point distance. + + References + ---------- + Zhang, "Iterative Point Matching for Registration of Free-Form Curve", IJCV1994 + )pbdoc") .def(py::init<>()) .def( "linearize", @@ -61,11 +72,11 @@ void define_factors(py::module& m) { py::arg("source_index"), py::arg("rejector"), R"pbdoc( - Linearize the factor. + Linearize the factor for the i-th source point. Parameters ---------- - target : PointCloud + target : :class:`PointCloud` Target point cloud. source : PointCloud Source point cloud. @@ -81,7 +92,7 @@ void define_factors(py::module& m) { Returns ------- success: bool - Success flag. + Success flag. If false, the correspondence is rejected. H : numpy.ndarray Hessian matrix (6x6). b : numpy.ndarray @@ -91,7 +102,13 @@ void define_factors(py::module& m) { )pbdoc"); // PointToPlaneICPFactor - py::class_(m, "PointToPlaneICPFactor", "Point-to-plane ICP per-point factor") + py::class_(m, "PointToPlaneICPFactor", R"pbdoc( + Point-to-plane ICP per-point factor based on the point-to-plane distance. + + References + ---------- + Zhang, "Iterative Point Matching for Registration of Free-Form Curve", IJCV1994 + )pbdoc") .def(py::init<>()) .def( "linearize", @@ -116,12 +133,12 @@ void define_factors(py::module& m) { py::arg("source_index"), py::arg("rejector"), R"pbdoc( - Linearize the factor. + Linearize the factor for the i-th source point. Parameters ---------- target : PointCloud - Target point cloud. + Target point cloud. Must have normals. source : PointCloud Source point cloud. kdtree : KdTree @@ -136,7 +153,7 @@ void define_factors(py::module& m) { Returns ------- success: bool - Success flag. + Success flag. If false, the correspondence is rejected. H : numpy.ndarray Hessian matrix (6x6). b : numpy.ndarray @@ -146,7 +163,13 @@ void define_factors(py::module& m) { )pbdoc"); // GICPFactor - py::class_(m, "GICPFactor", "Generalized ICP per-point factor") // + py::class_(m, "GICPFactor", R"pbdoc( + Generalized ICP per-point factor based on distribution-to-distribution distance. + + References + ---------- + Segal et al., "Generalized-ICP", RSS2005 + )pbdoc") // .def(py::init<>()) .def( "linearize", @@ -171,14 +194,14 @@ void define_factors(py::module& m) { py::arg("source_index"), py::arg("rejector"), R"pbdoc( - Linearize the factor. + Linearize the factor for the i-th source point. Parameters ---------- target : PointCloud - Target point cloud. + Target point cloud. Must have covariances. source : PointCloud - Source point cloud. + Source point cloud. Must have covariances. kdtree : KdTree KdTree for the target point cloud. T : numpy.ndarray @@ -191,7 +214,7 @@ void define_factors(py::module& m) { Returns ------- success: bool - Success flag. + Success flag. If false, the correspondence is rejected. H : numpy.ndarray Hessian matrix (6x6). b : numpy.ndarray diff --git a/src/python/kdtree.cpp b/src/python/kdtree.cpp index fcb8936c..3d9d8c0f 100644 --- a/src/python/kdtree.cpp +++ b/src/python/kdtree.cpp @@ -37,11 +37,11 @@ void define_kdtree(py::module& m) { R"""( KdTree(points: numpy.ndarray, num_threads: int = 1) - Construct a KdTree from numpy. + Construct a KdTree from a numpy array. Parameters ---------- - points : numpy.ndarray, shape (n, 3) or (n, 4) + points : :class:`numpy.ndarray`, shape (n, 3) or (n, 4) The input point cloud. num_threads : int, optional The number of threads to use for KdTree construction. Default is 1. @@ -54,11 +54,11 @@ void define_kdtree(py::module& m) { R"""( KdTree(points: PointCloud, num_threads: int = 1) - Construct a KdTree from a point cloud. + Construct a KdTree from a small_gicp.PointCloud. Parameters ---------- - points : PointCloud + points : :class:`PointCloud` The input point cloud. num_threads : int, optional The number of threads to use for KdTree construction. Default is 1. @@ -86,7 +86,7 @@ void define_kdtree(py::module& m) { found : int Whether a neighbor was found (1 if found, 0 if not). k_index : int - The index of the nearest neighbor in the point cloud. + The index of the nearest neighbor in the point cloud. If a neighbor was not found, the index is -1. k_sq_dist : float The squared distance to the nearest neighbor. )""") @@ -114,9 +114,9 @@ void define_kdtree(py::module& m) { Returns ------- k_indices : numpy.ndarray, shape (k,) - The indices of the k nearest neighbors in the point cloud. + The indices of the k nearest neighbors in the point cloud. If a neighbor was not found, the index is -1. k_sq_dists : numpy.ndarray, shape (k,) - The squared distances to the k nearest neighbors. + The squared distances to the k nearest neighbors (Sorted in ascending order). )""") .def( @@ -155,7 +155,7 @@ void define_kdtree(py::module& m) { Returns ------- k_indices : numpy.ndarray, shape (n,) - The indices of the nearest neighbors for each input point. + The indices of the nearest neighbors for each input point. If a neighbor was not found, the index is -1. k_sq_dists : numpy.ndarray, shape (n,) The squared distances to the nearest neighbors for each input point. )""") @@ -201,8 +201,8 @@ void define_kdtree(py::module& m) { Returns ------- k_indices : list of numpy.ndarray, shape (n,) - The list of indices of the k nearest neighbors for each input point. + The list of indices of the k nearest neighbors for each input point. If a neighbor was not found, the index is -1. k_sq_dists : list of numpy.ndarray, shape (n,) - The list of squared distances to the k nearest neighbors for each input point. + The list of squared distances to the k nearest neighbors for each input point (sorted in ascending order). )"""); } diff --git a/src/python/misc.cpp b/src/python/misc.cpp index d350b6d4..9e647352 100644 --- a/src/python/misc.cpp +++ b/src/python/misc.cpp @@ -22,6 +22,6 @@ void define_misc(py::module& m) { const auto points = read_ply(filename); return std::make_shared(points); }, - "Read PLY file. This function can only read simple point clouds with XYZ properties for testing. Do not use this for general PLY IO.", + "Read PLY file. This function can only read simple point clouds with XYZ properties for testing purposes. Do not use this for general PLY IO.", py::arg("filename")); } \ No newline at end of file diff --git a/src/python/pointcloud.cpp b/src/python/pointcloud.cpp index 61e18ada..39197f8f 100644 --- a/src/python/pointcloud.cpp +++ b/src/python/pointcloud.cpp @@ -41,7 +41,7 @@ void define_pointcloud(py::module& m) { R"""( PointCloud(points: numpy.ndarray) - Construct a PointCloud from numpy. + Construct a PointCloud from a numpy array. Parameters ---------- diff --git a/src/python/preprocess.cpp b/src/python/preprocess.cpp index d1d843c0..a849c2ad 100644 --- a/src/python/preprocess.cpp +++ b/src/python/preprocess.cpp @@ -35,9 +35,18 @@ void define_preprocess(py::module& m) { R"pbdoc( Voxelgrid downsampling. + Notes + ----- + When multi-threading is enabled, 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). + + 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. + Parameters ---------- - points : PointCloud + points : :class:`PointCloud` Input point cloud. resolution : float Voxel size. @@ -46,7 +55,7 @@ void define_preprocess(py::module& m) { Returns ------- - PointCloud + :class:`PointCloud` Downsampled point cloud. )pbdoc"); @@ -71,6 +80,15 @@ void define_preprocess(py::module& m) { R"pbdoc( Voxelgrid downsampling. + Notes + ----- + When multi-threading is enabled, 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). + + 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. + Parameters ---------- points : [np.float64] @@ -82,7 +100,7 @@ void define_preprocess(py::module& m) { Returns ------- - PointCloud + :class:`PointCloud` Downsampled point cloud. )pbdoc"); @@ -106,12 +124,13 @@ void define_preprocess(py::module& m) { py::arg("num_threads") = 1, R"pbdoc( Estimate point normals. + If a sufficient number of neighbor points (5 points) is not found, a zero vector is set to the point. Parameters ---------- - points : PointCloud + points : :class:`PointCloud` Input point cloud. Normals will be estimated in-place. (in/out) - tree : KdTree, optional + tree : :class:`KdTree`, optional Nearest neighbor search. If None, create a new KdTree (default: None) num_neighbors : int, optional Number of neighbors. (default: 20) @@ -139,12 +158,13 @@ void define_preprocess(py::module& m) { py::arg("num_threads") = 1, R"pbdoc( Estimate point covariances. + If a sufficient number of neighbor points (5 points) is not found, an identity matrix is set to the point. Parameters ---------- - points : PointCloud + points : :class:`PointCloud` Input point cloud. Covariances will be estimated in-place. (in/out) - tree : KdTree, optional + tree : :class:`KdTree`, optional Nearest neighbor search. If None, create a new KdTree (default: None) num_neighbors : int, optional Number of neighbors. (default: 20) @@ -172,12 +192,13 @@ void define_preprocess(py::module& m) { py::arg("num_threads") = 1, R"pbdoc( Estimate point normals and covariances. + If a sufficient number of neighbor points (5 points) is not found, a zero vector and an identity matrix is set to the point. Parameters ---------- - points : PointCloud + points : :class:`PointCloud` Input point cloud. Normals and covariances will be estimated in-place. (in/out) - tree : KdTree, optional + tree : :class:`KdTree`, optional Nearest neighbor search. If None, create a new KdTree (default: None) num_neighbors : int, optional Number of neighbors. (default: 20) @@ -215,13 +236,14 @@ void define_preprocess(py::module& m) { py::arg("num_threads") = 1, R"pbdoc( Preprocess point cloud (Downsampling and normal/covariance estimation). + See also: :func:`voxelgrid_sampling`, :func:`estimate_normals_covariances`. Parameters ---------- points : [np.float64] Input point cloud. Nx3 or Nx4. downsampling_resolution : float, optional - Resolution for downsampling the point clouds. (default: 0.25) + Resolution for downsampling the point clouds. Input points out of 21bit range after discretization are ignored (See also: :func:`voxelgrid_sampling`). (default: 0.25) num_neighbors : int, optional Number of neighbors used for attribute estimation. (default: 10) num_threads : int, optional @@ -229,9 +251,9 @@ void define_preprocess(py::module& m) { Returns ------- - PointCloud + :class:`PointCloud` Downsampled point cloud. - KdTree + :class:`KdTree` KdTree for the downsampled point cloud. )pbdoc"); @@ -255,13 +277,14 @@ void define_preprocess(py::module& m) { py::arg("num_threads") = 1, R"pbdoc( Preprocess point cloud (Downsampling and normal/covariance estimation). + See also: :func:`voxelgrid_sampling`, :func:`estimate_normals_covariances`. Parameters ---------- - points : PointCloud + points : :class:`PointCloud` Input point cloud. downsampling_resolution : float, optional - Resolution for downsampling the point clouds. (default: 0.25) + Resolution for downsampling the point clouds. Input points out of 21bit range after discretization are ignored (See also: :func:`voxelgrid_sampling`). (default: 0.25) num_neighbors : int, optional Number of neighbors used for attribute estimation. (default: 10) num_threads : int, optional @@ -269,9 +292,9 @@ void define_preprocess(py::module& m) { Returns ------- - PointCloud + :class:`PointCloud` Downsampled point cloud. - KdTree + :class:`KdTree` KdTree for the downsampled point cloud. )pbdoc"); } \ No newline at end of file diff --git a/src/python/voxelmap.cpp b/src/python/voxelmap.cpp index bcb20531..380b0f95 100644 --- a/src/python/voxelmap.cpp +++ b/src/python/voxelmap.cpp @@ -23,7 +23,12 @@ auto define_class(py::module& m, const std::string& name) { py::init(), py::arg("leaf_size"), R"pbdoc( - Construct a VoxelMap. + Construct a Incremental voxelmap. + + Notes + ----- + This class supports incremental point cloud insertion and LRU-based voxel deletion that removes voxels that are not recently referenced. + It can handle arbitrary number of voxels and arbitrary range of voxel coordinates (in 32-bit int range). Parameters ---------- @@ -55,11 +60,17 @@ auto define_class(py::module& m, const std::string& name) { py::arg("points"), py::arg("T") = Eigen::Matrix4d::Identity(), R"pbdoc( - Insert a point cloud into the voxel map. + Insert a point cloud into the voxel map and delete voxels that are not recently accessed. + + Note + ---- + If this class is based on FlatContainer (i.e., IncrementalVoxelMap*), input points are ignored if + 1) there are too many points in the cell or + 2) the input point is too close to existing points in the cell. Parameters ---------- - points : PointCloud + points : :class:`PointCloud` Input source point cloud. T : numpy.ndarray, optional Transformation matrix to be applied to the input point cloud (i.e., T_voxelmap_source). (default: identity) @@ -136,6 +147,6 @@ void define_voxelmap(py::module& m) { define_class, false, false>(m, "IncrementalVoxelMap"); define_class, true, false>(m, "IncrementalVoxelMapNormal"); define_class, false, true>(m, "IncrementalVoxelMapCov"); - define_class, true, true>(m, "FlatContainerNormalCov"); + define_class, true, true>(m, "IncrementalVoxelMapNormalCov"); define_class(m, "GaussianVoxelMap"); } \ No newline at end of file