default to knn_search

CMakeLists.txt

@@ -50,17 +50,32 @@ endif()
 #   ${Iridescence_LIBRARIES}
 # )
 
-add_executable(downsampling_test
-  src/downsampling_test.cpp
+add_library(small_gicp SHARED
+  src/small_gicp/registration/registration.cpp
 )
-target_include_directories(downsampling_test PUBLIC
+target_include_directories(small_gicp PUBLIC
+  include
+  ${PCL_INCLUDE_DIRS}
+  ${TBB_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+target_link_libraries(small_gicp
+  ${PCL_LIBRARIES}
+  ${TBB_LIBRARIES}
+)
+
+
+add_executable(odometry_benchmark
+  src/odometry_benchmark.cpp
+)
+target_include_directories(odometry_benchmark PUBLIC
   include
   ${PCL_INCLUDE_DIRS}
   ${TBB_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIR}
   ${Iridescence_INCLUDE_DIRS}
 )
-target_link_libraries(downsampling_test
+target_link_libraries(odometry_benchmark
   fmt::fmt
   ${PCL_LIBRARIES}
   ${TBB_LIBRARIES}

include/small_gicp/ann/traits.hpp

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

include/small_gicp/registration/optimizer.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <iostream>
 #include <small_gicp/util/lie.hpp>
 #include <small_gicp/registration/registration_result.hpp>
 

include/small_gicp/registration/registration.hpp

@@ -11,7 +11,7 @@
 namespace small_gicp {
 
 /// @brief Point cloud registration
-template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename Factor, typename CorrespondenceRejector, typename Reduction, typename Optimizer>
+template <typename Factor, typename CorrespondenceRejector, typename Reduction, typename Optimizer>
 struct Registration {
 public:
   /// @brief Align point clouds
@@ -20,6 +20,7 @@ public:
   /// @param target_tree  Nearest neighbor search for the target point cloud
   /// @param init_T       Initial guess
   /// @return             Registration result
+  template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree>
   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);

include/small_gicp/util/benchmark.hpp

@@ -2,6 +2,7 @@
 
 #include <chrono>
 #include <vector>
+#include <numeric>
 #include <algorithm>
 #include <filesystem>
 #include <Eigen/Core>

src/odometry_benchmark.cpp

@@ -0,0 +1,69 @@
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+#include <small_gicp/ann/kdtree.hpp>
+#include <small_gicp/util/downsampling_tbb.hpp>
+#include <small_gicp/util/normal_estimation_tbb.hpp>
+#include <small_gicp/points/point_cloud.hpp>
+#include <small_gicp/util/benchmark.hpp>
+
+#include <small_gicp/factors/icp_factor.hpp>
+#include <small_gicp/factors/plane_icp_factor.hpp>
+#include <small_gicp/factors/gicp_factor.hpp>
+#include <small_gicp/registration/reduction_tbb.hpp>
+#include <small_gicp/registration/registration.hpp>
+
+#include <guik/viewer/light_viewer.hpp>
+
+int main(int argc, char** argv) {
+  using namespace small_gicp;
+
+  std::cout << "SIMD in use: " << Eigen::SimdInstructionSetsInUse() << std::endl;
+  const std::string dataset_path = "/home/koide/datasets/velodyne";
+
+  std::cout << "Load dataset from " << dataset_path << std::endl;
+  KittiDataset kitti(dataset_path, 1000);
+  std::cout << "num_frames=" << kitti.points.size() << std::endl;
+  std::cout << fmt::format("num_points={} [points]", summarize(kitti.points, [](const auto& pts) { return pts.size(); })) << std::endl;
+
+  auto raw_points = kitti.convert<PointCloud>();
+  kitti.points.clear();
+
+  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
+  auto target = voxelgrid_sampling_tbb(*raw_points.front(), 0.25);
+  estimate_normals_covariances_tbb(*target, 10);
+  auto target_tree = std::make_shared<KdTree<PointCloud>>(target);
+
+  auto viewer = guik::viewer();
+  viewer->disable_vsync();
+
+  Stopwatch sw;
+  Summarizer preprocess_times;
+  Summarizer registration_times;
+
+  for (size_t i = 0; i < raw_points.size() && viewer->spin_once(); i++) {
+    sw.start();
+    auto points = voxelgrid_sampling_tbb(*raw_points[i], 0.25);
+    auto tree = std::make_shared<KdTree<PointCloud>>(points);
+    estimate_normals_covariances_tbb(*points, *tree, 10);
+    sw.lap();
+    preprocess_times.push(sw.msec());
+
+    Registration<GICPFactor, DistanceRejector, ParallelReductionTBB, LevenbergMarquardtOptimizer> registration;
+    auto result = registration.align(*target, *points, *target_tree, Eigen::Isometry3d::Identity());
+    sw.lap();
+    registration_times.push(sw.msec());
+
+    std::cout << "preprocess=" << preprocess_times.str() << "[msec]  registration=" << registration_times.str() << "[msec]" << std::endl;
+
+    T = T * result.T_target_source;
+
+    target = points;
+    target_tree = tree;
+
+    viewer->update_points("current", points->points, guik::FlatOrange(T).set_point_scale(2.0f));
+    viewer->update_points(guik::anon(), voxelgrid_sampling_tbb(*points, 1.0)->points, guik::Rainbow(T));
+  }
+
+  return 0;
+}

src/small_gicp/registration/registration.cpp

@@ -0,0 +1,3 @@
+#include <small_gicp/registration/registration.hpp>
+
+namespace small_gicp {}