Merge pull request #2 from koide3/focal

examples
2024-03-29 21:19:28 +09:00 · 2024-03-29 21:19:28 +09:00 · 0fca559c69
parent 66d61731b4 5e983ac3b2
commit 0fca559c69
17 changed files with 733 additions and 17 deletions
--- a/CMakeLists.txt
+++ b/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 ##
--- a/README.md
+++ b/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)
--- a/docs/assets/downsampling_comp.png
+++ b/docs/assets/downsampling_comp.png
--- a/docs/assets/downsampling_threads.png
+++ b/docs/assets/downsampling_threads.png
--- a/docs/assets/odometry_time.png
+++ b/docs/assets/odometry_time.png
--- a/include/small_gicp/benchmark/benchmark_odom.hpp
+++ b/include/small_gicp/benchmark/benchmark_odom.hpp
@ -19,7 +19,7 @@ public:
  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 @@ public:
        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);

--- a/include/small_gicp/pcl/pcl_proxy.hpp
+++ b/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
--- a/include/small_gicp/pcl/pcl_registration.hpp
+++ b/include/small_gicp/pcl/pcl_registration.hpp
@ -0,0 +1,84 @@
+#pragma once
+
+#include <pcl/point_cloud.h>
+#include <pcl/registration/registration.h>
+#include <small_gicp/ann/kdtree_omp.hpp>
+#include <small_gicp/ann/gaussian_voxelmap.hpp>
+
+namespace small_gicp {
+
+template <typename PointSource, typename PointTarget>
+class RegistrationPCL : public pcl::Registration<PointSource, PointTarget, float> {
+public:
+  using Scalar = float;
+  using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;
+
+  using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+  using Ptr = pcl::shared_ptr<RegistrationPCL<PointSource, PointTarget>>;
+  using ConstPtr = pcl::shared_ptr<const RegistrationPCL<PointSource, PointTarget>>;
+
+protected:
+  using pcl::Registration<PointSource, PointTarget, Scalar>::reg_name_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::target_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
+
+  using pcl::Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::max_iterations_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::final_transformation_;
+
+  using pcl::Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::converged_;
+
+public:
+  RegistrationPCL();
+  virtual ~RegistrationPCL();
+
+  void setNumThreads(int n) { num_threads_ = n; }
+  void setCorrespondenceRandomness(int k) { k_correspondences_ = k; }
+  void setVoxelResolution(double r) { voxel_resolution_ = r; }
+  void setRotationEpsilon(double eps) { rotation_epsilon_ = eps; }
+  void setRegistrationType(const std::string& type);
+
+  const Eigen::Matrix<double, 6, 6>& getFinalHessian() const { return final_hessian_; }
+
+  void setInputSource(const PointCloudSourceConstPtr& cloud) override;
+  void setInputTarget(const PointCloudTargetConstPtr& cloud) override;
+
+  void swapSourceAndTarget();
+  void clearSource();
+  void clearTarget();
+
+protected:
+  virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
+
+protected:
+  int num_threads_;
+  int k_correspondences_;
+  double rotation_epsilon_;
+  double voxel_resolution_;
+  bool verbose_;
+  std::string registration_type_;
+
+  std::shared_ptr<KdTreeOMP<pcl::PointCloud<PointSource>>> target_tree_;
+  std::shared_ptr<KdTreeOMP<pcl::PointCloud<PointSource>>> source_tree_;
+
+  std::shared_ptr<GaussianVoxelMap> target_voxelmap_;
+  std::shared_ptr<GaussianVoxelMap> source_voxelmap_;
+
+  std::vector<Eigen::Matrix4d> target_covs_;
+  std::vector<Eigen::Matrix4d> source_covs_;
+
+  Eigen::Matrix<double, 6, 6> final_hessian_;
+};
+
+}  // namespace small_gicp
+
+#include <small_gicp/pcl/pcl_registration_impl.hpp>
--- a/include/small_gicp/pcl/pcl_registration_impl.hpp
+++ b/include/small_gicp/pcl/pcl_registration_impl.hpp
@ -0,0 +1,140 @@
+#pragma once
+
+#include <small_gicp/pcl/pcl_registration.hpp>
+
+#include <small_gicp/pcl/pcl_proxy.hpp>
+#include <small_gicp/pcl/pcl_point_traits.hpp>
+#include <small_gicp/util/normal_estimation_omp.hpp>
+
+#include <small_gicp/factors/gicp_factor.hpp>
+#include <small_gicp/registration/reduction_omp.hpp>
+#include <small_gicp/registration/registration.hpp>
+
+namespace small_gicp {
+
+template <typename PointSource, typename PointTarget>
+RegistrationPCL<PointSource, PointTarget>::RegistrationPCL() {
+  reg_name_ = "RegistrationPCL";
+
+  num_threads_ = 4;
+  k_correspondences_ = 20;
+  corr_dist_threshold_ = 1000.0;
+  rotation_epsilon_ = 2e-3;
+  transformation_epsilon_ = 5e-4;
+
+  voxel_resolution_ = 1.0;
+  verbose_ = false;
+  registration_type_ = "GICP";
+
+  final_hessian_.setIdentity();
+}
+
+template <typename PointSource, typename PointTarget>
+RegistrationPCL<PointSource, PointTarget>::~RegistrationPCL() {}
+
+template <typename PointSource, typename PointTarget>
+void RegistrationPCL<PointSource, PointTarget>::setInputSource(const PointCloudSourceConstPtr& cloud) {
+  if (input_ == cloud) {
+    return;
+  }
+
+  pcl::Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);
+  source_tree_ = std::make_shared<KdTreeOMP<pcl::PointCloud<PointSource>>>(input_, num_threads_);
+  source_covs_.clear();
+  source_voxelmap_.reset();
+}
+
+template <typename PointSource, typename PointTarget>
+void RegistrationPCL<PointSource, PointTarget>::setInputTarget(const PointCloudTargetConstPtr& cloud) {
+  if (target_ == cloud) {
+    return;
+  }
+
+  pcl::Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
+  target_tree_ = std::make_shared<KdTreeOMP<pcl::PointCloud<PointTarget>>>(target_, num_threads_);
+  target_covs_.clear();
+  target_voxelmap_.reset();
+}
+
+template <typename PointSource, typename PointTarget>
+void RegistrationPCL<PointSource, PointTarget>::swapSourceAndTarget() {
+  input_.swap(target_);
+  source_tree_.swap(target_tree_);
+  source_covs_.swap(target_covs_);
+  source_voxelmap_.swap(target_voxelmap_);
+}
+
+template <typename PointSource, typename PointTarget>
+void RegistrationPCL<PointSource, PointTarget>::clearSource() {
+  input_.reset();
+  source_tree_.reset();
+  source_covs_.clear();
+  source_voxelmap_.reset();
+}
+
+template <typename PointSource, typename PointTarget>
+void RegistrationPCL<PointSource, PointTarget>::clearTarget() {
+  target_.reset();
+  target_tree_.reset();
+  target_covs_.clear();
+  target_voxelmap_.reset();
+}
+
+template <typename PointSource, typename PointTarget>
+void RegistrationPCL<PointSource, PointTarget>::setRegistrationType(const std::string& type) {
+  if (type == "GICP") {
+    registration_type_ = type;
+  } else if (type == "VGICP") {
+    registration_type_ = type;
+  } else {
+    PCL_ERROR("[RegistrationPCL::setRegistrationType] Invalid registration type: %s\n", type.c_str());
+  }
+}
+
+template <typename PointSource, typename PointTarget>
+void RegistrationPCL<PointSource, PointTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
+  if (output.points.data() == input_->points.data() || output.points.data() == target_->points.data()) {
+    throw std::invalid_argument("FastGICP: destination cloud cannot be identical to source or target");
+  }
+
+  PointCloudProxy<PointSource> source_proxy(*input_, source_covs_);
+  PointCloudProxy<PointTarget> target_proxy(*target_, target_covs_);
+
+  if (source_covs_.size() != input_->size()) {
+    estimate_covariances_omp(source_proxy, *source_tree_, k_correspondences_, num_threads_);
+  }
+  if (target_covs_.size() != target_->size()) {
+    estimate_covariances_omp(target_proxy, *target_tree_, k_correspondences_, num_threads_);
+  }
+
+  Registration<GICPFactor, ParallelReductionOMP> registration;
+  registration.reduction.num_threads = num_threads_;
+  registration.rejector.max_dist_sq = corr_dist_threshold_ * corr_dist_threshold_;
+  registration.optimizer.verbose = verbose_;
+  registration.optimizer.max_iterations = max_iterations_;
+
+  RegistrationResult result(Eigen::Isometry3d::Identity());
+  if (registration_type_ == "GICP") {
+    result = registration.align(target_proxy, source_proxy, *target_tree_, Eigen::Isometry3d(guess.template cast<double>()));
+  } else if (registration_type_ == "VGICP") {
+    if (!target_voxelmap_) {
+      target_voxelmap_ = std::make_shared<GaussianVoxelMap>(voxel_resolution_);
+      target_voxelmap_->insert(target_proxy);
+    }
+    if (!source_voxelmap_) {
+      source_voxelmap_ = std::make_shared<GaussianVoxelMap>(voxel_resolution_);
+      source_voxelmap_->insert(source_proxy);
+    }
+
+    result = registration.align(*target_voxelmap_, source_proxy, *target_voxelmap_, Eigen::Isometry3d(guess.template cast<double>()));
+  } else {
+    PCL_ERROR("[RegistrationPCL::computeTransformation] Invalid registration type: %s\n", registration_type_.c_str());
+    return;
+  }
+
+  final_transformation_ = result.T_target_source.matrix().template cast<float>();
+  final_hessian_ = result.H;
+  pcl::transformPointCloud(*input_, output, final_transformation_);
+}
+
+}  // namespace small_gicp
--- a/include/small_gicp/registration/registration_helper.hpp
+++ b/include/small_gicp/registration/registration_helper.hpp
@ -9,17 +9,17 @@ namespace small_gicp {

 /// @brief Preprocess point cloud (downsampling, kdtree creation, and normal and covariance estimation)
 /// @param points                Input points
-/// @param downsample_resolution Downsample resolution
+/// @param downsampling_resolution Downsample resolution
 /// @param num_neighbors         Number of neighbors for normal/covariance estimation
 /// @param num_threads           Number of threads
 std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>>
-preprocess_points(const PointCloud& points, double downsample_resolution, int num_neighbors = 10, int num_threads = 4);
+preprocess_points(const PointCloud& points, double downsampling_resolution, int num_neighbors = 10, int num_threads = 4);

 /// @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
 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 downsample_resolution, int num_neighbors = 10, int num_threads = 4);
+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
 /// @param points            Input points
@ -32,7 +32,7 @@ struct RegistrationSetting {

  RegistrationType type = GICP;              ///< Registration type
  double voxel_resolution = 1.0;             ///< Voxel resolution for VGICP
-  double downsample_resolution = 0.25;       ///< Downsample resolution (this will be used only in the Eigen-based interface)
+  double downsampling_resolution = 0.25;     ///< Downsample resolution (this will be used only in the Eigen-based interface)
  double max_correspondence_distance = 1.0;  ///< Maximum correspondence distance
  int num_threads = 4;                       ///< Number of threads
 };
--- a/scripts/plot_downsampling.py
+++ b/scripts/plot_downsampling.py
@ -94,6 +94,8 @@ def main():
  axes[0].set_ylabel('Processing time [msec/scan]')
  axes[2].legend(loc='upper center', bbox_to_anchor=(0.5, -0.11), ncol=3)

+  fig.savefig('downsampling_threads.svg')
+
  
  fig, axes = pyplot.subplots(1, 2)
  fig.set_size_inches(18, 3)
@ -116,6 +118,8 @@ def main():
  axes[0].grid()
  axes[1].grid()
  
+  fig.savefig('downsampling_comp.svg')
+  
  pyplot.show()


--- a/scripts/plot_odometry.py
+++ b/scripts/plot_odometry.py
@ -82,6 +82,7 @@ def main():
      axes[i, j].legend()
      axes[i, j].grid()
  
+  fig.savefig('odometry_time.svg')
  pyplot.show()

 if __name__ == "__main__":
--- a/src/example/01_basic_registration.cpp
+++ b/src/example/01_basic_registration.cpp
@ -0,0 +1,68 @@
+/// @brief Basic point cloud registration example with small_gicp::align()
+#include <iostream>
+#include <small_gicp/benchmark/read_points.hpp>
+#include <small_gicp/registration/registration_helper.hpp>
+
+using namespace small_gicp;
+
+/// @brief Most basic registration example.
+void example1(const std::vector<Eigen::Vector4f>& target_points, const std::vector<Eigen::Vector4f>& 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);
+
+  std::cout << "--- T_target_source ---" << std::endl << result.T_target_source.matrix() << std::endl;
+  std::cout << "converged:" << result.converged << std::endl;
+  std::cout << "error:" << result.error << std::endl;
+  std::cout << "iterations:" << result.iterations << std::endl;
+  std::cout << "num_inliers:" << result.num_inliers << std::endl;
+  std::cout << "--- H ---" << std::endl << result.H << std::endl;
+  std::cout << "--- b ---" << std::endl << result.b.transpose() << std::endl;
+}
+
+/// @brief Example to perform preprocessing and registration separately.
+void example2(const std::vector<Eigen::Vector4f>& target_points, const std::vector<Eigen::Vector4f>& 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);
+
+  std::cout << "--- T_target_source ---" << std::endl << result.T_target_source.matrix() << std::endl;
+  std::cout << "converged:" << result.converged << std::endl;
+  std::cout << "error:" << result.error << std::endl;
+  std::cout << "iterations:" << result.iterations << std::endl;
+  std::cout << "num_inliers:" << result.num_inliers << std::endl;
+  std::cout << "--- H ---" << std::endl << result.H << std::endl;
+  std::cout << "--- b ---" << std::endl << result.b.transpose() << std::endl;
+
+  // Preprocessed points and trees can be reused for the next registration for efficiency
+  RegistrationResult result2 = align(*source, *target, *source_tree, Eigen::Isometry3d::Identity(), setting);
+}
+
+int main(int argc, char** argv) {
+  std::vector<Eigen::Vector4f> target_points = read_ply("data/target.ply");
+  std::vector<Eigen::Vector4f> source_points = read_ply("data/source.ply");
+  if (target_points.empty() || source_points.empty()) {
+    std::cerr << "error: failed to read points from data/(target|source).ply" << std::endl;
+    return 1;
+  }
+
+  example1(target_points, source_points);
+  example2(target_points, source_points);
+
+  return 0;
+}
--- a/src/example/02_basic_registration_pcl.cpp
+++ b/src/example/02_basic_registration_pcl.cpp
@ -0,0 +1,107 @@
+/// @brief Basic point cloud registration example with PCL interfaces
+#include <small_gicp/pcl/pcl_point.hpp>
+#include <small_gicp/pcl/pcl_point_traits.hpp>
+#include <small_gicp/pcl/pcl_registration.hpp>
+#include <small_gicp/util/downsampling_omp.hpp>
+#include <small_gicp/benchmark/read_points.hpp>
+
+#include <glk/pointcloud_buffer_pcl.hpp>
+#include <guik/viewer/light_viewer.hpp>
+
+using namespace small_gicp;
+
+/// @brief Example of using RegistrationPCL that can be used as a drop-in replacement for pcl::GeneralizedIterativeClosestPoint.
+void example1(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& raw_target, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& 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);
+
+  std::cout << "--- T_target_source ---" << std::endl << reg.getFinalTransformation() << std::endl;
+  std::cout << "--- H ---" << std::endl << reg.getFinalHessian() << std::endl;
+
+  // 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);
+
+  std::cout << "--- T_target_source ---" << std::endl << reg.getFinalTransformation().inverse() << std::endl;
+}
+
+/// @brief Example to directly feed pcl::PointCloud<pcl::PointCovariance> to small_gicp::Registration.
+void example2(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& raw_target, const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& 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());
+
+  std::cout << "--- T_target_source ---" << std::endl << result.T_target_source.matrix() << std::endl;
+  std::cout << "converged:" << result.converged << std::endl;
+  std::cout << "error:" << result.error << std::endl;
+  std::cout << "iterations:" << result.iterations << std::endl;
+  std::cout << "num_inliers:" << result.num_inliers << std::endl;
+  std::cout << "--- H ---" << std::endl << result.H << std::endl;
+  std::cout << "--- b ---" << std::endl << result.b.transpose() << std::endl;
+
+  // Because this usage exposes all preprocessed data, you can easily re-use them to obtain the best efficiency.
+  auto result2 = registration.align(*source, *target, *source_tree, Eigen::Isometry3d::Identity());
+
+  std::cout << "--- T_target_source ---" << std::endl << result2.T_target_source.inverse().matrix() << std::endl;
+}
+
+int main(int argc, char** argv) {
+  std::vector<Eigen::Vector4f> target_points = read_ply("data/target.ply");
+  std::vector<Eigen::Vector4f> source_points = read_ply("data/source.ply");
+  if (target_points.empty() || source_points.empty()) {
+    std::cerr << "error: failed to read points from data/(target|source).ply" << std::endl;
+    return 1;
+  }
+
+  const auto convert_to_pcl = [](const std::vector<Eigen::Vector4f>& raw_points) {
+    auto points = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    points->resize(raw_points.size());
+    for (size_t i = 0; i < raw_points.size(); i++) {
+      points->at(i).getVector4fMap() = raw_points[i];
+    }
+    return points;
+  };
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr raw_target = convert_to_pcl(target_points);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr raw_source = convert_to_pcl(source_points);
+
+  example1(raw_target, raw_source);
+  example2(raw_target, raw_source);
+
+  return 0;
+}
--- a/src/odometry_benchmark.cpp
+++ b/src/odometry_benchmark.cpp
@ -43,7 +43,7 @@ int main(int argc, char** argv) {
    } else if (arg == "--num_neighbors") {
      params.num_neighbors = std::stoi(argv[i + 1]);
    } else if (arg == "--downsampling_resolution") {
-      params.downsample_resolution = std::stod(argv[i + 1]);
+      params.downsampling_resolution = std::stod(argv[i + 1]);
    } else if (arg == "--voxel_resolution") {
      params.voxel_resolution = std::stod(argv[i + 1]);
    } else if (arg == "--engine") {
@ -59,7 +59,7 @@ int main(int argc, char** argv) {
  std::cout << "registration_engine=" << engine << std::endl;
  std::cout << "num_threads=" << params.num_threads << std::endl;
  std::cout << "num_neighbors=" << params.num_neighbors << std::endl;
-  std::cout << "downsampling_resolution=" << params.downsample_resolution << std::endl;
+  std::cout << "downsampling_resolution=" << params.downsampling_resolution << std::endl;
  std::cout << "voxel_resolution=" << params.voxel_resolution << std::endl;
  std::cout << "visualize=" << params.visualize << std::endl;

--- a/src/odometry_benchmark_small_gicp_tbb_flow.cpp
+++ b/src/odometry_benchmark_small_gicp_tbb_flow.cpp
@ -46,7 +46,7 @@ public:
    tbb::flow::broadcast_node<InputFrame::Ptr> input_node(graph);
    tbb::flow::function_node<InputFrame::Ptr, InputFrame::Ptr> preprocess_node(graph, tbb::flow::unlimited, [&](const InputFrame::Ptr& input) {
      input->sw.start();
-      input->points = voxelgrid_sampling(*input->points, params.downsample_resolution);
+      input->points = voxelgrid_sampling(*input->points, params.downsampling_resolution);
      input->kdtree = std::make_shared<KdTree<PointCloud>>(input->points);
      estimate_covariances(*input->points, *input->kdtree, params.num_neighbors);
      return input;
--- a/src/small_gicp/registration/registration_helper.cpp
+++ b/src/small_gicp/registration/registration_helper.cpp
@ -17,14 +17,14 @@
 namespace small_gicp {

 // Preprocess points
-std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>> preprocess_points(const PointCloud& points, double downsample_resolution, int num_neighbors, int num_threads) {
+std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>> preprocess_points(const PointCloud& points, double downsampling_resolution, int num_neighbors, int num_threads) {
  if (num_threads == 1) {
-    auto downsampled = voxelgrid_sampling(points, downsample_resolution);
+    auto downsampled = voxelgrid_sampling(points, downsampling_resolution);
    auto kdtree = std::make_shared<KdTree<PointCloud>>(downsampled);
    estimate_normals_covariances(*downsampled, *kdtree, num_neighbors);
    return {downsampled, kdtree};
  } else {
-    auto downsampled = voxelgrid_sampling_omp(points, downsample_resolution);
+    auto downsampled = voxelgrid_sampling_omp(points, downsampling_resolution);
    auto kdtree = std::make_shared<KdTree<PointCloud>>(downsampled);
    estimate_normals_covariances_omp(*downsampled, *kdtree, num_neighbors, num_threads);
    return {downsampled, kdtree};
@ -34,8 +34,8 @@ std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>> preprocess_point
 // Preprocess points with Eigen input
 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 downsample_resolution, int num_neighbors, int num_threads) {
-  return preprocess_points(*std::make_shared<PointCloud>(points), downsample_resolution, num_neighbors, num_threads);
+preprocess_points(const std::vector<Eigen::Matrix<T, D, 1>>& points, double downsampling_resolution, int num_neighbors, int num_threads) {
+  return preprocess_points(*std::make_shared<PointCloud>(points), downsampling_resolution, num_neighbors, num_threads);
 }

 // Explicit instantiation
@ -55,8 +55,8 @@ GaussianVoxelMap::Ptr create_gaussian_voxelmap(const PointCloud& points, double
 template <typename T, int D>
 RegistrationResult
 align(const std::vector<Eigen::Matrix<T, D, 1>>& target, const std::vector<Eigen::Matrix<T, D, 1>>& source, const Eigen::Isometry3d& init_T, const RegistrationSetting& setting) {
-  auto [target_points, target_tree] = preprocess_points(*std::make_shared<PointCloud>(target), setting.downsample_resolution, 10, setting.num_threads);
-  auto [source_points, source_tree] = preprocess_points(*std::make_shared<PointCloud>(source), setting.downsample_resolution, 10, setting.num_threads);
+  auto [target_points, target_tree] = preprocess_points(*std::make_shared<PointCloud>(target), setting.downsampling_resolution, 10, setting.num_threads);
+  auto [source_points, source_tree] = preprocess_points(*std::make_shared<PointCloud>(source), setting.downsampling_resolution, 10, setting.num_threads);

  if (setting.type == RegistrationSetting::VGICP) {
    auto target_voxelmap = create_gaussian_voxelmap(*target_points, setting.voxel_resolution);