From 2617e4730b33ab51d9982f2199b7ba6cabe64639 Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Sun, 24 Mar 2024 18:36:31 +0900 Subject: [PATCH] default to knn_search --- CMakeLists.txt | 23 +++++-- include/small_gicp/ann/traits.hpp | 30 +++++--- include/small_gicp/registration/optimizer.hpp | 1 + .../small_gicp/registration/registration.hpp | 3 +- include/small_gicp/util/benchmark.hpp | 1 + src/odometry_benchmark.cpp | 69 +++++++++++++++++++ src/small_gicp/registration/registration.cpp | 3 + 7 files changed, 114 insertions(+), 16 deletions(-) create mode 100644 src/odometry_benchmark.cpp create mode 100644 src/small_gicp/registration/registration.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b8e1c94..1830d80 100644 --- a/CMakeLists.txt +++ b/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} diff --git a/include/small_gicp/ann/traits.hpp b/include/small_gicp/ann/traits.hpp index e8e3ced..e742543 100644 --- a/include/small_gicp/ann/traits.hpp +++ b/include/small_gicp/ann/traits.hpp @@ -9,17 +9,6 @@ namespace traits { template 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 -size_t nearest_neighbor_search(const T& tree, const Eigen::Vector4d& point, size_t* k_index, double* k_sq_dist) { - return Traits::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::knn_search(tree, point, k, k_indices, k_sq_dists); } +template +concept HasNearestNeighborSearch = requires { Traits::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 +size_t nearest_neighbor_search(const T& tree, const Eigen::Vector4d& point, size_t* k_index, double* k_sq_dist) { + return Traits::nearest_neighbor_search(tree, point, k_index, k_sq_dist); +} + +template +size_t nearest_neighbor_search(const T& tree, const Eigen::Vector4d& point, size_t* k_index, double* k_sq_dist) { + return Traits::knn_search(tree, point, 1, k_index, k_sq_dist); +} + } // namespace traits } // namespace small_gicp diff --git a/include/small_gicp/registration/optimizer.hpp b/include/small_gicp/registration/optimizer.hpp index aa29e07..34c5d0b 100644 --- a/include/small_gicp/registration/optimizer.hpp +++ b/include/small_gicp/registration/optimizer.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include diff --git a/include/small_gicp/registration/registration.hpp b/include/small_gicp/registration/registration.hpp index 4cfc78e..078f6e3 100644 --- a/include/small_gicp/registration/registration.hpp +++ b/include/small_gicp/registration/registration.hpp @@ -11,7 +11,7 @@ namespace small_gicp { /// @brief Point cloud registration -template +template 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 RegistrationResult align(const TargetPointCloud& target, const SourcePointCloud& source, const TargetTree& target_tree, const Eigen::Isometry3d& init_T) { std::vector factors(traits::size(source)); return optimizer.optimize(target, source, target_tree, rejector, criteria, reduction, init_T, factors); diff --git a/include/small_gicp/util/benchmark.hpp b/include/small_gicp/util/benchmark.hpp index 67ee0b2..0d5a993 100644 --- a/include/small_gicp/util/benchmark.hpp +++ b/include/small_gicp/util/benchmark.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include diff --git a/src/odometry_benchmark.cpp b/src/odometry_benchmark.cpp new file mode 100644 index 0000000..d6c2c31 --- /dev/null +++ b/src/odometry_benchmark.cpp @@ -0,0 +1,69 @@ +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +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(); + 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>(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>(points); + estimate_normals_covariances_tbb(*points, *tree, 10); + sw.lap(); + preprocess_times.push(sw.msec()); + + Registration 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; +} diff --git a/src/small_gicp/registration/registration.cpp b/src/small_gicp/registration/registration.cpp new file mode 100644 index 0000000..5910a2d --- /dev/null +++ b/src/small_gicp/registration/registration.cpp @@ -0,0 +1,3 @@ +#include + +namespace small_gicp {} \ No newline at end of file