init

.clang-format

@@ -0,0 +1,168 @@
+---
+Language:        Cpp
+# BasedOnStyle:  Google
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+AlignConsecutiveMacros: false
+AlignConsecutiveAssignments: false
+AlignConsecutiveDeclarations: false
+AlignEscapedNewlines: Left
+AlignOperands:   true
+AlignTrailingComments: true
+AllowAllArgumentsOnNextLine: false
+AllowAllConstructorInitializersOnNextLine: false
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortBlocksOnASingleLine: Never
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: Inline
+AllowShortLambdasOnASingleLine: All
+AllowShortIfStatementsOnASingleLine: WithoutElse
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: Yes
+BinPackArguments: false
+BinPackParameters: false
+BraceWrapping:
+  AfterCaseLabel:  false
+  AfterClass:      false
+  AfterControlStatement: false
+  AfterEnum:       false
+  AfterFunction:   false
+  AfterNamespace:  false
+  AfterObjCDeclaration: false
+  AfterStruct:     false
+  AfterUnion:      false
+  AfterExternBlock: false
+  BeforeCatch:     false
+  BeforeElse:      false
+  IndentBraces:    false
+  SplitEmptyFunction: true
+  SplitEmptyRecord: true
+  SplitEmptyNamespace: true
+BreakBeforeBinaryOperators: None
+BreakBeforeBraces: Attach
+BreakBeforeInheritanceComma: false
+BreakInheritanceList: BeforeColon
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+BreakConstructorInitializers: BeforeColon
+BreakAfterJavaFieldAnnotations: false
+BreakStringLiterals: true
+ColumnLimit:     180
+CommentPragmas:  '^ IWYU pragma:'
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+Cpp11BracedListStyle: true
+DeriveLineEnding: true
+DerivePointerAlignment: false
+DisableFormat:   false
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: true
+ForEachMacros:
+  - foreach
+  - Q_FOREACH
+  - BOOST_FOREACH
+IncludeBlocks:   Regroup
+IncludeCategories:
+  - Regex:           '^<ext/.*\.h>'
+    Priority:        2
+    SortPriority:    0
+  - Regex:           '^<.*\.h>'
+    Priority:        1
+    SortPriority:    0
+  - Regex:           '^<.*'
+    Priority:        2
+    SortPriority:    0
+  - Regex:           '.*'
+    Priority:        3
+    SortPriority:    0
+IncludeIsMainRegex: '([-_](test|unittest))?$'
+IncludeIsMainSourceRegex: ''
+IndentCaseLabels: true
+IndentGotoLabels: true
+IndentPPDirectives: None
+IndentWidth:     2
+IndentWrappedFunctionNames: false
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: false
+MacroBlockBegin: ''
+MacroBlockEnd:   ''
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBinPackProtocolList: Never
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakAssignment: 2
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyBreakTemplateDeclaration: 10
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+RawStringFormats:
+  - Language:        Cpp
+    Delimiters:
+      - cc
+      - CC
+      - cpp
+      - Cpp
+      - CPP
+      - 'c++'
+      - 'C++'
+    CanonicalDelimiter: ''
+    BasedOnStyle:    google
+  - Language:        TextProto
+    Delimiters:
+      - pb
+      - PB
+      - proto
+      - PROTO
+    EnclosingFunctions:
+      - EqualsProto
+      - EquivToProto
+      - PARSE_PARTIAL_TEXT_PROTO
+      - PARSE_TEST_PROTO
+      - PARSE_TEXT_PROTO
+      - ParseTextOrDie
+      - ParseTextProtoOrDie
+    CanonicalDelimiter: ''
+    BasedOnStyle:    google
+ReflowComments:  true
+SortIncludes:    false
+SortUsingDeclarations: true
+SpaceAfterCStyleCast: false
+SpaceAfterLogicalNot: false
+SpaceAfterTemplateKeyword: true
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeCpp11BracedList: false
+SpaceBeforeCtorInitializerColon: true
+SpaceBeforeInheritanceColon: true
+SpaceBeforeParens: ControlStatements
+SpaceBeforeRangeBasedForLoopColon: true
+SpaceInEmptyBlock: false
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 2
+SpacesInAngles:  false
+SpacesInConditionalStatement: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+SpaceBeforeSquareBrackets: false
+Standard:        Auto
+StatementMacros:
+  - Q_UNUSED
+  - QT_REQUIRE_VERSION
+TabWidth:        8
+UseCRLF:         false
+UseTab:          Never
+...
+

.gitignore

@@ -0,0 +1,2 @@
+.vscode/*
+build/*

CMakeLists.txt

@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 3.5.2)
+project(small_gicp)
+
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake")
+
+if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
+  set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Choose the type of build." FORCE)
+  set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo")
+endif()
+
+option(BUILD_TESTS "Build test" OFF)
+option(BUILD_WITH_MARCH_NATIVE "Build with -march=native" OFF)
+
+if(BUILD_WITH_MARCH_NATIVE)
+  add_compile_options(-march=native)
+  set(CMAKE_C_FLAGS "-march=native ${CMAKE_C_FLAGS}")
+  set(CMAKE_CXX_FLAGS "-march=native ${CMAKE_CXX_FLAGS}")
+endif()
+
+find_package(PCL REQUIRED)
+find_package(TBB REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(Iridescence REQUIRED)
+
+find_package(OpenMP)
+if (OPENMP_FOUND)
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+endif()
+
+###########
+## Build ##
+###########
+
+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}
+)

cmake/FindGTSAM.cmake

@@ -0,0 +1,26 @@
+find_path(GTSAM_INCLUDE_DIRS gtsam/inference/FactorGraph.h
+  HINTS /usr/local/include /usr/include
+  DOC "GTSAM include directories")
+
+find_library(GTSAM_LIB NAMES gtsam
+  HINTS /usr/local/lib /usr/lib
+  DOC "GTSAM libraries")
+
+find_library(GTSAM_UNSTABLE_LIB NAMES gtsam_unstable
+  HINTS /usr/local/lib /usr/lib
+  DOC "GTSAM_UNSTABLE libraries")
+
+find_library(TBB_LIB NAMES tbb
+  HINTS /usr/local/lib /usr/lib
+  DOC "TBB libraries")
+
+find_library(TBB_MALLOC_LIB NAMES tbbmalloc
+  HINTS /usr/local/lib /usr/lib
+  DOC "TBB malloc libraries")
+
+if(GTSAM_LIB AND GTSAM_UNSTABLE_LIB AND TBB_LIB)
+  set(GTSAM_LIBRARIES ${GTSAM_LIB} ${GTSAM_UNSTABLE_LIB} ${TBB_LIB} ${TBB_MALLOC_LIB})
+endif()
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(GTSAM DEFAULT_MSG GTSAM_INCLUDE_DIRS GTSAM_LIBRARIES)

cmake/FindIridescence.cmake

@@ -0,0 +1,16 @@
+find_path(Iridescence_INCLUDE_DIRS glk/drawable.hpp
+  HINTS /usr/local/include/iridescence /usr/include/iridescence
+  DOC "Iridescence include directories")
+
+find_library(Iridescence_LIBRARY NAMES iridescence
+  HINTS /usr/local/lib /usr/lib
+  DOC "Iridescence libraries")
+
+  find_library(gl_imgui_LIBRARY NAMES gl_imgui
+  HINTS /usr/local/lib /usr/lib
+  DOC "Iridescence libraries")
+
+set(Iridescence_LIBRARIES ${Iridescence_LIBRARY} ${gl_imgui_LIBRARY})
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Iridescence DEFAULT_MSG Iridescence_INCLUDE_DIRS Iridescence_LIBRARIES)

cmake/FindTBB.cmake

@@ -0,0 +1,15 @@
+find_path(TBB_INCLUDE_DIRS tbb/tbb.h
+  HINTS /usr/local/include /usr/include
+  DOC "oneTBB include directories")
+
+find_library(TBB_LIB NAMES tbb
+  HINTS /usr/local/lib /usr/lib /usr/lib/x86_64-linux-gnu
+  DOC "TBB libraries")
+
+# if(GTSAM_LIB AND GTSAM_UNSTABLE_LIB AND TBB_LIB)
+if(TBB_LIB)
+  set(TBB_LIBRARIES ${TBB_LIB})
+endif()
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(TBB DEFAULT_MSG TBB_INCLUDE_DIRS TBB_LIBRARIES)

include/small_gicp/ann/cached_kdtree.hpp

@@ -0,0 +1,62 @@
+#pragma once
+
+#include <atomic>
+#include <tbb/tbb.h>
+#include <small_gicp/ann/kdtree.hpp>
+#include <small_gicp/util/vector3i_hash.hpp>
+
+namespace small_gicp {
+
+template <typename PointCloud>
+class CachedKdTree {
+public:
+  using Ptr = std::shared_ptr<CachedKdTree>;
+  using ConstPtr = std::shared_ptr<const CachedKdTree>;
+
+  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 {
+    const Eigen::Vector3i coord = (pt * inv_leaf_size).array().floor().cast<int>().head<3>();
+
+    CacheTable::const_accessor ca;
+    if (cache.find(ca, coord)) {
+      std::ranges::copy(ca->second.first, k_indices);
+      std::ranges::copy(ca->second.second, k_sq_dists);
+      return ca->second.first.size();
+    }
+
+    const size_t n = kdtree.knn_search(pt, k, k_indices, k_sq_dists);
+    std::vector<size_t> indices(k_indices, k_indices + n);
+    std::vector<double> sq_dists(k_sq_dists, k_sq_dists + n);
+
+    CacheTable::accessor a;
+    if (cache.insert(a, coord)) {
+      a->second.first = std::move(indices);
+      a->second.second = std::move(sq_dists);
+    }
+
+    return n;
+  }
+
+public:
+  const double inv_leaf_size;
+
+  using KnnResult = std::pair<std::vector<size_t>, std::vector<double>>;
+  using CacheTable = tbb::concurrent_hash_map<Eigen::Vector3i, KnnResult, XORVector3iHash>;
+  mutable CacheTable cache;
+
+  UnsafeKdTree<PointCloud> kdtree;
+};
+
+namespace traits {
+
+template <typename PointCloud>
+struct Traits<CachedKdTree<PointCloud>> {
+  static size_t knn_search(const CachedKdTree<PointCloud>& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) {
+    return tree.knn_search(point, k, k_indices, k_sq_dists);
+  }
+};
+
+}  // namespace traits
+
+}  // namespace small_gicp

include/small_gicp/ann/kdtree.hpp

@@ -0,0 +1,69 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <small_gicp/points/traits.hpp>
+#include <small_gicp/ann/traits.hpp>
+#include <small_gicp/ann/nanoflann.hpp>
+
+namespace small_gicp {
+
+template <typename PointCloud>
+class UnsafeKdTree {
+public:
+  using Ptr = std::shared_ptr<UnsafeKdTree>;
+  using ConstPtr = std::shared_ptr<const UnsafeKdTree>;
+  using Index = nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<double, UnsafeKdTree<PointCloud>, double>, UnsafeKdTree<PointCloud>, 3, size_t>;
+
+  UnsafeKdTree(const PointCloud& points) : points(points), index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10)) { index.buildIndex(); }
+  ~UnsafeKdTree() {}
+
+  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]; }
+
+  template <class BBox>
+  bool kdtree_get_bbox(BBox&) const {
+    return false;
+  }
+
+  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;
+};
+
+template <typename PointCloud>
+class KdTree {
+public:
+  using Ptr = std::shared_ptr<KdTree>;
+  using ConstPtr = std::shared_ptr<const KdTree>;
+
+  KdTree(const std::shared_ptr<const PointCloud>& points) : points(points), tree(*points) {}
+  ~KdTree() {}
+
+  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;
+};
+
+namespace traits {
+
+template <typename PointCloud>
+struct Traits<UnsafeKdTree<PointCloud>> {
+  static size_t knn_search(const UnsafeKdTree<PointCloud>& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) {
+    return tree.knn_search(point, k, k_indices, k_sq_dists);
+  }
+};
+
+template <typename PointCloud>
+struct Traits<KdTree<PointCloud>> {
+  static size_t knn_search(const KdTree<PointCloud>& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) {
+    return tree.knn_search(point, k, k_indices, k_sq_dists);
+  }
+};
+
+}  // namespace traits
+
+}  // namespace small_gicp

include/small_gicp/ann/traits.hpp

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <Eigen/Core>
+
+namespace small_gicp {
+
+namespace traits {
+
+template <typename T>
+struct Traits;
+
+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);
+}
+
+}  // namespace traits
+
+}  // namespace small_gicp

include/small_gicp/factors/gicp_factor.hpp

@@ -0,0 +1,72 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <small_gicp/util/lie.hpp>
+#include <small_gicp/ann/traits.hpp>
+#include <small_gicp/points/traits.hpp>
+
+namespace small_gicp {
+
+struct GICPFactor {
+  GICPFactor() : target_index(std::numeric_limits<size_t>::max()), source_index(std::numeric_limits<size_t>::max()), mahalanobis(Eigen::Matrix4d::Zero()) {}
+
+  template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename CorrespondenceRejector>
+  bool linearize(
+    const TargetPointCloud& target,
+    const SourcePointCloud& source,
+    const TargetTree& target_tree,
+    const Eigen::Isometry3d& T,
+    size_t source_index,
+    const CorrespondenceRejector& rejector,
+    Eigen::Matrix<double, 6, 6>* H,
+    Eigen::Matrix<double, 6, 1>* b,
+    double* e) {
+    //
+    this->source_index = source_index;
+    this->target_index = std::numeric_limits<size_t>::max();
+
+    const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index);
+
+    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)) {
+      return false;
+    }
+
+    target_index = k_index;
+
+    const Eigen::Matrix4d RCR = traits::cov(target, target_index) + T.matrix() * traits::cov(source, source_index) * T.matrix().transpose();
+    mahalanobis.block<3, 3>(0, 0) = RCR.block<3, 3>(0, 0).inverse();
+
+    const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt;
+
+    Eigen::Matrix<double, 4, 6> J = Eigen::Matrix<double, 4, 6>::Zero();
+    J.block<3, 3>(0, 0) = T.linear() * skew(traits::point(source, source_index).template head<3>());
+    J.block<3, 3>(0, 3) = -T.linear();
+
+    *H = J.transpose() * mahalanobis * J;
+    *b = J.transpose() * mahalanobis * residual;
+    *e = 0.5 * residual.dot(mahalanobis * residual);
+
+    return true;
+  }
+
+  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()) {
+      return 0.0;
+    }
+
+    const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index);
+    const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt;
+    return 0.5 * residual.dot(mahalanobis * residual);
+  }
+
+  bool inlier() const { return target_index != std::numeric_limits<size_t>::max(); }
+
+  size_t target_index;
+  size_t source_index;
+  Eigen::Matrix4d mahalanobis;
+};
+}  // namespace small_gicp

include/small_gicp/factors/icp_factor.hpp

@@ -0,0 +1,66 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <small_gicp/util/lie.hpp>
+#include <small_gicp/ann/traits.hpp>
+#include <small_gicp/points/traits.hpp>
+
+namespace small_gicp {
+
+struct ICPFactor {
+  ICPFactor() : target_index(std::numeric_limits<size_t>::max()), source_index(std::numeric_limits<size_t>::max()) {}
+
+  template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename CorrespondenceRejector>
+  bool linearize(
+    const TargetPointCloud& target,
+    const SourcePointCloud& source,
+    const TargetTree& target_tree,
+    const Eigen::Isometry3d& T,
+    size_t source_index,
+    const CorrespondenceRejector& rejector,
+    Eigen::Matrix<double, 6, 6>* H,
+    Eigen::Matrix<double, 6, 1>* b,
+    double* e) {
+    //
+    this->source_index = source_index;
+    this->target_index = std::numeric_limits<size_t>::max();
+
+    const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index);
+
+    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)) {
+      return false;
+    }
+
+    target_index = k_index;
+    const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt;
+
+    Eigen::Matrix<double, 4, 6> J = Eigen::Matrix<double, 4, 6>::Zero();
+    J.block<3, 3>(0, 0) = T.linear() * skew(traits::point(source, source_index).template head<3>());
+    J.block<3, 3>(0, 3) = -T.linear();
+
+    *H = J.transpose() * J;
+    *b = J.transpose() * residual;
+    *e = 0.5 * residual.squaredNorm();
+
+    return true;
+  }
+
+  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()) {
+      return 0.0;
+    }
+
+    const Eigen::Vector4d residual = traits::point(target, target_index) - T * traits::point(source, source_index);
+    return 0.5 * residual.squaredNorm();
+  }
+
+  bool inlier() const { return target_index != std::numeric_limits<size_t>::max(); }
+
+  size_t target_index;
+  size_t source_index;
+};
+}  // namespace small_gicp

include/small_gicp/factors/plane_icp_factor.hpp

@@ -0,0 +1,71 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <small_gicp/util/lie.hpp>
+#include <small_gicp/ann/traits.hpp>
+#include <small_gicp/points/traits.hpp>
+
+namespace small_gicp {
+
+struct PointToPlaneICPFactor {
+  PointToPlaneICPFactor() : target_index(std::numeric_limits<size_t>::max()), source_index(std::numeric_limits<size_t>::max()) {}
+
+  template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename CorrespondenceRejector>
+  bool linearize(
+    const TargetPointCloud& target,
+    const SourcePointCloud& source,
+    const TargetTree& target_tree,
+    const Eigen::Isometry3d& T,
+    size_t source_index,
+    const CorrespondenceRejector& rejector,
+    Eigen::Matrix<double, 6, 6>* H,
+    Eigen::Matrix<double, 6, 1>* b,
+    double* e) {
+    //
+    this->source_index = source_index;
+    this->target_index = std::numeric_limits<size_t>::max();
+
+    const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index);
+
+    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)) {
+      return false;
+    }
+
+    target_index = k_index;
+    const auto& target_normal = traits::normal(target, target_index);
+
+    const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt;
+    const Eigen::Vector4d error = target_normal.array() * residual.array();
+
+    Eigen::Matrix<double, 4, 6> J = Eigen::Matrix<double, 4, 6>::Zero();
+    J.block<3, 3>(0, 0) = target_normal.template head<3>().asDiagonal() * T.linear() * skew(traits::point(source, source_index).template head<3>());
+    J.block<3, 3>(0, 3) = target_normal.template head<3>().asDiagonal() * (-T.linear());
+
+    *H = J.transpose() * J;
+    *b = J.transpose() * error;
+    *e = 0.5 * error.squaredNorm();
+
+    return true;
+  }
+
+  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()) {
+      return 0.0;
+    }
+
+    const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index);
+    const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt;
+    const Eigen::Vector4d error = traits::normal(target, target_index).array() * residual.array();
+    return 0.5 * error.squaredNorm();
+  }
+
+  bool inlier() const { return target_index != std::numeric_limits<size_t>::max(); }
+
+  size_t target_index;
+  size_t source_index;
+};
+}  // namespace small_gicp

include/small_gicp/points/gaussian_voxelmap.hpp

@@ -0,0 +1,135 @@
+#pragma once
+
+#include <unordered_map>
+
+#include <small_gicp/ann/traits.hpp>
+#include <small_gicp/points/traits.hpp>
+#include <small_gicp/util/vector3i_hash.hpp>
+
+namespace small_gicp {
+
+struct GaussianVoxel {
+public:
+  GaussianVoxel(const Eigen::Vector3i& coord) : finalized(false), lru(0), num_points(0), coord(coord), mean(Eigen::Vector4d::Zero()), cov(Eigen::Matrix4d::Zero()) {}
+  ~GaussianVoxel() {}
+
+  void add(const Eigen::Vector4d& mean, const Eigen::Matrix4d& cov, size_t lru) {
+    if (finalized) {
+      this->finalized = false;
+      this->mean *= num_points;
+      this->cov *= num_points;
+    }
+
+    num_points++;
+    this->mean += mean;
+    this->cov += cov;
+    this->lru = lru;
+  }
+
+  void finalize() {
+    if (finalized) {
+      return;
+    }
+
+    mean /= num_points;
+    cov /= num_points;
+    finalized = true;
+  }
+
+public:
+  bool finalized;
+  size_t lru;
+  size_t num_points;
+  Eigen::Vector3i coord;
+
+  Eigen::Vector4d mean;
+  Eigen::Matrix4d cov;
+};
+
+struct GaussianVoxelMap {
+public:
+  using Ptr = std::shared_ptr<GaussianVoxelMap>;
+  using ConstPtr = std::shared_ptr<const GaussianVoxelMap>;
+
+  GaussianVoxelMap(double leaf_size) : inv_leaf_size(1.0 / leaf_size), lru_horizon(100), lru_clear_cycle(10), lru_counter(0) {}
+  ~GaussianVoxelMap() {}
+
+  size_t size() const { return flat_voxels.size(); }
+
+  template <typename PointCloud>
+  void insert(const PointCloud& points, const Eigen::Isometry3d& T = Eigen::Isometry3d::Identity()) {
+    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>();
+
+      auto found = voxels.find(coord);
+      if (found == voxels.end()) {
+        found = voxels.emplace_hint(found, coord, flat_voxels.size());
+        flat_voxels.emplace_back(coord);
+      }
+
+      auto& voxel = flat_voxels[found->second];
+      const Eigen::Matrix4d cov = T.matrix() * traits::cov(points, i) * T.matrix().transpose();
+      voxel.add(pt, cov, lru_counter);
+    }
+
+    if ((lru_counter++) % lru_clear_cycle == 0) {
+      std::erase_if(flat_voxels, [&](const GaussianVoxel& voxel) { return voxel.lru + lru_horizon < lru_counter; });
+      voxels.clear();
+      for (size_t i = 0; i < flat_voxels.size(); i++) {
+        voxels[flat_voxels[i].coord] = i;
+      }
+    }
+
+    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;
+    }
+
+    const Eigen::Vector3i coord = (pt * inv_leaf_size).array().floor().cast<int>().head<3>();
+    const auto found = voxels.find(coord);
+    if (found == voxels.end()) {
+      return 0;
+    }
+
+    const GaussianVoxel& voxel = flat_voxels[found->second];
+    k_indices[0] = found->second;
+    k_sq_dists[0] = (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;
+
+  std::vector<GaussianVoxel> flat_voxels;
+  std::unordered_map<Eigen::Vector3i, size_t, XORVector3iHash> voxels;
+};
+
+namespace traits {
+
+template <>
+struct Traits<GaussianVoxelMap> {
+  static size_t size(const GaussianVoxelMap& voxelmap) { return voxelmap.size(); }
+
+  static bool has_points(const GaussianVoxelMap& voxelmap) { return !voxelmap.flat_voxels.empty(); }
+  static bool has_covs(const GaussianVoxelMap& voxelmap) { return !voxelmap.flat_voxels.empty(); }
+
+  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);
+  }
+};
+
+}  // namespace traits
+
+}  // namespace small_gicp

include/small_gicp/points/pcl_point.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <pcl/point_types.h>
+
+namespace pcl {
+
+using Matrix4fMap = Eigen::Map<Eigen::Matrix4f, Eigen::Aligned>;
+using Matrix4fMapConst = const Eigen::Map<const Eigen::Matrix4f, Eigen::Aligned>;
+
+struct EIGEN_ALIGN16 PointCovariance {
+  PCL_ADD_POINT4D;
+  Eigen::Matrix4f cov;
+
+  Matrix4fMap getCovariance4fMap() { return Matrix4fMap(cov.data()); }
+  Matrix4fMapConst getCovariance4fMap() const { return Matrix4fMapConst(cov.data()); }
+
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+struct EIGEN_ALIGN16 PointNormalCovariance {
+  PCL_ADD_POINT4D;
+  PCL_ADD_NORMAL4D
+  Eigen::Matrix4f cov;
+
+  Matrix4fMap getCovariance4fMap() { return Matrix4fMap(cov.data()); }
+  Matrix4fMapConst getCovariance4fMap() const { return Matrix4fMapConst(cov.data()); }
+
+  PCL_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}  // namespace pcl

include/small_gicp/points/pcl_point_traits.hpp

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+#include <small_gicp/points/traits.hpp>
+
+namespace small_gicp {
+
+namespace traits {
+
+template <typename PointType>
+struct Traits<pcl::PointCloud<PointType>> {
+  using Points = pcl::PointCloud<PointType>;
+
+  static size_t size(const Points& points) { return points.size(); }
+  static void resize(Points& points, size_t n) { points.resize(n); }
+
+  static bool has_points(const Points& points) { return !points.empty(); }
+  static bool has_normals(const Points& points) { return !points.empty(); }
+  static bool has_covs(const Points& points) { return !points.empty(); }
+
+  static void set_point(Points& points, size_t i, const Eigen::Vector4d& pt) { points.at(i).getVector4fMap() = pt.template cast<float>(); }
+  static void set_normal(Points& points, size_t i, const Eigen::Vector4d& pt) { points.at(i).getNormalVector4fMap() = pt.template cast<float>(); }
+  static void set_cov(Points& points, size_t i, const Eigen::Matrix4d& cov) { points.at(i).getCovariance4fMap() = cov.template cast<float>(); }
+
+  static Eigen::Vector4d point(const Points& points, size_t i) { return points.at(i).getVector4fMap().template cast<double>(); }
+  static Eigen::Vector4d normal(const Points& points, size_t i) { return points.at(i).getNormalVector4fMap().template cast<double>(); }
+  static Eigen::Matrix4d cov(const Points& points, size_t i) { return points.at(i).getCovariance4fMap().template cast<double>(); }
+};
+
+}  // namespace traits
+
+}  // namespace small_gicp

include/small_gicp/points/point_cloud.hpp

@@ -0,0 +1,69 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <small_gicp/points/traits.hpp>
+
+namespace small_gicp {
+
+struct PointCloud {
+public:
+  using Ptr = std::shared_ptr<PointCloud>;
+  using ConstPtr = std::shared_ptr<const PointCloud>;
+
+  PointCloud() {}
+  ~PointCloud() {}
+
+  template <typename T, int D, typename Allocator>
+  PointCloud(const std::vector<Eigen::Matrix<T, D, 1>, Allocator>& points) {
+    this->resize(points.size());
+    for (size_t i = 0; i < points.size(); i++) {
+      this->point(i) << points[i].template cast<double>().template head<3>(), 1.0;
+    }
+  }
+
+  size_t size() const { return points.size(); }
+
+  void resize(size_t n) {
+    points.resize(n);
+    normals.resize(n);
+    covs.resize(n);
+  }
+
+  Eigen::Vector4d& point(size_t i) { return points[i]; }
+  Eigen::Vector4d& normal(size_t i) { return normals[i]; }
+  Eigen::Matrix4d& cov(size_t i) { return covs[i]; }
+  const Eigen::Vector4d& point(size_t i) const { return points[i]; }
+  const Eigen::Vector4d& normal(size_t i) const { return normals[i]; }
+  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;
+};
+
+namespace traits {
+
+template <>
+struct Traits<PointCloud> {
+  using Points = PointCloud;
+
+  static size_t size(const Points& points) { return points.size(); }
+
+  static bool has_points(const Points& points) { return !points.points.empty(); }
+  static bool has_normals(const Points& points) { return !points.normals.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.point(i); }
+  static const Eigen::Vector4d& normal(const Points& points, size_t i) { return points.normal(i); }
+  static const Eigen::Matrix4d& cov(const Points& points, size_t i) { return points.cov(i); }
+
+  static void resize(Points& points, size_t n) { points.resize(n); }
+  static void set_point(Points& points, size_t i, const Eigen::Vector4d& pt) { points.point(i) = pt; }
+  static void set_normal(Points& points, size_t i, const Eigen::Vector4d& n) { points.normal(i) = n; }
+  static void set_cov(Points& points, size_t i, const Eigen::Matrix4d& cov) { points.cov(i) = cov; }
+};
+
+}  // namespace traits
+
+}  // namespace small_gicp

include/small_gicp/points/traits.hpp

@@ -0,0 +1,68 @@
+#pragma once
+
+#include <Eigen/Core>
+
+namespace small_gicp {
+
+namespace traits {
+
+template <typename T>
+struct Traits;
+
+template <typename T>
+size_t size(const T& points) {
+  return Traits<T>::size(points);
+}
+
+template <typename T>
+bool has_points(const T& points) {
+  return Traits<T>::has_points(points);
+}
+
+template <typename T>
+bool has_normals(const T& points) {
+  return Traits<T>::has_normals(points);
+}
+
+template <typename T>
+bool has_covs(const T& points) {
+  return Traits<T>::has_covs(points);
+}
+
+template <typename T>
+auto point(const T& points, size_t i) {
+  return Traits<T>::point(points, i);
+}
+
+template <typename T>
+auto normal(const T& points, size_t i) {
+  return Traits<T>::normal(points, i);
+}
+
+template <typename T>
+auto cov(const T& points, size_t i) {
+  return Traits<T>::cov(points, i);
+}
+
+template <typename T>
+void resize(T& points, size_t n) {
+  Traits<T>::resize(points, n);
+}
+
+template <typename T>
+void set_point(T& points, size_t i, const Eigen::Vector4d& pt) {
+  Traits<T>::set_point(points, i, pt);
+}
+
+template <typename T>
+void set_normal(T& points, size_t i, const Eigen::Vector4d& pt) {
+  Traits<T>::set_normal(points, i, pt);
+}
+
+template <typename T>
+void set_cov(T& points, size_t i, const Eigen::Matrix4d& cov) {
+  Traits<T>::set_cov(points, i, cov);
+}
+
+}  // namespace traits
+}  // namespace small_gicp

include/small_gicp/registration/optimizer.hpp

@@ -0,0 +1,131 @@
+#pragma once
+
+#include <small_gicp/util/lie.hpp>
+#include <small_gicp/registration/registration_result.hpp>
+
+namespace small_gicp {
+
+struct GaussNewtonOptimizer {
+  GaussNewtonOptimizer() : verbose(false), max_iterations(20), lambda(1e-6) {}
+
+  template <
+    typename TargetPointCloud,
+    typename SourcePointCloud,
+    typename TargetTree,
+    typename CorrespondenceRejector,
+    typename TerminationCriteria,
+    typename Reduction,
+    typename Factor>
+  RegistrationResult optimize(
+    const TargetPointCloud& target,
+    const SourcePointCloud& source,
+    const TargetTree& target_tree,
+    const CorrespondenceRejector& rejector,
+    const TerminationCriteria& criteria,
+    Reduction& reduction,
+    const Eigen::Isometry3d& init_T,
+    std::vector<Factor>& factors) {
+    //
+    if (verbose) {
+      std::cout << "--- GN optimization ---" << std::endl;
+    }
+
+    RegistrationResult result(init_T);
+    for (int i = 0; i < max_iterations && !result.converged; i++) {
+      const auto [H, b, e] = reduction.linearize(target, source, target_tree, rejector, result.T_target_source, factors);
+      const Eigen::Matrix<double, 6, 1> delta = (H + lambda * Eigen ::Matrix<double, 6, 6>::Identity()).ldlt().solve(-b);
+
+      if (verbose) {
+        std::cout << "iter=" << i << " e=" << e << " lambda=" << lambda << " dt=" << delta.tail<3>().norm() << " dr=" << delta.head<3>().norm() << std::endl;
+      }
+
+      result.converged = criteria.converged(delta);
+      result.T_target_source = result.T_target_source * se3_exp(delta);
+      result.iterations = i;
+      result.H = H;
+      result.b = b;
+      result.error = e;
+    }
+
+    result.num_inliers = std::ranges::count_if(factors, [](const auto& factor) { return factor.inlier(); });
+
+    return result;
+  }
+
+  bool verbose;
+  int max_iterations;
+  double lambda;
+};
+
+struct LevenbergMarquardtOptimizer {
+  LevenbergMarquardtOptimizer() : verbose(false), max_iterations(20), max_inner_iterations(10), init_lambda(1e-3), lambda_factor(10.0) {}
+
+  template <
+    typename TargetPointCloud,
+    typename SourcePointCloud,
+    typename TargetTree,
+    typename CorrespondenceRejector,
+    typename TerminationCriteria,
+    typename Reduction,
+    typename Factor>
+  RegistrationResult optimize(
+    const TargetPointCloud& target,
+    const SourcePointCloud& source,
+    const TargetTree& target_tree,
+    const CorrespondenceRejector& rejector,
+    const TerminationCriteria& criteria,
+    Reduction& reduction,
+    const Eigen::Isometry3d& init_T,
+    std::vector<Factor>& factors) {
+    //
+    if (verbose) {
+      std::cout << "--- LM optimization ---" << std::endl;
+    }
+
+    double lambda = init_lambda;
+    RegistrationResult result(init_T);
+    for (int i = 0; i < max_iterations && !result.converged; i++) {
+      const auto [H, b, e] = reduction.linearize(target, source, target_tree, rejector, result.T_target_source, factors);
+
+      bool success = false;
+      for (int j = 0; j < max_inner_iterations; j++) {
+        const Eigen::Matrix<double, 6, 1> delta = (H + lambda * Eigen ::Matrix<double, 6, 6>::Identity()).ldlt().solve(-b);
+        const Eigen::Isometry3d new_T = result.T_target_source * se3_exp(delta);
+        const double new_e = reduction.error(target, source, new_T, factors);
+
+        if (verbose) {
+          std::cout << "iter=" << i << " inner=" << j << " e=" << e << " new_e=" << new_e << " lambda=" << lambda << " dt=" << delta.tail<3>().norm()
+                    << " dr=" << delta.head<3>().norm() << std::endl;
+        }
+
+        if (new_e < e) {
+          result.converged = criteria.converged(delta);
+          result.T_target_source = new_T;
+          lambda /= lambda_factor;
+          success = true;
+
+          break;
+        } else {
+          lambda *= lambda_factor;
+        }
+      }
+
+      result.iterations = i;
+      result.H = H;
+      result.b = b;
+      result.error = e;
+    }
+
+    result.num_inliers = std::ranges::count_if(factors, [](const auto& factor) { return factor.inlier(); });
+
+    return result;
+  }
+
+  bool verbose;
+  int max_iterations;
+  int max_inner_iterations;
+  double init_lambda;
+  double lambda_factor;
+};
+
+}  // namespace small_gicp

include/small_gicp/registration/reduction.hpp

@@ -0,0 +1,47 @@
+#pragma once
+
+#include <Eigen/Core>
+
+namespace small_gicp {
+
+struct SerialReduction {
+  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,
+    const SourcePointCloud& source,
+    const TargetTree& target_tree,
+    const CorrespondenceRejector& rejector,
+    const Eigen::Isometry3d& T,
+    std::vector<Factor>& factors) {
+    Eigen::Matrix<double, 6, 6> sum_H = Eigen::Matrix<double, 6, 6>::Zero();
+    Eigen::Matrix<double, 6, 1> sum_b = Eigen::Matrix<double, 6, 1>::Zero();
+    double sum_e = 0.0;
+
+    for (size_t i = 0; i < factors.size(); i++) {
+      Eigen::Matrix<double, 6, 6> H;
+      Eigen::Matrix<double, 6, 1> b;
+      double e;
+
+      if (!factors[i].linearize(target, source, target_tree, T, i, rejector, &H, &b, &e)) {
+        continue;
+      }
+
+      sum_H += H;
+      sum_b += b;
+      sum_e += e;
+    }
+
+    return {sum_H, sum_b, sum_e};
+  }
+
+  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;
+    for (size_t i = 0; i < factors.size(); i++) {
+      sum_e += factors[i].error(target, source, T);
+    }
+    return sum_e;
+  }
+};
+
+}  // namespace small_gicp

include/small_gicp/registration/reduction_omp.hpp

@@ -0,0 +1,61 @@
+#pragma once
+
+#include <Eigen/Core>
+
+namespace small_gicp {
+
+struct ParallelReductionOMP {
+  ParallelReductionOMP() : num_threads(8) {}
+
+  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,
+    const SourcePointCloud& source,
+    const TargetTree& target_tree,
+    const CorrespondenceRejector& rejector,
+    const Eigen::Isometry3d& T,
+    std::vector<Factor>& factors) {
+    std::vector<Eigen::Matrix<double, 6, 6>> Hs(num_threads, Eigen::Matrix<double, 6, 6>::Zero());
+    std::vector<Eigen::Matrix<double, 6, 1>> bs(num_threads, Eigen::Matrix<double, 6, 1>::Zero());
+    std::vector<double> es(num_threads, 0.0);
+
+#pragma omp parallel for num_threads(num_threads) schedule(guided, 8)
+    for (size_t i = 0; i < factors.size(); i++) {
+      Eigen::Matrix<double, 6, 6> H;
+      Eigen::Matrix<double, 6, 1> b;
+      double e;
+
+      if (!factors[i].linearize(target, source, target_tree, T, i, rejector, &H, &b, &e)) {
+        continue;
+      }
+
+      const int thread_id = omp_get_thread_num();
+      Hs[thread_id] += H;
+      bs[thread_id] += b;
+      es[thread_id] += e;
+    }
+
+    for (int i = 1; i < num_threads; i++) {
+      Hs[0] += Hs[i];
+      bs[0] += bs[i];
+      es[0] += es[i];
+    }
+
+    return {Hs[0], bs[0], es[0]};
+  }
+
+  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;
+
+#pragma omp parallel for num_threads(num_threads) schedule(guided, 8) reduction(+ : sum_e)
+    for (size_t i = 0; i < factors.size(); i++) {
+      sum_e += factors[i].error(target, source, T);
+    }
+    return sum_e;
+  }
+
+  int num_threads;
+};
+
+}  // namespace small_gicp

include/small_gicp/registration/reduction_tbb.hpp

@@ -0,0 +1,136 @@
+#pragma once
+
+#include <tbb/tbb.h>
+#include <Eigen/Core>
+
+namespace small_gicp {
+
+template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename CorrespondenceRejector, typename Factor>
+struct LinearizeSum {
+  LinearizeSum(
+    const TargetPointCloud& target,
+    const SourcePointCloud& source,
+    const TargetTree& target_tree,
+    const CorrespondenceRejector& rejector,
+    const Eigen::Isometry3d& T,
+    std::vector<Factor>& factors)
+  : target(target),
+    source(source),
+    target_tree(target_tree),
+    rejector(rejector),
+    T(T),
+    factors(factors),
+    H(Eigen::Matrix<double, 6, 6>::Zero()),
+    b(Eigen::Matrix<double, 6, 1>::Zero()),
+    e(0.0) {}
+
+  LinearizeSum(LinearizeSum& x, tbb::split)
+  : target(x.target),
+    source(x.source),
+    target_tree(x.target_tree),
+    rejector(x.rejector),
+    T(x.T),
+    factors(x.factors),
+    H(Eigen::Matrix<double, 6, 6>::Zero()),
+    b(Eigen::Matrix<double, 6, 1>::Zero()),
+    e(0.0) {}
+
+  void operator()(const tbb::blocked_range<size_t>& r) {
+    Eigen::Matrix<double, 6, 6> Ht = H;
+    Eigen::Matrix<double, 6, 1> bt = b;
+    double et = e;
+
+    for (size_t i = r.begin(); i != r.end(); i++) {
+      Eigen::Matrix<double, 6, 6> Hi;
+      Eigen::Matrix<double, 6, 1> bi;
+      double ei;
+
+      if (!factors[i].linearize(target, source, target_tree, T, i, rejector, &Hi, &bi, &ei)) {
+        continue;
+      }
+
+      Ht += Hi;
+      bt += bi;
+      et += ei;
+    }
+
+    H = Ht;
+    b = bt;
+    e = et;
+  }
+
+  void join(const LinearizeSum& y) {
+    H += y.H;
+    b += y.b;
+    e += y.e;
+  }
+
+  const TargetPointCloud& target;
+  const SourcePointCloud& source;
+  const TargetTree& target_tree;
+  const CorrespondenceRejector& rejector;
+  const Eigen::Isometry3d& T;
+  std::vector<Factor>& factors;
+
+  Eigen::Matrix<double, 6, 6> H;
+  Eigen::Matrix<double, 6, 1> b;
+  double e;
+};
+
+template <typename TargetPointCloud, typename SourcePointCloud, typename Factor>
+struct ErrorSum {
+  ErrorSum(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector<Factor>& factors)
+  : target(target),
+    source(source),
+    T(T),
+    factors(factors),
+    e(0.0) {}
+
+  ErrorSum(ErrorSum& x, tbb::split) : target(x.target), source(x.source), T(x.T), factors(x.factors), e(0.0) {}
+
+  void operator()(const tbb::blocked_range<size_t>& r) {
+    double et = e;
+    for (size_t i = r.begin(); i != r.end(); i++) {
+      et += factors[i].error(target, source, T);
+    }
+    e = et;
+  }
+
+  void join(const ErrorSum& y) { e += y.e; }
+
+  const TargetPointCloud& target;
+  const SourcePointCloud& source;
+  const Eigen::Isometry3d& T;
+  std::vector<Factor>& factors;
+
+  double e;
+};
+
+struct ParallelReductionTBB {
+  ParallelReductionTBB() {}
+
+  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,
+    const SourcePointCloud& source,
+    const TargetTree& target_tree,
+    const CorrespondenceRejector& rejector,
+    const Eigen::Isometry3d& T,
+    std::vector<Factor>& factors) {
+    //
+    LinearizeSum<TargetPointCloud, SourcePointCloud, TargetTree, CorrespondenceRejector, Factor> sum(target, source, target_tree, rejector, T, factors);
+
+    tbb::parallel_reduce(tbb::blocked_range<size_t>(0, factors.size()), sum);
+
+    return {sum.H, sum.b, sum.e};
+  }
+
+  template <typename TargetPointCloud, typename SourcePointCloud, typename Factor>
+  double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector<Factor>& factors) {
+    ErrorSum<TargetPointCloud, SourcePointCloud, Factor> sum(target, source, T, factors);
+    tbb::parallel_reduce(tbb::blocked_range<size_t>(0, factors.size()), sum);
+    return sum.e;
+  }
+};
+
+}  // namespace small_gicp

include/small_gicp/registration/registration.hpp

@@ -0,0 +1,38 @@
+#pragma once
+
+#include <small_gicp/ann/traits.hpp>
+#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>
+
+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;
+};
+
+template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename Factor, typename CorrespondenceRejector, typename Reduction, typename Optimizer>
+struct Registration {
+public:
+  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;
+};
+
+}  // namespace small_gicp

include/small_gicp/registration/registration_result.hpp

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace small_gicp {
+
+struct RegistrationResult {
+  RegistrationResult(const Eigen::Isometry3d& T)
+  : T_target_source(T),
+    converged(false),
+    iterations(0),
+    num_inliers(0),
+    H(Eigen::Matrix<double, 6, 6>::Zero()),
+    b(Eigen::Matrix<double, 6, 1>::Zero()),
+    error(0.0) {}
+
+  Eigen::Isometry3d T_target_source;
+
+  bool converged;
+  size_t iterations;
+  size_t num_inliers;
+
+  Eigen::Matrix<double, 6, 6> H;
+  Eigen::Matrix<double, 6, 1> b;
+  double error;
+};
+
+}  // namespace small_gicp

include/small_gicp/registration/rejector.hpp

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace small_gicp {
+
+struct NullRejector {
+  bool operator()(const Eigen::Isometry3d& T, size_t target_index, size_t source_index, double sq_dist) const { return false; }
+};
+
+struct DistanceRejector {
+  DistanceRejector() : max_dist_sq(1.0) {}
+  DistanceRejector(double max_dist) : max_dist_sq(max_dist * max_dist) {}
+
+  bool operator()(const Eigen::Isometry3d& T, size_t target_index, size_t source_index, double sq_dist) const { return sq_dist > max_dist_sq; }
+
+  double max_dist_sq;
+};
+
+}  // namespace small_gicp

include/small_gicp/util/downsampling.hpp

@@ -0,0 +1,89 @@
+#pragma once
+
+#include <memory>
+#include <random>
+#include <unordered_map>
+#include <small_gicp/points/traits.hpp>
+#include <small_gicp/util/vector3i_hash.hpp>
+
+namespace small_gicp {
+
+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;
+  }
+
+  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;
+  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, std::make_shared<std::vector<size_t>>());
+      found->second->reserve(20);
+    }
+
+    found->second->emplace_back(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);
+
+  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]));
+  }
+
+  return downsampled;
+}
+
+}  // namespace small_gicp

include/small_gicp/util/downsampling_tbb.hpp

@@ -0,0 +1,68 @@
+#pragma once
+
+#include <memory>
+
+#include <tbb/tbb.h>
+#include <small_gicp/points/traits.hpp>
+#include <small_gicp/util/vector3i_hash.hpp>
+
+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.
+ */
+template <typename InputPointCloud, typename OutputPointCloud = InputPointCloud>
+std::shared_ptr<OutputPointCloud> voxelgrid_sampling_tbb(const InputPointCloud& points, double leaf_size) {
+  const double inv_leaf_size = 1.0 / leaf_size;
+
+  typedef tbb::concurrent_hash_map<Eigen::Vector3i, int, XORVector3iHash> VoxelMap;
+
+  std::atomic_uint64_t num_voxels = 0;
+  VoxelMap voxels;
+  std::vector<Eigen::Vector4d> voxel_values(traits::size(points), Eigen::Vector4d::Zero());
+
+  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());
+
+    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 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;
+      }
+    }
+
+    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 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());
+  }
+
+  return downsampled;
+}
+
+}  // namespace small_gicp

include/small_gicp/util/lie.hpp

@@ -0,0 +1,93 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace small_gicp {
+
+inline Eigen::Matrix3d skew(const Eigen::Vector3d& x) {
+  Eigen::Matrix3d skew = Eigen::Matrix3d::Zero();
+  skew(0, 1) = -x[2];
+  skew(0, 2) = x[1];
+  skew(1, 0) = x[2];
+  skew(1, 2) = -x[0];
+  skew(2, 0) = -x[1];
+  skew(2, 1) = x[0];
+
+  return skew;
+}
+
+/*
+ * SO3 expmap code taken from Sophus
+ * https://github.com/strasdat/Sophus/blob/593db47500ea1a2de5f0e6579c86147991509c59/sophus/so3.hpp#L585
+ *
+ * Copyright 2011-2017 Hauke Strasdat
+ *           2012-2017 Steven Lovegrove
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to
+ * deal in the Software without restriction, including without limitation the
+ * rights  to use, copy, modify, merge, publish, distribute, sublicense, and/or
+ * sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ */
+inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) {
+  double theta_sq = omega.dot(omega);
+
+  double theta;
+  double imag_factor;
+  double real_factor;
+  if (theta_sq < 1e-10) {
+    theta = 0;
+    double theta_quad = theta_sq * theta_sq;
+    imag_factor = 0.5 - 1.0 / 48.0 * theta_sq + 1.0 / 3840.0 * theta_quad;
+    real_factor = 1.0 - 1.0 / 8.0 * theta_sq + 1.0 / 384.0 * theta_quad;
+  } else {
+    theta = std::sqrt(theta_sq);
+    double half_theta = 0.5 * theta;
+    imag_factor = std::sin(half_theta) / theta;
+    real_factor = std::cos(half_theta);
+  }
+
+  return Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z());
+}
+
+// Rotation-first
+inline Eigen::Isometry3d se3_exp(const Eigen::Matrix<double, 6, 1>& a) {
+  using std::cos;
+  using std::sin;
+  const Eigen::Vector3d omega = a.head<3>();
+
+  double theta = std::sqrt(omega.dot(omega));
+  const Eigen::Quaterniond so3 = so3_exp(omega);
+  const Eigen::Matrix3d Omega = skew(omega);
+  const Eigen::Matrix3d Omega_sq = Omega * Omega;
+  Eigen::Matrix3d V;
+
+  if (theta < 1e-10) {
+    V = so3.matrix();
+    /// Note: That is an accurate expansion!
+  } else {
+    const double theta_sq = theta * theta;
+    V = (Eigen::Matrix3d::Identity() + (1.0 - cos(theta)) / (theta_sq)*Omega + (theta - sin(theta)) / (theta_sq * theta) * Omega_sq);
+  }
+
+  Eigen::Isometry3d se3 = Eigen::Isometry3d::Identity();
+  se3.linear() = so3.toRotationMatrix();
+  se3.translation() = V * a.tail<3>();
+
+  return se3;
+}
+
+}  // namespace small_gicp

include/small_gicp/util/normal_estimation.hpp

@@ -0,0 +1,105 @@
+#pragma once
+
+#include <Eigen/Eigen>
+#include <small_gicp/ann/kdtree.hpp>
+
+namespace small_gicp {
+
+template <typename PointCloud>
+struct NormalSetter {
+  static void set_invalid(PointCloud& cloud, size_t i) { traits::set_normal(cloud, i, Eigen::Vector4d::Zero()); }
+
+  static void set(PointCloud& cloud, size_t i, const Eigen::Matrix3d& eigenvectors) {
+    const Eigen::Vector4d normal = (Eigen::Vector4d() << eigenvectors.col(0).normalized(), 0.0).finished();
+    if (traits::point(cloud, i).dot(normal) > 0) {
+      traits::set_normal(cloud, i, -normal);
+    } else {
+      traits::set_normal(cloud, i, normal);
+    }
+  }
+};
+
+template <typename PointCloud>
+struct CovarianceSetter {
+  static void set_invalid(PointCloud& cloud, size_t i) {
+    Eigen::Matrix4d cov = Eigen::Matrix4d::Identity();
+    cov(3, 3) = 0.0;
+    traits::set_cov(cloud, i, cov);
+  }
+
+  static void set(PointCloud& cloud, size_t i, const Eigen::Matrix3d& eigenvectors) {
+    const Eigen::Vector3d values(1e-3, 1.0, 1.0);
+    Eigen::Matrix4d cov = Eigen::Matrix4d::Zero();
+    cov.block<3, 3>(0, 0) = eigenvectors * values.asDiagonal() * eigenvectors.transpose();
+    traits::set_cov(cloud, i, cov);
+  }
+};
+
+template <typename PointCloud>
+struct NormalCovarianceSetter {
+  static void set_invalid(PointCloud& cloud, size_t i) {
+    NormalSetter<PointCloud>::set_invalid(cloud, i);
+    CovarianceSetter<PointCloud>::set_invalid(cloud, i);
+  }
+
+  static void set(PointCloud& cloud, size_t i, const Eigen::Matrix3d& eigenvectors) {
+    NormalSetter<PointCloud>::set(cloud, i, eigenvectors);
+    CovarianceSetter<PointCloud>::set(cloud, i, eigenvectors);
+  }
+};
+
+template <typename PointCloud, typename Tree, typename Setter>
+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);
+  const size_t n = kdtree.knn_search(traits::point(cloud, point_index), num_neighbors, k_indices.data(), k_sq_dists.data());
+
+  if (n < 5) {
+    // Insufficient number of neighbors
+    Setter::set_invalid(cloud, point_index);
+    return;
+  }
+
+  Eigen::Vector4d sum_points = Eigen::Vector4d::Zero();
+  Eigen::Matrix4d sum_cross = Eigen::Matrix4d::Zero();
+  for (size_t i = 0; i < n; i++) {
+    const auto& pt = traits::point(cloud, k_indices[i]);
+    sum_points += pt;
+    sum_cross += pt * pt.transpose();
+  }
+
+  const Eigen::Vector4d mean = sum_points / n;
+  const Eigen::Matrix4d cov = (sum_cross - mean * sum_points.transpose()) / n;
+
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig;
+  eig.computeDirect(cov.block<3, 3>(0, 0));
+
+  Setter::set(cloud, point_index, eig.eigenvectors());
+}
+
+template <typename PointCloud, typename Setter>
+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);
+  }
+}
+
+template <typename PointCloud>
+void estimate_normals(PointCloud& cloud, int num_neighbors = 20) {
+  estimate_local_features<PointCloud, NormalSetter<PointCloud>>(cloud, num_neighbors);
+}
+
+template <typename PointCloud>
+void estimate_covariances(PointCloud& cloud, int num_neighbors = 20) {
+  estimate_local_features<PointCloud, CovarianceSetter<PointCloud>>(cloud, num_neighbors);
+}
+
+template <typename PointCloud>
+void estimate_normals_covariances(PointCloud& cloud, int num_neighbors = 20) {
+  estimate_local_features<PointCloud, NormalCovarianceSetter<PointCloud>>(cloud, num_neighbors);
+}
+
+}  // namespace small_gicp

include/small_gicp/util/normal_estimation_omp.hpp

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <small_gicp/util/normal_estimation.hpp>
+
+namespace small_gicp {
+
+template <typename PointCloud, typename Setter>
+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);
+  }
+}
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+}  // namespace small_gicp

include/small_gicp/util/normal_estimation_tbb.hpp

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <tbb/tbb.h>
+#include <small_gicp/util/normal_estimation.hpp>
+
+namespace small_gicp {
+
+template <typename PointCloud, typename Setter>
+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<PointCloud, UnsafeKdTree<PointCloud>, Setter>(cloud, kdtree, num_neighbors, i);
+    }
+  });
+}
+
+template <typename PointCloud>
+void estimate_normals_tbb(PointCloud& cloud, int num_neighbors = 20) {
+  estimate_local_features_tbb<PointCloud, NormalSetter<PointCloud>>(cloud, num_neighbors);
+}
+
+template <typename PointCloud>
+void estimate_covariances_tbb(PointCloud& cloud, int num_neighbors = 20) {
+  estimate_local_features_tbb<PointCloud, CovarianceSetter<PointCloud>>(cloud, num_neighbors);
+}
+
+template <typename PointCloud>
+void estimate_normals_covariances_tbb(PointCloud& cloud, int num_neighbors = 20) {
+  estimate_local_features_tbb<PointCloud, NormalCovarianceSetter<PointCloud>>(cloud, num_neighbors);
+}
+
+}  // namespace small_gicp

include/small_gicp/util/read_points.hpp

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <fstream>
+#include <Eigen/Core>
+
+namespace small_gicp {
+
+static std::vector<Eigen::Vector4f> read_points(const std::string& filename) {
+  std::ifstream ifs(filename, std::ios::binary | std::ios::ate);
+  if (!ifs) {
+    std::cerr << "error: failed to open " << filename << std::endl;
+    return {};
+  }
+
+  std::streamsize points_bytes = ifs.tellg();
+  size_t num_points = points_bytes / (sizeof(Eigen::Vector4f));
+
+  ifs.seekg(0, std::ios::beg);
+  std::vector<Eigen::Vector4f> points(num_points);
+  ifs.read(reinterpret_cast<char*>(points.data()), sizeof(Eigen::Vector4f) * num_points);
+
+  return points;
+}
+
+}  // namespace small_gicp

include/small_gicp/util/vector3i_hash.hpp

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <Eigen/Core>
+
+namespace small_gicp {
+
+/**
+ * @brief Spatial hashing function
+ *        Teschner et al., "Optimized Spatial Hashing for Collision Detection of Deformable Objects", VMV2003
+ */
+struct XORVector3iHash {
+public:
+  size_t operator()(const Eigen::Vector3i& x) const {
+    const size_t p1 = 73856093;
+    const size_t p2 = 19349669;  // 19349663 was not a prime number
+    const size_t p3 = 83492791;
+    return static_cast<size_t>((x[0] * p1) ^ (x[1] * p2) ^ (x[2] * p3));
+  }
+
+  static size_t hash(const Eigen::Vector3i& x) { return XORVector3iHash()(x); }
+  static bool equal(const Eigen::Vector3i& x1, const Eigen::Vector3i& x2) { return x1 == x2; }
+};
+
+}  // namespace small_gicp

src/small_gicp_test.cpp

@@ -0,0 +1,100 @@
+#include <iostream>
+#include <filesystem>
+
+#include <small_gicp/util/read_points.hpp>
+
+#include <small_gicp/points/pcl_point.hpp>
+#include <small_gicp/points/pcl_point_traits.hpp>
+#include <small_gicp/points/point_cloud.hpp>
+#include <small_gicp/points/gaussian_voxelmap.hpp>
+#include <small_gicp/ann/kdtree.hpp>
+#include <small_gicp/ann/cached_kdtree.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/registration/registration.hpp>
+#include <small_gicp/registration/reduction_omp.hpp>
+#include <small_gicp/registration/reduction_tbb.hpp>
+#include <small_gicp/factors/icp_factor.hpp>
+#include <small_gicp/factors/gicp_factor.hpp>
+#include <small_gicp/factors/plane_icp_factor.hpp>
+
+#include <glk/io/ply_io.hpp>
+#include <glk/pointcloud_buffer_pcl.hpp>
+#include <glk/normal_distributions.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 = 32;
+  tbb::task_scheduler_init init(num_threads);
+
+  const std::string dataset_path = "/home/koide/datasets/kitti/velodyne_filtered";
+  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();
+
+  GaussianVoxelMap::Ptr voxelmap;
+
+  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
+  for (const auto& filename : filenames) {
+    glim::EasyProfiler prof("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");
+    points = voxelgrid_sampling_tbb(*points, 0.1);
+    prof.push("estimate covs");
+    estimate_covariances_tbb(*points, 10);
+    std::cout << raw_points.size() << " => " << points->size() << std::endl;
+
+    if (voxelmap == nullptr) {
+      voxelmap = std::make_shared<GaussianVoxelMap>(1.0);
+      voxelmap->insert(*points, T);
+      continue;
+    }
+
+    //
+    prof.push("create_tbb");
+    Registration<GaussianVoxelMap, PointCloud, GaussianVoxelMap, GICPFactor, NullRejector, ParallelReductionTBB, LevenbergMarquardtOptimizer> registration_tbb;
+    registration_tbb.optimizer.verbose = true;
+    prof.push("registration_tbb");
+    auto result = registration_tbb.align(*voxelmap, *points, *voxelmap, T);
+
+    prof.push("update");
+    T = result.T_target_source;
+    voxelmap->insert(*points, T);
+
+    prof.push("show");
+    viewer->update_points("current", points->points, 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());
+
+    if (!viewer->spin_once()) {
+      break;
+    }
+  }
+
+  return 0;
+}