mirror of https://github.com/koide3/small_gicp.git
unified incremental voxelmap structure (#11)
* unified incremental voxelmap structure * comments for benchmark
This commit is contained in:
koide3
GitHub
parent
4d9923fd4f
commit
1afd997717
|
|
@ -126,6 +126,7 @@ if(BUILD_BENCHMARKS)
|
|||
src/benchmark/odometry_benchmark_small_vgicp_omp.cpp
|
||||
src/benchmark/odometry_benchmark_small_gicp_tbb.cpp
|
||||
src/benchmark/odometry_benchmark_small_vgicp_tbb.cpp
|
||||
src/benchmark/odometry_benchmark_small_gicp_model_tbb.cpp
|
||||
src/benchmark/odometry_benchmark_small_vgicp_model_tbb.cpp
|
||||
src/benchmark/odometry_benchmark_small_gicp_tbb_flow.cpp
|
||||
src/benchmark/odometry_benchmark.cpp
|
||||
|
|
|
|||
|
|
@ -240,6 +240,13 @@ See [03_registration_template.cpp](src/example/03_registration_template.cpp) fo
|
|||
|
||||
</details>
|
||||
|
||||
### Cookbook
|
||||
|
||||
- [Standard scan-to-scan GICP matching odometry](src/benchmark/odometry_benchmark_small_gicp_omp.cpp)
|
||||
- [Extremely scalable scan-to-scan matching odometry with data flow graph](src/benchmark/odometry_benchmark_small_gicp_tbb_flow.cpp)
|
||||
- [Scan-to-model matching odometry with incremental voxelmap (GICP + iVox)](src/benchmark/odometry_benchmark_small_gicp_model_tbb.cpp)
|
||||
- [Scan-to-model matching odometry with incremental Gaussian voxelmap (VGICP)](src/benchmark/odometry_benchmark_small_vgicp_model_tbb.cpp)
|
||||
|
||||
## Usage (Python) [basic_registration.py](src/example/basic_registration.py)
|
||||
|
||||
<details><summary>Expand</summary>
|
||||
|
|
|
|||
|
|
@ -1,72 +0,0 @@
|
|||
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
|
||||
// SPDX-License-Identifier: MIT
|
||||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <tbb/tbb.h>
|
||||
#include <small_gicp/ann/kdtree.hpp>
|
||||
#include <small_gicp/util/fast_floor.hpp>
|
||||
#include <small_gicp/util/vector3i_hash.hpp>
|
||||
|
||||
namespace small_gicp {
|
||||
|
||||
/// @brief KdTree with voxel-based caching.
|
||||
/// @note This class is usually useless.
|
||||
template <typename PointCloud>
|
||||
class CachedKdTree {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<CachedKdTree>;
|
||||
using ConstPtr = std::shared_ptr<const CachedKdTree>;
|
||||
|
||||
/// @brief Constructor
|
||||
/// @param points Input points
|
||||
/// @param leaf_size Cache voxel resolution
|
||||
CachedKdTree(const std::shared_ptr<const PointCloud>& points, double leaf_size) : inv_leaf_size(1.0 / leaf_size), kdtree(points) {}
|
||||
|
||||
size_t knn_search(const Eigen::Vector4d& pt, size_t k, size_t* k_indices, double* k_sq_dists) const {
|
||||
const Eigen::Vector3i coord = fast_floor(pt * inv_leaf_size).head<3>();
|
||||
|
||||
CacheTable::const_accessor ca;
|
||||
if (cache.find(ca, coord)) {
|
||||
const size_t n = std::min(ca->second.first.size(), k);
|
||||
|
||||
std::copy(ca->second.first.begin(), ca->second.first.begin() + n, k_indices);
|
||||
std::copy(ca->second.second.begin(), ca->second.second.begin() + n, k_sq_dists);
|
||||
return n;
|
||||
}
|
||||
|
||||
const size_t n = kdtree.knn_search(pt, k, k_indices, k_sq_dists);
|
||||
std::vector<size_t> indices(k_indices, k_indices + n);
|
||||
std::vector<double> sq_dists(k_sq_dists, k_sq_dists + n);
|
||||
|
||||
CacheTable::accessor a;
|
||||
if (cache.insert(a, coord)) {
|
||||
a->second.first = std::move(indices);
|
||||
a->second.second = std::move(sq_dists);
|
||||
}
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
public:
|
||||
const double inv_leaf_size;
|
||||
|
||||
using KnnResult = std::pair<std::vector<size_t>, std::vector<double>>;
|
||||
using CacheTable = tbb::concurrent_hash_map<Eigen::Vector3i, KnnResult, XORVector3iHash>;
|
||||
mutable CacheTable cache;
|
||||
|
||||
KdTree<PointCloud> kdtree;
|
||||
};
|
||||
|
||||
namespace traits {
|
||||
|
||||
template <typename PointCloud>
|
||||
struct Traits<CachedKdTree<PointCloud>> {
|
||||
static size_t knn_search(const CachedKdTree<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
|
||||
|
|
@ -0,0 +1,145 @@
|
|||
#pragma once
|
||||
|
||||
#include <queue>
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Geometry>
|
||||
#include <small_gicp/ann/traits.hpp>
|
||||
#include <small_gicp/points/traits.hpp>
|
||||
|
||||
namespace small_gicp {
|
||||
|
||||
/// @brief Flat point container
|
||||
/// @note IncrementalVoxelMap combined with FlastContainer is mostly the same as iVox.
|
||||
/// Bai et al., "Faster-LIO: Lightweight Tightly Coupled Lidar-Inertial Odometry Using Parallel Sparse Incremental Voxels", IEEE RA-L, 2022
|
||||
/// @tparam HasNormals If true, normals are stored
|
||||
/// @tparam HasCovs If true, covariances are stored
|
||||
template <bool HasNormals = false, bool HasCovs = false>
|
||||
struct FlatContainer {
|
||||
public:
|
||||
struct Setting {
|
||||
double min_sq_dist_in_cell = 0.1 * 0.1; ///< Minimum squared distance between points in a cell
|
||||
size_t max_num_points_in_cell = 10; ///< Maximum number of points in a cell
|
||||
};
|
||||
|
||||
/// @brief Constructor
|
||||
FlatContainer() { points.reserve(5); }
|
||||
|
||||
/// @brief Number of points
|
||||
size_t size() const { return points.size(); }
|
||||
|
||||
/// @brief Add a point to the container
|
||||
template <typename PointCloud>
|
||||
void add(const Setting& setting, const Eigen::Vector4d& transformed_pt, const PointCloud& points, size_t i, const Eigen::Isometry3d& T) {
|
||||
if (
|
||||
this->points.size() >= setting.max_num_points_in_cell || //
|
||||
std::any_of(this->points.begin(), this->points.end(), [&](const auto& pt) { return (pt - transformed_pt).squaredNorm() < setting.min_sq_dist_in_cell; }) //
|
||||
) {
|
||||
return;
|
||||
}
|
||||
|
||||
this->points.emplace_back(transformed_pt);
|
||||
if constexpr (HasNormals) {
|
||||
this->normals.emplace_back(T.matrix() * traits::normal(points, i));
|
||||
}
|
||||
if constexpr (HasCovs) {
|
||||
this->covs.emplace_back(T.matrix() * traits::cov(points, i) * T.matrix().transpose());
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Finalize the container (Nothing to do for FlatContainer)
|
||||
void finalize() {}
|
||||
|
||||
/// @brief Find the nearest neighbor
|
||||
/// @param pt Query point
|
||||
/// @param k_index Index of the nearest neighbor
|
||||
/// @param k_sq_dist Squared distance to the nearest neighbor
|
||||
/// @return Number of found points (0 or 1)
|
||||
size_t nearest_neighbor_search(const Eigen::Vector4d& pt, size_t* k_index, double* k_sq_dist) const {
|
||||
if (points.empty()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
size_t min_index = -1;
|
||||
double min_sq_dist = std::numeric_limits<double>::max();
|
||||
|
||||
for (size_t i = 0; i < points.size(); i++) {
|
||||
const double sq_dist = (points[i] - pt).squaredNorm();
|
||||
if (sq_dist < min_sq_dist) {
|
||||
min_index = i;
|
||||
min_sq_dist = sq_dist;
|
||||
}
|
||||
}
|
||||
|
||||
*k_index = min_index;
|
||||
*k_sq_dist = min_sq_dist;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/// @brief Find k nearest neighbors
|
||||
/// @param pt Query point
|
||||
/// @param k Number of neighbors
|
||||
/// @param k_index Indices of nearest neighbors
|
||||
/// @param k_sq_dist Squared distances to nearest neighbors
|
||||
/// @return Number of found points
|
||||
size_t knn_search(const Eigen::Vector4d& pt, int k, size_t* k_index, double* k_sq_dist) const {
|
||||
if (points.empty()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::priority_queue<std::pair<size_t, double>> queue;
|
||||
for (size_t i = 0; i < points.size(); i++) {
|
||||
const double sq_dist = (points[i] - pt).squaredNorm();
|
||||
queue.push({i, sq_dist});
|
||||
if (queue.size() > k) {
|
||||
queue.pop();
|
||||
}
|
||||
}
|
||||
|
||||
const size_t n = queue.size();
|
||||
while (!queue.empty()) {
|
||||
k_index[queue.size() - 1] = queue.top().first;
|
||||
k_sq_dist[queue.size() - 1] = queue.top().second;
|
||||
queue.pop();
|
||||
}
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
public:
|
||||
struct Empty {};
|
||||
|
||||
std::vector<Eigen::Vector4d> points;
|
||||
std::conditional_t<HasNormals, std::vector<Eigen::Vector4d>, Empty> normals;
|
||||
std::conditional_t<HasCovs, std::vector<Eigen::Matrix4d>, Empty> covs;
|
||||
};
|
||||
|
||||
using FlatContainerNormal = FlatContainer<true, false>;
|
||||
using FlatContainerCov = FlatContainer<false, true>;
|
||||
using FlatContainerNormalCov = FlatContainer<true, true>;
|
||||
|
||||
namespace traits {
|
||||
|
||||
template <bool HasNormals, bool HasCovs>
|
||||
struct Traits<FlatContainer<HasNormals, HasCovs>> {
|
||||
static size_t size(const FlatContainer<HasNormals, HasCovs>& container) { return container.size(); }
|
||||
static bool has_points(const FlatContainer<HasNormals, HasCovs>& container) { return container.size(); }
|
||||
static bool has_normals(const FlatContainer<HasNormals, HasCovs>& container) { return HasNormals && container.size(); }
|
||||
static bool has_covs(const FlatContainer<HasNormals, HasCovs>& container) { return HasCovs && container.size(); }
|
||||
|
||||
static const Eigen::Vector4d& point(const FlatContainer<HasNormals, HasCovs>& container, size_t i) { return container.points[i]; }
|
||||
static const Eigen::Vector4d& normal(const FlatContainer<HasNormals, HasCovs>& container, size_t i) { return container.normals[i]; }
|
||||
static const Eigen::Matrix4d& cov(const FlatContainer<HasNormals, HasCovs>& container, size_t i) { return container.covs[i]; }
|
||||
|
||||
static size_t nearest_neighbor_search(const FlatContainer<HasNormals, HasCovs>& container, const Eigen::Vector4d& pt, size_t* k_index, double* k_sq_dist) {
|
||||
return container.nearest_neighbor_search(pt, k_index, k_sq_dist);
|
||||
}
|
||||
|
||||
static size_t knn_search(const FlatContainer<HasNormals, HasCovs>& container, const Eigen::Vector4d& pt, size_t k, size_t* k_index, double* k_sq_dist) {
|
||||
return container.knn_search(pt, k, k_index, k_sq_dist);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace traits
|
||||
|
||||
} // namespace small_gicp
|
||||
|
|
@ -1,228 +0,0 @@
|
|||
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
|
||||
// SPDX-License-Identifier: MIT
|
||||
#pragma once
|
||||
|
||||
#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>
|
||||
#include <small_gicp/util/vector3i_hash.hpp>
|
||||
|
||||
namespace small_gicp {
|
||||
|
||||
struct FlatVoxelInfo {
|
||||
public:
|
||||
FlatVoxelInfo() : coord(0, 0, 0), num_indices(-1), index_loc(0) {}
|
||||
|
||||
Eigen::Vector3i coord;
|
||||
int num_indices;
|
||||
size_t index_loc;
|
||||
};
|
||||
|
||||
struct IndexDistance {
|
||||
bool operator<(const IndexDistance& rhs) const { return distance < rhs.distance; }
|
||||
|
||||
size_t index;
|
||||
double distance;
|
||||
};
|
||||
|
||||
template <typename PointCloud>
|
||||
struct FlatVoxelMap {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<FlatVoxelMap>;
|
||||
using ConstPtr = std::shared_ptr<const FlatVoxelMap>;
|
||||
|
||||
FlatVoxelMap(const std::shared_ptr<const PointCloud>& points, double leaf_size) : inv_leaf_size(1.0 / leaf_size), seek_count(2), points(points) {
|
||||
set_offset_pattern(7);
|
||||
create_table(*points);
|
||||
}
|
||||
~FlatVoxelMap() {}
|
||||
|
||||
void set_offset_pattern(int num_offsets) {
|
||||
offsets.clear();
|
||||
switch (num_offsets) {
|
||||
default:
|
||||
std::cerr << "warning: num_offsets must be 1, 7, or 9 (num_offsets=" << num_offsets << ")" << std::endl;
|
||||
case 1:
|
||||
offsets = {Eigen::Vector3i(0, 0, 0)};
|
||||
return;
|
||||
case 7:
|
||||
offsets = {
|
||||
Eigen::Vector3i(0, 0, 0),
|
||||
Eigen::Vector3i(-1, 0, 0),
|
||||
Eigen::Vector3i(0, -1, 0),
|
||||
Eigen::Vector3i(0, 0, -1),
|
||||
Eigen::Vector3i(1, 0, 0),
|
||||
Eigen::Vector3i(0, 1, 0),
|
||||
Eigen::Vector3i(0, 0, 1)};
|
||||
return;
|
||||
case 27:
|
||||
for (int i = -1; i <= 1; i++) {
|
||||
for (int j = -1; j <= 1; j++) {
|
||||
for (int k = -1; k <= 1; k++) {
|
||||
offsets.emplace_back(i, j, k);
|
||||
}
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
size_t knn_search(const Eigen::Vector4d& pt, size_t k, size_t* k_indices, double* k_sq_dists) const {
|
||||
const auto find_voxel = [&](const Eigen::Vector3i& coord) -> const FlatVoxelInfo* {
|
||||
const size_t hash = XORVector3iHash::hash(coord);
|
||||
for (size_t bucket_index = hash; bucket_index < hash + seek_count; bucket_index++) {
|
||||
const auto& voxel = voxels[bucket_index % voxels.size()];
|
||||
|
||||
if (voxel.num_indices < 0) {
|
||||
return nullptr;
|
||||
} else if (voxel.coord == coord) {
|
||||
return &voxel;
|
||||
}
|
||||
}
|
||||
|
||||
return nullptr;
|
||||
};
|
||||
|
||||
std::vector<IndexDistance> v;
|
||||
v.reserve(k);
|
||||
std::priority_queue<IndexDistance> queue(std::less<IndexDistance>(), std::move(v));
|
||||
|
||||
const Eigen::Vector3i center_coord = fast_floor(pt * inv_leaf_size).head<3>();
|
||||
for (const auto& offset : offsets) {
|
||||
const Eigen::Vector3i coord = center_coord + offset;
|
||||
const auto voxel = find_voxel(coord);
|
||||
if (voxel == nullptr || voxel->num_indices < 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const auto index_begin = indices.data() + voxel->index_loc;
|
||||
for (auto index_itr = index_begin; index_itr != index_begin + voxel->num_indices; index_itr++) {
|
||||
const double sq_dist = (traits::point(*points, *index_itr) - pt).squaredNorm();
|
||||
if (queue.size() < k) {
|
||||
queue.push(IndexDistance{*index_itr, sq_dist});
|
||||
} else if (sq_dist < queue.top().distance) {
|
||||
queue.pop();
|
||||
queue.push(IndexDistance{*index_itr, sq_dist});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const size_t n = queue.size();
|
||||
while (!queue.empty()) {
|
||||
const auto top = queue.top();
|
||||
queue.pop();
|
||||
|
||||
k_indices[queue.size()] = top.index;
|
||||
k_sq_dists[queue.size()] = top.distance;
|
||||
}
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
private:
|
||||
void create_table(const PointCloud& points) {
|
||||
// Here, we assume that the data structure of std::atomic_int64_t is the same as that of std::int64_t.
|
||||
// This is a dangerous assumption. If C++20 is available, should use std::atomic_ref<std::int64_t> instead.
|
||||
static_assert(sizeof(std::atomic_int64_t) == sizeof(std::int64_t), "We assume that std::atomic_int64_t is the same as std::int64_t.");
|
||||
|
||||
const double min_sq_dist_in_cell = 0.05 * 0.05;
|
||||
const int max_points_per_cell = 10;
|
||||
|
||||
const size_t buckets_size = traits::size(points);
|
||||
std::vector<std::atomic_int64_t> assignment_table(max_points_per_cell * buckets_size);
|
||||
memset(assignment_table.data(), -1, sizeof(std::atomic_int64_t) * max_points_per_cell * buckets_size);
|
||||
|
||||
std::vector<Eigen::Vector3i> coords(traits::size(points));
|
||||
tbb::parallel_for(static_cast<size_t>(0), static_cast<size_t>(traits::size(points)), [&](size_t i) {
|
||||
const Eigen::Vector4d pt = traits::point(points, i);
|
||||
const Eigen::Vector3i coord = fast_floor(pt * inv_leaf_size).template head<3>();
|
||||
coords[i] = coord;
|
||||
|
||||
const size_t hash = XORVector3iHash::hash(coord);
|
||||
for (size_t bucket_index = hash; bucket_index < hash + seek_count; bucket_index++) {
|
||||
auto slot_begin = assignment_table.data() + (bucket_index % buckets_size) * max_points_per_cell;
|
||||
|
||||
std::int64_t expected = -1;
|
||||
if (slot_begin->compare_exchange_strong(expected, static_cast<std::int64_t>(i))) {
|
||||
// Succeeded to insert the point in the first slot
|
||||
break;
|
||||
}
|
||||
|
||||
if (coords[expected] != coord) {
|
||||
// If the bucket is already occupied with another voxel coord, try the next bucket
|
||||
continue;
|
||||
}
|
||||
|
||||
const double sq_dist = (traits::point(points, expected) - pt).squaredNorm();
|
||||
if (sq_dist < min_sq_dist_in_cell) {
|
||||
break;
|
||||
}
|
||||
|
||||
for (auto slot = slot_begin + 1; slot != slot_begin + max_points_per_cell; slot++) {
|
||||
std::int64_t expected = -1;
|
||||
if (slot->compare_exchange_strong(expected, static_cast<std::int64_t>(i))) {
|
||||
// Succeeded to insert the point
|
||||
break;
|
||||
}
|
||||
|
||||
const double sq_dist = (traits::point(points, expected) - pt).squaredNorm();
|
||||
if (sq_dist < min_sq_dist_in_cell) {
|
||||
// There already exists a very close point
|
||||
break;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
});
|
||||
|
||||
indices.clear();
|
||||
indices.reserve(buckets_size * max_points_per_cell);
|
||||
|
||||
voxels.clear();
|
||||
voxels.resize(buckets_size);
|
||||
for (size_t i = 0; i < buckets_size; i++) {
|
||||
const auto slot_begin = assignment_table.data() + max_points_per_cell * i;
|
||||
if (*slot_begin < 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
FlatVoxelInfo v;
|
||||
v.coord = coords[*slot_begin];
|
||||
v.index_loc = indices.size();
|
||||
|
||||
const auto slot_end = std::find(slot_begin, slot_begin + max_points_per_cell, -1);
|
||||
v.num_indices = std::distance(slot_begin, slot_end);
|
||||
std::copy(slot_begin, slot_end, std::back_inserter(indices));
|
||||
|
||||
voxels[i] = v;
|
||||
}
|
||||
|
||||
indices.shrink_to_fit();
|
||||
}
|
||||
|
||||
public:
|
||||
const double inv_leaf_size;
|
||||
const int seek_count;
|
||||
std::vector<Eigen::Vector3i> offsets;
|
||||
|
||||
std::shared_ptr<const PointCloud> points;
|
||||
std::vector<FlatVoxelInfo> voxels;
|
||||
std::vector<size_t> indices;
|
||||
};
|
||||
|
||||
namespace traits {
|
||||
|
||||
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);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace traits
|
||||
|
||||
} // namespace small_gicp
|
||||
|
|
@ -2,30 +2,34 @@
|
|||
// SPDX-License-Identifier: MIT
|
||||
#pragma once
|
||||
|
||||
#include <unordered_map>
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include <small_gicp/ann/traits.hpp>
|
||||
#include <small_gicp/points/traits.hpp>
|
||||
#include <small_gicp/util/fast_floor.hpp>
|
||||
#include <small_gicp/util/vector3i_hash.hpp>
|
||||
#include <small_gicp/ann/incremental_voxelmap.hpp>
|
||||
|
||||
namespace small_gicp {
|
||||
|
||||
/// @brief Gaussian Voxel
|
||||
/// @brief Gaussian voxel.
|
||||
struct GaussianVoxel {
|
||||
public:
|
||||
/// @brief Constructor
|
||||
/// @param coord Voxel coordinates
|
||||
GaussianVoxel(const Eigen::Vector3i& coord) : finalized(false), lru(0), num_points(0), coord(coord), mean(Eigen::Vector4d::Zero()), cov(Eigen::Matrix4d::Zero()) {}
|
||||
~GaussianVoxel() {}
|
||||
struct Setting {};
|
||||
|
||||
/// @brief Add a point (Gaussian distribution) to the voxel
|
||||
/// @param mean Mean of the point
|
||||
/// @param cov Covariance of the point
|
||||
/// @param lru LRU cache counter
|
||||
void add(const Eigen::Vector4d& mean, const Eigen::Matrix4d& cov, size_t lru) {
|
||||
/// @brief Constructor
|
||||
GaussianVoxel() : finalized(false), num_points(0), mean(Eigen::Vector4d::Zero()), cov(Eigen::Matrix4d::Zero()) {}
|
||||
|
||||
/// @brief Number of points in the voxel (Always 1 for GaussianVoxel)
|
||||
size_t size() const { return 1; }
|
||||
|
||||
/// @brief Add a point to the voxel
|
||||
/// @param setting Setting
|
||||
/// @param transformed_pt Transformed point mean
|
||||
/// @param points Point cloud
|
||||
/// @param i Index of the point
|
||||
/// @param T Transformation matrix
|
||||
template <typename PointCloud>
|
||||
void add(const Setting& setting, const Eigen::Vector4d& transformed_pt, const PointCloud& points, size_t i, const Eigen::Isometry3d& T) {
|
||||
if (finalized) {
|
||||
this->finalized = false;
|
||||
this->mean *= num_points;
|
||||
|
|
@ -33,12 +37,11 @@ public:
|
|||
}
|
||||
|
||||
num_points++;
|
||||
this->mean += mean;
|
||||
this->cov += cov;
|
||||
this->lru = lru;
|
||||
this->mean += transformed_pt;
|
||||
this->cov += T.matrix() * traits::cov(points, i) * T.matrix().transpose();
|
||||
}
|
||||
|
||||
/// @brief Finalize mean and covariance
|
||||
/// @brief Finalize the voxel mean and covariance
|
||||
void finalize() {
|
||||
if (finalized) {
|
||||
return;
|
||||
|
|
@ -50,125 +53,36 @@ public:
|
|||
}
|
||||
|
||||
public:
|
||||
bool finalized; ///< If true, mean and cov are finalized, otherwise they represent the sum of input points
|
||||
size_t lru; ///< LRU counter
|
||||
size_t num_points; ///< Number of input points
|
||||
Eigen::Vector3i coord; ///< Voxel coordinates
|
||||
|
||||
bool finalized; ///< If true, mean and cov are finalized, otherwise they represent the sum of input points
|
||||
size_t num_points; ///< Number of input points
|
||||
Eigen::Vector4d mean; ///< Mean
|
||||
Eigen::Matrix4d cov; ///< Covariance
|
||||
};
|
||||
|
||||
/// @brief Gaussian VoxelMap.
|
||||
/// This class can be used as PointCloud as well as NearestNeighborSearch.
|
||||
/// It also supports incremental points insertion and LRU-based voxel deletion.
|
||||
struct GaussianVoxelMap {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<GaussianVoxelMap>;
|
||||
using ConstPtr = std::shared_ptr<const GaussianVoxelMap>;
|
||||
|
||||
/// @brief Constructor
|
||||
/// @param leaf_size Voxel resolution
|
||||
GaussianVoxelMap(double leaf_size) : inv_leaf_size(1.0 / leaf_size), lru_horizon(100), lru_clear_cycle(10), lru_counter(0) {}
|
||||
~GaussianVoxelMap() {}
|
||||
|
||||
/// @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::transform(flat_voxels.begin(), flat_voxels.end(), 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::transform(flat_voxels.begin(), flat_voxels.end(), covs.begin(), [](const GaussianVoxel& voxel) { return voxel.cov; });
|
||||
return covs;
|
||||
}
|
||||
|
||||
/// @brief Insert points to the voxelmap
|
||||
/// @param points Input points
|
||||
/// @param T Transformation
|
||||
template <typename PointCloud>
|
||||
void insert(const PointCloud& points, const Eigen::Isometry3d& T = Eigen::Isometry3d::Identity()) {
|
||||
// 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_floor(pt * inv_leaf_size).head<3>();
|
||||
|
||||
auto found = voxels.find(coord);
|
||||
if (found == voxels.end()) {
|
||||
found = voxels.emplace_hint(found, coord, flat_voxels.size());
|
||||
flat_voxels.emplace_back(coord);
|
||||
}
|
||||
|
||||
auto& voxel = flat_voxels[found->second];
|
||||
const Eigen::Matrix4d cov = T.matrix() * traits::cov(points, i) * T.matrix().transpose();
|
||||
voxel.add(pt, cov, lru_counter);
|
||||
}
|
||||
|
||||
if ((++lru_counter) % lru_clear_cycle == 0) {
|
||||
// Remove least recently used voxels
|
||||
auto remove_counter = std::remove_if(flat_voxels.begin(), flat_voxels.end(), [&](const GaussianVoxel& voxel) { return voxel.lru + lru_horizon < lru_counter; });
|
||||
flat_voxels.erase(remove_counter, flat_voxels.end());
|
||||
voxels.clear();
|
||||
|
||||
// Rehash
|
||||
for (size_t i = 0; i < flat_voxels.size(); i++) {
|
||||
voxels[flat_voxels[i].coord] = i;
|
||||
}
|
||||
}
|
||||
|
||||
// Finalize voxel means and covs
|
||||
for (auto& voxel : flat_voxels) {
|
||||
voxel.finalize();
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Find the nearest neighbor
|
||||
size_t nearest_neighbor_search(const Eigen::Vector4d& pt, size_t* k_index, double* k_sq_dist) const {
|
||||
const Eigen::Vector3i coord = fast_floor(pt * inv_leaf_size).head<3>();
|
||||
const auto found = voxels.find(coord);
|
||||
if (found == voxels.end()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
const GaussianVoxel& voxel = flat_voxels[found->second];
|
||||
*k_index = found->second;
|
||||
*k_sq_dist = (voxel.mean - pt).squaredNorm();
|
||||
return 1;
|
||||
}
|
||||
|
||||
public:
|
||||
const double inv_leaf_size; ///< Inverse of the voxel resolution
|
||||
size_t lru_horizon; ///< LRU horizon size
|
||||
size_t lru_clear_cycle; ///< LRU clear cycle
|
||||
size_t lru_counter; ///< LRU counter
|
||||
|
||||
std::vector<GaussianVoxel> flat_voxels; ///< Voxelmap contents
|
||||
std::unordered_map<Eigen::Vector3i, size_t, XORVector3iHash> voxels; ///< Voxel index map
|
||||
};
|
||||
|
||||
namespace traits {
|
||||
|
||||
template <>
|
||||
struct Traits<GaussianVoxelMap> {
|
||||
static size_t size(const GaussianVoxelMap& voxelmap) { return voxelmap.size(); }
|
||||
struct Traits<GaussianVoxel> {
|
||||
static size_t size(const GaussianVoxel& voxel) { return 1; }
|
||||
static bool has_points(const GaussianVoxel& voxel) { return true; }
|
||||
static bool has_covs(const GaussianVoxel& voxel) { return true; }
|
||||
|
||||
static bool has_points(const GaussianVoxelMap& voxelmap) { return !voxelmap.flat_voxels.empty(); }
|
||||
static bool has_covs(const GaussianVoxelMap& voxelmap) { return !voxelmap.flat_voxels.empty(); }
|
||||
static const Eigen::Vector4d& point(const GaussianVoxel& voxel, size_t i) { return voxel.mean; }
|
||||
static const Eigen::Matrix4d& cov(const GaussianVoxel& voxel, size_t i) { return voxel.cov; }
|
||||
|
||||
static const Eigen::Vector4d& point(const GaussianVoxelMap& voxelmap, size_t i) { return voxelmap.flat_voxels[i].mean; }
|
||||
static const Eigen::Matrix4d& cov(const GaussianVoxelMap& voxelmap, size_t i) { return voxelmap.flat_voxels[i].cov; }
|
||||
static size_t nearest_neighbor_search(const GaussianVoxel& voxel, const Eigen::Vector4d& pt, size_t* k_index, double* k_sq_dist) {
|
||||
*k_index = 0;
|
||||
*k_sq_dist = (voxel.mean - pt).squaredNorm();
|
||||
return 1;
|
||||
}
|
||||
|
||||
static size_t nearest_neighbor_search(const GaussianVoxelMap& voxelmap, const Eigen::Vector4d& point, size_t* k_index, double* k_sq_dist) {
|
||||
return voxelmap.nearest_neighbor_search(point, k_index, k_sq_dist);
|
||||
static size_t knn_search(const GaussianVoxel& voxel, const Eigen::Vector4d& pt, size_t k, size_t* k_index, double* k_sq_dist) {
|
||||
return nearest_neighbor_search(voxel, pt, k_index, k_sq_dist);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace traits
|
||||
|
||||
using GaussianVoxelMap = IncrementalVoxelMap<GaussianVoxel>;
|
||||
|
||||
} // namespace small_gicp
|
||||
|
|
|
|||
|
|
@ -0,0 +1,236 @@
|
|||
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
|
||||
// SPDX-License-Identifier: MIT
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <unordered_map>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include <small_gicp/ann/traits.hpp>
|
||||
#include <small_gicp/ann/flat_container.hpp>
|
||||
#include <small_gicp/points/traits.hpp>
|
||||
#include <small_gicp/util/fast_floor.hpp>
|
||||
#include <small_gicp/util/vector3i_hash.hpp>
|
||||
|
||||
namespace small_gicp {
|
||||
|
||||
/// @brief Voxel meta information
|
||||
struct VoxelInfo {
|
||||
public:
|
||||
/// @brief Constructor
|
||||
/// @param coord Voxel coordinate
|
||||
/// @param lru LRU counter for caching
|
||||
VoxelInfo(const Eigen::Vector3i& coord, size_t lru) : lru(lru), coord(coord) {}
|
||||
|
||||
public:
|
||||
size_t lru; ///< Last used time
|
||||
Eigen::Vector3i coord; ///< Voxel coordinate
|
||||
};
|
||||
|
||||
/// @brief Incremental voxelmap.
|
||||
/// This class supports incremental point cloud insertion and LRU-based voxel deletion.
|
||||
/// @note This class can be used as a point cloud as well as a neighbor search.
|
||||
template <typename VoxelContents>
|
||||
struct IncrementalVoxelMap {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<IncrementalVoxelMap>;
|
||||
using ConstPtr = std::shared_ptr<const IncrementalVoxelMap>;
|
||||
|
||||
/// @brief Constructor
|
||||
/// @param leaf_size Voxel size
|
||||
IncrementalVoxelMap(double leaf_size) : inv_leaf_size(1.0 / leaf_size), lru_horizon(100), lru_clear_cycle(10), lru_counter(0) {}
|
||||
|
||||
/// @brief Number of points in the voxelmap
|
||||
size_t size() const { return flat_voxels.size(); }
|
||||
|
||||
/// @brief Insert points to the voxelmap
|
||||
/// @param points Point cloud
|
||||
/// @param T Transformation matrix
|
||||
template <typename PointCloud>
|
||||
void insert(const PointCloud& points, const Eigen::Isometry3d& T = Eigen::Isometry3d::Identity()) {
|
||||
// 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_floor(pt * inv_leaf_size).head<3>();
|
||||
|
||||
auto found = voxels.find(coord);
|
||||
if (found == voxels.end()) {
|
||||
auto voxel = std::make_shared<std::pair<VoxelInfo, VoxelContents>>(VoxelInfo(coord, lru_counter), VoxelContents());
|
||||
|
||||
found = voxels.emplace_hint(found, coord, flat_voxels.size());
|
||||
flat_voxels.emplace_back(voxel);
|
||||
}
|
||||
|
||||
auto& [info, voxel] = *flat_voxels[found->second];
|
||||
info.lru = lru_counter;
|
||||
voxel.add(voxel_setting, pt, points, i, T);
|
||||
}
|
||||
|
||||
if ((++lru_counter) % lru_clear_cycle == 0) {
|
||||
// Remove least recently used voxels
|
||||
auto remove_counter = std::remove_if(flat_voxels.begin(), flat_voxels.end(), [&](const std::shared_ptr<std::pair<VoxelInfo, VoxelContents>>& voxel) {
|
||||
return voxel->first.lru + lru_horizon < lru_counter;
|
||||
});
|
||||
flat_voxels.erase(remove_counter, flat_voxels.end());
|
||||
|
||||
// Rehash
|
||||
voxels.clear();
|
||||
for (size_t i = 0; i < flat_voxels.size(); i++) {
|
||||
voxels[flat_voxels[i]->first.coord] = i;
|
||||
}
|
||||
}
|
||||
|
||||
// Finalize voxel means and covs
|
||||
for (auto& voxel : flat_voxels) {
|
||||
voxel->second.finalize();
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Find the nearest neighbor
|
||||
/// @param pt Query point
|
||||
/// @param index Index of the nearest neighbor
|
||||
/// @param sq_dist Squared distance to the nearest neighbor
|
||||
/// @return Number of found points (0 or 1)
|
||||
size_t nearest_neighbor_search(const Eigen::Vector4d& pt, size_t* index, double* sq_dist) const {
|
||||
const Eigen::Vector3i coord = fast_floor(pt * inv_leaf_size).head<3>();
|
||||
const auto found = voxels.find(coord);
|
||||
if (found == voxels.end()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
const size_t voxel_index = found->second;
|
||||
const auto& voxel = flat_voxels[voxel_index]->second;
|
||||
|
||||
size_t point_index;
|
||||
if (traits::nearest_neighbor_search(voxel, pt, &point_index, sq_dist) == 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
*index = calc_index(voxel_index, point_index);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/// @brief Find k nearest neighbors
|
||||
/// @param pt Query point
|
||||
/// @param k Number of neighbors
|
||||
/// @param k_indices Indices of nearest neighbors
|
||||
/// @param k_sq_dists Squared distances to nearest neighbors
|
||||
/// @return Number of found points
|
||||
size_t knn_search(const Eigen::Vector4d& pt, size_t k, size_t* k_indices, double* k_sq_dists) const {
|
||||
const Eigen::Vector3i coord = fast_floor(pt * inv_leaf_size).head<3>();
|
||||
const auto found = voxels.find(coord);
|
||||
if (found == voxels.end()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
const size_t voxel_index = found->second;
|
||||
const auto& voxel = flat_voxels[voxel_index]->second;
|
||||
|
||||
std::vector<size_t> point_indices(k);
|
||||
std::vector<double> sq_dists(k);
|
||||
const size_t num_found = traits::knn_search(voxel, pt, k, point_indices.data(), sq_dists.data());
|
||||
|
||||
for (size_t i = 0; i < num_found; i++) {
|
||||
k_indices[i] = calc_index(voxel_index, point_indices[i]);
|
||||
k_sq_dists[i] = sq_dists[i];
|
||||
}
|
||||
return num_found;
|
||||
}
|
||||
|
||||
inline size_t calc_index(const size_t voxel_id, const size_t point_id) const { return (voxel_id << point_id_bits) | point_id; }
|
||||
inline size_t voxel_id(const size_t i) const { return i >> point_id_bits; } ///< Extract the point ID from an index
|
||||
inline size_t point_id(const size_t i) const { return i & ((1ul << point_id_bits) - 1); } ///< Extract the voxel ID from an index
|
||||
|
||||
public:
|
||||
static_assert(sizeof(size_t) == 8, "size_t must be 64-bit");
|
||||
static constexpr int point_id_bits = 32; ///< Use the first 32 bits for point id
|
||||
static constexpr int voxel_id_bits = 64 - point_id_bits; ///< Use the remaining bits for voxel id
|
||||
const double inv_leaf_size; ///< Inverse of the voxel size
|
||||
|
||||
size_t lru_horizon; ///< LRU horizon size
|
||||
size_t lru_clear_cycle; ///< LRU clear cycle
|
||||
size_t lru_counter; ///< LRU counter
|
||||
|
||||
typename VoxelContents::Setting voxel_setting; ///< Voxel setting
|
||||
std::vector<std::shared_ptr<std::pair<VoxelInfo, VoxelContents>>> flat_voxels; ///< Voxel contents
|
||||
std::unordered_map<Eigen::Vector3i, size_t, XORVector3iHash> voxels; ///< Voxel index map
|
||||
};
|
||||
|
||||
namespace traits {
|
||||
|
||||
template <typename VoxelContents>
|
||||
struct Traits<IncrementalVoxelMap<VoxelContents>> {
|
||||
static size_t size(const IncrementalVoxelMap<VoxelContents>& voxelmap) { return voxelmap.size(); }
|
||||
|
||||
static Eigen::Vector4d point(const IncrementalVoxelMap<VoxelContents>& voxelmap, size_t i) {
|
||||
const size_t voxel_id = voxelmap.voxel_id(i);
|
||||
const size_t point_id = voxelmap.point_id(i);
|
||||
const auto& voxel = voxelmap.flat_voxels[voxel_id]->second;
|
||||
return traits::point(voxel, point_id);
|
||||
}
|
||||
static Eigen::Vector4d normal(const IncrementalVoxelMap<VoxelContents>& voxelmap, size_t i) {
|
||||
const size_t voxel_id = voxelmap.voxel_id(i);
|
||||
const size_t point_id = voxelmap.point_id(i);
|
||||
const auto& voxel = voxelmap.flat_voxels[voxel_id]->second;
|
||||
return traits::normal(voxel, point_id);
|
||||
}
|
||||
static Eigen::Matrix4d cov(const IncrementalVoxelMap<VoxelContents>& voxelmap, size_t i) {
|
||||
const size_t voxel_id = voxelmap.voxel_id(i);
|
||||
const size_t point_id = voxelmap.point_id(i);
|
||||
const auto& voxel = voxelmap.flat_voxels[voxel_id]->second;
|
||||
return traits::cov(voxel, point_id);
|
||||
}
|
||||
|
||||
static size_t nearest_neighbor_search(const IncrementalVoxelMap<VoxelContents>& voxelmap, const Eigen::Vector4d& pt, size_t* k_index, double* k_sq_dist) {
|
||||
return voxelmap.nearest_neighbor_search(pt, k_index, k_sq_dist);
|
||||
}
|
||||
|
||||
static size_t knn_search(const IncrementalVoxelMap<VoxelContents>& voxelmap, const Eigen::Vector4d& pt, int k, size_t* k_index, double* k_sq_dist) {
|
||||
return voxelmap.knn_search(pt, k, k_index, k_sq_dist);
|
||||
}
|
||||
};
|
||||
|
||||
template <typename VoxelContents>
|
||||
std::vector<Eigen::Vector4d> voxel_points(const IncrementalVoxelMap<VoxelContents>& voxelmap) {
|
||||
std::vector<Eigen::Vector4d> points;
|
||||
points.reserve(voxelmap.size() * 5);
|
||||
|
||||
for (const auto& voxel : voxelmap.flat_voxels) {
|
||||
for (size_t i = 0; i < traits::size(voxel->second); i++) {
|
||||
points.push_back(traits::point(voxel->second, i));
|
||||
}
|
||||
}
|
||||
return points;
|
||||
}
|
||||
|
||||
template <typename VoxelContents>
|
||||
std::vector<Eigen::Vector4d> voxel_normals(const IncrementalVoxelMap<VoxelContents>& voxelmap) {
|
||||
std::vector<Eigen::Vector4d> normals;
|
||||
normals.reserve(voxelmap.size() * 5);
|
||||
|
||||
for (const auto& voxel : voxelmap.flat_voxels) {
|
||||
for (size_t i = 0; i < traits::size(voxel->second); i++) {
|
||||
normals.push_back(traits::normal(voxel->second, i));
|
||||
}
|
||||
}
|
||||
return normals;
|
||||
}
|
||||
|
||||
template <typename VoxelContents>
|
||||
std::vector<Eigen::Matrix4d> voxel_covs(const IncrementalVoxelMap<VoxelContents>& voxelmap) {
|
||||
std::vector<Eigen::Matrix4d> covs;
|
||||
covs.reserve(voxelmap.size() * 5);
|
||||
|
||||
for (const auto& voxel : voxelmap.flat_voxels) {
|
||||
for (size_t i = 0; i < traits::size(voxel->second); i++) {
|
||||
covs.push_back(traits::cov(voxel->second, i));
|
||||
}
|
||||
}
|
||||
return covs;
|
||||
}
|
||||
|
||||
} // namespace traits
|
||||
|
||||
} // namespace small_gicp
|
||||
|
|
@ -81,11 +81,32 @@ public:
|
|||
return traj;
|
||||
}
|
||||
|
||||
void update_model_points(const Eigen::Isometry3d& T, const std::vector<Eigen::Vector4d>& points) {
|
||||
if (!params.visualize) {
|
||||
return;
|
||||
}
|
||||
|
||||
#ifdef BUILD_WITH_IRIDESCENCE
|
||||
if (!async_sub_initialized) {
|
||||
async_sub_initialized = true;
|
||||
async_sub = guik::async_viewer()->async_sub_viewer("model");
|
||||
}
|
||||
|
||||
async_sub.update_points("model", points, guik::Rainbow());
|
||||
async_sub.lookat(T.translation().cast<float>());
|
||||
#endif
|
||||
}
|
||||
|
||||
virtual Eigen::Isometry3d estimate(const PointCloud::Ptr& points) = 0;
|
||||
|
||||
protected:
|
||||
Eigen::Vector2f z_range;
|
||||
Summarizer total_times;
|
||||
|
||||
#ifdef BUILD_WITH_IRIDESCENCE
|
||||
bool async_sub_initialized = false;
|
||||
guik::AsyncLightViewerContext async_sub;
|
||||
#endif
|
||||
};
|
||||
|
||||
size_t register_odometry(const std::string& name, std::function<OdometryEstimation::Ptr(const OdometryEstimationParams&)> factory);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,74 @@
|
|||
#ifdef BUILD_WITH_TBB
|
||||
|
||||
#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 SmallGICPModelOnlineOdometryEstimationTBB : public OnlineOdometryEstimation {
|
||||
public:
|
||||
SmallGICPModelOnlineOdometryEstimationTBB(const OdometryEstimationParams& params)
|
||||
: OnlineOdometryEstimation(params),
|
||||
control(tbb::global_control::max_allowed_parallelism, params.num_threads),
|
||||
T_world_lidar(Eigen::Isometry3d::Identity()) {}
|
||||
|
||||
Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override {
|
||||
Stopwatch sw;
|
||||
sw.start();
|
||||
|
||||
// Estimate per-point covariances
|
||||
estimate_covariances_tbb(*points, params.num_neighbors);
|
||||
|
||||
if (voxelmap == nullptr) {
|
||||
// This is the very first frame
|
||||
voxelmap = std::make_shared<IncrementalVoxelMap<FlatContainerCov>>(params.voxel_resolution);
|
||||
voxelmap->insert(*points);
|
||||
return T_world_lidar;
|
||||
}
|
||||
|
||||
// Registration using GICP + TBB-based parallel reduction
|
||||
Registration<GICPFactor, ParallelReductionTBB> registration;
|
||||
auto result = registration.align(*voxelmap, *points, *voxelmap, T_world_lidar);
|
||||
|
||||
// Update T_world_lidar with the estimated transformation
|
||||
T_world_lidar = result.T_target_source;
|
||||
|
||||
// Insert points to the target voxel map
|
||||
voxelmap->insert(*points, T_world_lidar);
|
||||
|
||||
sw.stop();
|
||||
reg_times.push(sw.msec());
|
||||
|
||||
if (params.visualize) {
|
||||
update_model_points(T_world_lidar, traits::voxel_points(*voxelmap));
|
||||
}
|
||||
|
||||
return T_world_lidar;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
IncrementalVoxelMap<FlatContainerCov>::Ptr voxelmap; // Target voxel map that is an accumulation of past point clouds
|
||||
Eigen::Isometry3d T_world_lidar; // Current world-to-lidar transformation
|
||||
};
|
||||
|
||||
static auto small_gicp_model_tbb_registry =
|
||||
register_odometry("small_gicp_model_tbb", [](const OdometryEstimationParams& params) { return std::make_shared<SmallGICPModelOnlineOdometryEstimationTBB>(params); });
|
||||
|
||||
} // namespace small_gicp
|
||||
|
||||
#endif
|
||||
|
|
@ -11,35 +11,41 @@ namespace small_gicp {
|
|||
|
||||
class SmallGICPOnlineOdometryEstimationOMP : public OnlineOdometryEstimation {
|
||||
public:
|
||||
SmallGICPOnlineOdometryEstimationOMP(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) {}
|
||||
SmallGICPOnlineOdometryEstimationOMP(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T_world_lidar(Eigen::Isometry3d::Identity()) {}
|
||||
|
||||
Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override {
|
||||
Stopwatch sw;
|
||||
sw.start();
|
||||
|
||||
// Preprocess input points (kdtree construction & covariance estimation)
|
||||
// Note that input points here is already downsampled
|
||||
auto tree = std::make_shared<KdTreeOMP<PointCloud>>(points, params.num_threads);
|
||||
estimate_covariances_omp(*points, *tree, params.num_neighbors, params.num_threads);
|
||||
|
||||
if (target_points == nullptr) {
|
||||
// This is the very first frame
|
||||
target_points = points;
|
||||
target_tree = tree;
|
||||
return T;
|
||||
return T_world_lidar;
|
||||
}
|
||||
|
||||
// Registration using GICP + OMP-based parallel reduction
|
||||
Registration<GICPFactor, ParallelReductionOMP> registration;
|
||||
registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance;
|
||||
registration.reduction.num_threads = params.num_threads;
|
||||
|
||||
// Perform registration
|
||||
auto result = registration.align(*target_points, *points, *target_tree, Eigen::Isometry3d::Identity());
|
||||
|
||||
// Update T_world_lidar and target points
|
||||
T_world_lidar = T_world_lidar * result.T_target_source;
|
||||
target_points = points;
|
||||
target_tree = tree;
|
||||
|
||||
sw.stop();
|
||||
reg_times.push(sw.msec());
|
||||
|
||||
T = T * result.T_target_source;
|
||||
target_points = points;
|
||||
target_tree = tree;
|
||||
|
||||
return T;
|
||||
return T_world_lidar;
|
||||
}
|
||||
|
||||
void report() override { //
|
||||
|
|
@ -49,10 +55,10 @@ public:
|
|||
private:
|
||||
Summarizer reg_times;
|
||||
|
||||
PointCloud::Ptr target_points;
|
||||
KdTreeOMP<PointCloud>::Ptr target_tree;
|
||||
PointCloud::Ptr target_points; // Last point cloud to be registration target
|
||||
KdTreeOMP<PointCloud>::Ptr target_tree; // KdTree of the last point cloud
|
||||
|
||||
Eigen::Isometry3d T;
|
||||
Eigen::Isometry3d T_world_lidar; // T_world_lidar
|
||||
};
|
||||
|
||||
static auto small_gicp_omp_registry =
|
||||
|
|
|
|||
|
|
@ -15,16 +15,16 @@ class SmallGICPFlowEstimationTBB : 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;
|
||||
size_t id; // Frame sequential ID
|
||||
PointCloud::Ptr points; // Input point cloud
|
||||
KdTree<PointCloud>::Ptr kdtree; // KdTree for the input point cloud
|
||||
Eigen::Isometry3d T_last_current; // Transformation from the last frame to the current frame
|
||||
Stopwatch sw; // Stopwatch for performance measurement
|
||||
};
|
||||
|
||||
struct InputFramePair {
|
||||
InputFrame::Ptr target;
|
||||
InputFrame::Ptr source;
|
||||
InputFrame::Ptr target; // Target frame
|
||||
InputFrame::Ptr source; // Source frame
|
||||
};
|
||||
|
||||
SmallGICPFlowEstimationTBB(const OdometryEstimationParams& params)
|
||||
|
|
@ -48,38 +48,57 @@ public:
|
|||
}
|
||||
|
||||
std::vector<Eigen::Isometry3d> estimate(std::vector<PointCloud::Ptr>& points) override {
|
||||
// Output trajectory
|
||||
std::vector<Eigen::Isometry3d> traj;
|
||||
traj.reserve(points.size());
|
||||
|
||||
tbb::flow::graph graph;
|
||||
|
||||
// Input node
|
||||
tbb::flow::broadcast_node<InputFrame::Ptr> input_node(graph);
|
||||
|
||||
// Preprocessing node
|
||||
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.downsampling_resolution);
|
||||
input->kdtree = std::make_shared<KdTree<PointCloud>>(input->points);
|
||||
estimate_covariances(*input->points, *input->kdtree, params.num_neighbors);
|
||||
input->points = voxelgrid_sampling(*input->points, params.downsampling_resolution); // Downsampling
|
||||
input->kdtree = std::make_shared<KdTree<PointCloud>>(input->points); // KdTree construction
|
||||
estimate_covariances(*input->points, *input->kdtree, params.num_neighbors); // Covariance estimation
|
||||
return input;
|
||||
});
|
||||
|
||||
// Make preprocessed frames sequenced
|
||||
tbb::flow::sequencer_node<InputFrame::Ptr> postpre_sequencer_node(graph, [](const InputFrame::Ptr& input) { return input->id; });
|
||||
|
||||
// Make pairs of consecutive frames
|
||||
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;
|
||||
});
|
||||
|
||||
// Perform registration between the pair of frames
|
||||
tbb::flow::function_node<InputFramePair, InputFrame::Ptr> registration_node(graph, tbb::flow::unlimited, [&](const InputFramePair& pair) {
|
||||
if (pair.target == nullptr) {
|
||||
// This is the very first frame
|
||||
pair.source->T_last_current.setIdentity();
|
||||
return pair.source;
|
||||
}
|
||||
|
||||
// GICP + Single-thread reduction
|
||||
Registration<GICPFactor, SerialReduction> registration;
|
||||
registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance;
|
||||
|
||||
// Align points
|
||||
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;
|
||||
});
|
||||
|
||||
// Make registered frames sequenced
|
||||
tbb::flow::sequencer_node<InputFrame::Ptr> postreg_sequencer_node(graph, [](const InputFrame::Ptr& input) { return input->id; });
|
||||
|
||||
// Output node
|
||||
tbb::flow::function_node<InputFrame::Ptr> output_node(graph, 1, [&](const InputFrame::Ptr& input) {
|
||||
if (traj.empty()) {
|
||||
traj.emplace_back(input->T_last_current);
|
||||
|
|
@ -113,6 +132,7 @@ public:
|
|||
#endif
|
||||
});
|
||||
|
||||
// Connect nodes
|
||||
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);
|
||||
|
|
@ -122,14 +142,20 @@ public:
|
|||
|
||||
Stopwatch sw;
|
||||
sw.start();
|
||||
|
||||
// Feed input frames to the graph
|
||||
for (size_t i = 0; i < points.size(); i++) {
|
||||
auto frame = InputFrame::Ptr(new InputFrame);
|
||||
frame->id = i;
|
||||
frame->points = points[i];
|
||||
|
||||
// Feed the input frame to the input node
|
||||
if (!input_node.try_put(frame)) {
|
||||
std::cerr << "failed to input!!" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// Wait for the graph to finish
|
||||
graph.wait_for_all();
|
||||
|
||||
sw.stop();
|
||||
|
|
|
|||
|
|
@ -17,30 +17,40 @@ public:
|
|||
SmallVGICPModelOnlineOdometryEstimationTBB(const OdometryEstimationParams& params)
|
||||
: OnlineOdometryEstimation(params),
|
||||
control(tbb::global_control::max_allowed_parallelism, params.num_threads),
|
||||
T(Eigen::Isometry3d::Identity()) {}
|
||||
T_world_lidar(Eigen::Isometry3d::Identity()) {}
|
||||
|
||||
Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override {
|
||||
Stopwatch sw;
|
||||
sw.start();
|
||||
|
||||
// Estimate per-point covariances
|
||||
estimate_covariances_tbb(*points, params.num_neighbors);
|
||||
|
||||
if (voxelmap == nullptr) {
|
||||
// This is the very first frame
|
||||
voxelmap = std::make_shared<GaussianVoxelMap>(params.voxel_resolution);
|
||||
voxelmap->insert(*points);
|
||||
return T;
|
||||
return T_world_lidar;
|
||||
}
|
||||
|
||||
// Registration using GICP + TBB-based parallel reduction
|
||||
Registration<GICPFactor, ParallelReductionTBB> registration;
|
||||
auto result = registration.align(*voxelmap, *points, *voxelmap, T);
|
||||
auto result = registration.align(*voxelmap, *points, *voxelmap, T_world_lidar);
|
||||
|
||||
T = result.T_target_source;
|
||||
voxelmap->insert(*points, T);
|
||||
// Update T_world_lidar with the estimated transformation
|
||||
T_world_lidar = result.T_target_source;
|
||||
|
||||
// Insert points to the target voxel map
|
||||
voxelmap->insert(*points, T_world_lidar);
|
||||
|
||||
sw.stop();
|
||||
reg_times.push(sw.msec());
|
||||
|
||||
return T;
|
||||
if (params.visualize) {
|
||||
update_model_points(T_world_lidar, traits::voxel_points(*voxelmap));
|
||||
}
|
||||
|
||||
return T_world_lidar;
|
||||
}
|
||||
|
||||
void report() override { //
|
||||
|
|
@ -52,8 +62,8 @@ private:
|
|||
|
||||
Summarizer reg_times;
|
||||
|
||||
GaussianVoxelMap::Ptr voxelmap;
|
||||
Eigen::Isometry3d T;
|
||||
GaussianVoxelMap::Ptr voxelmap; // Target voxel map that is an accumulation of past point clouds
|
||||
Eigen::Isometry3d T_world_lidar; // Current world-to-lidar transformation
|
||||
};
|
||||
|
||||
static auto small_gicp_model_tbb_registry =
|
||||
|
|
|
|||
|
|
@ -7,8 +7,8 @@
|
|||
#include <small_gicp/ann/kdtree.hpp>
|
||||
#include <small_gicp/ann/kdtree_omp.hpp>
|
||||
#include <small_gicp/ann/kdtree_tbb.hpp>
|
||||
#include <small_gicp/ann/cached_kdtree.hpp>
|
||||
#include <small_gicp/ann/flat_voxelmap.hpp>
|
||||
#include <small_gicp/ann/gaussian_voxelmap.hpp>
|
||||
#include <small_gicp/ann/incremental_voxelmap.hpp>
|
||||
#include <small_gicp/util/downsampling.hpp>
|
||||
#include <small_gicp/points/point_cloud.hpp>
|
||||
#include <small_gicp/pcl/pcl_point_traits.hpp>
|
||||
|
|
@ -77,7 +77,7 @@ public:
|
|||
}
|
||||
|
||||
template <typename PointCloud, typename KdTree>
|
||||
void test_knn_(const PointCloud& points, const KdTree& tree, bool strict = true) {
|
||||
void test_kdtree(const PointCloud& points, const KdTree& tree) {
|
||||
for (size_t i = 0; i < queries.size(); i++) {
|
||||
// k-nearest neighbors search
|
||||
const auto& query = queries[i];
|
||||
|
|
@ -85,12 +85,10 @@ public:
|
|||
std::vector<double> sq_dists(k);
|
||||
const size_t num_results = traits::knn_search(tree, query, k, indices.data(), sq_dists.data());
|
||||
|
||||
if (strict) {
|
||||
EXPECT_EQ(num_results, k) << "num_neighbors must be k";
|
||||
for (size_t j = 0; j < k; j++) {
|
||||
EXPECT_EQ(indices[j], k_indices[i][j]);
|
||||
EXPECT_NEAR(sq_dists[j], k_sq_dists[i][j], 1e-3);
|
||||
}
|
||||
EXPECT_EQ(num_results, k) << "num_neighbors must be k";
|
||||
for (size_t j = 0; j < k; j++) {
|
||||
EXPECT_EQ(indices[j], k_indices[i][j]);
|
||||
EXPECT_NEAR(sq_dists[j], k_sq_dists[i][j], 1e-3);
|
||||
}
|
||||
|
||||
// Nearest neighbor search
|
||||
|
|
@ -98,12 +96,45 @@ public:
|
|||
double k_sq_dist;
|
||||
const size_t found = traits::nearest_neighbor_search(tree, query, &k_index, &k_sq_dist);
|
||||
|
||||
if (strict) {
|
||||
EXPECT_EQ(found, 1) << "num_neighbors must be 1";
|
||||
EXPECT_EQ(k_index, k_indices[i][0]);
|
||||
EXPECT_NEAR(k_sq_dist, k_sq_dists[i][0], 1e-3);
|
||||
EXPECT_EQ(found, 1) << "num_neighbors must be 1";
|
||||
EXPECT_EQ(k_index, k_indices[i][0]);
|
||||
EXPECT_NEAR(k_sq_dist, k_sq_dists[i][0], 1e-3);
|
||||
}
|
||||
}
|
||||
|
||||
template <typename PointCloud, typename VoxelMap>
|
||||
void test_voxelmap(const PointCloud& points, const VoxelMap& voxelmap) {
|
||||
size_t hit_count = 0;
|
||||
for (size_t i = 0; i < queries.size(); i++) {
|
||||
// k-nearest neighbors search
|
||||
const auto& query = queries[i];
|
||||
std::vector<size_t> indices(k);
|
||||
std::vector<double> sq_dists(k);
|
||||
const size_t num_results = traits::knn_search(voxelmap, query, k, indices.data(), sq_dists.data());
|
||||
|
||||
EXPECT_LE(num_results, k) << "num_neighbors must be less than or equal to k";
|
||||
for (size_t j = 0; j < num_results; j++) {
|
||||
const Eigen::Vector4d pt = traits::point(voxelmap, indices[j]);
|
||||
const double sq_dist = (pt - query).squaredNorm();
|
||||
EXPECT_NEAR(sq_dists[j], sq_dist, 1e-3);
|
||||
}
|
||||
|
||||
// Nearest neighbor search
|
||||
size_t nn_index;
|
||||
double nn_sq_dist;
|
||||
const size_t found = traits::nearest_neighbor_search(voxelmap, query, &nn_index, &nn_sq_dist);
|
||||
|
||||
EXPECT_LE(found, 1) << "num_neighbors must be less than or equal to 1";
|
||||
if (found) {
|
||||
const Eigen::Vector4d pt = traits::point(voxelmap, nn_index);
|
||||
const double sq_dist = (pt - query).squaredNorm();
|
||||
EXPECT_NEAR(nn_sq_dist, sq_dist, 1e-3);
|
||||
hit_count++;
|
||||
}
|
||||
}
|
||||
|
||||
const double net_tp = queries.size() * 2.0 / 3.0;
|
||||
EXPECT_GE(hit_count, net_tp * 0.5) << "Hit_count must be greater than or equal to " << net_tp;
|
||||
}
|
||||
|
||||
protected:
|
||||
|
|
@ -131,7 +162,7 @@ TEST_F(KdTreeTest, LoadCheck) {
|
|||
}
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(KdTreeTest, KdTreeTest, testing::Values("SMALL", "TBB", "OMP", "CACHED", "FLAT"), [](const auto& info) { return info.param; });
|
||||
INSTANTIATE_TEST_SUITE_P(KdTreeTest, KdTreeTest, testing::Values("SMALL", "TBB", "OMP", "IVOX", "GVOX"), [](const auto& info) { return info.param; });
|
||||
|
||||
// Check if kdtree works correctly for empty points
|
||||
TEST_P(KdTreeTest, EmptyTest) {
|
||||
|
|
@ -147,44 +178,34 @@ TEST_P(KdTreeTest, KnnTest) {
|
|||
const auto method = GetParam();
|
||||
if (method == "SMALL") {
|
||||
auto kdtree = std::make_shared<KdTree<PointCloud>>(points);
|
||||
test_knn_(*points, *kdtree);
|
||||
test_kdtree(*points, *kdtree);
|
||||
|
||||
auto kdtree_pcl = std::make_shared<KdTree<pcl::PointCloud<pcl::PointXYZ>>>(points_pcl);
|
||||
test_knn_(*points_pcl, *kdtree_pcl);
|
||||
test_kdtree(*points_pcl, *kdtree_pcl);
|
||||
} else if (method == "TBB") {
|
||||
auto kdtree = std::make_shared<KdTreeTBB<PointCloud>>(points);
|
||||
test_knn_(*points, *kdtree);
|
||||
test_kdtree(*points, *kdtree);
|
||||
|
||||
auto kdtree_pcl = std::make_shared<KdTreeTBB<pcl::PointCloud<pcl::PointXYZ>>>(points_pcl);
|
||||
test_knn_(*points_pcl, *kdtree_pcl);
|
||||
test_kdtree(*points_pcl, *kdtree_pcl);
|
||||
} else if (method == "OMP") {
|
||||
auto kdtree = std::make_shared<KdTreeOMP<PointCloud>>(points, 4);
|
||||
test_knn_(*points, *kdtree);
|
||||
test_kdtree(*points, *kdtree);
|
||||
|
||||
auto kdtree_pcl = std::make_shared<KdTreeOMP<pcl::PointCloud<pcl::PointXYZ>>>(points_pcl, 4);
|
||||
test_knn_(*points_pcl, *kdtree_pcl);
|
||||
} else if (method == "CACHED") {
|
||||
// This is approximated and no guarantee about the search result
|
||||
const bool strict = false;
|
||||
test_kdtree(*points_pcl, *kdtree_pcl);
|
||||
} else if (method == "IVOX") {
|
||||
auto voxelmap = std::make_shared<IncrementalVoxelMap<FlatContainerNormalCov>>(1.0);
|
||||
voxelmap->insert(*points);
|
||||
test_voxelmap(*points, *voxelmap);
|
||||
|
||||
auto kdtree = std::make_shared<CachedKdTree<PointCloud>>(points, 0.025);
|
||||
test_knn_(*points, *kdtree, strict);
|
||||
|
||||
auto kdtree_pcl = std::make_shared<CachedKdTree<pcl::PointCloud<pcl::PointXYZ>>>(points_pcl, 0.025);
|
||||
test_knn_(*points_pcl, *kdtree_pcl, strict);
|
||||
} else if (method == "FLAT") {
|
||||
// This is approximated and no guarantee about the search result
|
||||
const bool strict = false;
|
||||
|
||||
auto kdtree = std::make_shared<FlatVoxelMap<PointCloud>>(points, 0.5);
|
||||
kdtree->set_offset_pattern(1);
|
||||
kdtree->set_offset_pattern(7);
|
||||
kdtree->set_offset_pattern(27);
|
||||
test_knn_(*points, *kdtree, strict);
|
||||
|
||||
auto kdtree_pcl = std::make_shared<FlatVoxelMap<pcl::PointCloud<pcl::PointXYZ>>>(points_pcl, 0.5);
|
||||
kdtree_pcl->set_offset_pattern(27);
|
||||
test_knn_(*points_pcl, *kdtree_pcl, strict);
|
||||
auto voxelmap_pcl = std::make_shared<IncrementalVoxelMap<FlatContainer<>>>(1.0);
|
||||
voxelmap_pcl->insert(*points_pcl);
|
||||
test_voxelmap(*points, *voxelmap_pcl);
|
||||
} else if (method == "GVOX") {
|
||||
auto voxelmap = std::make_shared<GaussianVoxelMap>(1.0);
|
||||
voxelmap->insert(*points);
|
||||
test_voxelmap(*points, *voxelmap);
|
||||
} else {
|
||||
throw std::runtime_error("Invalid method: " + method);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue