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

efficient voxelgrid

This commit is contained in:
k.koide 2024-03-24 15:33:09 +09:00
parent e09e5b8aea
commit dbc3efe0ae
32 changed files with 918 additions and 207 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
CMakeLists.txt
View File

@ -33,17 +33,33 @@ endif()
## Build ##
###########
add_executable(small_gicp_test
src/small_gicp_test.cpp
# add_executable(small_gicp_test
# src/small_gicp_test.cpp
# )
# target_include_directories(small_gicp_test PUBLIC
# include
# ${PCL_INCLUDE_DIRS}
# ${TBB_INCLUDE_DIRS}
# ${EIGEN3_INCLUDE_DIR}
# ${Iridescence_INCLUDE_DIRS}
# )
# target_link_libraries(small_gicp_test
# ${PCL_LIBRARIES}
# ${TBB_LIBRARIES}
# ${Iridescence_LIBRARIES}
# )
add_executable(downsampling_benchmark
src/downsampling_benchmark.cpp
)
target_include_directories(small_gicp_test PUBLIC
target_include_directories(downsampling_benchmark PUBLIC
include
${PCL_INCLUDE_DIRS}
${TBB_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
${Iridescence_INCLUDE_DIRS}
)
target_link_libraries(small_gicp_test
target_link_libraries(downsampling_benchmark
${PCL_LIBRARIES}
${TBB_LIBRARIES}
${Iridescence_LIBRARIES}

5
include/small_gicp/ann/cached_kdtree.hpp
View File

@ -7,12 +7,17 @@
namespace small_gicp {
/// @brief KdTree with voxel-based caching.
/// @note This class is usually useless.
template <typename PointCloud>
class CachedKdTree {
public:
using Ptr = std::shared_ptr<CachedKdTree>;
using ConstPtr = std::shared_ptr<const CachedKdTree>;
/// @brief Constructor
/// @param points Input points
/// @param leaf_size Cache voxel resolution
CachedKdTree(const PointCloud& points, double leaf_size) : inv_leaf_size(1.0 / leaf_size), kdtree(points) {}
size_t knn_search(const Eigen::Vector4d& pt, size_t k, size_t* k_indices, double* k_sq_dists) const {

224
include/small_gicp/ann/flat_voxelmap.hpp Normal file
View File

@ -0,0 +1,224 @@
#pragma once
#include <tbb/tbb.h>
#include <memory>
#include <small_gicp/ann/traits.hpp>
#include <small_gicp/points/traits.hpp>
#include <small_gicp/util/vector3i_hash.hpp>
namespace small_gicp {
struct FlatVoxelInfo {
public:
FlatVoxelInfo() : coord(0, 0, 0), num_indices(-1), index_loc(0) {}
Eigen::Vector3i coord;
int num_indices;
size_t index_loc;
};
struct IndexDistance {
bool operator<(const IndexDistance& rhs) const { return distance < rhs.distance; }
size_t index;
double distance;
};
struct FlatVoxelMap {
public:
using Ptr = std::shared_ptr<FlatVoxelMap>;
using ConstPtr = std::shared_ptr<const FlatVoxelMap>;
template <typename PointCloud>
FlatVoxelMap(double leaf_size, const PointCloud& points) : inv_leaf_size(1.0 / leaf_size),
seek_count(2),
points(points) {
set_offset_pattern(7);
create_table(points);
}
~FlatVoxelMap() {}
void set_offset_pattern(int num_offsets) {
offsets.clear();
switch (num_offsets) {
default:
std::cerr << "warning: num_offsets must be 1, 7, or 9 (num_offsets=" << num_offsets << ")" << std::endl;
case 1:
offsets = {Eigen::Vector3i(0, 0, 0)};
return;
case 7:
offsets = {
Eigen::Vector3i(0, 0, 0),
Eigen::Vector3i(-1, 0, 0),
Eigen::Vector3i(0, -1, 0),
Eigen::Vector3i(0, 0, -1),
Eigen::Vector3i(1, 0, 0),
Eigen::Vector3i(0, 1, 0),
Eigen::Vector3i(0, 0, 1)};
return;
case 27:
for (int i = -1; i <= 1; i++) {
for (int j = -1; j <= 1; j++) {
for (int k = -1; k <= 1; k++) {
offsets.emplace_back(i, j, k);
}
}
}
return;
}
}
size_t knn_search(const Eigen::Vector4d& pt, size_t k, size_t* k_indices, double* k_sq_dists) const {
const auto find_voxel = [&](const Eigen::Vector3i& coord) -> const FlatVoxelInfo* {
const size_t hash = XORVector3iHash::hash(coord);
for (size_t bucket_index = hash; bucket_index < hash + seek_count; bucket_index++) {
const auto& voxel = voxels[bucket_index % voxels.size()];
if (voxel.num_indices < 0) {
return nullptr;
} else if (voxel.coord == coord) {
return &voxel;
}
}
return nullptr;
};
std::vector<IndexDistance> v;
v.reserve(k);
std::priority_queue<IndexDistance> queue(std::less<IndexDistance>(), std::move(v));
const Eigen::Vector3i center_coord = (pt * inv_leaf_size).array().floor().cast<int>().head<3>();
for (const auto& offset : offsets) {
const Eigen::Vector3i coord = center_coord + offset;
const auto voxel = find_voxel(coord);
if (voxel == nullptr || voxel->num_indices < 0) {
continue;
}
const auto index_begin = indices.data() + voxel->index_loc;
for (auto index_itr = index_begin; index_itr != index_begin + voxel->num_indices; index_itr++) {
const double sq_dist = (traits::point(points, *index_itr) - pt).squaredNorm();
if (queue.size() < k) {
queue.push(IndexDistance{*index_itr, sq_dist});
} else if (sq_dist < queue.top().distance) {
queue.pop();
queue.push(IndexDistance{*index_itr, sq_dist});
}
}
}
const size_t n = queue.size();
while (!queue.empty()) {
const auto top = queue.top();
queue.pop();
k_indices[queue.size()] = top.index;
k_sq_dists[queue.size()] = top.distance;
}
return n;
}
private:
template <typename PointCloud>
void create_table(const PointCloud& points) {
const double min_sq_dist_in_cell = 0.05 * 0.05;
const int max_points_per_cell = 10;
const size_t buckets_size = traits::size(points);
std::vector<std::int64_t> assignment_table(max_points_per_cell * buckets_size, -1);
std::vector<Eigen::Vector3i> coords(traits::size(points));
tbb::parallel_for(static_cast<size_t>(0), static_cast<size_t>(traits::size(points)), [&](size_t i) {
const Eigen::Vector4d pt = traits::point(points, i);
const Eigen::Vector3i coord = (pt * inv_leaf_size).array().floor().template cast<int>().template head<3>();
coords[i] = coord;
const size_t hash = XORVector3iHash::hash(coord);
for (size_t bucket_index = hash; bucket_index < hash + seek_count; bucket_index++) {
auto slot_begin = assignment_table.data() + (bucket_index % buckets_size) * max_points_per_cell;
std::atomic_ref<std::int64_t> slot_begin_a(*slot_begin);
std::int64_t expected = -1;
if (slot_begin_a.compare_exchange_strong(expected, static_cast<std::int64_t>(i))) {
// Succeeded to insert the point in the first slot
break;
}
if (coords[expected] != coord) {
// If the bucket is already occupied with another voxel coord, try the next bucket
continue;
}
const double sq_dist = (traits::point(points, expected) - pt).squaredNorm();
if (sq_dist < min_sq_dist_in_cell) {
break;
}
for (auto slot = slot_begin + 1; slot != slot_begin + max_points_per_cell; slot++) {
std::int64_t expected = -1;
std::atomic_ref<std::int64_t> slot_a(*slot);
if (slot_a.compare_exchange_strong(expected, static_cast<std::int64_t>(i))) {
// Succeeded to insert the point
break;
}
const double sq_dist = (traits::point(points, expected) - pt).squaredNorm();
if (sq_dist < min_sq_dist_in_cell) {
// There already exists a very close point
break;
}
}
break;
}
});
indices.clear();
indices.reserve(buckets_size * max_points_per_cell);
voxels.clear();
voxels.resize(buckets_size);
for (size_t i = 0; i < buckets_size; i++) {
const auto slot_begin = assignment_table.data() + max_points_per_cell * i;
if (*slot_begin < 0) {
continue;
}
FlatVoxelInfo v;
v.coord = coords[*slot_begin];
v.index_loc = indices.size();
const auto slot_end = std::find(slot_begin, slot_begin + max_points_per_cell, -1);
v.num_indices = std::distance(slot_begin, slot_end);
std::copy(slot_begin, slot_end, std::back_inserter(indices));
voxels[i] = v;
}
indices.shrink_to_fit();
}
public:
const double inv_leaf_size;
const int seek_count;
std::vector<Eigen::Vector3i> offsets;
const PointCloud points;
std::vector<FlatVoxelInfo> voxels;
std::vector<size_t> indices;
};
namespace traits {
template <>
struct Traits<FlatVoxelMap> {
static size_t knn_search(const FlatVoxelMap& voxelmap, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) {
return voxelmap.knn_search(point, k, k_indices, k_sq_dists);
}
};
} // namespace traits
} // namespace small_gicp

63
include/small_gicp/points/gaussian_voxelmap.hpp → include/small_gicp/ann/gaussian_voxelmap.hpp
View File

@ -8,11 +8,18 @@
namespace small_gicp {
/// @brief Gaussian Voxel
struct GaussianVoxel {
public:
/// @brief Constructor
/// @param coord Voxel coordinates
GaussianVoxel(const Eigen::Vector3i& coord) : finalized(false), lru(0), num_points(0), coord(coord), mean(Eigen::Vector4d::Zero()), cov(Eigen::Matrix4d::Zero()) {}
~GaussianVoxel() {}
/// @brief Add a point (Gaussian distribution) to the voxel
/// @param mean Mean of the point
/// @param cov Covariance of the point
/// @param lru LRU cache counter
void add(const Eigen::Vector4d& mean, const Eigen::Matrix4d& cov, size_t lru) {
if (finalized) {
this->finalized = false;
@ -26,6 +33,7 @@ public:
this->lru = lru;
}
/// @brief Finalize mean and covariance
void finalize() {
if (finalized) {
return;
@ -37,27 +45,37 @@ public:
}
public:
bool finalized;
size_t lru;
size_t num_points;
Eigen::Vector3i coord;
bool finalized; ///< If true, mean and cov are finalized, otherwise they represent the sum of input points
size_t lru; ///< LRU counter
size_t num_points; ///< Number of input points
Eigen::Vector3i coord; ///< Voxel coordinates
Eigen::Vector4d mean;
Eigen::Matrix4d cov;
Eigen::Vector4d mean; ///< Mean
Eigen::Matrix4d cov; ///< Covariance
};
/// @brief Gaussian VoxelMap.
/// This class can be used as PointCloud as well as NearestNeighborSearch.
/// It also supports incremental points insertion and LRU-based voxel deletion.
struct GaussianVoxelMap {
public:
using Ptr = std::shared_ptr<GaussianVoxelMap>;
using ConstPtr = std::shared_ptr<const GaussianVoxelMap>;
/// @brief Constructor
/// @param leaf_size Voxel resolution
GaussianVoxelMap(double leaf_size) : inv_leaf_size(1.0 / leaf_size), lru_horizon(100), lru_clear_cycle(10), lru_counter(0) {}
~GaussianVoxelMap() {}
/// @brief Number of voxels
size_t size() const { return flat_voxels.size(); }
/// @brief Insert points to the voxelmap
/// @param points Input points
/// @param T Transformation
template <typename PointCloud>
void insert(const PointCloud& points, const Eigen::Isometry3d& T = Eigen::Isometry3d::Identity()) {
// Insert points to the voxelmap
for (size_t i = 0; i < traits::size(points); i++) {
const Eigen::Vector4d pt = T * traits::point(points, i);
const Eigen::Vector3i coord = (pt * inv_leaf_size).array().floor().cast<int>().head<3>();
@ -73,24 +91,25 @@ public:
voxel.add(pt, cov, lru_counter);
}
if ((lru_counter++) % lru_clear_cycle == 0) {
if ((++lru_counter) % lru_clear_cycle == 0) {
// Remove least recently used voxels
std::erase_if(flat_voxels, [&](const GaussianVoxel& voxel) { return voxel.lru + lru_horizon < lru_counter; });
voxels.clear();
// Rehash
for (size_t i = 0; i < flat_voxels.size(); i++) {
voxels[flat_voxels[i].coord] = i;
}
}
// Finalize voxel means and covs
for (auto& voxel : flat_voxels) {
voxel.finalize();
}
}
size_t knn_search(const Eigen::Vector4d& pt, size_t k, size_t* k_indices, double* k_sq_dists) const {
if (k != 1) {
std::cerr << "warning:!!" << std::endl;
}
/// @brief Find the nearest neighbor
size_t nearest_neighbor_search(const Eigen::Vector4d& pt, size_t* k_index, double* k_sq_dist) const {
const Eigen::Vector3i coord = (pt * inv_leaf_size).array().floor().cast<int>().head<3>();
const auto found = voxels.find(coord);
if (found == voxels.end()) {
@ -98,19 +117,19 @@ public:
}
const GaussianVoxel& voxel = flat_voxels[found->second];
k_indices[0] = found->second;
k_sq_dists[0] = (voxel.mean - pt).squaredNorm();
*k_index = found->second;
*k_sq_dist = (voxel.mean - pt).squaredNorm();
return 1;
}
public:
const double inv_leaf_size;
size_t lru_horizon;
size_t lru_clear_cycle;
size_t lru_counter;
const double inv_leaf_size; ///< Inverse of the voxel resolution
size_t lru_horizon; ///< LRU horizon size
size_t lru_clear_cycle; ///< LRU clear cycle
size_t lru_counter; ///< LRU counter
std::vector<GaussianVoxel> flat_voxels;
std::unordered_map<Eigen::Vector3i, size_t, XORVector3iHash> voxels;
std::vector<GaussianVoxel> flat_voxels; ///< Voxelmap contents
std::unordered_map<Eigen::Vector3i, size_t, XORVector3iHash> voxels; ///< Voxel index map
};
namespace traits {
@ -125,8 +144,8 @@ struct Traits<GaussianVoxelMap> {
static const Eigen::Vector4d& point(const GaussianVoxelMap& voxelmap, size_t i) { return voxelmap.flat_voxels[i].mean; }
static const Eigen::Matrix4d& cov(const GaussianVoxelMap& voxelmap, size_t i) { return voxelmap.flat_voxels[i].cov; }
static size_t knn_search(const GaussianVoxelMap& voxelmap, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) {
return voxelmap.knn_search(point, k, k_indices, k_sq_dists);
static size_t nearest_neighbor_search(const GaussianVoxelMap& voxelmap, const Eigen::Vector4d& point, size_t* k_index, double* k_sq_dist) {
return voxelmap.nearest_neighbor_search(point, k_index, k_sq_dist);
}
};

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

@ -7,6 +7,9 @@
namespace small_gicp {
/// @brief Unsafe KdTree
/// @note This class does not hold the ownership of the input points.
/// You must keep the input points along with this class.
template <typename PointCloud>
class UnsafeKdTree {
public:
@ -14,9 +17,12 @@ public:
using ConstPtr = std::shared_ptr<const UnsafeKdTree>;
using Index = nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<double, UnsafeKdTree<PointCloud>, double>, UnsafeKdTree<PointCloud>, 3, size_t>;
/// @brief Constructor
/// @param points Input points
UnsafeKdTree(const PointCloud& points) : points(points), index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10)) { index.buildIndex(); }
~UnsafeKdTree() {}
// Interfaces for nanoflann
size_t kdtree_get_point_count() const { return traits::size(points); }
double kdtree_get_pt(const size_t idx, const size_t dim) const { return traits::point(points, idx)[dim]; }
@ -25,27 +31,32 @@ public:
return false;
}
/// @brief Find k-nearest neighbors
size_t knn_search(const Eigen::Vector4d& pt, size_t k, size_t* k_indices, double* k_sq_dists) const { return index.knnSearch(pt.data(), k, k_indices, k_sq_dists); }
private:
const PointCloud& points;
Index index;
const PointCloud& points; ///< Input points
Index index; ///< KdTree index
};
/// @brief KdTree
template <typename PointCloud>
class KdTree {
public:
using Ptr = std::shared_ptr<KdTree>;
using ConstPtr = std::shared_ptr<const KdTree>;
/// @brief Constructor
/// @param points Input points
KdTree(const std::shared_ptr<const PointCloud>& points) : points(points), tree(*points) {}
~KdTree() {}
/// @brief Find k-nearest neighbors
size_t knn_search(const Eigen::Vector4d& pt, size_t k, size_t* k_indices, double* k_sq_dists) const { return tree.knn_search(pt, k, k_indices, k_sq_dists); }
private:
const std::shared_ptr<const PointCloud> points;
const UnsafeKdTree<PointCloud> tree;
const std::shared_ptr<const PointCloud> points; ///< Input points
const UnsafeKdTree<PointCloud> tree; ///< KdTree
};
namespace traits {

13
include/small_gicp/ann/traits.hpp
View File

@ -9,11 +9,24 @@ namespace traits {
template <typename T>
struct Traits;
/// @brief Find the nearest neighbor
/// @param tree Nearest neighbor search (e.g., KdTree)
/// @param point Query point
/// @param k_index [out] Index of the nearest neighbor
/// @param k_sq_dist [out] Squared distance to the nearest neighbor
/// @return 1 if a neighbor is found else 0
template <typename T>
size_t nearest_neighbor_search(const T& tree, const Eigen::Vector4d& point, size_t* k_index, double* k_sq_dist) {
return Traits<T>::nearest_neighbor_search(tree, point, k_index, k_sq_dist);
}
/// @brief Find k-nearest neighbors
/// @param tree Nearest neighbor search (e.g., KdTree)
/// @param point Query point
/// @param k Number of neighbors
/// @param k_index [out] Index of the nearest neighbor
/// @param k_sq_dist [out] Squared distance to the nearest neighbor
/// @return Number of found neighbors
template <typename T>
size_t knn_search(const T& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) {
return Traits<T>::knn_search(tree, point, k, k_indices, k_sq_dists);

27
include/small_gicp/factors/gicp_factor.hpp
View File

@ -8,9 +8,22 @@
namespace small_gicp {
/// @brief GICP (distribution-to-distribution) error factor
struct GICPFactor {
/// @brief Constructor
GICPFactor() : target_index(std::numeric_limits<size_t>::max()), source_index(std::numeric_limits<size_t>::max()), mahalanobis(Eigen::Matrix4d::Zero()) {}
/// @brief Linearize the factor
/// @param target Target point cloud
/// @param source Source point cloud
/// @param target_tree Nearest neighbor search for the target point cloud
/// @param T Linearization point
/// @param source_index Source point index
/// @param rejector Correspondence rejector
/// @param H Linearized precision matrix
/// @param b Linearized information vector
/// @param e Error at the linearization point
/// @return
template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename CorrespondenceRejector>
bool linearize(
const TargetPointCloud& target,
@ -30,7 +43,7 @@ struct GICPFactor {
size_t k_index;
double k_sq_dist;
if (!traits::knn_search(target_tree, transed_source_pt, 1, &k_index, &k_sq_dist) || rejector(T, k_index, source_index, k_sq_dist)) {
if (!traits::nearest_neighbor_search(target_tree, transed_source_pt, &k_index, &k_sq_dist) || rejector(T, k_index, source_index, k_sq_dist)) {
return false;
}
@ -52,6 +65,11 @@ struct GICPFactor {
return true;
}
/// @brief Evaluate error
/// @param target Target point cloud
/// @param source Source point cloud
/// @param T Evaluation point
/// @return Error
template <typename TargetPointCloud, typename SourcePointCloud>
double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T) const {
if (target_index == std::numeric_limits<size_t>::max()) {
@ -63,10 +81,11 @@ struct GICPFactor {
return 0.5 * residual.dot(mahalanobis * residual);
}
/// @brief Returns true if this factor is not rejected as an outlier
bool inlier() const { return target_index != std::numeric_limits<size_t>::max(); }
size_t target_index;
size_t source_index;
Eigen::Matrix4d mahalanobis;
size_t target_index; ///< Target point index
size_t source_index; ///< Source point index
Eigen::Matrix4d mahalanobis; ///< Fused precision matrix
};
} // namespace small_gicp

3
include/small_gicp/factors/icp_factor.hpp
View File

@ -8,6 +8,7 @@
namespace small_gicp {
/// @brief Point-to-point error factor
struct ICPFactor {
ICPFactor() : target_index(std::numeric_limits<size_t>::max()), source_index(std::numeric_limits<size_t>::max()) {}
@ -30,7 +31,7 @@ struct ICPFactor {
size_t k_index;
double k_sq_dist;
if (!traits::knn_search(target_tree, transed_source_pt, 1, &k_index, &k_sq_dist) || rejector(T, k_index, source_index, k_sq_dist)) {
if (!traits::nearest_neighbor_search(target_tree, transed_source_pt, &k_index, &k_sq_dist) || rejector(T, k_index, source_index, k_sq_dist)) {
return false;
}

3
include/small_gicp/factors/plane_icp_factor.hpp
View File

@ -8,6 +8,7 @@
namespace small_gicp {
/// @brief Point-to-plane error factor
struct PointToPlaneICPFactor {
PointToPlaneICPFactor() : target_index(std::numeric_limits<size_t>::max()), source_index(std::numeric_limits<size_t>::max()) {}
@ -30,7 +31,7 @@ struct PointToPlaneICPFactor {
size_t k_index;
double k_sq_dist;
if (!traits::knn_search(target_tree, transed_source_pt, 1, &k_index, &k_sq_dist) || rejector(T, k_index, source_index, k_sq_dist)) {
if (!traits::nearest_neighbor_search(target_tree, transed_source_pt, &k_index, &k_sq_dist) || rejector(T, k_index, source_index, k_sq_dist)) {
return false;
}

2
include/small_gicp/points/pcl_point.hpp
View File

@ -7,6 +7,7 @@ namespace pcl {
using Matrix4fMap = Eigen::Map<Eigen::Matrix4f, Eigen::Aligned>;
using Matrix4fMapConst = const Eigen::Map<const Eigen::Matrix4f, Eigen::Aligned>;
/// @brief Point with covariance
struct PointCovariance {
PCL_ADD_POINT4D;
Eigen::Matrix4f cov;
@ -17,6 +18,7 @@ struct PointCovariance {
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
/// @brief Point with normal and covariance
struct PointNormalCovariance {
PCL_ADD_POINT4D;
PCL_ADD_NORMAL4D

30
include/small_gicp/points/point_cloud.hpp
View File

@ -5,14 +5,19 @@
namespace small_gicp {
/**
* @brief Point cloud
*/
struct PointCloud {
public:
using Ptr = std::shared_ptr<PointCloud>;
using ConstPtr = std::shared_ptr<const PointCloud>;
/// @brief Constructor
PointCloud() {}
~PointCloud() {}
/// @brief Constructor
/// @param points Points to initialize the point cloud
template <typename T, int D, typename Allocator>
PointCloud(const std::vector<Eigen::Matrix<T, D, 1>, Allocator>& points) {
this->resize(points.size());
@ -21,25 +26,42 @@ public:
}
}
/// @brief Destructor
~PointCloud() {}
/// @brief Number of points
size_t size() const { return points.size(); }
/// @brief Resize point/normal/cov buffers
/// @param n Number of points
void resize(size_t n) {
points.resize(n);
normals.resize(n);
covs.resize(n);
}
/// @brief Get i-th point
Eigen::Vector4d& point(size_t i) { return points[i]; }
/// @brief Get i-th normal
Eigen::Vector4d& normal(size_t i) { return normals[i]; }
/// @brief Get i-th covariance
Eigen::Matrix4d& cov(size_t i) { return covs[i]; }
/// @brief Get i-th point (const)
const Eigen::Vector4d& point(size_t i) const { return points[i]; }
/// @brief Get i-th normal (const)
const Eigen::Vector4d& normal(size_t i) const { return normals[i]; }
/// @brief Get i-th covariance (const)
const Eigen::Matrix4d& cov(size_t i) const { return covs[i]; }
public:
std::vector<Eigen::Vector4d> points;
std::vector<Eigen::Vector4d> normals;
std::vector<Eigen::Matrix4d> covs;
std::vector<Eigen::Vector4d> points; ///< Point coordinates
std::vector<Eigen::Vector4d> normals; ///< Point normals
std::vector<Eigen::Matrix4d> covs; ///< Point covariances
};
namespace traits {

11
include/small_gicp/points/traits.hpp
View File

@ -9,56 +9,67 @@ namespace traits {
template <typename T>
struct Traits;
/// @brief Get the number of points
template <typename T>
size_t size(const T& points) {
return Traits<T>::size(points);
}
/// @brief Check if the point cloud has points
template <typename T>
bool has_points(const T& points) {
return Traits<T>::has_points(points);
}
/// @brief Check if the point cloud has normals
template <typename T>
bool has_normals(const T& points) {
return Traits<T>::has_normals(points);
}
/// @brief Check if the point cloud has covariances
template <typename T>
bool has_covs(const T& points) {
return Traits<T>::has_covs(points);
}
/// @brief Get i-th point
template <typename T>
auto point(const T& points, size_t i) {
return Traits<T>::point(points, i);
}
/// @brief Get i-th normal
template <typename T>
auto normal(const T& points, size_t i) {
return Traits<T>::normal(points, i);
}
/// @brief Get i-th covariance
template <typename T>
auto cov(const T& points, size_t i) {
return Traits<T>::cov(points, i);
}
/// @brief Resize the point cloud (this function should resize all attributes)
template <typename T>
void resize(T& points, size_t n) {
Traits<T>::resize(points, n);
}
/// @brief Set i-th point
template <typename T>
void set_point(T& points, size_t i, const Eigen::Vector4d& pt) {
Traits<T>::set_point(points, i, pt);
}
/// @brief Set i-th normal
template <typename T>
void set_normal(T& points, size_t i, const Eigen::Vector4d& pt) {
Traits<T>::set_normal(points, i, pt);
}
/// @brief Set i-th covariance
template <typename T>
void set_cov(T& points, size_t i, const Eigen::Matrix4d& cov) {
Traits<T>::set_cov(points, i, cov);

18
include/small_gicp/registration/optimizer.hpp
View File

@ -5,6 +5,7 @@
namespace small_gicp {
/// @brief GaussNewton optimizer
struct GaussNewtonOptimizer {
GaussNewtonOptimizer() : verbose(false), max_iterations(20), lambda(1e-6) {}
@ -52,11 +53,12 @@ struct GaussNewtonOptimizer {
return result;
}
bool verbose;
int max_iterations;
double lambda;
bool verbose; ///< If true, print debug messages
int max_iterations; ///< Max number of optimization iterations
double lambda; ///< Damping factor (Increasing this makes optimization slow but stable)
};
/// @brief LevenbergMarquardt optimizer
struct LevenbergMarquardtOptimizer {
LevenbergMarquardtOptimizer() : verbose(false), max_iterations(20), max_inner_iterations(10), init_lambda(1e-3), lambda_factor(10.0) {}
@ -121,11 +123,11 @@ struct LevenbergMarquardtOptimizer {
return result;
}
bool verbose;
int max_iterations;
int max_inner_iterations;
double init_lambda;
double lambda_factor;
bool verbose; ///< If true, print debug messages
int max_iterations; ///< Max number of optimization iterations
int max_inner_iterations; ///< Max number of inner iterations (lambda-trial)
double init_lambda; ///< Initial lambda (damping factor)
double lambda_factor; ///< Lambda increase factor
};
} // namespace small_gicp

15
include/small_gicp/registration/reduction.hpp
View File

@ -4,7 +4,16 @@
namespace small_gicp {
/// @brief Single-thread reduction
struct SerialReduction {
/// @brief Sum up linearized systems
/// @param target Target point cloud
/// @param source Source point cloud
/// @param target_tree Nearest neighbor search for the target point cloud
/// @param rejector Correspondence rejector
/// @param T Linearization point
/// @param factors Factors to be linearized
/// @return Sum of the linearized systems (Precision matrix, information vector, and error)
template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename CorrespondenceRejector, typename Factor>
std::tuple<Eigen::Matrix<double, 6, 6>, Eigen::Matrix<double, 6, 1>, double> linearize(
const TargetPointCloud& target,
@ -34,6 +43,12 @@ struct SerialReduction {
return {sum_H, sum_b, sum_e};
}
/// @brief Sum up evaluated errors
/// @param target Target point cloud
/// @param source Source point cloud
/// @param T Evaluation point
/// @param factors Factors to be evaluated
/// @return Sum of the evaluated errors
template <typename TargetPointCloud, typename SourcePointCloud, typename Factor>
double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector<Factor>& factors) {
double sum_e = 0.0;

3
include/small_gicp/registration/reduction_omp.hpp
View File

@ -4,6 +4,7 @@
namespace small_gicp {
/// @brief Parallel reduction with OpenMP backend
struct ParallelReductionOMP {
ParallelReductionOMP() : num_threads(8) {}
@ -55,7 +56,7 @@ struct ParallelReductionOMP {
return sum_e;
}
int num_threads;
int num_threads; ///< Number of threads
};
} // namespace small_gicp

3
include/small_gicp/registration/reduction_tbb.hpp
View File

@ -5,6 +5,7 @@
namespace small_gicp {
/// @brief Summation for linearized systems
template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename CorrespondenceRejector, typename Factor>
struct LinearizeSum {
LinearizeSum(
@ -77,6 +78,7 @@ struct LinearizeSum {
double e;
};
/// @brief Summation for evaluated errors
template <typename TargetPointCloud, typename SourcePointCloud, typename Factor>
struct ErrorSum {
ErrorSum(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector<Factor>& factors)
@ -106,6 +108,7 @@ struct ErrorSum {
double e;
};
/// @brief Parallel reduction with TBB backend
struct ParallelReductionTBB {
ParallelReductionTBB() {}

29
include/small_gicp/registration/registration.hpp
View File

@ -4,35 +4,32 @@
#include <small_gicp/points/traits.hpp>
#include <small_gicp/registration/rejector.hpp>
#include <small_gicp/registration/reduction.hpp>
#include <small_gicp/registration/registration_result.hpp>
#include <small_gicp/registration/optimizer.hpp>
#include <guik/viewer/light_viewer.hpp>
#include <small_gicp/registration/termination_criteria.hpp>
#include <small_gicp/registration/registration_result.hpp>
namespace small_gicp {
struct TerminationCriteria {
TerminationCriteria() : translation_eps(1e-3), rotation_eps(0.1 * M_PI / 180.0) {}
bool converged(const Eigen::Matrix<double, 6, 1>& delta) const { return delta.template head<3>().norm() < rotation_eps && delta.template tail<3>().norm() < translation_eps; }
double translation_eps;
double rotation_eps;
};
/// @brief Point cloud registration
template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename Factor, typename CorrespondenceRejector, typename Reduction, typename Optimizer>
struct Registration {
public:
/// @brief Align point clouds
/// @param target Target point cloud
/// @param source Source point cloud
/// @param target_tree Nearest neighbor search for the target point cloud
/// @param init_T Initial guess
/// @return Registration result
RegistrationResult align(const TargetPointCloud& target, const SourcePointCloud& source, const TargetTree& target_tree, const Eigen::Isometry3d& init_T) {
std::vector<Factor> factors(traits::size(source));
return optimizer.optimize(target, source, target_tree, rejector, criteria, reduction, init_T, factors);
}
public:
TerminationCriteria criteria;
CorrespondenceRejector rejector;
Reduction reduction;
Optimizer optimizer;
TerminationCriteria criteria; ///< Termination criteria
CorrespondenceRejector rejector; ///< Correspondence rejector
Reduction reduction; ///< Reduction
Optimizer optimizer; ///< Optimizer
};
} // namespace small_gicp

15
include/small_gicp/registration/registration_result.hpp
View File

@ -5,6 +5,7 @@
namespace small_gicp {
/// @brief Registration result
struct RegistrationResult {
RegistrationResult(const Eigen::Isometry3d& T)
: T_target_source(T),
@ -15,15 +16,15 @@ struct RegistrationResult {
b(Eigen::Matrix<double, 6, 1>::Zero()),
error(0.0) {}
Eigen::Isometry3d T_target_source;
Eigen::Isometry3d T_target_source; ///< Estimated transformation
bool converged;
size_t iterations;
size_t num_inliers;
bool converged; ///< If the optimization converged
size_t iterations; ///< Number of optimization iterations
size_t num_inliers; ///< Number of inliear points
Eigen::Matrix<double, 6, 6> H;
Eigen::Matrix<double, 6, 1> b;
double error;
Eigen::Matrix<double, 6, 6> H; ///< Final precision matrix
Eigen::Matrix<double, 6, 1> b; ///< Final information vector
double error; ///< Final error
};
} // namespace small_gicp

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

@ -5,10 +5,12 @@
namespace small_gicp {
/// @brief Null correspondence rejector. This class accepts all input correspondences.
struct NullRejector {
bool operator()(const Eigen::Isometry3d& T, size_t target_index, size_t source_index, double sq_dist) const { return false; }
};
/// @brief Rejecting correspondences with large distances.
struct DistanceRejector {
DistanceRejector() : max_dist_sq(1.0) {}
DistanceRejector(double max_dist) : max_dist_sq(max_dist * max_dist) {}

21
include/small_gicp/registration/termination_criteria.hpp Normal file
View File

@ -0,0 +1,21 @@
#pragma once
#include <Eigen/Core>
namespace small_gicp {
/// @brief Registration termination criteria
struct TerminationCriteria {
/// @brief Constructor
TerminationCriteria() : translation_eps(1e-3), rotation_eps(0.1 * M_PI / 180.0) {}
/// @brief Check the convergence
/// @param delta Transformation update vector
/// @return True if converged
bool converged(const Eigen::Matrix<double, 6, 1>& delta) const { return delta.template head<3>().norm() < rotation_eps && delta.template tail<3>().norm() < translation_eps; }
double translation_eps; ///< Rotation tolerance [rad]
double rotation_eps; ///< Translation tolerance
};
} // namespace small_gicp

65
include/small_gicp/util/benchmark.hpp Normal file
View File

@ -0,0 +1,65 @@
#pragma once
#include <chrono>
#include <vector>
#include <algorithm>
#include <filesystem>
#include <Eigen/Core>
#include <small_gicp/points/traits.hpp>
#include <small_gicp/util/read_points.hpp>
namespace small_gicp {
struct Stopwatch {
public:
void start() { t1 = std::chrono::high_resolution_clock::now(); }
void stop() { t2 = std::chrono::high_resolution_clock::now(); }
double elapsed_sec() const { return std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e9; }
double elapsed_msec() const { return std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e6; }
public:
std::chrono::high_resolution_clock::time_point t1;
std::chrono::high_resolution_clock::time_point t2;
};
struct KittiDataset {
public:
KittiDataset(const std::string& dataset_path, size_t max_num_data = std::numeric_limits<size_t>::max()) {
std::vector<std::string> filenames;
for (auto path : std::filesystem::directory_iterator(dataset_path)) {
if (path.path().extension() != ".bin") {
continue;
}
filenames.emplace_back(path.path().string());
}
std::ranges::sort(filenames);
if (filenames.size() > max_num_data) {
filenames.resize(max_num_data);
}
points.resize(max_num_data);
std::ranges::transform(filenames, points.begin(), [](const std::string& filename) { return read_points(filename); });
std::erase_if(points, [](const auto& p) { return p.empty(); });
}
template <typename PointCloud>
std::vector<std::shared_ptr<PointCloud>> convert() const {
std::vector<std::shared_ptr<PointCloud>> converted(points.size());
std::ranges::transform(points, converted.begin(), [](const auto& raw_points) {
auto points = std::make_shared<PointCloud>();
traits::resize(*points, raw_points.size());
for (size_t i = 0; i < raw_points.size(); i++) {
traits::set_point(*points, i, raw_points[i].template cast<double>());
}
return points;
});
return converted;
}
public:
std::vector<std::vector<Eigen::Vector4f>> points;
};
} // namespace small_gicp

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

@ -8,81 +8,58 @@
namespace small_gicp {
/// @brief Voxelgrid downsampling
/// @param points Input points
/// @param leaf_size Downsampling resolution
/// @return Downsampled points
template <typename InputPointCloud, typename OutputPointCloud = InputPointCloud>
std::shared_ptr<OutputPointCloud> voxelgrid_sampling(const InputPointCloud& points, double leaf_size) {
const double inv_leaf_size = 1.0 / leaf_size;
std::unordered_map<Eigen::Vector3i, Eigen::Vector4d, XORVector3iHash> voxels;
for (size_t i = 0; i < traits::size(points); i++) {
const auto& pt = traits::point(points, i);
const Eigen::Vector3i coord = (pt * inv_leaf_size).array().floor().template cast<int>().template head<3>();
auto found = voxels.find(coord);
if (found == voxels.end()) {
found = voxels.emplace_hint(found, coord, Eigen::Vector4d::Zero());
}
found->second += pt;
}
auto downsampled = std::make_shared<OutputPointCloud>();
traits::resize(*downsampled, voxels.size());
size_t i = 0;
for (const auto& v : voxels) {
traits::set_point(*downsampled, i++, v.second / v.second.w());
}
return downsampled;
}
template <typename InputPointCloud, typename OutputPointCloud = InputPointCloud>
std::shared_ptr<OutputPointCloud> randomgrid_sampling(const InputPointCloud& points, double leaf_size, size_t target_num_points, std::mt19937& rng) {
if (traits::size(points) <= target_num_points) {
auto downsampled = std::make_shared<OutputPointCloud>();
traits::resize(*downsampled, traits::size(points));
for (size_t i = 0; i < traits::size(points); i++) {
traits::set_point(*downsampled, i, traits::point(points, i));
}
return downsampled;
if (traits::size(points) == 0) {
std::cerr << "warning: empty input points!!" << std::endl;
return std::make_shared<OutputPointCloud>();
}
const double inv_leaf_size = 1.0 / leaf_size;
using Indices = std::shared_ptr<std::vector<size_t>>;
std::unordered_map<Eigen::Vector3i, Indices, XORVector3iHash> voxels;
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
std::vector<std::pair<std::uint64_t, size_t>> coord_pt(points.size());
for (size_t i = 0; i < traits::size(points); i++) {
// TODO: Check if coord in 21bit range
const auto& pt = traits::point(points, i);
const Eigen::Array4i coord = (pt * inv_leaf_size).array().floor().template cast<int>() + coord_offset;
const Eigen::Vector3i coord = (pt * inv_leaf_size).array().floor().template cast<int>().template head<3>();
auto found = voxels.find(coord);
if (found == voxels.end()) {
found = voxels.emplace_hint(found, coord, std::make_shared<std::vector<size_t>>());
found->second->reserve(20);
}
found->second->emplace_back(i);
// Compute voxel coord bits (0|1bit, z|21bit, y|21bit, x|21bit)
const std::uint64_t bits = //
((coord[0] & coord_bit_mask) << (coord_bit_size * 0)) | //
((coord[1] & coord_bit_mask) << (coord_bit_size * 1)) | //
((coord[2] & coord_bit_mask) << (coord_bit_size * 2));
coord_pt[i] = {bits, i};
}
const size_t points_per_voxel = std::ceil(static_cast<double>(target_num_points) / voxels.size());
std::vector<size_t> indices;
indices.reserve(points_per_voxel * voxels.size());
for (const auto& voxel : voxels) {
const auto& voxel_indices = *voxel.second;
if (voxel_indices.size() <= points_per_voxel) {
indices.insert(indices.end(), voxel_indices.begin(), voxel_indices.end());
} else {
std::ranges::sample(voxel_indices, std::back_inserter(indices), points_per_voxel, rng);
}
}
std::ranges::sort(indices);
// Sort by voxel coord
const auto compare = [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; };
std::ranges::sort(coord_pt, compare);
auto downsampled = std::make_shared<OutputPointCloud>();
traits::resize(*downsampled, indices.size());
for (size_t i = 0; i < indices.size(); i++) {
traits::set_point(*downsampled, i, traits::point(points, indices[i]));
traits::resize(*downsampled, traits::size(points));
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 - 1].first != coord_pt[i].first) {
traits::set_point(*downsampled, num_points++, sum_pt / sum_pt.w());
sum_pt.setZero();
}
sum_pt += traits::point(points, coord_pt[i].second);
}
traits::set_point(*downsampled, num_points++, sum_pt / sum_pt.w());
traits::resize(*downsampled, num_points);
return downsampled;
}

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

@ -1,5 +1,6 @@
#pragma once
#include <atomic>
#include <memory>
#include <tbb/tbb.h>
@ -8,59 +9,66 @@
namespace small_gicp {
/**
* @brief Voxel grid downsampling using TBB.
* @note This TBB version brings only a minor speedup compared to the single-thread version (e.g., 32-threads -> 1.4x speedup), and is not worth using usually.
*/
/// @brief Voxel grid downsampling using TBB.
/// @param points Input points
/// @param leaf_size Downsampling resolution
/// @return Downsampled points
template <typename InputPointCloud, typename OutputPointCloud = InputPointCloud>
std::shared_ptr<OutputPointCloud> voxelgrid_sampling_tbb(const InputPointCloud& points, double leaf_size) {
if (traits::size(points) == 0) {
std::cerr << "warning: empty input points!!" << std::endl;
return std::make_shared<OutputPointCloud>();
}
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
typedef tbb::concurrent_hash_map<Eigen::Vector3i, int, XORVector3iHash> VoxelMap;
std::vector<std::pair<std::uint64_t, size_t>> coord_pt(points.size());
tbb::parallel_for(size_t(0), size_t(traits::size(points)), [&](size_t i) {
// TODO: Check if coord in 21bit range
const Eigen::Vector4d pt = traits::point(points, i);
const Eigen::Array4i coord = (pt * inv_leaf_size).array().floor().template cast<int>() + coord_offset;
std::atomic_uint64_t num_voxels = 0;
VoxelMap voxels;
std::vector<Eigen::Vector4d> voxel_values(traits::size(points), Eigen::Vector4d::Zero());
// Compute voxel coord bits (0|1bit, z|21bit, y|21bit, x|21bit)
const std::uint64_t bits = //
((coord[0] & coord_bit_mask) << (coord_bit_size * 0)) | //
((coord[1] & coord_bit_mask) << (coord_bit_size * 1)) | //
((coord[2] & coord_bit_mask) << (coord_bit_size * 2));
coord_pt[i] = {bits, i};
});
const int chunk_size = 8;
tbb::parallel_for(tbb::blocked_range<size_t>(0, traits::size(points), chunk_size), [&](const tbb::blocked_range<size_t>& range) {
std::vector<Eigen::Vector3i> coords;
std::vector<Eigen::Vector4d> values;
coords.reserve(range.size());
values.reserve(range.size());
// Sort by voxel coords
tbb::parallel_sort(coord_pt, [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; });
for (size_t i = range.begin(); i < range.end(); i++) {
const Eigen::Vector4d pt = traits::point(points, i);
const Eigen::Vector3i coord = (pt * inv_leaf_size).array().floor().template cast<int>().template head<3>();
auto downsampled = std::make_shared<OutputPointCloud>();
traits::resize(*downsampled, traits::size(points));
auto found = std::ranges::find(coords, coord);
if (found == coords.end()) {
coords.emplace_back(coord);
values.emplace_back(pt);
} else {
values[std::distance(coords.begin(), found)] += pt;
// Take block-wise sum
const int block_size = 1024;
std::atomic_uint64_t num_points = 0;
tbb::parallel_for(tbb::blocked_range<size_t>(0, traits::size(points), block_size), [&](const tbb::blocked_range<size_t>& range) {
std::vector<Eigen::Vector4d> sub_points;
sub_points.reserve(block_size);
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 - 1].first != coord_pt[i].first) {
sub_points.emplace_back(sum_pt / sum_pt.w());
sum_pt.setZero();
}
sum_pt += traits::point(points, coord_pt[i].second);
}
sub_points.emplace_back(sum_pt / sum_pt.w());
for (size_t i = 0; i < coords.size(); i++) {
VoxelMap::accessor a;
if (voxels.insert(a, coords[i])) {
a->second = num_voxels++;
voxel_values[a->second] = values[i];
} else {
voxel_values[a->second] += values[i];
}
const size_t point_index_begin = num_points.fetch_add(sub_points.size());
for (size_t i = 0; i < sub_points.size(); i++) {
traits::set_point(*downsampled, point_index_begin + i, sub_points[i]);
}
});
const int N = num_voxels;
auto downsampled = std::make_shared<OutputPointCloud>();
traits::resize(*downsampled, N);
for (size_t i = 0; i < N; i++) {
const Eigen::Vector4d pt = voxel_values[i];
traits::set_point(*downsampled, i, pt / pt.w());
}
traits::resize(*downsampled, num_points);
return downsampled;
}

3
include/small_gicp/util/lie.hpp
View File

@ -5,6 +5,9 @@
namespace small_gicp {
/// @brief Create skew symmetric matrix
/// @param x Rotation vector
/// @return Skew symmetric matrix
inline Eigen::Matrix3d skew(const Eigen::Vector3d& x) {
Eigen::Matrix3d skew = Eigen::Matrix3d::Zero();
skew(0, 1) = -x[2];

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

@ -48,7 +48,7 @@ struct NormalCovarianceSetter {
}
};
template <typename PointCloud, typename Tree, typename Setter>
template <typename Setter, typename PointCloud, typename Tree>
void estimate_local_features(PointCloud& cloud, Tree& kdtree, int num_neighbors, size_t point_index) {
std::vector<size_t> k_indices(num_neighbors);
std::vector<double> k_sq_dists(num_neighbors);
@ -77,29 +77,73 @@ void estimate_local_features(PointCloud& cloud, Tree& kdtree, int num_neighbors,
Setter::set(cloud, point_index, eig.eigenvectors());
}
template <typename PointCloud, typename Setter>
template <typename Setter, typename PointCloud, typename KdTree>
void estimate_local_features(PointCloud& cloud, const KdTree& kdtree, int num_neighbors) {
traits::resize(cloud, traits::size(cloud));
for (size_t i = 0; i < traits::size(cloud); i++) {
estimate_local_features<Setter>(cloud, kdtree, num_neighbors, i);
}
}
template <typename Setter, typename PointCloud>
void estimate_local_features(PointCloud& cloud, int num_neighbors) {
traits::resize(cloud, traits::size(cloud));
KdTree<PointCloud> kdtree(cloud);
for (size_t i = 0; i < traits::size(cloud); i++) {
estimate_local_features<PointCloud, KdTree<PointCloud>, Setter>(cloud, kdtree, num_neighbors, i);
estimate_local_features<Setter>(cloud, kdtree, num_neighbors, i);
}
}
/// @brief Estimate point normals
/// @param cloud [in/out] Point cloud
/// @param num_neighbors Number of neighbors used for attribute estimation
template <typename PointCloud>
void estimate_normals(PointCloud& cloud, int num_neighbors = 20) {
estimate_local_features<PointCloud, NormalSetter<PointCloud>>(cloud, num_neighbors);
estimate_local_features<NormalSetter<PointCloud>>(cloud, num_neighbors);
}
/// @brief Estimate point normals
/// @param cloud [in/out] Point cloud
/// @param kdtree Nearest neighbor search
/// @param num_neighbors Number of neighbors used for attribute estimation
template <typename PointCloud, typename KdTree>
void estimate_normals(PointCloud& cloud, KdTree& kdtree, int num_neighbors = 20) {
estimate_local_features<NormalSetter<PointCloud>>(cloud, kdtree, num_neighbors);
}
/// @brief Estimate point covariances
/// @param cloud [in/out] Point cloud
/// @param num_neighbors Number of neighbors used for attribute estimation
template <typename PointCloud>
void estimate_covariances(PointCloud& cloud, int num_neighbors = 20) {
estimate_local_features<PointCloud, CovarianceSetter<PointCloud>>(cloud, num_neighbors);
estimate_local_features<CovarianceSetter<PointCloud>>(cloud, num_neighbors);
}
/// @brief Estimate point covariances
/// @param cloud [in/out] Point cloud
/// @param kdtree Nearest neighbor search
/// @param num_neighbors Number of neighbors used for attribute estimation
template <typename PointCloud, typename KdTree>
void estimate_covariances(PointCloud& cloud, KdTree& kdtree, int num_neighbors = 20) {
estimate_local_features<CovarianceSetter<PointCloud>>(cloud, kdtree, num_neighbors);
}
/// @brief Estimate point normals and covariances
/// @param cloud [in/out] Point cloud
/// @param num_neighbors Number of neighbors used for attribute estimation
template <typename PointCloud>
void estimate_normals_covariances(PointCloud& cloud, int num_neighbors = 20) {
estimate_local_features<PointCloud, NormalCovarianceSetter<PointCloud>>(cloud, num_neighbors);
estimate_local_features<NormalCovarianceSetter<PointCloud>>(cloud, num_neighbors);
}
/// @brief Estimate point normals and covariances
/// @param cloud [in/out] Point cloud
/// @param kdtree Nearest neighbor search
/// @param num_neighbors Number of neighbors used for attribute estimation
template <typename PointCloud, typename KdTree>
void estimate_normals_covariances(PointCloud& cloud, KdTree& kdtree, int num_neighbors = 20) {
estimate_local_features<NormalCovarianceSetter<PointCloud>>(cloud, kdtree, num_neighbors);
}
} // namespace small_gicp

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

@ -4,31 +4,80 @@
namespace small_gicp {
template <typename PointCloud, typename Setter>
template <typename Setter, typename PointCloud>
void estimate_local_features_omp(PointCloud& cloud, int num_neighbors, int num_threads) {
traits::resize(cloud, traits::size(cloud));
UnsafeKdTree<PointCloud> kdtree(cloud);
#pragma omp parallel for num_threads(num_threads)
for (size_t i = 0; i < traits::size(cloud); i++) {
estimate_local_features<PointCloud, UnsafeKdTree<PointCloud>, Setter>(cloud, kdtree, num_neighbors, i);
estimate_local_features<Setter>(cloud, kdtree, num_neighbors, i);
}
}
template <typename Setter, typename PointCloud, typename KdTree>
void estimate_local_features_omp(PointCloud& cloud, KdTree& kdtree, int num_neighbors, int num_threads) {
traits::resize(cloud, traits::size(cloud));
#pragma omp parallel for num_threads(num_threads)
for (size_t i = 0; i < traits::size(cloud); i++) {
estimate_local_features<Setter>(cloud, kdtree, num_neighbors, i);
}
}
/// @brief Estimate point normals with OpenMP
/// @param cloud [in/out] Point cloud
/// @param num_neighbors Number of neighbors used for attribute estimation
/// @param num_threads Number of threads
template <typename PointCloud>
void estimate_normals_omp(PointCloud& cloud, int num_neighbors = 20, int num_threads = 4) {
estimate_local_features_omp<PointCloud, NormalSetter<PointCloud>>(cloud, num_neighbors, num_threads);
estimate_local_features_omp<NormalSetter<PointCloud>>(cloud, num_neighbors, num_threads);
}
/// @brief Estimate point normals with OpenMP
/// @param cloud [in/out] Point cloud
/// @param kdtree Nearest neighbor search
/// @param num_neighbors Number of neighbors used for attribute estimation
/// @param num_threads Number of threads
template <typename PointCloud, typename KdTree>
void estimate_normals_omp(PointCloud& cloud, KdTree& kdtree, int num_neighbors = 20, int num_threads = 4) {
estimate_local_features_omp<NormalSetter<PointCloud>>(cloud, kdtree, num_neighbors, num_threads);
}
/// @brief Estimate point covariances with OpenMP
/// @param cloud [in/out] Point cloud
/// @param num_neighbors Number of neighbors used for attribute estimation
/// @param num_threads Number of threads
template <typename PointCloud>
void estimate_covariances_omp(PointCloud& cloud, int num_neighbors = 20, int num_threads = 4) {
estimate_local_features_omp<PointCloud, CovarianceSetter<PointCloud>>(cloud, num_neighbors, num_threads);
estimate_local_features_omp<CovarianceSetter<PointCloud>>(cloud, num_neighbors, num_threads);
}
/// @brief Estimate point covariances with OpenMP
/// @param cloud [in/out] Point cloud
/// @param kdtree Nearest neighbor search
/// @param num_neighbors Number of neighbors used for attribute estimation
/// @param num_threads Number of threads
template <typename PointCloud, typename KdTree>
void estimate_covariances_omp(PointCloud& cloud, KdTree& kdtree, int num_neighbors = 20, int num_threads = 4) {
estimate_local_features_omp<CovarianceSetter<PointCloud>>(cloud, kdtree, num_neighbors, num_threads);
}
/// @brief Estimate point normals and covariances with OpenMP
/// @param cloud [in/out] Point cloud
/// @param num_neighbors Number of neighbors used for attribute estimation
/// @param num_threads Number of threads
template <typename PointCloud>
void estimate_normals_covariances_omp(PointCloud& cloud, int num_neighbors = 20, int num_threads = 4) {
estimate_local_features_omp<PointCloud, NormalCovarianceSetter<PointCloud>>(cloud, num_neighbors, num_threads);
estimate_local_features_omp<NormalCovarianceSetter<PointCloud>>(cloud, num_neighbors, num_threads);
}
/// @brief Estimate point normals and covariances with OpenMP
/// @param cloud [in/out] Point cloud
/// @param kdtree Nearest neighbor search
/// @param num_neighbors Number of neighbors used for attribute estimation
/// @param num_threads Number of threads
template <typename PointCloud, typename KdTree>
void estimate_normals_covariances_omp(PointCloud& cloud, KdTree& kdtree, int num_neighbors = 20, int num_threads = 4) {
estimate_local_features_omp<NormalCovarianceSetter<PointCloud>>(cloud, kdtree, num_neighbors, num_threads);
}
} // namespace small_gicp

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

@ -5,32 +5,77 @@
namespace small_gicp {
template <typename PointCloud, typename Setter>
void estimate_local_features_tbb(PointCloud& cloud, int num_neighbors) {
template <typename Setter, typename PointCloud, typename KdTree>
void estimate_local_features_tbb(PointCloud& cloud, KdTree& kdtree, int num_neighbors) {
traits::resize(cloud, traits::size(cloud));
UnsafeKdTree<PointCloud> kdtree(cloud);
tbb::parallel_for(tbb::blocked_range<size_t>(0, traits::size(cloud)), [&](const tbb::blocked_range<size_t>& range) {
for (size_t i = range.begin(); i < range.end(); i++) {
estimate_local_features<PointCloud, UnsafeKdTree<PointCloud>, Setter>(cloud, kdtree, num_neighbors, i);
estimate_local_features<Setter>(cloud, kdtree, num_neighbors, i);
}
});
}
template <typename Setter, typename PointCloud>
void estimate_local_features_tbb(PointCloud& cloud, int num_neighbors) {
traits::resize(cloud, traits::size(cloud));
UnsafeKdTree<PointCloud> kdtree(cloud);
tbb::parallel_for(tbb::blocked_range<size_t>(0, traits::size(cloud)), [&](const tbb::blocked_range<size_t>& range) {
for (size_t i = range.begin(); i < range.end(); i++) {
estimate_local_features<Setter>(cloud, kdtree, num_neighbors, i);
}
});
}
/// @brief Estimate point normals with TBB
/// @param cloud [in/out] Point cloud
/// @param num_neighbors Number of neighbors used for attribute estimation
template <typename PointCloud>
void estimate_normals_tbb(PointCloud& cloud, int num_neighbors = 20) {
estimate_local_features_tbb<PointCloud, NormalSetter<PointCloud>>(cloud, num_neighbors);
estimate_local_features_tbb<NormalSetter<PointCloud>>(cloud, num_neighbors);
}
/// @brief Estimate point normals with TBB
/// @param cloud [in/out] Point cloud
/// @param kdtree Nearest neighbor search
/// @param num_neighbors Number of neighbors used for attribute estimation
template <typename PointCloud, typename KdTree>
void estimate_normals_tbb(PointCloud& cloud, KdTree& kdtree, int num_neighbors = 20) {
estimate_local_features_tbb<NormalSetter<PointCloud>>(cloud, kdtree, num_neighbors);
}
/// @brief Estimate point covariances with TBB
/// @param cloud [in/out] Point cloud
/// @param num_neighbors Number of neighbors used for attribute estimation
template <typename PointCloud>
void estimate_covariances_tbb(PointCloud& cloud, int num_neighbors = 20) {
estimate_local_features_tbb<PointCloud, CovarianceSetter<PointCloud>>(cloud, num_neighbors);
estimate_local_features_tbb<CovarianceSetter<PointCloud>>(cloud, num_neighbors);
}
/// @brief Estimate point covariances with TBB
/// @param cloud [in/out] Point cloud
/// @param kdtree Nearest neighbor search
/// @param num_neighbors Number of neighbors used for attribute estimation
template <typename PointCloud, typename KdTree>
void estimate_covariances_tbb(PointCloud& cloud, KdTree& kdtree, int num_neighbors = 20) {
estimate_local_features_tbb<CovarianceSetter<PointCloud>>(cloud, kdtree, num_neighbors);
}
/// @brief Estimate point normals and covariances with TBB
/// @param cloud [in/out] Point cloud
/// @param num_neighbors Number of neighbors used for attribute estimation
template <typename PointCloud>
void estimate_normals_covariances_tbb(PointCloud& cloud, int num_neighbors = 20) {
estimate_local_features_tbb<PointCloud, NormalCovarianceSetter<PointCloud>>(cloud, num_neighbors);
estimate_local_features_tbb<NormalCovarianceSetter<PointCloud>>(cloud, num_neighbors);
}
/// @brief Estimate point normals and covariances with TBB
/// @param cloud [in/out] Point cloud
/// @param kdtree Nearest neighbor search
/// @param num_neighbors Number of neighbors used for attribute estimation
template <typename PointCloud, typename KdTree>
void estimate_normals_covariances_tbb(PointCloud& cloud, KdTree& kdtree, int num_neighbors = 20) {
estimate_local_features_tbb<NormalCovarianceSetter<PointCloud>>(cloud, kdtree, num_neighbors);
}
} // namespace small_gicp

4
include/small_gicp/util/read_points.hpp
View File

@ -1,10 +1,14 @@
#pragma once
#include <fstream>
#include <iostream>
#include <Eigen/Core>
namespace small_gicp {
/// @brief Read points from file (simple float4 array)
/// @param filename Filename
/// @return Points
static std::vector<Eigen::Vector4f> read_points(const std::string& filename) {
std::ifstream ifs(filename, std::ios::binary | std::ios::ate);
if (!ifs) {

4
include/small_gicp/util/vector3i_hash.hpp
View File

@ -5,8 +5,8 @@
namespace small_gicp {
/**
* @brief Spatial hashing function
* Teschner et al., "Optimized Spatial Hashing for Collision Detection of Deformable Objects", VMV2003
* @brief Spatial hashing function.
* Teschner et al., "Optimized Spatial Hashing for Collision Detection of Deformable Objects", VMV2003.
*/
struct XORVector3iHash {
public:

62
src/downsampling_benchmark.cpp Normal file
View File

@ -0,0 +1,62 @@
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <small_gicp/util/benchmark.hpp>
#include <small_gicp/util/downsampling.hpp>
#include <small_gicp/util/downsampling_tbb.hpp>
#include <small_gicp/util/normal_estimation_tbb.hpp>
#include <small_gicp/points/point_cloud.hpp>
#include <small_gicp/points/pcl_point.hpp>
#include <small_gicp/points/pcl_point_traits.hpp>
#include <guik/viewer/light_viewer.hpp>
int main(int argc, char** argv) {
using namespace small_gicp;
std::cout << "SIMD=" << Eigen::SimdInstructionSetsInUse() << std::endl;
const std::string dataset_path = "/home/koide/datasets/velodyne";
std::cout << "Load dataset from " << dataset_path << std::endl;
Stopwatch sw;
sw.start();
KittiDataset kitti(dataset_path, 1000);
sw.stop();
std::cout << "load=" << sw.elapsed_sec() << "s" << std::endl;
size_t sum_num_points = 0;
const auto points_pcl = kitti.convert<pcl::PointCloud<pcl::PointXYZ>>();
sw.start();
sum_num_points = 0;
for (size_t i = 0; i < points_pcl.size(); i++) {
auto downsampled = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
pcl::VoxelGrid<pcl::PointXYZ> voxelgrid;
voxelgrid.setLeafSize(0.2, 0.2, 0.2);
voxelgrid.setInputCloud(points_pcl[i]);
voxelgrid.filter(*downsampled);
sum_num_points += downsampled->size();
}
sw.stop();
std::cout << "filter=" << sw.elapsed_sec() << "s avg_num_points=" << static_cast<double>(sum_num_points) / points_pcl.size() << std::endl;
const auto points = kitti.convert<PointCloud>();
sw.start();
sum_num_points = 0;
for (size_t i = 0; i < points.size(); i++) {
auto downsampled = voxelgrid_sampling_tbb(*points[i], 0.2);
sum_num_points += downsampled->size();
}
sw.stop();
std::cout << "filter=" << sw.elapsed_sec() << "s avg_num_points=" << static_cast<double>(sum_num_points) / points.size() << std::endl;
return 0;
}

68
src/downsampling_test.cpp Normal file
View File

@ -0,0 +1,68 @@
#include <iostream>
#include <filesystem>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <small_gicp/util/read_points.hpp>
#include <small_gicp/points/point_cloud.hpp>
#include <small_gicp/util/downsampling.hpp>
#include <small_gicp/util/downsampling_tbb.hpp>
#include <small_gicp/util/normal_estimation_omp.hpp>
#include <small_gicp/util/normal_estimation_tbb.hpp>
#include <small_gicp/ann/flat_voxelmap.hpp>
#include <guik/viewer/light_viewer.hpp>
#include <easy_profiler.hpp>
int main(int argc, char** argv) {
using namespace small_gicp;
const int num_threads = 6;
tbb::task_arena arena(num_threads);
// tbb::task_scheduler_init init(num_threads);
const std::string dataset_path = "/home/koide/datasets/velodyne";
std::vector<std::string> filenames;
for (const auto& path : std::filesystem::directory_iterator(dataset_path)) {
if (path.path().extension() != ".bin") {
continue;
}
filenames.emplace_back(path.path().string());
}
std::ranges::sort(filenames);
auto viewer = guik::viewer();
viewer->disable_vsync();
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
for (const auto& filename : filenames) {
EasyProfiler prof;
prof.push("read_points");
const auto raw_points = read_points(filename);
prof.push("copy");
auto points = std::make_shared<PointCloud>(raw_points);
prof.push("downsample");
auto downsampled = approx_voxelgrid_sampling_tbb(*points, 0.2);
prof.push("estimate covs");
estimate_covariances_omp(*downsampled, 10);
prof.push("create flat voxels");
auto voxels = std::make_shared<FlatVoxelMap>(0.5, *downsampled);
prof.push("estimate covs2");
estimate_covariances_tbb(*downsampled, *voxels, 10);
prof.push("search");
prof.push("done");
viewer->update_points("points", downsampled->points, guik::Rainbow());
if (!viewer->spin_once()) {
break;
}
}
return 0;
}

24
src/small_gicp_test.cpp
View File

@ -29,10 +29,10 @@
int main(int argc, char** argv) {
using namespace small_gicp;
const int num_threads = 32;
tbb::task_scheduler_init init(num_threads);
const int num_threads = 8;
// tbb::task_scheduler_init init(num_threads);
const std::string dataset_path = "/home/koide/datasets/kitti/velodyne_filtered";
const std::string dataset_path = "/home/koide/datasets/velodyne";
std::vector<std::string> filenames;
for (const auto& path : std::filesystem::directory_iterator(dataset_path)) {
if (path.path().extension() != ".bin") {
@ -50,7 +50,7 @@ int main(int argc, char** argv) {
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
for (const auto& filename : filenames) {
glim::EasyProfiler prof("prof");
EasyProfiler prof;
prof.push("read_points");
const auto raw_points = read_points(filename);
@ -80,15 +80,15 @@ int main(int argc, char** argv) {
voxelmap->insert(*points, T);
prof.push("show");
// viewer->update_points("current", raw_points[0].data(), sizeof(float) * 4, raw_points.size(), guik::FlatOrange(T).set_point_scale(2.0f));
viewer->update_points("current", raw_points[0].data(), sizeof(float) * 4, raw_points.size(), guik::FlatOrange(T).set_point_scale(2.0f));
// std::vector<Eigen::Vector4d> means;
// std::vector<Eigen::Matrix4d> covs;
// for (const auto& voxel : voxelmap->flat_voxels) {
// means.emplace_back(voxel.mean);
// covs.emplace_back(voxel.cov);
// }
// viewer->update_normal_dists("target", means, covs, 0.5, guik::Rainbow());
std::vector<Eigen::Vector4d> means;
std::vector<Eigen::Matrix4d> covs;
for (const auto& voxel : voxelmap->flat_voxels) {
means.emplace_back(voxel.mean);
covs.emplace_back(voxel.cov);
}
viewer->update_normal_dists("target", means, covs, 0.5, guik::Rainbow());
std::cout << "--- T ---" << std::endl << T.matrix() << std::endl;