mirror
/
small_gicp
mirror of https://github.com/koide3/small_gicp.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Documentation of detailed behaviors (#82) 

* detailed documentation (cpp)

* doc for invalid normal estimation results

* docs of detailed algorithm behavior for py
This commit is contained in:
koide3 2024-08-06 10:56:32 +09:00 committed by GitHub
parent aec35190eb
commit 0a2617d035
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
20 changed files with 230 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/small_gicp/ann/flat_container.hpp
View File

@ -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 @@ public:
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 @@ public:
/// @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 @@ public:
/// @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 {

6
include/small_gicp/ann/incremental_voxelmap.hpp
View File

@ -31,8 +31,9 @@ public:
};
/// @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 @@ public:
/// @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 @@ public:
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:

12
include/small_gicp/ann/kdtree.hpp
View File

@ -154,8 +154,8 @@ public:
/// @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 @@ public:
/// @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 @@ public:
/// @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 @@ public:
/// @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 @@ public:
/// @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 {

1
include/small_gicp/ann/knn_result.hpp
View File

@ -76,6 +76,7 @@ public:
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;

2
include/small_gicp/ann/projection.hpp
View File

@ -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 @@ public:
};
/// @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.

13
include/small_gicp/registration/registration_helper.hpp
View File

@ -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

2
include/small_gicp/registration/rejector.hpp
View File

@ -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

17
include/small_gicp/util/downsampling.hpp
View File

@ -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();

18
include/small_gicp/util/downsampling_omp.hpp
View File

@ -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<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)
@ -35,7 +41,7 @@ std::shared_ptr<OutputPointCloud> 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<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();

20
include/small_gicp/util/downsampling_tbb.hpp
View File

@ -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<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) {
@ -35,12 +41,12 @@ std::shared_ptr<OutputPointCloud> 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<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();

15
include/small_gicp/util/normal_estimation.hpp
View File

@ -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 <typename PointCloud>
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 <typename PointCloud>
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 <typename PointCloud>
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 <typename PointCloud>
@ -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 <typename PointCloud>
@ -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 <typename PointCloud>
@ -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

12
include/small_gicp/util/normal_estimation_omp.hpp
View File

@ -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

12
include/small_gicp/util/normal_estimation_tbb.hpp
View File

@ -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 <typename PointCloud>
@ -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 <typename PointCloud>
@ -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 <typename PointCloud>
@ -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

32
src/python/align.cpp
View File

@ -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<PointCloud>::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");
}

51
src/python/factors.cpp
View File

@ -20,7 +20,12 @@ using namespace small_gicp;
void define_factors(py::module& m) {
// DistanceRejector
py::class_<DistanceRejector>(m, "DistanceRejector", "Correspondence rejection based on the distance between points.")
py::class_<DistanceRejector>(
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_<ICPFactor>(m, "ICPFactor", "ICP per-point factor")
py::class_<ICPFactor>(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_<PointToPlaneICPFactor>(m, "PointToPlaneICPFactor", "Point-to-plane ICP per-point factor")
py::class_<PointToPlaneICPFactor>(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_<GICPFactor>(m, "GICPFactor", "Generalized ICP per-point factor") //
py::class_<GICPFactor>(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

20
src/python/kdtree.cpp
View File

@ -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).
)""");
}

2
src/python/misc.cpp
View File

@ -22,6 +22,6 @@ void define_misc(py::module& m) {
const auto points = read_ply(filename);
return std::make_shared<PointCloud>(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"));
}

2
src/python/pointcloud.cpp
View File

@ -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
----------

55
src/python/preprocess.cpp
View File

@ -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");
}

19
src/python/voxelmap.cpp
View File

@ -23,7 +23,12 @@ auto define_class(py::module& m, const std::string& name) {
py::init<double>(),
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<IncrementalVoxelMap<FlatContainerPoints>, false, false>(m, "IncrementalVoxelMap");
define_class<IncrementalVoxelMap<FlatContainerNormal>, true, false>(m, "IncrementalVoxelMapNormal");
define_class<IncrementalVoxelMap<FlatContainerCov>, false, true>(m, "IncrementalVoxelMapCov");
define_class<IncrementalVoxelMap<FlatContainerNormalCov>, true, true>(m, "FlatContainerNormalCov");
define_class<IncrementalVoxelMap<FlatContainerNormalCov>, true, true>(m, "IncrementalVoxelMapNormalCov");
define_class<GaussianVoxelMap, false, true>(m, "GaussianVoxelMap");
}