mirror of https://github.com/koide3/small_gicp.git
kdtree_omp
This commit is contained in:
k.koide
parent
eb709de524
commit
6ee032a5a8
|
|
@ -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}
|
||||
)
|
||||
|
||||
|
||||
##########
|
||||
## 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()
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@
|
|||
#include <tbb/tbb.h>
|
||||
|
||||
#include <memory>
|
||||
#include <iostream>
|
||||
#include <small_gicp/ann/traits.hpp>
|
||||
#include <small_gicp/points/traits.hpp>
|
||||
#include <small_gicp/util/fast_floor.hpp>
|
||||
|
|
@ -26,15 +27,13 @@ struct IndexDistance {
|
|||
double distance;
|
||||
};
|
||||
|
||||
template <typename PointCloud>
|
||||
struct FlatVoxelMap {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<FlatVoxelMap>;
|
||||
using ConstPtr = std::shared_ptr<const FlatVoxelMap>;
|
||||
|
||||
template <typename PointCloud>
|
||||
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 <typename PointCloud>
|
||||
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<Eigen::Vector3i> offsets;
|
||||
|
||||
const PointCloud points;
|
||||
const PointCloud& points;
|
||||
std::vector<FlatVoxelInfo> voxels;
|
||||
std::vector<size_t> indices;
|
||||
};
|
||||
|
||||
namespace traits {
|
||||
|
||||
template <>
|
||||
struct Traits<FlatVoxelMap> {
|
||||
static size_t knn_search(const FlatVoxelMap& voxelmap, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) {
|
||||
template <typename PointCloud>
|
||||
struct Traits<FlatVoxelMap<PointCloud>> {
|
||||
static size_t knn_search(const FlatVoxelMap<PointCloud>& 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);
|
||||
}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -1,6 +1,8 @@
|
|||
#pragma once
|
||||
|
||||
#include <unordered_map>
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include <small_gicp/ann/traits.hpp>
|
||||
#include <small_gicp/points/traits.hpp>
|
||||
|
|
@ -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()) {
|
||||
|
|
|
|||
|
|
@ -1,5 +1,6 @@
|
|||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <Eigen/Core>
|
||||
#include <small_gicp/points/traits.hpp>
|
||||
#include <small_gicp/ann/traits.hpp>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,80 @@
|
|||
#pragma once
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <small_gicp/points/traits.hpp>
|
||||
#include <small_gicp/ann/traits.hpp>
|
||||
#include <small_gicp/ann/nanoflann.hpp>
|
||||
#include <small_gicp/ann/nanoflann_omp.hpp>
|
||||
|
||||
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 <typename PointCloud>
|
||||
class UnsafeKdTreeOMP {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<UnsafeKdTreeOMP>;
|
||||
using ConstPtr = std::shared_ptr<const UnsafeKdTreeOMP>;
|
||||
using Index = nanoflann::KDTreeSingleIndexAdaptorOMP<nanoflann::L2_Simple_Adaptor<double, UnsafeKdTreeOMP<PointCloud>, double>, UnsafeKdTreeOMP<PointCloud>, 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 <class BBox>
|
||||
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 <typename PointCloud>
|
||||
class KdTreeOMP {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<KdTreeOMP>;
|
||||
using ConstPtr = std::shared_ptr<const KdTreeOMP>;
|
||||
|
||||
/// @brief Constructor
|
||||
/// @param points Input points
|
||||
KdTreeOMP(const std::shared_ptr<const PointCloud>& 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<const PointCloud> points; ///< Input points
|
||||
const UnsafeKdTreeOMP<PointCloud> tree; ///< KdTree
|
||||
};
|
||||
|
||||
namespace traits {
|
||||
|
||||
template <typename PointCloud>
|
||||
struct Traits<UnsafeKdTreeOMP<PointCloud>> {
|
||||
static size_t knn_search(const UnsafeKdTreeOMP<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<KdTreeOMP<PointCloud>> {
|
||||
static size_t knn_search(const KdTreeOMP<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
|
||||
|
|
@ -4,23 +4,23 @@
|
|||
#include <small_gicp/points/traits.hpp>
|
||||
#include <small_gicp/ann/traits.hpp>
|
||||
#include <small_gicp/ann/nanoflann.hpp>
|
||||
#include <small_gicp/ann/nanoflann_mt.hpp>
|
||||
#include <small_gicp/ann/nanoflann_tbb.hpp>
|
||||
|
||||
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 <typename PointCloud>
|
||||
class UnsafeKdTreeMT {
|
||||
class UnsafeKdTreeTBB {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<UnsafeKdTreeMT>;
|
||||
using ConstPtr = std::shared_ptr<const UnsafeKdTreeMT>;
|
||||
using Index = nanoflann::KDTreeSingleIndexAdaptorMT<nanoflann::L2_Simple_Adaptor<double, UnsafeKdTreeMT<PointCloud>, double>, UnsafeKdTreeMT<PointCloud>, 3, size_t>;
|
||||
using Ptr = std::shared_ptr<UnsafeKdTreeTBB>;
|
||||
using ConstPtr = std::shared_ptr<const UnsafeKdTreeTBB>;
|
||||
using Index = nanoflann::KDTreeSingleIndexAdaptorTBB<nanoflann::L2_Simple_Adaptor<double, UnsafeKdTreeTBB<PointCloud>, double>, UnsafeKdTreeTBB<PointCloud>, 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 <typename PointCloud>
|
||||
class KdTreeMT {
|
||||
class KdTreeTBB {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<KdTreeMT>;
|
||||
using ConstPtr = std::shared_ptr<const KdTreeMT>;
|
||||
using Ptr = std::shared_ptr<KdTreeTBB>;
|
||||
using ConstPtr = std::shared_ptr<const KdTreeTBB>;
|
||||
|
||||
/// @brief Constructor
|
||||
/// @param points Input points
|
||||
KdTreeMT(const std::shared_ptr<const PointCloud>& points, int num_threads = 4) : points(points), tree(*points, num_threads) {}
|
||||
~KdTreeMT() {}
|
||||
KdTreeTBB(const std::shared_ptr<const PointCloud>& 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<const PointCloud> points; ///< Input points
|
||||
const UnsafeKdTreeMT<PointCloud> tree; ///< KdTree
|
||||
const UnsafeKdTreeTBB<PointCloud> tree; ///< KdTree
|
||||
};
|
||||
|
||||
namespace traits {
|
||||
|
||||
template <typename PointCloud>
|
||||
struct Traits<UnsafeKdTreeMT<PointCloud>> {
|
||||
static size_t knn_search(const UnsafeKdTreeMT<PointCloud>& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) {
|
||||
struct Traits<UnsafeKdTreeTBB<PointCloud>> {
|
||||
static size_t knn_search(const UnsafeKdTreeTBB<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<KdTreeMT<PointCloud>> {
|
||||
static size_t knn_search(const KdTreeMT<PointCloud>& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) {
|
||||
struct Traits<KdTreeTBB<PointCloud>> {
|
||||
static size_t knn_search(const KdTreeTBB<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);
|
||||
}
|
||||
};
|
||||
|
|
@ -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</a>
|
||||
*/
|
||||
|
||||
#ifndef NANOFLANN_MT_HPP_
|
||||
#define NANOFLANN_MT_HPP_
|
||||
#ifndef NANOFLANN_OMP_HPP_
|
||||
#define NANOFLANN_OMP_HPP_
|
||||
|
||||
#include <tbb/tbb.h>
|
||||
#include <atomic>
|
||||
#include <small_gicp/ann/nanoflann.hpp>
|
||||
|
||||
namespace nanoflann {
|
||||
|
|
@ -69,7 +69,7 @@ namespace nanoflann {
|
|||
*/
|
||||
|
||||
template <class Derived, typename Distance, class DatasetAdaptor, int DIM = -1, typename IndexType = size_t>
|
||||
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<Node> pool;
|
||||
std::atomic_uint64_t pool_count;
|
||||
std::vector<Node> 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<IndexType>(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 <typename Distance, class DatasetAdaptor, int DIM = -1, typename IndexType = size_t>
|
||||
class KDTreeSingleIndexAdaptorMT : public KDTreeBaseClassMT<KDTreeSingleIndexAdaptorMT<Distance, DatasetAdaptor, DIM, IndexType>, Distance, DatasetAdaptor, DIM, IndexType> {
|
||||
class KDTreeSingleIndexAdaptorOMP : public KDTreeBaseClassOMP<KDTreeSingleIndexAdaptorOMP<Distance, DatasetAdaptor, DIM, IndexType>, Distance, DatasetAdaptor, DIM, IndexType> {
|
||||
public:
|
||||
/** Deleted copy constructor*/
|
||||
KDTreeSingleIndexAdaptorMT(const KDTreeSingleIndexAdaptorMT<Distance, DatasetAdaptor, DIM, IndexType>&) = delete;
|
||||
KDTreeSingleIndexAdaptorOMP(const KDTreeSingleIndexAdaptorOMP<Distance, DatasetAdaptor, DIM, IndexType>&) = delete;
|
||||
|
||||
/**
|
||||
* The dataset used by this index
|
||||
|
|
@ -381,7 +387,7 @@ public:
|
|||
|
||||
Distance distance;
|
||||
|
||||
typedef typename nanoflann::KDTreeBaseClassMT<nanoflann::KDTreeSingleIndexAdaptorMT<Distance, DatasetAdaptor, DIM, IndexType>, Distance, DatasetAdaptor, DIM, IndexType>
|
||||
typedef typename nanoflann::KDTreeBaseClassOMP<nanoflann::KDTreeSingleIndexAdaptorOMP<Distance, DatasetAdaptor, DIM, IndexType>, 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)
|
||||
{
|
||||
|
|
@ -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 <nanoflann.hpp> in your code.
|
||||
*
|
||||
* See:
|
||||
* - <a href="modules.html" >C++ API organized by modules</a>
|
||||
* - <a href="https://github.com/jlblancoc/nanoflann" >Online README</a>
|
||||
* - <a href="http://jlblancoc.github.io/nanoflann/" >Doxygen
|
||||
* documentation</a>
|
||||
*/
|
||||
|
||||
#ifndef NANOFLANN_TBB_HPP_
|
||||
#define NANOFLANN_TBB_HPP_
|
||||
|
||||
#include <tbb/tbb.h>
|
||||
#include <small_gicp/ann/nanoflann.hpp>
|
||||
|
||||
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 Derived, typename Distance, class DatasetAdaptor, int DIM = -1, typename IndexType = size_t>
|
||||
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<IndexType> 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<DIM, Interval>::container_t BoundingBox;
|
||||
|
||||
/** Define "distance_vector_t" as a fixed-size or variable-size container
|
||||
* depending on "DIM" */
|
||||
typedef typename array_or_vector_selector<DIM, DistanceType>::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<Node> 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<size_t>(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<IndexType>(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<DistanceType>(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
|
||||
* dataset[ind[lim1..lim2-1]][cutfeat]==cutval
|
||||
* dataset[ind[lim2..count]][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 <class BBOX> 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 <typename Distance, class DatasetAdaptor, int DIM = -1, typename IndexType = size_t>
|
||||
class KDTreeSingleIndexAdaptorTBB : public KDTreeBaseClassTBB<KDTreeSingleIndexAdaptorTBB<Distance, DatasetAdaptor, DIM, IndexType>, Distance, DatasetAdaptor, DIM, IndexType> {
|
||||
public:
|
||||
/** Deleted copy constructor*/
|
||||
KDTreeSingleIndexAdaptorTBB(const KDTreeSingleIndexAdaptorTBB<Distance, DatasetAdaptor, DIM, IndexType>&) = 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<nanoflann::KDTreeSingleIndexAdaptorTBB<Distance, DatasetAdaptor, DIM, IndexType>, 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<DistanceType>
|
||||
* \return True if the requested neighbors could be found.
|
||||
* \sa knnSearch, radiusSearch
|
||||
*/
|
||||
template <typename RESULTSET>
|
||||
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<decltype(result.worstDist())>(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<DistanceType, IndexType> 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<std::pair<IndexType, DistanceType>>& IndicesDists, const SearchParams& searchParams)
|
||||
const {
|
||||
RadiusResultSet<DistanceType, IndexType> 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 <class SEARCH_CALLBACK>
|
||||
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<DistanceType>
|
||||
* \return true if the search should be continued, false if the results are
|
||||
* sufficient
|
||||
*/
|
||||
template <class RESULTSET>
|
||||
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_ */
|
||||
|
|
@ -22,7 +22,7 @@ size_t knn_search(const T& tree, const Eigen::Vector4d& point, size_t k, size_t*
|
|||
}
|
||||
|
||||
template <typename T>
|
||||
concept HasNearestNeighborSearch = requires { Traits<T>::nearest_neighbor_search(T(), Eigen::Vector4d(), 1, nullptr, nullptr); };
|
||||
concept HasNearestNeighborSearch = requires(const T& tree) { Traits<T>::nearest_neighbor_search(tree, Eigen::Vector4d(), nullptr, nullptr); };
|
||||
|
||||
/// @brief Find the nearest neighbor
|
||||
/// @param tree Nearest neighbor search (e.g., KdTree)
|
||||
|
|
|
|||
|
|
@ -1,7 +1,6 @@
|
|||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <deque>
|
||||
#include <vector>
|
||||
#include <numeric>
|
||||
#include <algorithm>
|
||||
|
|
@ -9,7 +8,7 @@
|
|||
#include <Eigen/Core>
|
||||
#include <fmt/format.h>
|
||||
#include <small_gicp/points/traits.hpp>
|
||||
#include <small_gicp/util/read_points.hpp>
|
||||
#include <small_gicp/benchmark/read_points.hpp>
|
||||
|
||||
namespace small_gicp {
|
||||
|
||||
|
|
@ -0,0 +1,82 @@
|
|||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Geometry>
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/util/downsampling.hpp>
|
||||
#include <small_gicp/benchmark/benchmark.hpp>
|
||||
|
||||
#include <guik/viewer/async_light_viewer.hpp>
|
||||
|
||||
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>;
|
||||
|
||||
OdometryEstimation(const OdometryEstimationParams& params) : params(params) {}
|
||||
virtual ~OdometryEstimation() = default;
|
||||
|
||||
virtual std::vector<Eigen::Isometry3d> estimate(std::vector<PointCloud::Ptr>& 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<Eigen::Isometry3d> estimate(std::vector<PointCloud::Ptr>& points) override {
|
||||
std::vector<Eigen::Isometry3d> 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<OdometryEstimation::Ptr(const OdometryEstimationParams&)> factory);
|
||||
|
||||
std::vector<std::string> odometry_names();
|
||||
|
||||
OdometryEstimation::Ptr create_odometry(const std::string& name, const OdometryEstimationParams& params);
|
||||
|
||||
} // namespace small_gicp
|
||||
|
|
@ -11,7 +11,7 @@
|
|||
namespace small_gicp {
|
||||
|
||||
/// @brief Point cloud registration
|
||||
template <typename Factor, typename CorrespondenceRejector, typename Reduction, typename Optimizer>
|
||||
template <typename Factor, typename Reduction, typename CorrespondenceRejector = DistanceRejector, typename Optimizer = LevenbergMarquardtOptimizer>
|
||||
struct Registration {
|
||||
public:
|
||||
/// @brief Align point clouds
|
||||
|
|
@ -21,7 +21,8 @@ public:
|
|||
/// @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) {
|
||||
RegistrationResult
|
||||
align(const TargetPointCloud& target, const SourcePointCloud& source, const TargetTree& target_tree, const Eigen::Isometry3d& init_T = Eigen::Isometry3d::Identity()) {
|
||||
std::vector<Factor> factors(traits::size(source));
|
||||
return optimizer.optimize(target, source, target_tree, rejector, criteria, reduction, init_T, factors);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,6 +2,7 @@
|
|||
|
||||
#include <memory>
|
||||
#include <random>
|
||||
#include <iostream>
|
||||
#include <unordered_map>
|
||||
#include <small_gicp/points/traits.hpp>
|
||||
#include <small_gicp/util/fast_floor.hpp>
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
|
||||
#include <small_gicp/points/traits.hpp>
|
||||
#include <small_gicp/util/fast_floor.hpp>
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
|
||||
#include <tbb/tbb.h>
|
||||
#include <small_gicp/points/traits.hpp>
|
||||
|
|
|
|||
|
|
@ -89,7 +89,7 @@ template <typename Setter, typename PointCloud>
|
|||
void estimate_local_features(PointCloud& cloud, int num_neighbors) {
|
||||
traits::resize(cloud, traits::size(cloud));
|
||||
|
||||
KdTree<PointCloud> kdtree(cloud);
|
||||
UnsafeKdTree<PointCloud> kdtree(cloud);
|
||||
for (size_t i = 0; i < traits::size(cloud); i++) {
|
||||
estimate_local_features<Setter>(cloud, kdtree, num_neighbors, i);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,148 @@
|
|||
#include <small_gicp/ann/kdtree.hpp>
|
||||
#include <small_gicp/ann/kdtree_mt.hpp>
|
||||
#include <small_gicp/ann/flat_voxelmap.hpp>
|
||||
#include <small_gicp/ann/gaussian_voxelmap.hpp>
|
||||
#include <small_gicp/util/downsampling.hpp>
|
||||
#include <small_gicp/util/downsampling_tbb.hpp>
|
||||
#include <small_gicp/util/normal_estimation.hpp>
|
||||
#include <small_gicp/util/normal_estimation_omp.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/registration/optimizer.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_omp.hpp>
|
||||
#include <small_gicp/registration/reduction_tbb.hpp>
|
||||
#include <small_gicp/registration/registration.hpp>
|
||||
|
||||
#include <guik/viewer/light_viewer.hpp>
|
||||
#include <guik/viewer/async_light_viewer.hpp>
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
using namespace small_gicp;
|
||||
|
||||
// if (argc < 3) {
|
||||
// std::cout << "usage: odometry_benchmark <dataset_path> <output_path> (--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<PointCloud>(true);
|
||||
|
||||
auto async_viewer = guik::async_viewer();
|
||||
async_viewer->use_orbit_camera_control(250.0);
|
||||
|
||||
struct InputFrame {
|
||||
using Ptr = std::shared_ptr<InputFrame>;
|
||||
size_t id;
|
||||
PointCloud::Ptr points;
|
||||
KdTree<PointCloud>::Ptr kdtree;
|
||||
Eigen::Isometry3d T_last_current;
|
||||
};
|
||||
struct InputFramePair {
|
||||
InputFrame::Ptr source;
|
||||
InputFrame::Ptr target;
|
||||
};
|
||||
|
||||
tbb::flow::graph graph;
|
||||
tbb::flow::broadcast_node<InputFrame::Ptr> input(graph);
|
||||
tbb::flow::function_node<InputFrame::Ptr, InputFrame::Ptr> preprocess_node(graph, tbb::flow::unlimited, [=](const InputFrame::Ptr& input) {
|
||||
input->points = voxelgrid_sampling(*input->points, downsampling_resolution);
|
||||
input->kdtree = std::make_shared<KdTree<PointCloud>>(input->points);
|
||||
estimate_covariances(*input->points, *input->kdtree, 10);
|
||||
return input;
|
||||
});
|
||||
tbb::flow::sequencer_node<InputFrame::Ptr> postpre_sequencer_node(graph, [](const InputFrame::Ptr& input) { return input->id; });
|
||||
|
||||
tbb::flow::function_node<InputFrame::Ptr, InputFramePair> 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<InputFramePair, InputFrame::Ptr> registration_node(graph, tbb::flow::unlimited, [&](const InputFramePair& pair) {
|
||||
if (pair.target == nullptr) {
|
||||
pair.source->T_last_current.setIdentity();
|
||||
return pair.source;
|
||||
}
|
||||
|
||||
Registration<GICPFactor, DistanceRejector, SerialReduction, LevenbergMarquardtOptimizer> 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<InputFrame::Ptr> postreg_sequencer_node(graph, [](const InputFrame::Ptr& input) { return input->id; });
|
||||
|
||||
tbb::flow::function_node<InputFrame::Ptr> 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;
|
||||
}
|
||||
|
|
@ -5,13 +5,13 @@
|
|||
#include <pcl/filters/voxel_grid.h>
|
||||
#include <pcl/filters/approximate_voxel_grid.h>
|
||||
|
||||
#include <small_gicp/util/benchmark.hpp>
|
||||
#include <small_gicp/util/downsampling.hpp>
|
||||
#include <small_gicp/util/downsampling_omp.hpp>
|
||||
#include <small_gicp/util/downsampling_tbb.hpp>
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/points/pcl_point.hpp>
|
||||
#include <small_gicp/points/pcl_point_traits.hpp>
|
||||
#include <small_gicp/benchmark/benchmark.hpp>
|
||||
|
||||
namespace small_gicp {
|
||||
|
||||
|
|
|
|||
|
|
@ -1,133 +1,31 @@
|
|||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <small_gicp/util/downsampling.hpp>
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/benchmark/benchmark.hpp>
|
||||
#include <small_gicp/benchmark/benchmark_odom.hpp>
|
||||
|
||||
#include <small_gicp/ann/kdtree.hpp>
|
||||
#include <small_gicp/ann/kdtree_mt.hpp>
|
||||
#include <small_gicp/util/downsampling.hpp>
|
||||
#include <small_gicp/util/downsampling_tbb.hpp>
|
||||
#include <small_gicp/util/normal_estimation.hpp>
|
||||
#include <small_gicp/util/normal_estimation_omp.hpp>
|
||||
#include <small_gicp/util/normal_estimation_tbb.hpp>
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/points/pcl_point.hpp>
|
||||
#include <small_gicp/points/pcl_point_traits.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_omp.hpp>
|
||||
#include <small_gicp/registration/reduction_tbb.hpp>
|
||||
#include <small_gicp/registration/registration.hpp>
|
||||
|
||||
#include <fast_gicp/gicp/fast_gicp.hpp>
|
||||
#include <fast_gicp/gicp/impl/fast_gicp_impl.hpp>
|
||||
#include <fast_gicp/gicp/impl/lsq_registration_impl.hpp>
|
||||
|
||||
#include <glk/pointcloud_buffer_pcl.hpp>
|
||||
#include <guik/viewer/light_viewer.hpp>
|
||||
|
||||
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<pcl::PointCloud<pcl::PointXYZ>>();
|
||||
points_pcl->resize(points->size());
|
||||
for (size_t i = 0; i < points->size(); i++) {
|
||||
points_pcl->at(i).getVector4fMap() = points->point(i).template cast<float>();
|
||||
}
|
||||
|
||||
Stopwatch sw;
|
||||
sw.start();
|
||||
|
||||
if (!gicp.getInputTarget()) {
|
||||
gicp.setInputTarget(points_pcl);
|
||||
return Eigen::Isometry3d::Identity();
|
||||
}
|
||||
|
||||
gicp.setInputSource(points_pcl);
|
||||
pcl::PointCloud<pcl::PointXYZ> aligned;
|
||||
gicp.align(aligned);
|
||||
|
||||
sw.stop();
|
||||
reg_times.push(sw.msec());
|
||||
|
||||
T = T * Eigen::Isometry3d(gicp.getFinalTransformation().cast<double>());
|
||||
gicp.swapSourceAndTarget();
|
||||
|
||||
return T;
|
||||
}
|
||||
|
||||
private:
|
||||
fast_gicp::FastGICP<pcl::PointXYZ, pcl::PointXYZ> 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<KdTreeMT<PointCloud>>(points);
|
||||
estimate_covariances_tbb(*points, *tree, 10);
|
||||
|
||||
if (!target_points) {
|
||||
target_points = points;
|
||||
target_tree = tree;
|
||||
return Eigen::Isometry3d::Identity();
|
||||
}
|
||||
|
||||
Registration<GICPFactor, DistanceRejector, ParallelReductionTBB, LevenbergMarquardtOptimizer> 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<PointCloud>::Ptr target_tree;
|
||||
|
||||
Eigen::Isometry3d T;
|
||||
};
|
||||
|
||||
} // namespace small_gicp
|
||||
#include <small_gicp/ann/kdtree_omp.hpp>
|
||||
#include <small_gicp/ann/kdtree_tbb.hpp>
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
using namespace small_gicp;
|
||||
|
||||
if (argc < 3) {
|
||||
std::cout << "usage: odometry_benchmark <dataset_path> <output_path> (--engine small|fast) (--num_threads 4) (--resolution 0.25)" << std::endl;
|
||||
std::cout << "USAGE: odometry_benchmark <dataset_path> <output_path> [options]" << std::endl;
|
||||
std::cout << "OPTIONS:" << std::endl;
|
||||
std::cout << " --num_threads <value> (default: 4)" << std::endl;
|
||||
std::cout << " --resolution <value> (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<OdometryEstimater> odom;
|
||||
if (engine == "small") {
|
||||
odom = std::make_shared<SmallGICPOdometryEstimater>(num_threads);
|
||||
} else if (engine == "fast") {
|
||||
odom = std::make_shared<FastGICPOdometryEstimater>(num_threads);
|
||||
} else {
|
||||
std::cerr << "Unknown engine: " << engine << std::endl;
|
||||
OdometryEstimationParams params{num_threads, downsampling_resolution};
|
||||
std::shared_ptr<OdometryEstimation> 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<PointCloud>(true);
|
||||
std::vector<Eigen::Isometry3d> traj = odom->estimate(raw_points);
|
||||
|
||||
std::vector<Eigen::Isometry3d> 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) {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,62 @@
|
|||
#include <small_gicp/benchmark/benchmark_odom.hpp>
|
||||
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/registration/gicp.h>
|
||||
|
||||
#include <fast_gicp/gicp/fast_gicp.hpp>
|
||||
#include <fast_gicp/gicp/impl/fast_gicp_impl.hpp>
|
||||
#include <fast_gicp/gicp/impl/lsq_registration_impl.hpp>
|
||||
|
||||
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<pcl::PointCloud<pcl::PointXYZ>>();
|
||||
points_pcl->resize(points->size());
|
||||
for (size_t i = 0; i < points->size(); i++) {
|
||||
points_pcl->at(i).getVector4fMap() = points->point(i).template cast<float>();
|
||||
}
|
||||
|
||||
Stopwatch sw;
|
||||
sw.start();
|
||||
|
||||
if (!gicp.getInputTarget()) {
|
||||
gicp.setInputTarget(points_pcl);
|
||||
return T;
|
||||
}
|
||||
|
||||
gicp.setInputSource(points_pcl);
|
||||
pcl::PointCloud<pcl::PointXYZ> aligned;
|
||||
gicp.align(aligned);
|
||||
|
||||
sw.stop();
|
||||
reg_times.push(sw.msec());
|
||||
|
||||
T = T * Eigen::Isometry3d(gicp.getFinalTransformation().cast<double>());
|
||||
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<pcl::PointXYZ, pcl::PointXYZ> gicp;
|
||||
Eigen::Isometry3d T;
|
||||
};
|
||||
|
||||
static auto fast_gicp_registry = register_odometry("fast_gicp", [](const OdometryEstimationParams& params) { return std::make_shared<FastGICPOdometryEstimation>(params); });
|
||||
|
||||
} // namespace small_gicp
|
||||
|
|
@ -0,0 +1,64 @@
|
|||
#include <small_gicp/benchmark/benchmark_odom.hpp>
|
||||
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/registration/gicp.h>
|
||||
|
||||
#include <fast_gicp/gicp/fast_gicp.hpp>
|
||||
#include <fast_gicp/gicp/fast_vgicp.hpp>
|
||||
#include <fast_gicp/gicp/impl/fast_gicp_impl.hpp>
|
||||
#include <fast_gicp/gicp/impl/fast_vgicp_impl.hpp>
|
||||
#include <fast_gicp/gicp/impl/lsq_registration_impl.hpp>
|
||||
|
||||
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<pcl::PointCloud<pcl::PointXYZ>>();
|
||||
points_pcl->resize(points->size());
|
||||
for (size_t i = 0; i < points->size(); i++) {
|
||||
points_pcl->at(i).getVector4fMap() = points->point(i).template cast<float>();
|
||||
}
|
||||
|
||||
Stopwatch sw;
|
||||
sw.start();
|
||||
|
||||
if (!vgicp.getInputTarget()) {
|
||||
vgicp.setInputTarget(points_pcl);
|
||||
return T;
|
||||
}
|
||||
|
||||
vgicp.setInputSource(points_pcl);
|
||||
pcl::PointCloud<pcl::PointXYZ> aligned;
|
||||
vgicp.align(aligned);
|
||||
|
||||
sw.stop();
|
||||
reg_times.push(sw.msec());
|
||||
|
||||
T = T * Eigen::Isometry3d(vgicp.getFinalTransformation().cast<double>());
|
||||
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<pcl::PointXYZ, pcl::PointXYZ> vgicp;
|
||||
Eigen::Isometry3d T;
|
||||
};
|
||||
|
||||
static auto fast_vgicp_registry = register_odometry("fast_vgicp", [](const OdometryEstimationParams& params) { return std::make_shared<FastVGICPOdometryEstimation>(params); });
|
||||
|
||||
} // namespace small_gicp
|
||||
|
|
@ -0,0 +1,59 @@
|
|||
#include <small_gicp/benchmark/benchmark_odom.hpp>
|
||||
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/registration/gicp.h>
|
||||
|
||||
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<pcl::PointCloud<pcl::PointXYZ>>();
|
||||
points_pcl->resize(points->size());
|
||||
for (size_t i = 0; i < points->size(); i++) {
|
||||
points_pcl->at(i).getVector4fMap() = points->point(i).template cast<float>();
|
||||
}
|
||||
|
||||
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<pcl::PointXYZ> aligned;
|
||||
gicp.align(aligned);
|
||||
|
||||
sw.stop();
|
||||
reg_times.push(sw.msec());
|
||||
|
||||
T = T * Eigen::Isometry3d(gicp.getFinalTransformation().cast<double>());
|
||||
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<pcl::PointXYZ, pcl::PointXYZ> gicp;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr target_points;
|
||||
Eigen::Isometry3d T;
|
||||
};
|
||||
|
||||
static auto pcl_odom_registry = register_odometry("pcl", [](const OdometryEstimationParams& params) { return std::make_shared<PCLOnlineOdometryEstimation>(params); });
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,60 @@
|
|||
#include <small_gicp/benchmark/benchmark_odom.hpp>
|
||||
|
||||
#include <small_gicp/factors/gicp_factor.hpp>
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/ann/kdtree.hpp>
|
||||
#include <small_gicp/util/normal_estimation.hpp>
|
||||
#include <small_gicp/registration/reduction.hpp>
|
||||
#include <small_gicp/registration/registration.hpp>
|
||||
|
||||
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<KdTree<PointCloud>>(points);
|
||||
estimate_covariances(*points, *tree, 10);
|
||||
|
||||
if (target_points == nullptr) {
|
||||
target_points = points;
|
||||
target_tree = tree;
|
||||
return T;
|
||||
}
|
||||
|
||||
Registration<GICPFactor, SerialReduction, DistanceRejector, LevenbergMarquardtOptimizer> 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<PointCloud>::Ptr target_tree;
|
||||
|
||||
Eigen::Isometry3d T;
|
||||
};
|
||||
|
||||
static auto small_gicp_tbb_registry =
|
||||
register_odometry("small_gicp", [](const OdometryEstimationParams& params) { return std::make_shared<SmallGICPOnlineOdometryEstimation>(params); });
|
||||
|
||||
} // namespace small_gicp
|
||||
|
|
@ -0,0 +1,61 @@
|
|||
#include <small_gicp/benchmark/benchmark_odom.hpp>
|
||||
|
||||
#include <small_gicp/factors/gicp_factor.hpp>
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/ann/kdtree_omp.hpp>
|
||||
#include <small_gicp/util/normal_estimation_omp.hpp>
|
||||
#include <small_gicp/registration/reduction_omp.hpp>
|
||||
#include <small_gicp/registration/registration.hpp>
|
||||
|
||||
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<KdTree<PointCloud>>(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<GICPFactor, ParallelReductionOMP, DistanceRejector, LevenbergMarquardtOptimizer> 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<PointCloud>::Ptr target_tree;
|
||||
|
||||
Eigen::Isometry3d T;
|
||||
};
|
||||
|
||||
static auto small_gicp_omp_registry =
|
||||
register_odometry("small_gicp_omp", [](const OdometryEstimationParams& params) { return std::make_shared<SmallGICPOnlineOdometryEstimationOMP>(params); });
|
||||
|
||||
} // namespace small_gicp
|
||||
|
|
@ -0,0 +1,66 @@
|
|||
#include <small_gicp/benchmark/benchmark_odom.hpp>
|
||||
|
||||
#include <tbb/tbb.h>
|
||||
#include <small_gicp/factors/gicp_factor.hpp>
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/ann/kdtree_tbb.hpp>
|
||||
#include <small_gicp/util/normal_estimation_tbb.hpp>
|
||||
#include <small_gicp/registration/reduction_tbb.hpp>
|
||||
#include <small_gicp/registration/registration.hpp>
|
||||
|
||||
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<KdTreeTBB<PointCloud>>(points);
|
||||
estimate_covariances_tbb(*points, *tree, 10);
|
||||
|
||||
if (target_points == nullptr) {
|
||||
target_points = points;
|
||||
target_tree = tree;
|
||||
return T;
|
||||
}
|
||||
|
||||
Registration<GICPFactor, ParallelReductionTBB, DistanceRejector, LevenbergMarquardtOptimizer> 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<PointCloud>::Ptr target_tree;
|
||||
|
||||
Eigen::Isometry3d T;
|
||||
};
|
||||
|
||||
static auto small_gicp_tbb_registry =
|
||||
register_odometry("small_gicp_tbb", [](const OdometryEstimationParams& params) { return std::make_shared<SmallGICPOnlineOdometryEstimationTBB>(params); });
|
||||
|
||||
} // namespace small_gicp
|
||||
|
|
@ -0,0 +1,132 @@
|
|||
#include <small_gicp/benchmark/benchmark_odom.hpp>
|
||||
|
||||
#include <tbb/tbb.h>
|
||||
#include <small_gicp/factors/gicp_factor.hpp>
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/ann/kdtree.hpp>
|
||||
#include <small_gicp/util/normal_estimation.hpp>
|
||||
#include <small_gicp/registration/registration.hpp>
|
||||
|
||||
namespace small_gicp {
|
||||
|
||||
class BatchedSmallGICPOnlineOdometryEstimationTBB : public OdometryEstimation {
|
||||
public:
|
||||
struct InputFrame {
|
||||
using Ptr = std::shared_ptr<InputFrame>;
|
||||
size_t id;
|
||||
PointCloud::Ptr points;
|
||||
KdTree<PointCloud>::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<Eigen::Isometry3d> estimate(std::vector<PointCloud::Ptr>& points) override {
|
||||
auto async_viewer = guik::async_viewer();
|
||||
|
||||
std::vector<Eigen::Isometry3d> traj;
|
||||
traj.reserve(points.size());
|
||||
|
||||
tbb::flow::graph graph;
|
||||
tbb::flow::broadcast_node<InputFrame::Ptr> input_node(graph);
|
||||
tbb::flow::function_node<InputFrame::Ptr, InputFrame::Ptr> 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<KdTree<PointCloud>>(input->points);
|
||||
estimate_covariances(*input->points, *input->kdtree, 10);
|
||||
return input;
|
||||
});
|
||||
tbb::flow::sequencer_node<InputFrame::Ptr> postpre_sequencer_node(graph, [](const InputFrame::Ptr& input) { return input->id; });
|
||||
tbb::flow::function_node<InputFrame::Ptr, InputFramePair> 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<InputFramePair, InputFrame::Ptr> registration_node(graph, tbb::flow::unlimited, [&](const InputFramePair& pair) {
|
||||
if (pair.target == nullptr) {
|
||||
pair.source->T_last_current.setIdentity();
|
||||
return pair.source;
|
||||
}
|
||||
|
||||
Registration<GICPFactor, SerialReduction> 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<InputFrame::Ptr> postreg_sequencer_node(graph, [](const InputFrame::Ptr& input) { return input->id; });
|
||||
tbb::flow::function_node<InputFrame::Ptr> 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<std::chrono::nanoseconds>(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<BatchedSmallGICPOnlineOdometryEstimationTBB>(params); });
|
||||
|
||||
} // namespace small_gicp
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
#include <small_gicp/benchmark/benchmark_odom.hpp>
|
||||
|
||||
#include <tbb/tbb.h>
|
||||
#include <small_gicp/factors/gicp_factor.hpp>
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/ann/gaussian_voxelmap.hpp>
|
||||
#include <small_gicp/util/normal_estimation_tbb.hpp>
|
||||
#include <small_gicp/registration/reduction_tbb.hpp>
|
||||
#include <small_gicp/registration/registration.hpp>
|
||||
|
||||
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<GaussianVoxelMap>(params.voxel_resolution);
|
||||
voxelmap->insert(*points);
|
||||
|
||||
if (target_points == nullptr) {
|
||||
target_points = points;
|
||||
target_voxelmap = voxelmap;
|
||||
return T;
|
||||
}
|
||||
|
||||
Registration<GICPFactor, ParallelReductionTBB, DistanceRejector, LevenbergMarquardtOptimizer> 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<SmallVGICPOnlineOdometryEstimationTBB>(params); });
|
||||
|
||||
} // namespace small_gicp
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
#include <small_gicp/benchmark/benchmark_odom.hpp>
|
||||
|
||||
namespace small_gicp {
|
||||
|
||||
std::vector<std::pair<std::string, std::function<OdometryEstimation::Ptr(const OdometryEstimationParams&)>>> odometry_registry;
|
||||
|
||||
size_t register_odometry(const std::string& name, std::function<OdometryEstimation::Ptr(const OdometryEstimationParams&)> factory) {
|
||||
odometry_registry.emplace_back(name, factory);
|
||||
return odometry_registry.size() - 1;
|
||||
}
|
||||
|
||||
std::vector<std::string> odometry_names() {
|
||||
std::vector<std::string> 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
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
#include <gtest/gtest.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/points/pcl_point_traits.hpp>
|
||||
#include <small_gicp/util/downsampling.hpp>
|
||||
#include <small_gicp/util/downsampling_omp.hpp>
|
||||
#include <small_gicp/util/downsampling_tbb.hpp>
|
||||
|
||||
using namespace small_gicp;
|
||||
|
||||
TEST(DownsamplingTest, NullTest) {
|
||||
auto points = std::make_shared<small_gicp::PointCloud>();
|
||||
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<pcl::PointCloud<pcl::PointXYZ>>();
|
||||
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);
|
||||
}
|
||||
|
|
@ -0,0 +1,61 @@
|
|||
#include <random>
|
||||
#include <algorithm>
|
||||
#include <fmt/format.h>
|
||||
#include <small_gicp/util/sort_omp.hpp>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
using namespace small_gicp;
|
||||
|
||||
bool identical(const std::vector<int>& arr1, const std::vector<int>& 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<int> data(size_dist(mt));
|
||||
std::ranges::generate(data, [&] { return data_dist(mt); });
|
||||
|
||||
std::vector<int> sorted = data;
|
||||
std::ranges::sort(sorted);
|
||||
|
||||
std::vector<int> sorted_omp = data;
|
||||
merge_sort_omp(sorted_omp.begin(), sorted_omp.end(), std::less<int>(), 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<int> data(size_dist(mt));
|
||||
std::ranges::generate(data, [&] { return data_dist(mt); });
|
||||
|
||||
std::vector<int> sorted = data;
|
||||
std::ranges::sort(sorted);
|
||||
|
||||
std::vector<int> sorted_omp = data;
|
||||
quick_sort_omp(sorted_omp.begin(), sorted_omp.end(), std::less<int>(), 4);
|
||||
|
||||
EXPECT_TRUE(identical(sorted, sorted_omp)) << fmt::format("i={} N={}", i, data.size());
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue