mirror
/
small_gicp
mirror of https://github.com/koide3/small_gicp.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

generic kdtree template

This commit is contained in:
k.koide 2024-03-27 09:29:16 +09:00
parent 3752a64d21
commit ef72d7ce9b
14 changed files with 168 additions and 600 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
CMakeLists.txt
View File

@ -77,7 +77,8 @@ add_executable(odometry_benchmark
src/odometry_benchmark_small_vgicp_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_small_vgicp_model_tbb.cpp
src/odometry_benchmark_small_gicp_tbb_flow.cpp
src/odometry_benchmark.cpp
)
target_include_directories(odometry_benchmark PUBLIC

14
include/small_gicp/ann/gaussian_voxelmap.hpp
View File

@ -73,6 +73,20 @@ public:
/// @brief Number of voxels
size_t size() const { return flat_voxels.size(); }
/// @brief Get voxel means
std::vector<Eigen::Vector4d> voxel_means() const {
std::vector<Eigen::Vector4d> means(flat_voxels.size());
std::ranges::transform(flat_voxels, means.begin(), [](const GaussianVoxel& voxel) { return voxel.mean; });
return means;
}
/// @brief Get voxel covariances
std::vector<Eigen::Matrix4d> voxel_covs() const {
std::vector<Eigen::Matrix4d> covs(flat_voxels.size());
std::ranges::transform(flat_voxels, covs.begin(), [](const GaussianVoxel& voxel) { return voxel.cov; });
return covs;
}
/// @brief Insert points to the voxelmap
/// @param points Input points
/// @param T Transformation

69
include/small_gicp/ann/kdtree.hpp
View File

@ -8,20 +8,30 @@
namespace small_gicp {
/// @brief Unsafe KdTree
template <typename Distance, class DatasetAdaptor, int DIM = -1, typename IndexType = size_t>
class KDTreeSingleIndexAdaptor;
/// @brief Unsafe KdTree with arbitrary nanoflann Adaptor
/// @note This class does not hold the ownership of the input points.
/// You must keep the input points along with this class.
template <typename PointCloud>
class UnsafeKdTree {
template <class PointCloud, template <typename, typename, int, typename> class Adaptor>
class UnsafeKdTreeGeneric {
public:
using Ptr = std::shared_ptr<UnsafeKdTree>;
using ConstPtr = std::shared_ptr<const UnsafeKdTree>;
using Index = nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<double, UnsafeKdTree<PointCloud>, double>, UnsafeKdTree<PointCloud>, 3, size_t>;
using Ptr = std::shared_ptr<UnsafeKdTreeGeneric>;
using ConstPtr = std::shared_ptr<const UnsafeKdTreeGeneric>;
using ThisClass = UnsafeKdTreeGeneric<PointCloud, Adaptor>;
using Index = Adaptor<nanoflann::L2_Simple_Adaptor<double, ThisClass, double>, ThisClass, 3, size_t>;
/// @brief Constructor
/// @param points Input points
UnsafeKdTree(const PointCloud& points) : points(points), index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10)) { index.buildIndex(); }
~UnsafeKdTree() {}
UnsafeKdTreeGeneric(const PointCloud& points) : points(points), index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10)) { index.buildIndex(); }
/// @brief Constructor
/// @param points Input points
/// @params num_threads Number of threads used for building the index (This argument is only valid for OMP implementation)
UnsafeKdTreeGeneric(const PointCloud& points, int num_threads) : points(points), index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10)) { index.buildIndex(num_threads); }
~UnsafeKdTreeGeneric() {}
// Interfaces for nanoflann
size_t kdtree_get_point_count() const { return traits::size(points); }
@ -40,38 +50,51 @@ private:
Index index; ///< KdTree index
};
/// @brief KdTree
template <typename PointCloud>
class KdTree {
/// @brief KdTree with arbitrary nanoflann Adaptor
template <class PointCloud, template <typename, typename, int, typename> class Adaptor>
class KdTreeGeneric {
public:
using Ptr = std::shared_ptr<KdTree>;
using ConstPtr = std::shared_ptr<const KdTree>;
using Ptr = std::shared_ptr<KdTreeGeneric>;
using ConstPtr = std::shared_ptr<const KdTreeGeneric>;
/// @brief Constructor
/// @param points Input points
KdTree(const std::shared_ptr<const PointCloud>& points) : points(points), tree(*points) {}
~KdTree() {}
KdTreeGeneric(const std::shared_ptr<const PointCloud>& points) : points(points), tree(*points) {}
/// @brief Constructor
/// @param points Input points
KdTreeGeneric(const std::shared_ptr<const PointCloud>& points, int num_threads) : points(points), tree(*points, num_threads) {}
~KdTreeGeneric() {}
/// @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 UnsafeKdTree<PointCloud> tree; ///< KdTree
const std::shared_ptr<const PointCloud> points; ///< Input points
const UnsafeKdTreeGeneric<PointCloud, Adaptor> tree; ///< KdTree
};
/// @brief Standard KdTree (unsafe)
template <class PointCloud>
using UnsafeKdTree = UnsafeKdTreeGeneric<PointCloud, nanoflann::KDTreeSingleIndexAdaptor>;
/// @brief Standard KdTree
template <class PointCloud>
using KdTree = KdTreeGeneric<PointCloud, nanoflann::KDTreeSingleIndexAdaptor>;
namespace traits {
template <typename PointCloud>
struct Traits<UnsafeKdTree<PointCloud>> {
static size_t knn_search(const UnsafeKdTree<PointCloud>& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) {
template <class PointCloud, template <typename, typename, int, typename> class Adaptor>
struct Traits<UnsafeKdTreeGeneric<PointCloud, Adaptor>> {
static size_t knn_search(const UnsafeKdTreeGeneric<PointCloud, Adaptor>& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) {
return tree.knn_search(point, k, k_indices, k_sq_dists);
}
};
template <typename PointCloud>
struct Traits<KdTree<PointCloud>> {
static size_t knn_search(const KdTree<PointCloud>& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) {
template <class PointCloud, template <typename, typename, int, typename> class Adaptor>
struct Traits<KdTreeGeneric<PointCloud, Adaptor>> {
static size_t knn_search(const KdTreeGeneric<PointCloud, Adaptor>& 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);
}
};

76
include/small_gicp/ann/kdtree_omp.hpp
View File

@ -2,79 +2,19 @@
#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/kdtree.hpp>
#include <small_gicp/ann/nanoflann_omp.hpp>
namespace small_gicp {
/// @brief Unsafe KdTree with multi-thread tree construction.
/// @brief Unsafe KdTree with multi-thread tree construction with OpenMP backend.
/// @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>;
template <class PointCloud>
using UnsafeKdTreeOMP = UnsafeKdTreeGeneric<PointCloud, nanoflann::KDTreeSingleIndexAdaptorOMP>;
/// @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
/// @brief KdTree with multi-thread tree construction with OpenMP backend.
/// @note This class only parallelizes the tree construction. The search is still single-threaded as in the normal KdTree.
template <class PointCloud>
using KdTreeOMP = KdTreeGeneric<PointCloud, nanoflann::KDTreeSingleIndexAdaptorOMP>;
} // namespace small_gicp

76
include/small_gicp/ann/kdtree_tbb.hpp
View File

@ -2,79 +2,19 @@
#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/kdtree.hpp>
#include <small_gicp/ann/nanoflann_tbb.hpp>
namespace small_gicp {
/// @brief Unsafe KdTree with multi-thread tree construction.
/// @brief Unsafe KdTree with multi-thread tree construction with TBB backend.
/// @note This class only parallelizes the tree construction. The search is still single-threaded as in the normal KdTree.
template <typename PointCloud>
class UnsafeKdTreeTBB {
public:
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>;
template <class PointCloud>
using UnsafeKdTreeTBB = UnsafeKdTreeGeneric<PointCloud, nanoflann::KDTreeSingleIndexAdaptorTBB>;
/// @brief Constructor
/// @param points Input points
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); }
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 KdTreeTBB {
public:
using Ptr = std::shared_ptr<KdTreeTBB>;
using ConstPtr = std::shared_ptr<const KdTreeTBB>;
/// @brief Constructor
/// @param points Input points
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 UnsafeKdTreeTBB<PointCloud> tree; ///< KdTree
};
namespace traits {
template <typename PointCloud>
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<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);
}
};
} // namespace traits
/// @brief KdTree with multi-thread tree construction with TBB backend.
/// @note This class only parallelizes the tree construction. The search is still single-threaded as in the normal KdTree.
template <class PointCloud>
using KdTreeTBB = KdTreeGeneric<PointCloud, nanoflann::KDTreeSingleIndexAdaptorTBB>;
} // namespace small_gicp

2
include/small_gicp/ann/nanoflann_omp.hpp
View File

@ -437,7 +437,7 @@ public:
/**
* Builds the index
*/
void buildIndex(int num_threads = 4) {
void buildIndex(int num_threads) {
BaseClassRef::m_size = dataset.kdtree_get_point_count();
BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
init_vind();

7
include/small_gicp/benchmark/benchmark_odom.hpp
View File

@ -37,7 +37,7 @@ protected:
class OnlineOdometryEstimation : public OdometryEstimation {
public:
OnlineOdometryEstimation(const OdometryEstimationParams& params) : OdometryEstimation(params) {}
OnlineOdometryEstimation(const OdometryEstimationParams& params) : OdometryEstimation(params), z_range(-5.0f, 5.0f) {}
~OnlineOdometryEstimation() { guik::async_destroy(); }
std::vector<Eigen::Isometry3d> estimate(std::vector<PointCloud::Ptr>& points) override {
@ -55,8 +55,12 @@ public:
traj.emplace_back(T);
auto async_viewer = guik::async_viewer();
z_range[0] = std::min<double>(z_range[0], T.translation().z() - 5.0f);
z_range[1] = std::max<double>(z_range[1], T.translation().z() + 5.0f);
async_viewer->invoke([=] { guik::viewer()->shader_setting().add("z_range", z_range); });
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));
async_viewer->lookat(T.translation().cast<float>());
points[i].reset();
@ -70,6 +74,7 @@ public:
virtual Eigen::Isometry3d estimate(const PointCloud::Ptr& points) = 0;
protected:
Eigen::Vector2f z_range;
Summarizer total_times;
};

148
src/batch_test.cpp
View File

@ -1,148 +0,0 @@
#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;
}

68
src/downsampling_test.cpp
View File

@ -1,68 +0,0 @@
#include <iostream>
#include <filesystem>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <small_gicp/util/read_points.hpp>
#include <small_gicp/points/point_cloud.hpp>
#include <small_gicp/util/downsampling.hpp>
#include <small_gicp/util/downsampling_tbb.hpp>
#include <small_gicp/util/normal_estimation_omp.hpp>
#include <small_gicp/util/normal_estimation_tbb.hpp>
#include <small_gicp/ann/flat_voxelmap.hpp>
#include <guik/viewer/light_viewer.hpp>
#include <easy_profiler.hpp>
int main(int argc, char** argv) {
using namespace small_gicp;
const int num_threads = 6;
tbb::task_arena arena(num_threads);
// tbb::task_scheduler_init init(num_threads);
const std::string dataset_path = "/home/koide/datasets/velodyne";
std::vector<std::string> filenames;
for (const auto& path : std::filesystem::directory_iterator(dataset_path)) {
if (path.path().extension() != ".bin") {
continue;
}
filenames.emplace_back(path.path().string());
}
std::ranges::sort(filenames);
auto viewer = guik::viewer();
viewer->disable_vsync();
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
for (const auto& filename : filenames) {
EasyProfiler prof;
prof.push("read_points");
const auto raw_points = read_points(filename);
prof.push("copy");
auto points = std::make_shared<PointCloud>(raw_points);
prof.push("downsample");
auto downsampled = voxelgrid_sampling_tbb(*points, 0.2);
prof.push("estimate covs");
estimate_covariances_omp(*downsampled, 10);
prof.push("create flat voxels");
auto voxels = std::make_shared<FlatVoxelMap>(0.5, *downsampled);
prof.push("estimate covs2");
estimate_covariances_tbb(*downsampled, *voxels, 10);
prof.push("search");
prof.push("done");
viewer->update_points("points", downsampled->points, guik::Rainbow());
if (!viewer->spin_once()) {
break;
}
}
return 0;
}

11
src/odometry_benchmark.cpp
View File

@ -14,7 +14,8 @@ int main(int argc, char** argv) {
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;
std::cout << " --downsample_resolution <value> (default: 0.25)" << std::endl;
std::cout << " --voxel_resolution <value> (default: 2.0)" << std::endl;
const auto odom_names = odometry_names();
std::stringstream sst;
@ -34,13 +35,16 @@ int main(int argc, char** argv) {
int num_threads = 4;
double downsampling_resolution = 0.25;
double voxel_resolution = 2.0;
std::string engine = "small_gicp";
for (auto arg = argv + 3; arg != argv + argc; arg++) {
if (std::string(*arg) == "--num_threads") {
num_threads = std::stoi(*(arg + 1));
} else if (std::string(*arg) == "--resolution") {
} else if (std::string(*arg) == "--downsampling_resolution") {
downsampling_resolution = std::stod(*(arg + 1));
} else if (std::string(*arg) == "--voxel_resolution") {
voxel_resolution = std::stod(*(arg + 1));
} else if (std::string(*arg) == "--engine") {
engine = *(arg + 1);
}
@ -51,8 +55,9 @@ int main(int argc, char** argv) {
std::cout << "registration_engine=" << engine << std::endl;
std::cout << "num_threads=" << num_threads << std::endl;
std::cout << "downsampling_resolution=" << downsampling_resolution << std::endl;
std::cout << "voxel_resolution=" << voxel_resolution << std::endl;
OdometryEstimationParams params{num_threads, downsampling_resolution};
OdometryEstimationParams params{num_threads, downsampling_resolution, voxel_resolution};
std::shared_ptr<OdometryEstimation> odom = create_odometry(engine, params);
if (odom == nullptr) {
return 1;

10
src/odometry_benchmark_small_gicp_tbb_batch.cpp → src/odometry_benchmark_small_gicp_tbb_flow.cpp
View File

@ -9,7 +9,7 @@
namespace small_gicp {
class BatchedSmallGICPOnlineOdometryEstimationTBB : public OdometryEstimation {
class SmallGICPFlowEstimationTBB : public OdometryEstimation {
public:
struct InputFrame {
using Ptr = std::shared_ptr<InputFrame>;
@ -25,12 +25,12 @@ public:
InputFrame::Ptr source;
};
BatchedSmallGICPOnlineOdometryEstimationTBB(const OdometryEstimationParams& params)
SmallGICPFlowEstimationTBB(const OdometryEstimationParams& params)
: OdometryEstimation(params),
control(tbb::global_control::max_allowed_parallelism, params.num_threads),
throughput(0.0) {}
~BatchedSmallGICPOnlineOdometryEstimationTBB() { guik::async_destroy(); }
~SmallGICPFlowEstimationTBB() { guik::async_destroy(); }
std::vector<Eigen::Isometry3d> estimate(std::vector<PointCloud::Ptr>& points) override {
auto async_viewer = guik::async_viewer();
@ -126,7 +126,7 @@ private:
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); });
static auto small_gicp_tbb_flow_registry =
register_odometry("small_gicp_tbb_flow", [](const OdometryEstimationParams& params) { return std::make_shared<SmallGICPFlowEstimationTBB>(params); });
} // namespace small_gicp

70
src/odometry_benchmark_small_vgicp_model_tbb.cpp Normal file
View File

@ -0,0 +1,70 @@
#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>
#include <guik/viewer/async_light_viewer.hpp>
namespace small_gicp {
class SmallVGICPModelOnlineOdometryEstimationTBB : public OnlineOdometryEstimation {
public:
SmallVGICPModelOnlineOdometryEstimationTBB(const OdometryEstimationParams& params)
: OnlineOdometryEstimation(params),
control(tbb::global_control::max_allowed_parallelism, params.num_threads),
T(Eigen::Isometry3d::Identity()) {
sub_viewer = guik::async_viewer()->async_sub_viewer("model points");
}
Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override {
Stopwatch sw;
sw.start();
estimate_covariances_tbb(*points, 10);
if (voxelmap == nullptr) {
voxelmap = std::make_shared<GaussianVoxelMap>(params.voxel_resolution);
voxelmap->insert(*points);
return T;
}
Registration<GICPFactor, ParallelReductionTBB, DistanceRejector, LevenbergMarquardtOptimizer> registration;
auto result = registration.align(*voxelmap, *points, *voxelmap, T);
T = result.T_target_source;
voxelmap->insert(*points, T);
sub_viewer.update_points("current", points->points, guik::FlatOrange(T).set_point_scale(2.0f));
sub_viewer.update_normal_dists("voxelmap", voxelmap->voxel_means(), voxelmap->voxel_covs(), 1.0, guik::Rainbow());
sub_viewer.lookat(T.translation().cast<float>());
sw.stop();
reg_times.push(sw.msec());
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:
guik::AsyncLightViewerContext sub_viewer;
tbb::global_control control;
Summarizer reg_times;
GaussianVoxelMap::Ptr voxelmap;
Eigen::Isometry3d T;
};
static auto small_gicp_model_tbb_registry =
register_odometry("small_vgicp_model_tbb", [](const OdometryEstimationParams& params) { return std::make_shared<SmallVGICPModelOnlineOdometryEstimationTBB>(params); });
} // namespace small_gicp

101
src/small_gicp_test.cpp
View File

@ -1,101 +0,0 @@
#include <iostream>
#include <filesystem>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <small_gicp/util/read_points.hpp>
#include <small_gicp/points/point_cloud.hpp>
#include <small_gicp/points/gaussian_voxelmap.hpp>
#include <small_gicp/ann/kdtree.hpp>
#include <small_gicp/ann/cached_kdtree.hpp>
#include <small_gicp/util/downsampling.hpp>
#include <small_gicp/util/downsampling_tbb.hpp>
#include <small_gicp/util/normal_estimation_omp.hpp>
#include <small_gicp/util/normal_estimation_tbb.hpp>
#include <small_gicp/registration/registration.hpp>
#include <small_gicp/registration/reduction_omp.hpp>
#include <small_gicp/registration/reduction_tbb.hpp>
#include <small_gicp/factors/icp_factor.hpp>
#include <small_gicp/factors/gicp_factor.hpp>
#include <small_gicp/factors/plane_icp_factor.hpp>
#include <glk/io/ply_io.hpp>
#include <glk/normal_distributions.hpp>
#include <guik/viewer/light_viewer.hpp>
#include <easy_profiler.hpp>
int main(int argc, char** argv) {
using namespace small_gicp;
const int num_threads = 8;
// tbb::task_scheduler_init init(num_threads);
const std::string dataset_path = "/home/koide/datasets/velodyne";
std::vector<std::string> filenames;
for (const auto& path : std::filesystem::directory_iterator(dataset_path)) {
if (path.path().extension() != ".bin") {
continue;
}
filenames.emplace_back(path.path().string());
}
std::ranges::sort(filenames);
auto viewer = guik::viewer();
viewer->disable_vsync();
GaussianVoxelMap::Ptr voxelmap;
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
for (const auto& filename : filenames) {
EasyProfiler prof;
prof.push("read_points");
const auto raw_points = read_points(filename);
prof.push("copy");
auto points = std::make_shared<PointCloud>(raw_points);
prof.push("downsample");
points = voxelgrid_sampling_tbb(*points, 0.1);
prof.push("estimate covs");
estimate_covariances_tbb(*points, 10);
std::cout << raw_points.size() << " => " << points->size() << std::endl;
if (voxelmap == nullptr) {
voxelmap = std::make_shared<GaussianVoxelMap>(1.0);
voxelmap->insert(*points, T);
continue;
}
//
prof.push("create_tbb");
Registration<GaussianVoxelMap, PointCloud, GaussianVoxelMap, GICPFactor, NullRejector, ParallelReductionTBB, LevenbergMarquardtOptimizer> registration_tbb;
registration_tbb.optimizer.verbose = true;
prof.push("registration_tbb");
auto result = registration_tbb.align(*voxelmap, *points, *voxelmap, T);
prof.push("update");
T = result.T_target_source;
voxelmap->insert(*points, T);
prof.push("show");
viewer->update_points("current", raw_points[0].data(), sizeof(float) * 4, raw_points.size(), guik::FlatOrange(T).set_point_scale(2.0f));
std::vector<Eigen::Vector4d> means;
std::vector<Eigen::Matrix4d> covs;
for (const auto& voxel : voxelmap->flat_voxels) {
means.emplace_back(voxel.mean);
covs.emplace_back(voxel.cov);
}
viewer->update_normal_dists("target", means, covs, 0.5, guik::Rainbow());
std::cout << "--- T ---" << std::endl << T.matrix() << std::endl;
if (!viewer->spin_once()) {
break;
}
}
return 0;
}

113
src/sort_test.cpp
View File

@ -1,113 +0,0 @@
#include <random>
#include <vector>
#include <fstream>
#include <iostream>
#include <filesystem>
#include <algorithm>
#include <tbb/tbb.h>
#include <fmt/format.h>
#include <small_gicp/util/benchmark.hpp>
#include <small_gicp/util/sort_omp.hpp>
int main(int argc, char** argv) {
std::vector<std::vector<std::pair<std::uint64_t, size_t>>> dataset(1000);
std::cout << "read dataset" << std::endl;
#pragma omp parallel for
for (int i = 0; i < dataset.size(); i++) {
std::ifstream ifs(fmt::format("/tmp/coord_pt_{:06d}.txt", i));
if (!ifs) {
std::cerr << "failed to open " << i << std::endl;
abort();
}
std::vector<std::pair<std::uint64_t, size_t>> coord_pt;
coord_pt.reserve(200000);
std::string line;
while (!ifs.eof() && std::getline(ifs, line) && !line.empty()) {
std::stringstream sst(line);
std::uint64_t coord;
size_t pt;
sst >> coord >> pt;
coord_pt.emplace_back(coord, pt);
}
dataset[i] = std::move(coord_pt);
}
using namespace small_gicp;
Stopwatch sw;
const int num_threads = 8;
tbb::global_control control(tbb::global_control::max_allowed_parallelism, num_threads);
std::vector<std::vector<std::pair<std::uint64_t, size_t>>> omp_sorted_dataset = dataset;
Summarizer omp_times;
sw.start();
for (auto& coord_pt : omp_sorted_dataset) {
quick_sort_omp(
coord_pt.begin(),
coord_pt.end(),
[](const auto& a, const auto& b) { return a.first < b.first; },
num_threads);
sw.lap();
omp_times.push(sw.msec());
}
std::cout << "omp=" << omp_times.str() << std::endl;
std::vector<std::vector<std::pair<std::uint64_t, size_t>>> std_sorted_dataset = dataset;
Summarizer std_times;
sw.start();
for (auto& coord_pt : std_sorted_dataset) {
std::ranges::sort(coord_pt, [](const auto& a, const auto& b) { return a.first < b.first; });
sw.lap();
std_times.push(sw.msec());
}
std::cout << "std=" << std_times.str() << std::endl;
std::vector<std::vector<std::pair<std::uint64_t, size_t>>> stable_sorted_dataset = dataset;
Summarizer stable_times;
sw.start();
for (auto& coord_pt : std_sorted_dataset) {
std::ranges::stable_sort(coord_pt, [](const auto& a, const auto& b) { return a.first < b.first; });
sw.lap();
stable_times.push(sw.msec());
}
std::cout << "stable=" << stable_times.str() << std::endl;
std::vector<std::vector<std::pair<std::uint64_t, size_t>>> tbb_sorted_dataset = dataset;
Summarizer tbb_times;
sw.start();
for (auto& coord_pt : tbb_sorted_dataset) {
tbb::parallel_sort(coord_pt, [](const auto& a, const auto& b) { return a.first < b.first; });
sw.lap();
tbb_times.push(sw.msec());
}
std::cout << "tbb=" << tbb_times.str() << std::endl;
std::cout << "validate" << std::endl;
for (size_t i = 0; i < std_sorted_dataset.size(); i++) {
for (size_t j = 0; j < std_sorted_dataset[i].size(); j++) {
if (std_sorted_dataset[i][j].first != tbb_sorted_dataset[i][j].first) {
std::cerr << "error: " << i << " " << j << std::endl;
abort();
}
if (std_sorted_dataset[i][j].first != omp_sorted_dataset[i][j].first) {
std::cerr << "error: " << i << " " << j << std::endl;
abort();
}
}
}
return 0;
}