diff --git a/CMakeLists.txt b/CMakeLists.txt
index 54345b1..68a7b26 100644
--- 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 ##
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..a5c2840
--- /dev/null
+++ 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.
+
+[](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))
+
+The helper library (`registration_helper.hpp`) enables processing point clouds represented as `std::vector` easily.
+
+`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
+
+std::vector target_points = ...; // Any of Eigen::Vector(3|4)(f|d) can be used
+std::vector 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 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
+
+std::vector target_points = ...; // Any of Eigen::Vector(3|4)(f|d) can be used
+std::vector 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::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 H = result.H; // Final Hessian matrix (6x6)
+```
+
+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`.
+
+```cpp
+#include
+
+pcl::PointCloud::Ptr raw_target = ...;
+pcl::PointCloud::Ptr raw_source = ...;
+
+// small_gicp::voxelgrid_downsampling can directly operate on pcl::PointCloud.
+pcl::PointCloud::Ptr target = voxelgrid_sampling_omp(*raw_target, 0.25);
+pcl::PointCloud::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 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>();
+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
+
+pcl::PointCloud::Ptr raw_target = ...;
+pcl::PointCloud::Ptr raw_source = ...;
+
+// Downsample points and convert them into pcl::PointCloud.
+pcl::PointCloud::Ptr target = voxelgrid_sampling_omp, pcl::PointCloud>(*raw_target, 0.25);
+pcl::PointCloud::Ptr source = voxelgrid_sampling_omp, pcl::PointCloud>(*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>>(target, num_threads);
+auto source_tree = std::make_shared>>(source, num_threads);
+
+Registration 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.
+auto result = registration.align(*target, *source, *target_tree, Eigen::Isometry3d::Identity());
+```
+
+If you want to fine-control and customize the registration process, use `small_gicp::Registration` template that allows changing the inner algorithms and parameters.
+
+```cpp
+#include
+#include
+#include
+#include
+#include
+#include
+
+std::vector target_points = ...; // Any of Eigen::Vector(3|4)(f|d) can be used
+std::vector 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(target_points);
+auto source = std::make_shared(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>(target, num_threads);
+auto source_tree = std::make_shared>(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 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 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 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
+```
+
+