Skip to content

Commit

Permalink
Documentation of detailed behaviors (#82)
Browse files Browse the repository at this point in the history
* detailed documentation (cpp)

* doc for invalid normal estimation results

* docs of detailed algorithm behavior for py
  • Loading branch information
koide3 authored Aug 6, 2024
1 parent aec3519 commit 0a2617d
Show file tree
Hide file tree
Showing 20 changed files with 230 additions and 87 deletions.
6 changes: 4 additions & 2 deletions include/small_gicp/ann/flat_container.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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
Expand Down Expand Up @@ -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()) {
Expand All @@ -93,7 +95,7 @@ struct FlatContainer {
/// @param pt Query point
/// @param k Number of neighbors
/// @param k_index Indices of nearest neighbors
/// @param k_sq_dist Squared distances to nearest neighbors
/// @param k_sq_dist Squared distances to nearest neighbors (sorted in ascending order)
/// @return Number of found points
template <typename Result>
void knn_search(const Eigen::Vector4d& pt, Result& result) const {
Expand Down
6 changes: 4 additions & 2 deletions include/small_gicp/ann/incremental_voxelmap.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,9 @@ struct VoxelInfo {
};

/// @brief Incremental voxelmap.
/// This class supports incremental point cloud insertion and LRU-based voxel deletion.
/// This class supports incremental point cloud insertion and LRU-based voxel deletion that removes voxels that are not recently referenced.
/// @note This class can be used as a point cloud as well as a neighbor search structure.
/// @note This class can handle arbitrary number of voxels and arbitrary range of voxel coordinates (in 32-bit int range).
template <typename VoxelContents>
struct IncrementalVoxelMap {
public:
Expand Down Expand Up @@ -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>();
Expand Down Expand Up @@ -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:
Expand Down
12 changes: 6 additions & 6 deletions include/small_gicp/ann/kdtree.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand All @@ -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 {
Expand All @@ -178,7 +178,7 @@ struct UnsafeKdTree {
/// @brief Find k-nearest neighbors. This method uses fixed and static memory allocation. Might be faster for small k.
/// @param query Query point
/// @param k_indices Indices of neighbors
/// @param k_sq_dists Squared distances to neighbors
/// @param k_sq_dists Squared distances to neighbors (sorted in ascending order)
/// @param setting KNN search setting
/// @return Number of found neighbors
template <int N>
Expand Down Expand Up @@ -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 {
Expand All @@ -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 {
Expand Down
1 change: 1 addition & 0 deletions include/small_gicp/ann/knn_result.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 2 additions & 0 deletions include/small_gicp/ann/projection.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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.
Expand Down
13 changes: 9 additions & 4 deletions include/small_gicp/registration/registration_helper.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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);
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion include/small_gicp/registration/rejector.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 changes: 12 additions & 5 deletions include/small_gicp/util/downsampling.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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;
}

Expand All @@ -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();
Expand Down
18 changes: 14 additions & 4 deletions include/small_gicp/util/downsampling_omp.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -25,17 +29,19 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling_omp(const InputPointCloud&
}

const double inv_leaf_size = 1.0 / leaf_size;
const int coord_bit_size = 21; // Bits to represent each voxel coordinate (pack 21x3 = 63bits in 64bit int)
const size_t coord_bit_mask = (1 << 21) - 1; // Bit mask
const int coord_offset = 1 << (coord_bit_size - 1); // Coordinate offset to make values positive

constexpr std::uint64_t invalid_coord = std::numeric_limits<std::uint64_t>::max();
constexpr int coord_bit_size = 21; // Bits to represent each voxel coordinate (pack 21x3 = 63bits in 64bit int)
constexpr size_t coord_bit_mask = (1 << 21) - 1; // Bit mask
constexpr int coord_offset = 1 << (coord_bit_size - 1); // Coordinate offset to make values positive

std::vector<std::pair<std::uint64_t, size_t>> coord_pt(traits::size(points));
#pragma omp parallel for num_threads(num_threads) schedule(guided, 32)
for (std::int64_t i = 0; i < traits::size(points); i++) {
const Eigen::Array4i coord = fast_floor(traits::point(points, i) * inv_leaf_size) + coord_offset;
if ((coord < 0).any() || (coord > coord_bit_mask).any()) {
std::cerr << "warning: voxel coord is out of range!!" << std::endl;
coord_pt[i] = {0, i};
coord_pt[i] = {invalid_coord, i};
continue;
}
// Compute voxel coord bits (0|1bit, z|21bit, y|21bit, x|21bit)
Expand Down Expand Up @@ -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();
Expand Down
20 changes: 15 additions & 5 deletions include/small_gicp/util/downsampling_tbb.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -25,22 +29,24 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling_tbb(const InputPointCloud&
}

const double inv_leaf_size = 1.0 / leaf_size;
const int coord_bit_size = 21; // Bits to represent each voxel coordinate (pack 21x3 = 63bits in 64bit int)
const size_t coord_bit_mask = (1 << 21) - 1; // Bit mask
const int coord_offset = 1 << (coord_bit_size - 1); // Coordinate offset to make values positive

constexpr std::uint64_t invalid_coord = std::numeric_limits<std::uint64_t>::max();
constexpr int coord_bit_size = 21; // Bits to represent each voxel coordinate (pack 21x3 = 63bits in 64bit int)
constexpr size_t coord_bit_mask = (1 << 21) - 1; // Bit mask
constexpr int coord_offset = 1 << (coord_bit_size - 1); // Coordinate offset to make values positive

std::vector<std::pair<std::uint64_t, size_t>> coord_pt(traits::size(points));
tbb::parallel_for(tbb::blocked_range<size_t>(0, traits::size(points), 64), [&](const tbb::blocked_range<size_t>& range) {
for (size_t i = range.begin(); i != range.end(); i++) {
const Eigen::Array4i coord = fast_floor(traits::point(points, i) * inv_leaf_size) + coord_offset;
if ((coord < 0).any() || (coord > coord_bit_mask).any()) {
std::cerr << "warning: voxel coord is out of range!!" << std::endl;
coord_pt[i] = {0, i};
coord_pt[i] = {invalid_coord, i};
continue;
}

// Compute voxel coord bits (0|1bit, z|21bit, y|21bit, x|21bit)
const std::uint64_t bits = //
const std::uint64_t bits = //
(static_cast<std::uint64_t>(coord[0] & coord_bit_mask) << (coord_bit_size * 0)) | //
(static_cast<std::uint64_t>(coord[1] & coord_bit_mask) << (coord_bit_size * 1)) | //
(static_cast<std::uint64_t>(coord[2] & coord_bit_mask) << (coord_bit_size * 2));
Expand All @@ -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();
Expand Down
Loading

0 comments on commit 0a2617d

Please sign in to comment.