examples

CMakeLists.txt

@@ -16,7 +16,8 @@ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
 
 option(BUILD_HELPER "Build helper library" ON)
-option(BUILD_TESTS "Build test" OFF)
+option(BUILD_TESTS "Build tests" OFF)
+option(BUILD_EXAMPLES "Build examples" OFF)
 option(BUILD_BENCHMARKS "Build benchmarks" OFF)
 option(BUILD_WITH_TBB "Build with TBB" ON)
 option(BUILD_WITH_PCL "Build with PCL (required for benchmark and test only)" OFF)
@@ -124,6 +125,37 @@ if(BUILD_BENCHMARKS)
   endif()
 endif()
 
+#############
+## Example ##
+#############
+if(BUILD_EXAMPLES)
+find_package(fmt REQUIRED)
+find_package(PCL REQUIRED)
+find_package(TBB REQUIRED)
+find_package(Iridescence REQUIRED)
+
+file(GLOB EXAMPLE_SOURCES "src/example/*.cpp")
+foreach(EXAMPLE_SOURCE ${EXAMPLE_SOURCES})
+  get_filename_component(EXAMPLE_NAME ${EXAMPLE_SOURCE} NAME_WE)
+  add_executable(${EXAMPLE_NAME} ${EXAMPLE_SOURCE})
+  target_include_directories(${EXAMPLE_NAME} PUBLIC
+    include
+    ${PCL_INCLUDE_DIRS}
+    ${TBB_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIR}
+    ${Iridescence_INCLUDE_DIRS}
+  )
+  target_link_libraries(${EXAMPLE_NAME}
+    small_gicp
+    fmt::fmt
+    ${PCL_LIBRARIES}
+    ${TBB_LIBRARIES}
+    ${Iridescence_LIBRARIES}
+  )
+endforeach()
+
+
+endif()
 
 ##########
 ## Test ##

README.md

@@ -0,0 +1,245 @@
+# small_gicp (fast_gicp2)
+
+**small_gicp** is a header-only C++ library that provides efficient and parallelized fine point cloud registration algorithms (ICP, Point-to-Plane ICP, GICP, VGICP, etc.). It is essentially an optimized and refined version of its predecessor, [fast_gicp](https://github.com/SMRT-AIST/fast_gicp), with the following features. 
+
+- **Highly optimized** : The implementation of the core registration algorithm is further optimized from that in fast_gicp. It can provide up to 2x speed up compared to fast_gicp.
+- **All parallerized** : small_gicp provides parallelized implementations of several algorithms in the point cloud registration process (Downsampling, KdTree construction, Normal/covariance estimation). As a parallelism backend, either (or both) of [OpenMP](https://www.openmp.org/) and [Intel TBB](https://github.com/oneapi-src/oneTBB) can be used. 
+- **Minimum dependency** : small_gicp is a header-only library and requires only [Eigen](https://eigen.tuxfamily.org/) (and bundled [nanoflann](https://github.com/jlblancoc/nanoflann) and [Sophus](https://github.com/strasdat/Sophus)) at a minimum. Optionally, it provides the [PCL](https://pointclouds.org/) registration interface so that it can be used as a drop-in replacement in many systems.
+- **Customizable** : small_gicp is implemented with the trait mechanism that allows feeding any custom point cloud class to the registration algorithm. Furthermore, the template-based implementation allows customizing the regisration process with your custom correspondence estimator and registration factors.
+
+[![Build](https://github.com/koide3/small_gicp/actions/workflows/build.yml/badge.svg)](https://github.com/koide3/small_gicp/actions/workflows/build.yml)
+
+## Installation
+
+This is a header-only library. You can just download and put it in your project directory to use all capabilities.
+
+Meanwhile, if you just want to perform basic point cloud registration without fine customization, you can build and install the helper library (see `small_gicp/registration/registration_helper.hpp` for details) as follows.
+
+```cpp
+sudo apt-get install libeigen3-dev libomp-dev
+
+cd small_gicp
+mkdir build && cd build
+cmake .. -DCMAKE_BUILD_TYPE=Release && make -j
+sudo make install
+```
+
+## Usage
+
+The following examples assume `using namespace small_gicp`.
+
+### Using helper library ([01_basic_resigtration.cpp](https://github.com/koide3/small_gicp/blob/master/src/example/01_basic_registration.cpp))
+
+<details><summary>The helper library (`registration_helper.hpp`) enables processing point clouds represented as `std::vector<Eigen::Vector(3|4)(f|d)>` easily. </summary>
+
+`small_gicp::align` takes two point clouds (`std::vectors` of `Eigen::Vector(3|4)(f|d)`) and returns a registration result (estimated transformation and some information on the optimization result). This is the easiest way to use **small_gicp** but causes an overhead for duplicated preprocessing.
+
+```cpp
+#include <small_gicp/registration/registration_helper.hpp>
+
+std::vector<Eigen::Vector3d> target_points = ...;   // Any of Eigen::Vector(3|4)(f|d) can be used
+std::vector<Eigen::Vector3d> source_points = ...;   // 
+
+RegistrationSetting setting;
+setting.num_threads = 4;                    // Number of threads to be used
+setting.downsampling_resolution = 0.25;     // Downsampling resolution
+setting.max_correspondence_distance = 1.0;  // Maximum correspondence distance between points (e.g., triming threshold)
+
+Eigen::Isometry3d init_T_target_source = Eigen::Isometry3d::Identity();
+RegistrationResult result = align(target_points, source_points, init_T_target_source, setting);
+
+Eigen::Isometry3d T = result.T_target_source;  // Estimated transformation
+size_t num_inliers = result.num_inliers;       // Number of inlier source points
+Eigen::Matrix<double, 6, 6> H = result.H;      // Final Hessian matrix (6x6)
+```
+
+There is also a way to perform preprocessing and registration separately. This enables saving the time for preprocessing in case registration is performed several times for a same point cloud (e.g., typical odometry estimation based on scan-to-scan matching).
+
+```cpp
+#include <small_gicp/registration/registration_helper.hpp>
+
+std::vector<Eigen::Vector3d> target_points = ...;   // Any of Eigen::Vector(3|4)(f|d) can be used
+std::vector<Eigen::Vector3d> source_points = ...;   // 
+
+int num_threads = 4;                    // Number of threads to be used
+double downsampling_resolution = 0.25;    // Downsampling resolution
+int num_neighbors = 10;                 // Number of neighbor points used for normal and covariance estimation
+
+// std::pair<PointCloud::Ptr, KdTree<PointCloud>::Ptr>
+auto [target, target_tree] = preprocess_points(target_points, downsampling_resolution, num_neighbors, num_threads);
+auto [source, source_tree] = preprocess_points(source_points, downsampling_resolution, num_neighbors, num_threads);
+
+RegistrationSetting setting;
+setting.num_threads = num_threads;
+setting.max_correspondence_distance = 1.0;  // Maximum correspondence distance between points (e.g., triming threshold)
+
+Eigen::Isometry3d init_T_target_source = Eigen::Isometry3d::Identity();
+RegistrationResult result = align(*target, *source, *target_tree, init_T_target_source, setting);
+
+Eigen::Isometry3d T = result.T_target_source;  // Estimated transformation
+size_t num_inliers = result.num_inliers;       // Number of inlier source points
+Eigen::Matrix<double, 6, 6> H = result.H;      // Final Hessian matrix (6x6)
+```
+
+</details>
+
+### Using with PCL interface ([02_basic_resigtration_pcl.cpp](https://github.com/koide3/small_gicp/blob/master/src/example/02_basic_resigtration_pcl.cpp))
+
+<details><summary>The PCL interface allows using small_gicp as a drop-in replacement for `pcl::GeneralizedIterativeClosestPoint`. It is also possible to directly feed `pcl::PointCloud` to `small_gicp::Registration`.</summary>
+
+```cpp
+#include <small_gicp/pcl/pcl_registration.hpp>
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr raw_target = ...;
+pcl::PointCloud<pcl::PointXYZ>::Ptr raw_source = ...;
+
+// small_gicp::voxelgrid_downsampling can directly operate on pcl::PointCloud.
+pcl::PointCloud<pcl::PointXYZ>::Ptr target = voxelgrid_sampling_omp(*raw_target, 0.25);
+pcl::PointCloud<pcl::PointXYZ>::Ptr source = voxelgrid_sampling_omp(*raw_source, 0.25);
+
+// RegistrationPCL is derived from pcl::Registration and has mostly the same interface as pcl::GeneralizedIterativeClosestPoint.
+RegistrationPCL<pcl::PointXYZ, pcl::PointXYZ> reg;
+reg.setNumThreads(4);
+reg.setCorrespondenceRandomness(20);
+reg.setMaxCorrespondenceDistance(1.0);
+reg.setVoxelResolution(1.0);
+reg.setRegistrationType("VGICP");  // or "GICP" (default = "GICP")
+
+// Set input point clouds.
+reg.setInputTarget(target);
+reg.setInputSource(source);
+
+// Align point clouds.
+auto aligned = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+reg.align(*aligned);
+
+// Swap source and target and align again.
+// This is useful when you want to re-use preprocessed point clouds for successive registrations (e.g., odometry estimation).
+reg.swapSourceAndTarget();
+reg.align(*aligned);
+```
+
+It is also possible to directly feed `pcl::PointCloud` to `small_gicp::Registration`. Because all preprocessed data are exposed in this way, you can easily re-use them to obtain the best efficiency.
+
+```cpp
+#include <small_gicp/pcl/pcl_registration.hpp>
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr raw_target = ...;
+pcl::PointCloud<pcl::PointXYZ>::Ptr raw_source = ...;
+
+// Downsample points and convert them into pcl::PointCloud<pcl::PointCovariance>.
+pcl::PointCloud<pcl::PointCovariance>::Ptr target = voxelgrid_sampling_omp<pcl::PointCloud<pcl::PointXYZ>, pcl::PointCloud<pcl::PointCovariance>>(*raw_target, 0.25);
+pcl::PointCloud<pcl::PointCovariance>::Ptr source = voxelgrid_sampling_omp<pcl::PointCloud<pcl::PointXYZ>, pcl::PointCloud<pcl::PointCovariance>>(*raw_source, 0.25);
+
+// Estimate covariances of points.
+const int num_threads = 4;
+const int num_neighbors = 20;
+estimate_covariances_omp(*target, num_neighbors, num_threads);
+estimate_covariances_omp(*source, num_neighbors, num_threads);
+
+// Create KdTree for target and source.
+auto target_tree = std::make_shared<KdTreeOMP<pcl::PointCloud<pcl::PointCovariance>>>(target, num_threads);
+auto source_tree = std::make_shared<KdTreeOMP<pcl::PointCloud<pcl::PointCovariance>>>(source, num_threads);
+
+Registration<GICPFactor, ParallelReductionOMP> registration;
+registration.reduction.num_threads = num_threads;
+registration.rejector.max_dist_sq = 1.0;
+
+// Align point clouds. Note that the input point clouds are pcl::PointCloud<pcl::PointCovariance>.
+auto result = registration.align(*target, *source, *target_tree, Eigen::Isometry3d::Identity());
+```
+
+</details>
+
+### Using `small_gicp::Registration` template (`registration.hpp`)
+
+<details><summary>If you want to fine-control and customize the registration process, use `small_gicp::Registration` template that allows changing the inner algorithms and parameters.</summary>
+
+```cpp
+#include <small_gicp/ann/kdtree_omp.hpp>
+#include <small_gicp/points/point_cloud.hpp>
+#include <small_gicp/factors/gicp_factor.hpp>
+#include <small_gicp/util/normal_estimation_omp.hpp>
+#include <small_gicp/registration/reduction_omp.hpp>
+#include <small_gicp/registration/registration.hpp>
+
+std::vector<Eigen::Vector3d> target_points = ...;   // Any of Eigen::Vector(3|4)(f|d) can be used
+std::vector<Eigen::Vector3d> source_points = ...;   // 
+
+int num_threads = 4;
+double downsampling_resolution = 0.25;
+int num_neighbors = 10;
+double max_correspondence_distance = 1.0;
+
+// Convert to small_gicp::PointCloud
+auto target = std::make_shared<PointCloud>(target_points);
+auto source = std::make_shared<PointCloud>(source_points);
+
+// Downsampling
+target = voxelgrid_downsampling_omp(*target, downsampling_resolution, num_threads);
+source = voxelgrid_downsampling_omp(*source, downsampling_resolution, num_threads);
+
+// Create KdTree
+auto target_tree = std::make_shared<KdTreeOMP<PointCloud>>(target, num_threads);
+auto source_tree = std::make_shared<KdTreeOMP<PointCloud>>(source, num_threads);
+
+// Estimate point covariances
+estimate_covariances_omp(*target, *target_tree, num_neighbors, num_threads);
+estimate_covariances_omp(*source, *source_tree, num_neighbors, num_threads);
+
+// GICP + OMP-based parallel reduction
+Registration<GICPFactor, ParallelReductionOMP> registration;
+registration.reduction.num_threads = num_threads;
+registration.rejector.max_dist_sq = max_correspondence_distance * max_correspondence_distance;
+
+// Align point clouds
+Eigen::Isometry3d init_T_target_source = Eigen::Isometry3d::Identity();
+auto result = registration.align(*target, *source, *target_tree, init_T_target_source);
+
+Eigen::Isometry3d T = result.T_target_source;  // Estimated transformation
+size_t num_inliers = result.num_inliers;       // Number of inlier source points
+Eigen::Matrix<double, 6, 6> H = result.H;      // Final Hessian matrix (6x6)
+```
+
+Custom registration example:
+
+```cpp
+using PerPointFactor = PointToPlaneICPFactor;       // Point-to-plane ICP
+using GeneralFactor = RestrictDoFFactor;            // DoF restriction
+using Reduction = ParallelReductionTBB;             // TBB-based parallel reduction
+using CorrespondenceRejector = DistanceRejector;    // Distance-based correspondence rejection
+using Optimizer = LevenbergMarquardtOptimizer;      // Levenberg marquardt optimizer
+
+Registration<PerPointFactor, Reduction, GeneralFactor, CorrespondenceRejector, Optimizer> registration;
+registration.general_factor.set_translation_mask(Eigen::Vector3d(1.0, 1.0, 0.0));   // XY-translation only
+registration.general_factor.set_ratation_mask(Eigen::Vector3d(0.0, 0.0, 1.0));      // Z-rotation only
+registration.optimizer.init_lambda = 1e-3;                                          // Initial damping scale
+```
+
+</details>
+
+## Benchmark
+
+### Downsampling
+
+- Single-threaded `small_gicp::voxelgrid_sampling` is about 1.3x faster than `pcl::VoxelGrid`.
+- Multi-threaded `small_gicp::voxelgrid_sampling_tbb` (6 threads) is about 3.2x faster than `pcl::VoxelGrid`.
+- `small_gicp::voxelgrid_sampling` gives accurate downsampling results (almost identical to those of `pcl::VoxelGrid`) while `pcl::ApproximateVoxelGrid` yields spurious points (up to 2x points).
+- `small_gicp::voxelgrid_sampling` can process a larger point cloud with a fine voxel resolution compared to `pcl::VoxelGrid`.
+
+![downsampling_comp](docs/assets/downsampling_comp.png)
+
+### Odometry estimation
+
+- Single-threaded `small_gicp::GICP` is about 2.4x and 1.9x faster than `pcl::GICP` and `fast_gicp::GICP`, respectively.
+- `small_gicp` shows a better scalability to many-threads situations compared to `fast_gicp`.
+- `small_gicp::GICP` with TBB flow graph shows an excellent multi-thread scalablity but with a latency degradation.
+
+![odometry_time](docs/assets/odometry_time.png)
+
+## Papers
+- Kenji Koide, Masashi Yokozuka, Shuji Oishi, and Atsuhiko Banno, Voxelized GICP for Fast and Accurate 3D Point Cloud Registration, ICRA2021
+
+## Contact
+
+[Kenji Koide](https://staff.aist.go.jp/k.koide/), National Institute of Advanced Industrial Science and Technology (AIST)

include/small_gicp/benchmark/benchmark_odom.hpp

@@ -19,7 +19,7 @@ struct OdometryEstimationParams {
   bool visualize = false;
   int num_threads = 4;
   int num_neighbors = 20;
-  double downsample_resolution = 0.25;
+  double downsampling_resolution = 0.25;
   double voxel_resolution = 1.0;
   double max_correspondence_distance = 1.0;
 };
@@ -54,7 +54,7 @@ class OnlineOdometryEstimation : public OdometryEstimation {
         report();
       }
 
-      auto downsampled = voxelgrid_sampling(*points[i], params.downsample_resolution);
+      auto downsampled = voxelgrid_sampling(*points[i], params.downsampling_resolution);
       const Eigen::Isometry3d T = estimate(downsampled);
       traj.emplace_back(T);
 

include/small_gicp/pcl/pcl_proxy.hpp

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <pcl/point_cloud.h>
+#include <small_gicp/points/traits.hpp>
+
+namespace small_gicp {
+
+template <typename PointT>
+struct PointCloudProxy {
+  PointCloudProxy(const pcl::PointCloud<PointT>& cloud, std::vector<Eigen::Matrix4d>& covs) : cloud(cloud), covs(covs) {}
+
+  const pcl::PointCloud<PointT>& cloud;
+  std::vector<Eigen::Matrix4d>& covs;
+};
+
+namespace traits {
+template <typename PointT>
+struct Traits<PointCloudProxy<PointT>> {
+  using Points = PointCloudProxy<PointT>;
+
+  static size_t size(const Points& points) { return points.cloud.size(); }
+
+  static bool has_points(const Points& points) { return !points.cloud.points.empty(); }
+  static bool has_covs(const Points& points) { return !points.covs.empty(); }
+
+  static const Eigen::Vector4d point(const Points& points, size_t i) { return points.cloud.at(i).getVector4fMap().template cast<double>(); }
+  static const Eigen::Matrix4d& cov(const Points& points, size_t i) { return points.covs[i]; }
+
+  static void resize(Points& points, size_t n) { points.covs.resize(n); }
+  static void set_cov(Points& points, size_t i, const Eigen::Matrix4d& cov) { points.covs[i] = cov; }
+};
+
+}  // namespace traits
+
+}  // namespace small_gicp