diff --git a/CMakeLists.txt b/CMakeLists.txt index e39ab02..e446e6b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,6 +68,15 @@ target_link_libraries(small_gicp add_executable(odometry_benchmark + src/small_gicp/util/benchmark_odom.cpp + src/odometry_benchmark_pcl.cpp + src/odometry_benchmark_fast_gicp.cpp + src/odometry_benchmark_fast_vgicp.cpp + src/odometry_benchmark_small_gicp.cpp + src/odometry_benchmark_small_gicp_omp.cpp + src/odometry_benchmark_small_gicp_tbb.cpp + src/odometry_benchmark_small_vgicp_tbb.cpp + src/odometry_benchmark_small_gicp_tbb_batch.cpp src/odometry_benchmark.cpp ) target_include_directories(odometry_benchmark PUBLIC @@ -100,17 +109,37 @@ target_link_libraries(downsampling_benchmark ${TBB_LIBRARIES} ) -add_executable(sort_test - src/sort_test.cpp -) -target_include_directories(sort_test PUBLIC - include - ${PCL_INCLUDE_DIRS} - ${TBB_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} -) -target_link_libraries(sort_test - fmt::fmt - ${PCL_LIBRARIES} - ${TBB_LIBRARIES} -) \ No newline at end of file + + +########## +## Test ## +########## +option(BUILD_TESTS "Build test" ON) +if(BUILD_TESTS) + include(FetchContent) + FetchContent_Declare(googletest URL https://github.com/google/googletest/archive/03597a01ee50ed33e9dfd640b249b4be3799d395.zip) + FetchContent_MakeAvailable(googletest) + include(GoogleTest) + + enable_testing() + file(GLOB TEST_SOURCES "src/test/*.cpp") + # Generate test target for each test source file + foreach(TEST_SOURCE ${TEST_SOURCES}) + get_filename_component(TEST_NAME ${TEST_SOURCE} NAME_WE) + add_executable(${TEST_NAME} ${TEST_SOURCE}) + target_include_directories(${TEST_NAME} PUBLIC + include + ${PCL_INCLUDE_DIRS} + ${TBB_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ) + target_link_libraries(${TEST_NAME} + fmt::fmt + GTest::gtest_main + ${PCL_LIBRARIES} + ${TBB_LIBRARIES} + ) + + gtest_discover_tests(${TEST_NAME}) + endforeach() +endif() diff --git a/include/small_gicp/ann/flat_voxelmap.hpp b/include/small_gicp/ann/flat_voxelmap.hpp index a0731c8..a4e9683 100644 --- a/include/small_gicp/ann/flat_voxelmap.hpp +++ b/include/small_gicp/ann/flat_voxelmap.hpp @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -26,15 +27,13 @@ struct IndexDistance { double distance; }; +template struct FlatVoxelMap { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - template - FlatVoxelMap(double leaf_size, const PointCloud& points) : inv_leaf_size(1.0 / leaf_size), - seek_count(2), - points(points) { + FlatVoxelMap(double leaf_size, const PointCloud& points) : inv_leaf_size(1.0 / leaf_size), seek_count(2), points(points) { set_offset_pattern(7); create_table(points); } @@ -123,7 +122,6 @@ public: } private: - template void create_table(const PointCloud& points) { const double min_sq_dist_in_cell = 0.05 * 0.05; const int max_points_per_cell = 10; @@ -206,16 +204,16 @@ public: const int seek_count; std::vector offsets; - const PointCloud points; + const PointCloud& points; std::vector voxels; std::vector indices; }; namespace traits { -template <> -struct Traits { - static size_t knn_search(const FlatVoxelMap& voxelmap, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) { +template +struct Traits> { + static size_t knn_search(const FlatVoxelMap& 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); } }; diff --git a/include/small_gicp/ann/gaussian_voxelmap.hpp b/include/small_gicp/ann/gaussian_voxelmap.hpp index 59e8e4f..8205755 100644 --- a/include/small_gicp/ann/gaussian_voxelmap.hpp +++ b/include/small_gicp/ann/gaussian_voxelmap.hpp @@ -1,6 +1,8 @@ #pragma once #include +#include +#include #include #include @@ -79,7 +81,7 @@ public: // Insert points to the voxelmap for (size_t i = 0; i < traits::size(points); i++) { const Eigen::Vector4d pt = T * traits::point(points, i); - const Eigen::Vector3i coord = fast_loor(pt * inv_leaf_size).head<3>(); + const Eigen::Vector3i coord = fast_floor(pt * inv_leaf_size).head<3>(); auto found = voxels.find(coord); if (found == voxels.end()) { diff --git a/include/small_gicp/ann/kdtree.hpp b/include/small_gicp/ann/kdtree.hpp index 70b60d7..52242e3 100644 --- a/include/small_gicp/ann/kdtree.hpp +++ b/include/small_gicp/ann/kdtree.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include diff --git a/include/small_gicp/ann/kdtree_omp.hpp b/include/small_gicp/ann/kdtree_omp.hpp new file mode 100644 index 0000000..b6f6d2a --- /dev/null +++ b/include/small_gicp/ann/kdtree_omp.hpp @@ -0,0 +1,80 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace small_gicp { + +/// @brief Unsafe KdTree with multi-thread tree construction. +/// @note This class only parallelizes the tree construction. The search is still single-threaded as in the normal KdTree. +template +class UnsafeKdTreeOMP { +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using Index = nanoflann::KDTreeSingleIndexAdaptorOMP, double>, UnsafeKdTreeOMP, 3, size_t>; + + /// @brief Constructor + /// @param points Input points + UnsafeKdTreeOMP(const PointCloud& points, int num_threads = 4) : points(points), index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10)) { index.buildIndex(num_threads); } + ~UnsafeKdTreeOMP() {} + + // Interfaces for nanoflann + 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 + bool kdtree_get_bbox(BBox&) const { + return false; + } + + /// @brief Find k-nearest neighbors + 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; ///< Input points + Index index; ///< KdTree index +}; + +/// @brief KdTree +template +class KdTreeOMP { +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + /// @brief Constructor + /// @param points Input points + KdTreeOMP(const std::shared_ptr& points, int num_threads = 4) : points(points), tree(*points, num_threads) {} + ~KdTreeOMP() {} + + /// @brief Find k-nearest neighbors + 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 points; ///< Input points + const UnsafeKdTreeOMP tree; ///< KdTree +}; + +namespace traits { + +template +struct Traits> { + static size_t knn_search(const UnsafeKdTreeOMP& 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 +struct Traits> { + static size_t knn_search(const KdTreeOMP& 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 diff --git a/include/small_gicp/ann/kdtree_mt.hpp b/include/small_gicp/ann/kdtree_tbb.hpp similarity index 58% rename from include/small_gicp/ann/kdtree_mt.hpp rename to include/small_gicp/ann/kdtree_tbb.hpp index d3d3899..56db5dd 100644 --- a/include/small_gicp/ann/kdtree_mt.hpp +++ b/include/small_gicp/ann/kdtree_tbb.hpp @@ -4,23 +4,23 @@ #include #include #include -#include +#include namespace small_gicp { /// @brief Unsafe KdTree with multi-thread tree construction. /// @note This class only parallelizes the tree construction. The search is still single-threaded as in the normal KdTree. template -class UnsafeKdTreeMT { +class UnsafeKdTreeTBB { public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - using Index = nanoflann::KDTreeSingleIndexAdaptorMT, double>, UnsafeKdTreeMT, 3, size_t>; + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using Index = nanoflann::KDTreeSingleIndexAdaptorTBB, double>, UnsafeKdTreeTBB, 3, size_t>; /// @brief Constructor /// @param points Input points - UnsafeKdTreeMT(const PointCloud& points, int num_threads = 4) : points(points), index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10)) { index.buildIndex(num_threads); } - ~UnsafeKdTreeMT() {} + UnsafeKdTreeTBB(const PointCloud& points) : points(points), index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10)) { index.buildIndex(); } + ~UnsafeKdTreeTBB() {} // Interfaces for nanoflann size_t kdtree_get_point_count() const { return traits::size(points); } @@ -41,36 +41,36 @@ private: /// @brief KdTree template -class KdTreeMT { +class KdTreeTBB { public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; /// @brief Constructor /// @param points Input points - KdTreeMT(const std::shared_ptr& points, int num_threads = 4) : points(points), tree(*points, num_threads) {} - ~KdTreeMT() {} + KdTreeTBB(const std::shared_ptr& points) : points(points), tree(*points) {} + ~KdTreeTBB() {} /// @brief Find k-nearest neighbors 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 points; ///< Input points - const UnsafeKdTreeMT tree; ///< KdTree + const UnsafeKdTreeTBB tree; ///< KdTree }; namespace traits { template -struct Traits> { - static size_t knn_search(const UnsafeKdTreeMT& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) { +struct Traits> { + static size_t knn_search(const UnsafeKdTreeTBB& 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 -struct Traits> { - static size_t knn_search(const KdTreeMT& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) { +struct Traits> { + static size_t knn_search(const KdTreeTBB& 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); } }; diff --git a/include/small_gicp/ann/nanoflann_mt.hpp b/include/small_gicp/ann/nanoflann_omp.hpp similarity index 95% rename from include/small_gicp/ann/nanoflann_mt.hpp rename to include/small_gicp/ann/nanoflann_omp.hpp index c086f54..034a4ec 100644 --- a/include/small_gicp/ann/nanoflann_mt.hpp +++ b/include/small_gicp/ann/nanoflann_omp.hpp @@ -31,7 +31,7 @@ *************************************************************************/ /** - * This nanoflann_mt.hpp is derived from nanoflann.hpp to parallelize the tree construction with OpenMP and TBB + * This nanoflann_mt.hpp is derived from nanoflann.hpp to parallelize the tree construction with OpenMP. */ /** \mainpage nanoflann C++ API documentation @@ -48,10 +48,10 @@ * documentation */ -#ifndef NANOFLANN_MT_HPP_ -#define NANOFLANN_MT_HPP_ +#ifndef NANOFLANN_OMP_HPP_ +#define NANOFLANN_OMP_HPP_ -#include +#include #include namespace nanoflann { @@ -69,7 +69,7 @@ namespace nanoflann { */ template -class KDTreeBaseClassMT { +class KDTreeBaseClassOMP { public: /** Frees the previously-built index. Automatically called within * buildIndex(). */ @@ -136,8 +136,8 @@ public: * than allocating memory directly when there is a large * number small of memory allocations. */ - // NOTE: Should remove TBB to minimize the dependencies - tbb::concurrent_vector pool; + std::atomic_uint64_t pool_count; + std::vector pool; /** Returns number of points in dataset */ size_t size(const Derived& obj) const { return obj.m_size; } @@ -166,7 +166,12 @@ public: * @param right index of the last vector */ NodePtr divideTree(Derived& obj, const IndexType left, const IndexType right, BoundingBox& bbox) { - NodePtr node = &(*pool.emplace_back()); + const size_t pool_loc = pool_count++; + if (pool_loc >= pool.size()) { + std::cerr << "error: pool_loc=" << pool_loc << " >= pool.size()=" << pool.size() << std::endl; + abort(); + } + NodePtr node = pool.data() + pool_loc; /* If too few exemplars remain, then make this a leaf node. */ if ((right - left) <= static_cast(obj.m_leaf_max_size)) { @@ -206,6 +211,7 @@ public: } else { // I did my best to check that the following parallelization does not cause race conditions. // But, still not 100% sure if it is correct. + #pragma omp task shared(obj, left_bbox) node->child1 = divideTree(obj, left, left + idx, left_bbox); #pragma omp task shared(obj, right_bbox) @@ -367,10 +373,10 @@ public: * be typically size_t or int */ template -class KDTreeSingleIndexAdaptorMT : public KDTreeBaseClassMT, Distance, DatasetAdaptor, DIM, IndexType> { +class KDTreeSingleIndexAdaptorOMP : public KDTreeBaseClassOMP, Distance, DatasetAdaptor, DIM, IndexType> { public: /** Deleted copy constructor*/ - KDTreeSingleIndexAdaptorMT(const KDTreeSingleIndexAdaptorMT&) = delete; + KDTreeSingleIndexAdaptorOMP(const KDTreeSingleIndexAdaptorOMP&) = delete; /** * The dataset used by this index @@ -381,7 +387,7 @@ public: Distance distance; - typedef typename nanoflann::KDTreeBaseClassMT, Distance, DatasetAdaptor, DIM, IndexType> + typedef typename nanoflann::KDTreeBaseClassOMP, Distance, DatasetAdaptor, DIM, IndexType> BaseClassRef; typedef typename BaseClassRef::ElementType ElementType; @@ -413,7 +419,7 @@ public: * @param inputData Dataset with the input features * @param params Basically, the maximum leaf node size */ - KDTreeSingleIndexAdaptorMT(const int dimensionality, const DatasetAdaptor& inputData, const KDTreeSingleIndexAdaptorParams& params = KDTreeSingleIndexAdaptorParams()) + KDTreeSingleIndexAdaptorOMP(const int dimensionality, const DatasetAdaptor& inputData, const KDTreeSingleIndexAdaptorParams& params = KDTreeSingleIndexAdaptorParams()) : dataset(inputData), index_params(params), distance(inputData) { @@ -440,7 +446,8 @@ public: if (BaseClassRef::m_size == 0) return; computeBoundingBox(BaseClassRef::root_bbox); - BaseClassRef::pool.reserve(BaseClassRef::m_size); + BaseClassRef::pool_count = 0; + BaseClassRef::pool.resize(BaseClassRef::m_size); #pragma omp parallel num_threads(num_threads) { diff --git a/include/small_gicp/ann/nanoflann_tbb.hpp b/include/small_gicp/ann/nanoflann_tbb.hpp new file mode 100644 index 0000000..be51b13 --- /dev/null +++ b/include/small_gicp/ann/nanoflann_tbb.hpp @@ -0,0 +1,639 @@ +/*********************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. + * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. + * Copyright 2011-2021 Jose Luis Blanco (joseluisblancoc@gmail.com). + * All rights reserved. + * + * THE BSD LICENSE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *************************************************************************/ + +/** + * This nanoflann_mt.hpp is derived from nanoflann.hpp to parallelize the tree construction with TBB + */ + +/** \mainpage nanoflann C++ API documentation + * nanoflann is a C++ header-only library for building KD-Trees, mostly + * optimized for 2D or 3D point clouds. + * + * nanoflann does not require compiling or installing, just an + * #include in your code. + * + * See: + * - C++ API organized by modules + * - Online README + * - Doxygen + * documentation + */ + +#ifndef NANOFLANN_TBB_HPP_ +#define NANOFLANN_TBB_HPP_ + +#include +#include + +namespace nanoflann { + +/** kd-tree base-class + * + * Contains the member functions common to the classes KDTreeSingleIndexAdaptor + * and KDTreeSingleIndexDynamicAdaptor_. + * + * \tparam Derived The name of the class which inherits this class. + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use, these are all classes derived + * from nanoflann::Metric \tparam DIM Dimensionality of data points (e.g. 3 for + * 3D points) \tparam IndexType Will be typically size_t or int + */ + +template +class KDTreeBaseClassTBB { +public: + /** Frees the previously-built index. Automatically called within + * buildIndex(). */ + void freeIndex(Derived& obj) { + obj.root_node = NULL; + obj.m_size_at_index_build = 0; + } + + typedef typename Distance::ElementType ElementType; + typedef typename Distance::DistanceType DistanceType; + + /*--------------------- Internal Data Structures --------------------------*/ + struct Node { + /** Union used because a node can be either a LEAF node or a non-leaf node, + * so both data fields are never used simultaneously */ + union { + struct leaf { + IndexType left, right; //!< Indices of points in leaf node + } lr; + struct nonleaf { + int divfeat; //!< Dimension used for subdivision. + DistanceType divlow, divhigh; //!< The values used for subdivision. + } sub; + } node_type; + Node *child1, *child2; //!< Child nodes (both=NULL mean its a leaf node) + }; + + typedef Node* NodePtr; + + struct Interval { + ElementType low, high; + }; + + /** + * Array of indices to vectors in the dataset. + */ + std::vector vind; + + NodePtr root_node; + + size_t m_leaf_max_size; + + size_t m_size; //!< Number of current points in the dataset + size_t m_size_at_index_build; //!< Number of points in the dataset when the + //!< index was built + int dim; //!< Dimensionality of each data point + + /** Define "BoundingBox" as a fixed-size or variable-size container depending + * on "DIM" */ + typedef typename array_or_vector_selector::container_t BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + typedef typename array_or_vector_selector::container_t distance_vector_t; + + /** The KD-tree used to find neighbours */ + + BoundingBox root_bbox; + + /** + * Pooled memory allocator. + * + * Using a pooled memory allocator is more efficient + * than allocating memory directly when there is a large + * number small of memory allocations. + */ + tbb::concurrent_vector pool; + + /** Returns number of points in dataset */ + size_t size(const Derived& obj) const { return obj.m_size; } + + /** Returns the length of each point in the dataset */ + size_t veclen(const Derived& obj) { return static_cast(DIM > 0 ? DIM : obj.dim); } + + /// Helper accessor to the dataset points: + inline ElementType dataset_get(const Derived& obj, size_t idx, int component) const { return obj.dataset.kdtree_get_pt(idx, component); } + + void computeMinMax(const Derived& obj, IndexType* ind, IndexType count, int element, ElementType& min_elem, ElementType& max_elem) { + min_elem = dataset_get(obj, ind[0], element); + max_elem = dataset_get(obj, ind[0], element); + for (IndexType i = 1; i < count; ++i) { + ElementType val = dataset_get(obj, ind[i], element); + if (val < min_elem) min_elem = val; + if (val > max_elem) max_elem = val; + } + } + + /** + * Create a tree node that subdivides the list of vecs from vind[first] + * to vind[last]. The routine is called recursively on each sublist. + * + * @param left index of the first vector + * @param right index of the last vector + */ + NodePtr divideTree(Derived& obj, const IndexType left, const IndexType right, BoundingBox& bbox) { + NodePtr node = &(*pool.emplace_back()); + + /* If too few exemplars remain, then make this a leaf node. */ + if ((right - left) <= static_cast(obj.m_leaf_max_size)) { + node->child1 = node->child2 = NULL; /* Mark as leaf node. */ + node->node_type.lr.left = left; + node->node_type.lr.right = right; + + // compute bounding-box of leaf points + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + bbox[i].low = dataset_get(obj, obj.vind[left], i); + bbox[i].high = dataset_get(obj, obj.vind[left], i); + } + for (IndexType k = left + 1; k < right; ++k) { + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + if (bbox[i].low > dataset_get(obj, obj.vind[k], i)) bbox[i].low = dataset_get(obj, obj.vind[k], i); + if (bbox[i].high < dataset_get(obj, obj.vind[k], i)) bbox[i].high = dataset_get(obj, obj.vind[k], i); + } + } + } else { + IndexType idx; + int cutfeat; + DistanceType cutval; + middleSplit_(obj, &obj.vind[0] + left, right - left, idx, cutfeat, cutval, bbox); + + node->node_type.sub.divfeat = cutfeat; + + BoundingBox left_bbox(bbox); + left_bbox[cutfeat].high = cutval; + + BoundingBox right_bbox(bbox); + right_bbox[cutfeat].low = cutval; + + if ((right - left) <= 512) { + // Do not parallelize if there are less than 512 points + node->child1 = divideTree(obj, left, left + idx, left_bbox); + node->child2 = divideTree(obj, left + idx, right, right_bbox); + } else { + // I did my best to check that the following parallelization does not cause race conditions. + // But, still not 100% sure if it is correct. + + tbb::parallel_invoke([&] { node->child1 = divideTree(obj, left, left + idx, left_bbox); }, [&] { node->child2 = divideTree(obj, left + idx, right, right_bbox); }); + } + + node->node_type.sub.divlow = left_bbox[cutfeat].high; + node->node_type.sub.divhigh = right_bbox[cutfeat].low; + + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low); + bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high); + } + } + + return node; + } + + void middleSplit_(Derived& obj, IndexType* ind, IndexType count, IndexType& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox) { + const DistanceType EPS = static_cast(0.00001); + ElementType max_span = bbox[0].high - bbox[0].low; + for (int i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i) { + ElementType span = bbox[i].high - bbox[i].low; + if (span > max_span) { + max_span = span; + } + } + ElementType max_spread = -1; + cutfeat = 0; + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + ElementType span = bbox[i].high - bbox[i].low; + if (span > (1 - EPS) * max_span) { + ElementType min_elem, max_elem; + computeMinMax(obj, ind, count, i, min_elem, max_elem); + ElementType spread = max_elem - min_elem; + if (spread > max_spread) { + cutfeat = i; + max_spread = spread; + } + } + } + // split in the middle + DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2; + ElementType min_elem, max_elem; + computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem); + + if (split_val < min_elem) + cutval = min_elem; + else if (split_val > max_elem) + cutval = max_elem; + else + cutval = split_val; + + IndexType lim1, lim2; + planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2); + + if (lim1 > count / 2) + index = lim1; + else if (lim2 < count / 2) + index = lim2; + else + index = count / 2; + } + + /** + * Subdivide the list of points by a plane perpendicular on axe corresponding + * to the 'cutfeat' dimension at 'cutval' position. + * + * On return: + * dataset[ind[0..lim1-1]][cutfeat]cutval + */ + void planeSplit(Derived& obj, IndexType* ind, const IndexType count, int cutfeat, DistanceType& cutval, IndexType& lim1, IndexType& lim2) { + /* Move vector indices for left subtree to front of list. */ + IndexType left = 0; + IndexType right = count - 1; + for (;;) { + while (left <= right && dataset_get(obj, ind[left], cutfeat) < cutval) ++left; + while (right && left <= right && dataset_get(obj, ind[right], cutfeat) >= cutval) --right; + if (left > right || !right) break; // "!right" was added to support unsigned Index types + std::swap(ind[left], ind[right]); + ++left; + --right; + } + /* If either list is empty, it means that all remaining features + * are identical. Split in the middle to maintain a balanced tree. + */ + lim1 = left; + right = count - 1; + for (;;) { + while (left <= right && dataset_get(obj, ind[left], cutfeat) <= cutval) ++left; + while (right && left <= right && dataset_get(obj, ind[right], cutfeat) > cutval) --right; + if (left > right || !right) break; // "!right" was added to support unsigned Index types + std::swap(ind[left], ind[right]); + ++left; + --right; + } + lim2 = left; + } + + DistanceType computeInitialDistances(const Derived& obj, const ElementType* vec, distance_vector_t& dists) const { + assert(vec); + DistanceType distsq = DistanceType(); + + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + if (vec[i] < obj.root_bbox[i].low) { + dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i); + distsq += dists[i]; + } + if (vec[i] > obj.root_bbox[i].high) { + dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i); + distsq += dists[i]; + } + } + return distsq; + } +}; + +/** @addtogroup kdtrees_grp KD-tree classes and adaptors + * @{ */ + +/** kd-tree static index + * + * Contains the k-d trees and other information for indexing a set of points + * for nearest-neighbor matching. + * + * The class "DatasetAdaptor" must provide the following interface (can be + * non-virtual, inlined methods): + * + * \code + * // Must return the number of data poins + * inline size_t kdtree_get_point_count() const { ... } + * + * + * // Must return the dim'th component of the idx'th point in the class: + * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } + * + * // Optional bounding-box computation: return false to default to a standard + * bbox computation loop. + * // Return true if the BBOX was already computed by the class and returned + * in "bb" so it can be avoided to redo it again. + * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 + * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const + * { + * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits + * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits + * ... + * return true; + * } + * + * \endcode + * + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM + * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will + * be typically size_t or int + */ +template +class KDTreeSingleIndexAdaptorTBB : public KDTreeBaseClassTBB, Distance, DatasetAdaptor, DIM, IndexType> { +public: + /** Deleted copy constructor*/ + KDTreeSingleIndexAdaptorTBB(const KDTreeSingleIndexAdaptorTBB&) = delete; + + /** + * The dataset used by this index + */ + const DatasetAdaptor& dataset; //!< The source of our data + + const KDTreeSingleIndexAdaptorParams index_params; + + Distance distance; + + typedef typename nanoflann::KDTreeBaseClassTBB, Distance, DatasetAdaptor, DIM, IndexType> + BaseClassRef; + + typedef typename BaseClassRef::ElementType ElementType; + typedef typename BaseClassRef::DistanceType DistanceType; + + typedef typename BaseClassRef::Node Node; + typedef Node* NodePtr; + + typedef typename BaseClassRef::Interval Interval; + /** Define "BoundingBox" as a fixed-size or variable-size container depending + * on "DIM" */ + typedef typename BaseClassRef::BoundingBox BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + typedef typename BaseClassRef::distance_vector_t distance_vector_t; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 + * for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexAdaptorTBB(const int dimensionality, const DatasetAdaptor& inputData, const KDTreeSingleIndexAdaptorParams& params = KDTreeSingleIndexAdaptorParams()) + : dataset(inputData), + index_params(params), + distance(inputData) { + BaseClassRef::root_node = NULL; + BaseClassRef::m_size = dataset.kdtree_get_point_count(); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + BaseClassRef::dim = dimensionality; + if (DIM > 0) BaseClassRef::dim = DIM; + BaseClassRef::m_leaf_max_size = params.leaf_max_size; + + // Create a permutable array of indices to the input vectors. + init_vind(); + } + + /** + * Builds the index + */ + void buildIndex() { + BaseClassRef::m_size = dataset.kdtree_get_point_count(); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + init_vind(); + this->freeIndex(*this); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + if (BaseClassRef::m_size == 0) return; + computeBoundingBox(BaseClassRef::root_bbox); + + BaseClassRef::pool.reserve(BaseClassRef::m_size); + + BaseClassRef::root_node = this->divideTree(*this, 0, BaseClassRef::m_size, + BaseClassRef::root_bbox); // construct the tree + } + + /** \name Query methods + * @{ */ + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * + * Params: + * result = the result object in which the indices of the + * nearest-neighbors are stored vec = the vector for which to search the + * nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + */ + template + bool findNeighbors(RESULTSET& result, const ElementType* vec, const SearchParams& searchParams) const { + assert(vec); + if (this->size(*this) == 0) return false; + if (!BaseClassRef::root_node) throw std::runtime_error("[nanoflann] findNeighbors() called before building the index."); + float epsError = 1 + searchParams.eps; + + distance_vector_t dists; // fixed or variable-sized container (depending on DIM) + auto zero = static_cast(0); + assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), + zero); // Fill it with zeros. + DistanceType distsq = this->computeInitialDistances(*this, vec, dists); + searchLevel( + result, + vec, + BaseClassRef::root_node, + distsq, + dists, + epsError); // "count_leaf" parameter removed since was neither + // used nor returned to the user. + return result.full(); + } + + /** + * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. + * Their indices are stored inside the result object. \sa radiusSearch, + * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility + * with the original FLANN interface. \return Number `N` of valid points in + * the result set. Only the first `N` entries in `out_indices` and + * `out_distances_sq` will be valid. Return may be less than `num_closest` + * only if the number of elements in the tree is less than `num_closest`. + */ + size_t knnSearch(const ElementType* query_point, const size_t num_closest, IndexType* out_indices, DistanceType* out_distances_sq, const int /* nChecks_IGNORED */ = 10) const { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + return resultSet.size(); + } + + /** + * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. + * The output is given as a vector of pairs, of which the first element is a + * point index and the second the corresponding distance. Previous contents of + * \a IndicesDists are cleared. + * + * If searchParams.sorted==true, the output list is sorted by ascending + * distances. + * + * For a better performance, it is advisable to do a .reserve() on the vector + * if you have any wild guess about the number of expected matches. + * + * \sa knnSearch, findNeighbors, radiusSearchCustomCallback + * \return The number of points within the given radius (i.e. indices.size() + * or dists.size() ) + */ + size_t radiusSearch(const ElementType* query_point, const DistanceType& radius, std::vector>& IndicesDists, const SearchParams& searchParams) + const { + RadiusResultSet resultSet(radius, IndicesDists); + const size_t nFound = radiusSearchCustomCallback(query_point, resultSet, searchParams); + if (searchParams.sorted) std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); + return nFound; + } + + /** + * Just like radiusSearch() but with a custom callback class for each point + * found in the radius of the query. See the source of RadiusResultSet<> as a + * start point for your own classes. \sa radiusSearch + */ + template + size_t radiusSearchCustomCallback(const ElementType* query_point, SEARCH_CALLBACK& resultSet, const SearchParams& searchParams = SearchParams()) const { + this->findNeighbors(resultSet, query_point, searchParams); + return resultSet.size(); + } + + /** @} */ + +public: + /** Make sure the auxiliary list \a vind has the same size than the current + * dataset, and re-generate if size has changed. */ + void init_vind() { + // Create a permutable array of indices to the input vectors. + BaseClassRef::m_size = dataset.kdtree_get_point_count(); + if (BaseClassRef::vind.size() != BaseClassRef::m_size) BaseClassRef::vind.resize(BaseClassRef::m_size); + for (size_t i = 0; i < BaseClassRef::m_size; i++) BaseClassRef::vind[i] = i; + } + + void computeBoundingBox(BoundingBox& bbox) { + resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); + if (dataset.kdtree_get_bbox(bbox)) { + // Done! It was implemented in derived class + } else { + const size_t N = dataset.kdtree_get_point_count(); + if (!N) + throw std::runtime_error( + "[nanoflann] computeBoundingBox() called but " + "no data points found."); + for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + bbox[i].low = bbox[i].high = this->dataset_get(*this, 0, i); + } + for (size_t k = 1; k < N; ++k) { + for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + if (this->dataset_get(*this, k, i) < bbox[i].low) bbox[i].low = this->dataset_get(*this, k, i); + if (this->dataset_get(*this, k, i) > bbox[i].high) bbox[i].high = this->dataset_get(*this, k, i); + } + } + } + } + + /** + * Performs an exact search in the tree starting from a node. + * \tparam RESULTSET Should be any ResultSet + * \return true if the search should be continued, false if the results are + * sufficient + */ + template + bool searchLevel(RESULTSET& result_set, const ElementType* vec, const NodePtr node, DistanceType mindistsq, distance_vector_t& dists, const float epsError) const { + /* If this is a leaf node, then do check and return. */ + if ((node->child1 == NULL) && (node->child2 == NULL)) { + // count_leaf += (node->lr.right-node->lr.left); // Removed since was + // neither used nor returned to the user. + DistanceType worst_dist = result_set.worstDist(); + for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; ++i) { + const IndexType index = BaseClassRef::vind[i]; // reorder... : i; + DistanceType dist = distance.evalMetric(vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); + if (dist < worst_dist) { + if (!result_set.addPoint(dist, BaseClassRef::vind[i])) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return false; + } + } + } + return true; + } + + /* Which child branch should be taken first? */ + int idx = node->node_type.sub.divfeat; + ElementType val = vec[idx]; + DistanceType diff1 = val - node->node_type.sub.divlow; + DistanceType diff2 = val - node->node_type.sub.divhigh; + + NodePtr bestChild; + NodePtr otherChild; + DistanceType cut_dist; + if ((diff1 + diff2) < 0) { + bestChild = node->child1; + otherChild = node->child2; + cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); + } else { + bestChild = node->child2; + otherChild = node->child1; + cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); + } + + /* Call recursively to search next level down. */ + if (!searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError)) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return false; + } + + DistanceType dst = dists[idx]; + mindistsq = mindistsq + cut_dist - dst; + dists[idx] = cut_dist; + if (mindistsq * epsError <= result_set.worstDist()) { + if (!searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError)) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return false; + } + } + dists[idx] = dst; + return true; + } +}; + +} // namespace nanoflann + +#endif /* NANOFLANN_HPP_ */ diff --git a/include/small_gicp/ann/traits.hpp b/include/small_gicp/ann/traits.hpp index e742543..9868270 100644 --- a/include/small_gicp/ann/traits.hpp +++ b/include/small_gicp/ann/traits.hpp @@ -22,7 +22,7 @@ size_t knn_search(const T& tree, const Eigen::Vector4d& point, size_t k, size_t* } template -concept HasNearestNeighborSearch = requires { Traits::nearest_neighbor_search(T(), Eigen::Vector4d(), 1, nullptr, nullptr); }; +concept HasNearestNeighborSearch = requires(const T& tree) { Traits::nearest_neighbor_search(tree, Eigen::Vector4d(), nullptr, nullptr); }; /// @brief Find the nearest neighbor /// @param tree Nearest neighbor search (e.g., KdTree) diff --git a/include/small_gicp/util/benchmark.hpp b/include/small_gicp/benchmark/benchmark.hpp similarity index 98% rename from include/small_gicp/util/benchmark.hpp rename to include/small_gicp/benchmark/benchmark.hpp index 19fbffb..6e7229b 100644 --- a/include/small_gicp/util/benchmark.hpp +++ b/include/small_gicp/benchmark/benchmark.hpp @@ -1,7 +1,6 @@ #pragma once #include -#include #include #include #include @@ -9,7 +8,7 @@ #include #include #include -#include +#include namespace small_gicp { diff --git a/include/small_gicp/benchmark/benchmark_odom.hpp b/include/small_gicp/benchmark/benchmark_odom.hpp new file mode 100644 index 0000000..4aafe1d --- /dev/null +++ b/include/small_gicp/benchmark/benchmark_odom.hpp @@ -0,0 +1,82 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace small_gicp { + +struct OdometryEstimationParams { +public: + int num_threads = 4; + double downsample_resolution = 0.25; + double voxel_resolution = 1.0; + double max_correspondence_distance = 1.0; +}; + +class OdometryEstimation { +public: + using Ptr = std::shared_ptr; + + OdometryEstimation(const OdometryEstimationParams& params) : params(params) {} + virtual ~OdometryEstimation() = default; + + virtual std::vector estimate(std::vector& points) = 0; + + virtual void report() {} + +protected: + const OdometryEstimationParams params; +}; + +class OnlineOdometryEstimation : public OdometryEstimation { +public: + OnlineOdometryEstimation(const OdometryEstimationParams& params) : OdometryEstimation(params) {} + ~OnlineOdometryEstimation() { guik::async_destroy(); } + + std::vector estimate(std::vector& points) override { + std::vector traj; + + Stopwatch sw; + sw.start(); + for (size_t i = 0; i < points.size(); i++) { + if (i && i % 256 == 0) { + report(); + } + + auto downsampled = voxelgrid_sampling(*points[i], params.downsample_resolution); + const Eigen::Isometry3d T = estimate(downsampled); + traj.emplace_back(T); + + auto async_viewer = guik::async_viewer(); + async_viewer->update_points("current", downsampled->points, guik::FlatOrange(T).set_point_scale(2.0f)); + async_viewer->update_points(guik::anon(), voxelgrid_sampling(*downsampled, 1.0)->points, guik::Rainbow(T)); + + points[i].reset(); + + sw.lap(); + total_times.push(sw.msec()); + } + + return traj; + } + + virtual Eigen::Isometry3d estimate(const PointCloud::Ptr& points) = 0; + +protected: + Summarizer total_times; +}; + +size_t register_odometry(const std::string& name, std::function factory); + +std::vector odometry_names(); + +OdometryEstimation::Ptr create_odometry(const std::string& name, const OdometryEstimationParams& params); + +} // namespace small_gicp diff --git a/include/small_gicp/util/read_points.hpp b/include/small_gicp/benchmark/read_points.hpp similarity index 100% rename from include/small_gicp/util/read_points.hpp rename to include/small_gicp/benchmark/read_points.hpp diff --git a/include/small_gicp/registration/registration.hpp b/include/small_gicp/registration/registration.hpp index 078f6e3..67c7f54 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 @@ -21,7 +21,8 @@ public: /// @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) { + RegistrationResult + align(const TargetPointCloud& target, const SourcePointCloud& source, const TargetTree& target_tree, const Eigen::Isometry3d& init_T = Eigen::Isometry3d::Identity()) { 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/downsampling.hpp b/include/small_gicp/util/downsampling.hpp index 2bddc78..6539c11 100644 --- a/include/small_gicp/util/downsampling.hpp +++ b/include/small_gicp/util/downsampling.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include diff --git a/include/small_gicp/util/downsampling_omp.hpp b/include/small_gicp/util/downsampling_omp.hpp index 33e1686..4274f7b 100644 --- a/include/small_gicp/util/downsampling_omp.hpp +++ b/include/small_gicp/util/downsampling_omp.hpp @@ -2,7 +2,7 @@ #include #include -#include +#include #include #include diff --git a/include/small_gicp/util/downsampling_tbb.hpp b/include/small_gicp/util/downsampling_tbb.hpp index dcc2e54..8f42dcd 100644 --- a/include/small_gicp/util/downsampling_tbb.hpp +++ b/include/small_gicp/util/downsampling_tbb.hpp @@ -2,7 +2,7 @@ #include #include -#include +#include #include #include diff --git a/include/small_gicp/util/normal_estimation.hpp b/include/small_gicp/util/normal_estimation.hpp index db02cee..58a7017 100644 --- a/include/small_gicp/util/normal_estimation.hpp +++ b/include/small_gicp/util/normal_estimation.hpp @@ -89,7 +89,7 @@ template void estimate_local_features(PointCloud& cloud, int num_neighbors) { traits::resize(cloud, traits::size(cloud)); - KdTree kdtree(cloud); + UnsafeKdTree kdtree(cloud); for (size_t i = 0; i < traits::size(cloud); i++) { estimate_local_features(cloud, kdtree, num_neighbors, i); } diff --git a/src/batch_test.cpp b/src/batch_test.cpp new file mode 100644 index 0000000..d75de3e --- /dev/null +++ b/src/batch_test.cpp @@ -0,0 +1,148 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +int main(int argc, char** argv) { + using namespace small_gicp; + + // if (argc < 3) { + // std::cout << "usage: odometry_benchmark (--engine small|fast|pcl) (--num_threads 4) (--resolution 0.25)" << std::endl; + // return 0; + // } + + const std::string dataset_path = "/home/koide/datasets/kitti/velodyne_filtered"; + const std::string output_path = "/tmp/traj_lidar.txt"; + + int num_threads = 128; + double downsampling_resolution = 0.25; + + /* + for (auto arg = argv; arg != argv + argc; arg++) { + if (std::string(*arg) == "--num_threads") { + num_threads = std::stoi(*(arg + 1)); + } else if (std::string(*arg) == "--resolution") { + downsampling_resolution = std::stod(*(arg + 1)); + } else if (std::string(*arg) == "--engine") { + engine = *(arg + 1); + } + } + */ + + std::cout << "dataset_path=" << dataset_path << std::endl; + std::cout << "output_path=" << output_path << std::endl; + std::cout << "num_threads=" << num_threads << std::endl; + std::cout << "downsampling_resolution=" << downsampling_resolution << std::endl; + + tbb::global_control control(tbb::global_control::max_allowed_parallelism, num_threads); + + KittiDataset kitti(dataset_path); + 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(true); + + auto async_viewer = guik::async_viewer(); + async_viewer->use_orbit_camera_control(250.0); + + struct InputFrame { + using Ptr = std::shared_ptr; + size_t id; + PointCloud::Ptr points; + KdTree::Ptr kdtree; + Eigen::Isometry3d T_last_current; + }; + struct InputFramePair { + InputFrame::Ptr source; + InputFrame::Ptr target; + }; + + tbb::flow::graph graph; + tbb::flow::broadcast_node input(graph); + tbb::flow::function_node preprocess_node(graph, tbb::flow::unlimited, [=](const InputFrame::Ptr& input) { + input->points = voxelgrid_sampling(*input->points, downsampling_resolution); + input->kdtree = std::make_shared>(input->points); + estimate_covariances(*input->points, *input->kdtree, 10); + return input; + }); + tbb::flow::sequencer_node postpre_sequencer_node(graph, [](const InputFrame::Ptr& input) { return input->id; }); + + tbb::flow::function_node pairing_node(graph, 1, [&](const InputFrame::Ptr& input) { + static InputFrame::Ptr last_frame; + InputFramePair pair; + pair.source = input; + pair.target = last_frame; + last_frame = input; + return pair; + }); + + tbb::flow::function_node registration_node(graph, tbb::flow::unlimited, [&](const InputFramePair& pair) { + if (pair.target == nullptr) { + pair.source->T_last_current.setIdentity(); + return pair.source; + } + + Registration registration; + const auto result = registration.align(*pair.target->points, *pair.source->points, *pair.target->kdtree, Eigen::Isometry3d::Identity()); + pair.source->T_last_current = result.T_target_source; + return pair.source; + }); + + tbb::flow::sequencer_node postreg_sequencer_node(graph, [](const InputFrame::Ptr& input) { return input->id; }); + + tbb::flow::function_node viewer_node(graph, 1, [&](const InputFrame::Ptr& input) { + static size_t counter = 0; + static Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); + T = T * input->T_last_current; + + async_viewer->update_points(fmt::format("{}", counter++), input->points->points, guik::Rainbow(T)); + async_viewer->update_points("points", input->points->points, guik::FlatOrange(T).set_point_scale(2.0f)); + }); + + tbb::flow::make_edge(input, preprocess_node); + tbb::flow::make_edge(preprocess_node, postpre_sequencer_node); + tbb::flow::make_edge(postpre_sequencer_node, pairing_node); + tbb::flow::make_edge(pairing_node, registration_node); + tbb::flow::make_edge(registration_node, postreg_sequencer_node); + tbb::flow::make_edge(postreg_sequencer_node, viewer_node); + + std::cout << "Run" << std::endl; + Stopwatch sw; + sw.start(); + for (size_t i = 0; i < raw_points.size(); i++) { + auto frame = InputFrame::Ptr(new InputFrame); + frame->id = i; + frame->points = raw_points[i]; + if (!input.try_put(frame)) { + std::cerr << "failed to input!!" << std::endl; + } + } + + std::cout << "wait_for_all" << std::endl; + graph.wait_for_all(); + + sw.stop(); + std::cout << "Elapsed time: " << sw.msec() << "[ms] " << sw.msec() / raw_points.size() << "[msec/scan]" << std::endl; + + guik::async_wait(); + + return 0; +} diff --git a/src/downsampling_benchmark.cpp b/src/downsampling_benchmark.cpp index 4e474fa..391a71d 100644 --- a/src/downsampling_benchmark.cpp +++ b/src/downsampling_benchmark.cpp @@ -5,13 +5,13 @@ #include #include -#include #include #include #include #include #include #include +#include namespace small_gicp { diff --git a/src/odometry_benchmark.cpp b/src/odometry_benchmark.cpp index 7fa7538..dcae920 100644 --- a/src/odometry_benchmark.cpp +++ b/src/odometry_benchmark.cpp @@ -1,133 +1,31 @@ -#include -#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -namespace small_gicp { - -class OdometryEstimater { -public: - OdometryEstimater() = default; - virtual ~OdometryEstimater() = default; - - virtual Eigen::Isometry3d estimate(const PointCloud::Ptr& points) = 0; - - const Summarizer& registration_times() const { return reg_times; } - -protected: - Summarizer reg_times; -}; - -class FastGICPOdometryEstimater : public OdometryEstimater { -public: - FastGICPOdometryEstimater(int num_threads) : T(Eigen::Isometry3d::Identity()) { - gicp.setCorrespondenceRandomness(10); - gicp.setMaxCorrespondenceDistance(1.0); - gicp.setNumThreads(num_threads); - } - - Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { - auto points_pcl = pcl::make_shared>(); - points_pcl->resize(points->size()); - for (size_t i = 0; i < points->size(); i++) { - points_pcl->at(i).getVector4fMap() = points->point(i).template cast(); - } - - Stopwatch sw; - sw.start(); - - if (!gicp.getInputTarget()) { - gicp.setInputTarget(points_pcl); - return Eigen::Isometry3d::Identity(); - } - - gicp.setInputSource(points_pcl); - pcl::PointCloud aligned; - gicp.align(aligned); - - sw.stop(); - reg_times.push(sw.msec()); - - T = T * Eigen::Isometry3d(gicp.getFinalTransformation().cast()); - gicp.swapSourceAndTarget(); - - return T; - } - -private: - fast_gicp::FastGICP gicp; - Eigen::Isometry3d T; -}; - -class SmallGICPOdometryEstimater : public OdometryEstimater { -public: - SmallGICPOdometryEstimater(int num_threads) : num_threads(num_threads), T(Eigen::Isometry3d::Identity()) {} - - Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { - Stopwatch sw; - sw.start(); - - auto tree = std::make_shared>(points); - estimate_covariances_tbb(*points, *tree, 10); - - if (!target_points) { - target_points = points; - target_tree = tree; - return Eigen::Isometry3d::Identity(); - } - - Registration registration; - const auto result = registration.align(*target_points, *points, *target_tree, Eigen::Isometry3d::Identity()); - - sw.stop(); - reg_times.push(sw.msec()); - - T = T * result.T_target_source; - target_points = points; - target_tree = tree; - - return T; - } - -private: - int num_threads; - PointCloud::ConstPtr target_points; - KdTreeMT::Ptr target_tree; - - Eigen::Isometry3d T; -}; - -} // namespace small_gicp +#include +#include int main(int argc, char** argv) { using namespace small_gicp; if (argc < 3) { - std::cout << "usage: odometry_benchmark (--engine small|fast) (--num_threads 4) (--resolution 0.25)" << std::endl; + std::cout << "USAGE: odometry_benchmark [options]" << std::endl; + std::cout << "OPTIONS:" << std::endl; + std::cout << " --num_threads (default: 4)" << std::endl; + std::cout << " --resolution (default: 0.25)" << std::endl; + + const auto odom_names = odometry_names(); + std::stringstream sst; + for (size_t i = 0; i < odom_names.size(); i++) { + if (i) { + sst << "|"; + } + sst << odom_names[i]; + } + + std::cout << " --engine <" << sst.str() << "> (default: small_gicp)" << std::endl; return 0; } @@ -136,7 +34,7 @@ int main(int argc, char** argv) { int num_threads = 4; double downsampling_resolution = 0.25; - std::string engine = "small"; + std::string engine = "small_gicp"; for (auto arg = argv + 3; arg != argv + argc; arg++) { if (std::string(*arg) == "--num_threads") { @@ -154,15 +52,9 @@ int main(int argc, char** argv) { std::cout << "num_threads=" << num_threads << std::endl; std::cout << "downsampling_resolution=" << downsampling_resolution << std::endl; - tbb::global_control control(tbb::global_control::max_allowed_parallelism, num_threads); - - std::shared_ptr odom; - if (engine == "small") { - odom = std::make_shared(num_threads); - } else if (engine == "fast") { - odom = std::make_shared(num_threads); - } else { - std::cerr << "Unknown engine: " << engine << std::endl; + OdometryEstimationParams params{num_threads, downsampling_resolution}; + std::shared_ptr odom = create_odometry(engine, params); + if (odom == nullptr) { return 1; } @@ -171,19 +63,10 @@ int main(int argc, char** argv) { std::cout << fmt::format("num_points={} [points]", summarize(kitti.points, [](const auto& pts) { return pts.size(); })) << std::endl; auto raw_points = kitti.convert(true); + std::vector traj = odom->estimate(raw_points); - std::vector traj; - for (size_t i = 0; i < raw_points.size(); i++) { - auto downsampled = voxelgrid_sampling(*raw_points[i], downsampling_resolution); - const Eigen::Isometry3d T = odom->estimate(downsampled); - - if (i && i % 256 == 0) { - std::cout << fmt::format("{}/{} : {}", i, raw_points.size(), odom->registration_times().str()) << std::endl; - } - traj.emplace_back(T); - } - - std::cout << "registration_time_stats=" << odom->registration_times().str() << std::endl; + std::cout << "done!" << std::endl; + odom->report(); std::ofstream ofs(output_path); for (const auto& T : traj) { diff --git a/src/odometry_benchmark_fast_gicp.cpp b/src/odometry_benchmark_fast_gicp.cpp new file mode 100644 index 0000000..0e5602e --- /dev/null +++ b/src/odometry_benchmark_fast_gicp.cpp @@ -0,0 +1,62 @@ +#include + +#include +#include +#include + +#include +#include +#include + +namespace small_gicp { + +class FastGICPOdometryEstimation : public OnlineOdometryEstimation { +public: + FastGICPOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) { + gicp.setCorrespondenceRandomness(10); + gicp.setMaxCorrespondenceDistance(params.max_correspondence_distance); + gicp.setNumThreads(params.num_threads); + } + + Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { + auto points_pcl = pcl::make_shared>(); + points_pcl->resize(points->size()); + for (size_t i = 0; i < points->size(); i++) { + points_pcl->at(i).getVector4fMap() = points->point(i).template cast(); + } + + Stopwatch sw; + sw.start(); + + if (!gicp.getInputTarget()) { + gicp.setInputTarget(points_pcl); + return T; + } + + gicp.setInputSource(points_pcl); + pcl::PointCloud aligned; + gicp.align(aligned); + + sw.stop(); + reg_times.push(sw.msec()); + + T = T * Eigen::Isometry3d(gicp.getFinalTransformation().cast()); + gicp.swapSourceAndTarget(); + + return T; + } + + void report() override { // + std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; + } + +private: + Summarizer reg_times; + + fast_gicp::FastGICP gicp; + Eigen::Isometry3d T; +}; + +static auto fast_gicp_registry = register_odometry("fast_gicp", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); + +} // namespace small_gicp \ No newline at end of file diff --git a/src/odometry_benchmark_fast_vgicp.cpp b/src/odometry_benchmark_fast_vgicp.cpp new file mode 100644 index 0000000..f28f377 --- /dev/null +++ b/src/odometry_benchmark_fast_vgicp.cpp @@ -0,0 +1,64 @@ +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace small_gicp { + +class FastVGICPOdometryEstimation : public OnlineOdometryEstimation { +public: + FastVGICPOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) { + vgicp.setCorrespondenceRandomness(10); + vgicp.setResolution(params.voxel_resolution); + vgicp.setNumThreads(params.num_threads); + } + + Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { + auto points_pcl = pcl::make_shared>(); + points_pcl->resize(points->size()); + for (size_t i = 0; i < points->size(); i++) { + points_pcl->at(i).getVector4fMap() = points->point(i).template cast(); + } + + Stopwatch sw; + sw.start(); + + if (!vgicp.getInputTarget()) { + vgicp.setInputTarget(points_pcl); + return T; + } + + vgicp.setInputSource(points_pcl); + pcl::PointCloud aligned; + vgicp.align(aligned); + + sw.stop(); + reg_times.push(sw.msec()); + + T = T * Eigen::Isometry3d(vgicp.getFinalTransformation().cast()); + vgicp.swapSourceAndTarget(); + + return T; + } + + void report() override { // + std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; + } + +private: + Summarizer reg_times; + + fast_gicp::FastVGICP vgicp; + Eigen::Isometry3d T; +}; + +static auto fast_vgicp_registry = register_odometry("fast_vgicp", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); + +} // namespace small_gicp \ No newline at end of file diff --git a/src/odometry_benchmark_pcl.cpp b/src/odometry_benchmark_pcl.cpp new file mode 100644 index 0000000..15c7bca --- /dev/null +++ b/src/odometry_benchmark_pcl.cpp @@ -0,0 +1,59 @@ +#include + +#include +#include +#include + +namespace small_gicp { + +class PCLOnlineOdometryEstimation : public OnlineOdometryEstimation { +public: + PCLOnlineOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) { + gicp.setCorrespondenceRandomness(10); + gicp.setMaxCorrespondenceDistance(params.max_correspondence_distance); + } + + Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { + auto points_pcl = pcl::make_shared>(); + points_pcl->resize(points->size()); + for (size_t i = 0; i < points->size(); i++) { + points_pcl->at(i).getVector4fMap() = points->point(i).template cast(); + } + + Stopwatch sw; + sw.start(); + + if (!target_points) { + target_points = points_pcl; + return Eigen::Isometry3d::Identity(); + } + + gicp.setInputTarget(target_points); + gicp.setInputSource(points_pcl); + pcl::PointCloud aligned; + gicp.align(aligned); + + sw.stop(); + reg_times.push(sw.msec()); + + T = T * Eigen::Isometry3d(gicp.getFinalTransformation().cast()); + target_points = points_pcl; + + return T; + } + + void report() override { // + std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; + } + +private: + Summarizer reg_times; + + pcl::GeneralizedIterativeClosestPoint gicp; + pcl::PointCloud::Ptr target_points; + Eigen::Isometry3d T; +}; + +static auto pcl_odom_registry = register_odometry("pcl", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); + +} \ No newline at end of file diff --git a/src/odometry_benchmark_small_gicp.cpp b/src/odometry_benchmark_small_gicp.cpp new file mode 100644 index 0000000..c960b5a --- /dev/null +++ b/src/odometry_benchmark_small_gicp.cpp @@ -0,0 +1,60 @@ +#include + +#include +#include +#include +#include +#include +#include + +namespace small_gicp { + +class SmallGICPOnlineOdometryEstimation : public OnlineOdometryEstimation { +public: + SmallGICPOnlineOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) {} + + Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { + Stopwatch sw; + sw.start(); + + auto tree = std::make_shared>(points); + estimate_covariances(*points, *tree, 10); + + if (target_points == nullptr) { + target_points = points; + target_tree = tree; + return T; + } + + Registration registration; + registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance; + + auto result = registration.align(*target_points, *points, *target_tree, Eigen::Isometry3d::Identity()); + + sw.stop(); + reg_times.push(sw.msec()); + + T = T * result.T_target_source; + target_points = points; + target_tree = tree; + + return T; + } + + void report() override { // + std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; + } + +private: + Summarizer reg_times; + + PointCloud::Ptr target_points; + KdTree::Ptr target_tree; + + Eigen::Isometry3d T; +}; + +static auto small_gicp_tbb_registry = + register_odometry("small_gicp", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); + +} // namespace small_gicp \ No newline at end of file diff --git a/src/odometry_benchmark_small_gicp_omp.cpp b/src/odometry_benchmark_small_gicp_omp.cpp new file mode 100644 index 0000000..972202d --- /dev/null +++ b/src/odometry_benchmark_small_gicp_omp.cpp @@ -0,0 +1,61 @@ +#include + +#include +#include +#include +#include +#include +#include + +namespace small_gicp { + +class SmallGICPOnlineOdometryEstimationOMP : public OnlineOdometryEstimation { +public: + SmallGICPOnlineOdometryEstimationOMP(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) {} + + Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { + Stopwatch sw; + sw.start(); + + auto tree = std::make_shared>(points, params.num_threads); + estimate_covariances_omp(*points, *tree, 10, params.num_threads); + + if (target_points == nullptr) { + target_points = points; + target_tree = tree; + return T; + } + + Registration registration; + registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance; + registration.reduction.num_threads = params.num_threads; + + auto result = registration.align(*target_points, *points, *target_tree, Eigen::Isometry3d::Identity()); + + sw.stop(); + reg_times.push(sw.msec()); + + T = T * result.T_target_source; + target_points = points; + target_tree = tree; + + return T; + } + + void report() override { // + std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; + } + +private: + Summarizer reg_times; + + PointCloud::Ptr target_points; + KdTree::Ptr target_tree; + + Eigen::Isometry3d T; +}; + +static auto small_gicp_omp_registry = + register_odometry("small_gicp_omp", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); + +} // namespace small_gicp \ No newline at end of file diff --git a/src/odometry_benchmark_small_gicp_tbb.cpp b/src/odometry_benchmark_small_gicp_tbb.cpp new file mode 100644 index 0000000..247c37a --- /dev/null +++ b/src/odometry_benchmark_small_gicp_tbb.cpp @@ -0,0 +1,66 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace small_gicp { + +class SmallGICPOnlineOdometryEstimationTBB : public OnlineOdometryEstimation { +public: + SmallGICPOnlineOdometryEstimationTBB(const OdometryEstimationParams& params) + : OnlineOdometryEstimation(params), + control(tbb::global_control::max_allowed_parallelism, params.num_threads), + T(Eigen::Isometry3d::Identity()) {} + + Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { + Stopwatch sw; + sw.start(); + + auto tree = std::make_shared>(points); + estimate_covariances_tbb(*points, *tree, 10); + + if (target_points == nullptr) { + target_points = points; + target_tree = tree; + return T; + } + + Registration registration; + registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance; + + auto result = registration.align(*target_points, *points, *target_tree, Eigen::Isometry3d::Identity()); + + sw.stop(); + reg_times.push(sw.msec()); + + T = T * result.T_target_source; + target_points = points; + target_tree = tree; + + return T; + } + + void report() override { // + std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; + } + +private: + tbb::global_control control; + + Summarizer reg_times; + + PointCloud::Ptr target_points; + KdTreeTBB::Ptr target_tree; + + Eigen::Isometry3d T; +}; + +static auto small_gicp_tbb_registry = + register_odometry("small_gicp_tbb", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); + +} // namespace small_gicp \ No newline at end of file diff --git a/src/odometry_benchmark_small_gicp_tbb_batch.cpp b/src/odometry_benchmark_small_gicp_tbb_batch.cpp new file mode 100644 index 0000000..945569a --- /dev/null +++ b/src/odometry_benchmark_small_gicp_tbb_batch.cpp @@ -0,0 +1,132 @@ +#include + +#include +#include +#include +#include +#include +#include + +namespace small_gicp { + +class BatchedSmallGICPOnlineOdometryEstimationTBB : public OdometryEstimation { +public: + struct InputFrame { + using Ptr = std::shared_ptr; + size_t id; + PointCloud::Ptr points; + KdTree::Ptr kdtree; + Eigen::Isometry3d T_last_current; + Stopwatch sw; + }; + + struct InputFramePair { + InputFrame::Ptr target; + InputFrame::Ptr source; + }; + + BatchedSmallGICPOnlineOdometryEstimationTBB(const OdometryEstimationParams& params) + : OdometryEstimation(params), + control(tbb::global_control::max_allowed_parallelism, params.num_threads), + throughput(0.0) {} + + ~BatchedSmallGICPOnlineOdometryEstimationTBB() { guik::async_destroy(); } + + std::vector estimate(std::vector& points) override { + auto async_viewer = guik::async_viewer(); + + std::vector traj; + traj.reserve(points.size()); + + tbb::flow::graph graph; + tbb::flow::broadcast_node input_node(graph); + tbb::flow::function_node preprocess_node(graph, tbb::flow::unlimited, [&](const InputFrame::Ptr& input) { + input->sw.start(); + input->points = voxelgrid_sampling(*input->points, params.downsample_resolution); + input->kdtree = std::make_shared>(input->points); + estimate_covariances(*input->points, *input->kdtree, 10); + return input; + }); + tbb::flow::sequencer_node postpre_sequencer_node(graph, [](const InputFrame::Ptr& input) { return input->id; }); + tbb::flow::function_node pairing_node(graph, 1, [&](const InputFrame::Ptr& input) { + static InputFrame::Ptr last_frame; + InputFramePair pair = {last_frame, input}; + last_frame = input; + return pair; + }); + tbb::flow::function_node registration_node(graph, tbb::flow::unlimited, [&](const InputFramePair& pair) { + if (pair.target == nullptr) { + pair.source->T_last_current.setIdentity(); + return pair.source; + } + + Registration registration; + registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance; + const auto result = registration.align(*pair.target->points, *pair.source->points, *pair.target->kdtree, Eigen::Isometry3d::Identity()); + pair.source->T_last_current = result.T_target_source; + return pair.source; + }); + tbb::flow::sequencer_node postreg_sequencer_node(graph, [](const InputFrame::Ptr& input) { return input->id; }); + tbb::flow::function_node viewer_node(graph, 1, [&](const InputFrame::Ptr& input) { + if (traj.empty()) { + traj.emplace_back(input->T_last_current); + } else { + traj.emplace_back(traj.back() * input->T_last_current); + } + const Eigen::Isometry3d T = traj.back(); + + input->sw.stop(); + reg_times.push(input->sw.msec()); + + static auto t0 = input->sw.t1; + const double elapsed_msec = std::chrono::duration_cast(input->sw.t2 - t0).count() / 1e6; + throughput = elapsed_msec / (input->id + 1); + + if (input->id && input->id % 1024 == 0) { + report(); + } + + async_viewer->update_points(guik::anon(), input->points->points, guik::Rainbow(T)); + async_viewer->update_points("points", input->points->points, guik::FlatOrange(T).set_point_scale(2.0f)); + }); + + tbb::flow::make_edge(input_node, preprocess_node); + tbb::flow::make_edge(preprocess_node, postpre_sequencer_node); + tbb::flow::make_edge(postpre_sequencer_node, pairing_node); + tbb::flow::make_edge(pairing_node, registration_node); + tbb::flow::make_edge(registration_node, postreg_sequencer_node); + tbb::flow::make_edge(postreg_sequencer_node, viewer_node); + + Stopwatch sw; + sw.start(); + for (size_t i = 0; i < points.size(); i++) { + auto frame = InputFrame::Ptr(new InputFrame); + frame->id = i; + frame->points = points[i]; + if (!input_node.try_put(frame)) { + std::cerr << "failed to input!!" << std::endl; + } + } + graph.wait_for_all(); + + sw.stop(); + throughput = sw.msec() / points.size(); + + return traj; + } + + void report() override { // + std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << throughput << " [msec/scan]" << std::endl; + } + +private: + tbb::global_control control; + + double throughput; + Summarizer reg_times; +}; + +static auto small_gicp_tbb_batch_registry = + register_odometry("small_gicp_tbb_batch", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); + +} // namespace small_gicp \ No newline at end of file diff --git a/src/odometry_benchmark_small_vgicp_tbb.cpp b/src/odometry_benchmark_small_vgicp_tbb.cpp new file mode 100644 index 0000000..6f10b72 --- /dev/null +++ b/src/odometry_benchmark_small_vgicp_tbb.cpp @@ -0,0 +1,69 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace small_gicp { + +class SmallVGICPOnlineOdometryEstimationTBB : public OnlineOdometryEstimation { +public: + SmallVGICPOnlineOdometryEstimationTBB(const OdometryEstimationParams& params) + : OnlineOdometryEstimation(params), + control(tbb::global_control::max_allowed_parallelism, params.num_threads), + T(Eigen::Isometry3d::Identity()) {} + + Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { + Stopwatch sw; + sw.start(); + + estimate_covariances_tbb(*points, 10); + + auto voxelmap = std::make_shared(params.voxel_resolution); + voxelmap->insert(*points); + + if (target_points == nullptr) { + target_points = points; + target_voxelmap = voxelmap; + return T; + } + + Registration registration; + registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance; + + auto result = registration.align(*target_voxelmap, *points, *target_voxelmap, Eigen::Isometry3d::Identity()); + + sw.stop(); + reg_times.push(sw.msec()); + + T = T * result.T_target_source; + + target_points = points; + target_voxelmap = voxelmap; + + return T; + } + + void report() override { // + std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; + } + +private: + tbb::global_control control; + + Summarizer reg_times; + + PointCloud::Ptr target_points; + GaussianVoxelMap::Ptr target_voxelmap; + + Eigen::Isometry3d T; +}; + +static auto small_gicp_tbb_registry = + register_odometry("small_vgicp_tbb", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); + +} // namespace small_gicp \ No newline at end of file diff --git a/src/small_gicp/util/benchmark_odom.cpp b/src/small_gicp/util/benchmark_odom.cpp new file mode 100644 index 0000000..8db4e1a --- /dev/null +++ b/src/small_gicp/util/benchmark_odom.cpp @@ -0,0 +1,27 @@ +#include + +namespace small_gicp { + +std::vector>> odometry_registry; + +size_t register_odometry(const std::string& name, std::function factory) { + odometry_registry.emplace_back(name, factory); + return odometry_registry.size() - 1; +} + +std::vector odometry_names() { + std::vector names(odometry_registry.size()); + std::ranges::transform(odometry_registry.begin(), odometry_registry.end(), names.begin(), [](const auto& p) { return p.first; }); + return names; +} + +OdometryEstimation::Ptr create_odometry(const std::string& name, const OdometryEstimationParams& params) { + auto found = std::find_if(odometry_registry.begin(), odometry_registry.end(), [&](const auto& p) { return p.first == name; }); + if (found == odometry_registry.end()) { + std::cerr << "error: unknown odometry engine: " << name << std::endl; + return nullptr; + } + return found->second(params); +} + +} // namespace small_gicp diff --git a/src/test/downsampling_test.cpp b/src/test/downsampling_test.cpp new file mode 100644 index 0000000..4193013 --- /dev/null +++ b/src/test/downsampling_test.cpp @@ -0,0 +1,23 @@ +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace small_gicp; + +TEST(DownsamplingTest, NullTest) { + auto points = std::make_shared(); + EXPECT_EQ(small_gicp::voxelgrid_sampling(*points, 0.1)->size(), 0); + EXPECT_EQ(small_gicp::voxelgrid_sampling_omp(*points, 0.1)->size(), 0); + EXPECT_EQ(small_gicp::voxelgrid_sampling_tbb(*points, 0.1)->size(), 0); + + auto points_pcl = std::make_shared>(); + EXPECT_EQ(small_gicp::voxelgrid_sampling(*points_pcl, 0.1)->size(), 0); + EXPECT_EQ(small_gicp::voxelgrid_sampling_omp(*points_pcl, 0.1)->size(), 0); + EXPECT_EQ(small_gicp::voxelgrid_sampling_tbb(*points_pcl, 0.1)->size(), 0); +} diff --git a/src/test/sort_omp_test.cpp b/src/test/sort_omp_test.cpp new file mode 100644 index 0000000..4712eb1 --- /dev/null +++ b/src/test/sort_omp_test.cpp @@ -0,0 +1,61 @@ +#include +#include +#include +#include + +#include + +using namespace small_gicp; + +bool identical(const std::vector& arr1, const std::vector& arr2) { + if (arr1.size() != arr2.size()) { + return false; + } + + for (size_t i = 0; i < arr1.size(); i++) { + if (arr1[i] != arr2[i]) { + return false; + } + } + return true; +} + +TEST(SortOMP, MergeSortTest) { + std::mt19937 mt; + + std::uniform_int_distribution<> data_dist(-100, 100); + std::uniform_int_distribution<> size_dist(0, 1024); + + for (int i = 0; i < 100; i++) { + std::vector data(size_dist(mt)); + std::ranges::generate(data, [&] { return data_dist(mt); }); + + std::vector sorted = data; + std::ranges::sort(sorted); + + std::vector sorted_omp = data; + merge_sort_omp(sorted_omp.begin(), sorted_omp.end(), std::less(), 4); + + EXPECT_TRUE(identical(sorted, sorted_omp)) << fmt::format("i={} N={}", i, data.size()); + } +} + +TEST(SortOMP, QuickSortTest) { + std::mt19937 mt; + + std::uniform_int_distribution<> data_dist(-100, 100); + std::uniform_int_distribution<> size_dist(0, 1024); + + for (int i = 0; i < 100; i++) { + std::vector data(size_dist(mt)); + std::ranges::generate(data, [&] { return data_dist(mt); }); + + std::vector sorted = data; + std::ranges::sort(sorted); + + std::vector sorted_omp = data; + quick_sort_omp(sorted_omp.begin(), sorted_omp.end(), std::less(), 4); + + EXPECT_TRUE(identical(sorted, sorted_omp)) << fmt::format("i={} N={}", i, data.size()); + } +}