-
Notifications
You must be signed in to change notification settings - Fork 25
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Introduce Bonxai as Map representation #22
base: main
Are you sure you want to change the base?
Changes from all commits
7a19274
0084554
4adc4e0
01188b4
be18fae
11b9c5a
a7ddd20
7e9726f
6ed4c01
c4c1d90
2af1115
b76abda
f456e76
bbb3ecc
57977f1
45c1b2b
9859082
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Large diffs are not rendered by default.
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,32 @@ | ||
# MIT License | ||
# | ||
# Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill | ||
# Stachniss. | ||
# | ||
# Permission is hereby granted, free of charge, to any person obtaining a copy | ||
# of this software and associated documentation files (the "Software"), to deal | ||
# in the Software without restriction, including without limitation the rights | ||
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
# copies of the Software, and to permit persons to whom the Software is | ||
# furnished to do so, subject to the following conditions: | ||
# | ||
# The above copyright notice and this permission notice shall be included in all | ||
# copies or substantial portions of the Software. | ||
# | ||
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE | ||
# SOFTWARE. | ||
# Silence timestamp warning | ||
if(CMAKE_VERSION VERSION_GREATER 3.24) | ||
cmake_policy(SET CMP0135 OLD) | ||
endif() | ||
|
||
include(FetchContent) | ||
FetchContent_Declare( | ||
bonxai URL https://github.com/facontidavide/Bonxai/archive/refs/tags/v0.6.0.tar.gz SOURCE_SUBDIR bonxai_core | ||
PATCH_COMMAND patch -p1 < ${CMAKE_CURRENT_LIST_DIR}/bonxai.patch UPDATE_DISCONNECTED 1) | ||
FetchContent_MakeAvailable(bonxai) |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,52 @@ | ||
diff --git a/bonxai_core/include/bonxai/bonxai.hpp b/bonxai_core/include/bonxai/bonxai.hpp | ||
index 7d360cf..e43771e 100644 | ||
--- a/bonxai_core/include/bonxai/bonxai.hpp | ||
+++ b/bonxai_core/include/bonxai/bonxai.hpp | ||
@@ -367,7 +367,7 @@ inline size_t Grid<DataT>::memUsage() const { | ||
template <typename DataT> | ||
inline void VoxelGrid<DataT>::releaseUnusedMemory() { | ||
std::vector<CoordT> keys_to_delete; | ||
- for (const auto& [key, inner_grid] : root_map) { | ||
+ for (auto& [key, inner_grid] : root_map) { | ||
for (auto inner_it = inner_grid.mask().beginOn(); inner_it; ++inner_it) { | ||
const int32_t inner_index = *inner_it; | ||
auto& leaf_grid = inner_grid.cell(inner_index); | ||
@@ -404,9 +404,9 @@ inline VoxelGrid<DataT>::VoxelGrid(double voxel_size, uint8_t inner_bits, uint8_ | ||
template <typename DataT> | ||
inline CoordT VoxelGrid<DataT>::posToCoord(double x, double y, double z) const { | ||
return { | ||
- static_cast<int32_t>(std::nearbyint(x * inv_resolution)), | ||
nachovizzo marked this conversation as resolved.
Show resolved
Hide resolved
|
||
- static_cast<int32_t>(std::nearbyint(y * inv_resolution)), | ||
- static_cast<int32_t>(std::nearbyint(z * inv_resolution))}; | ||
+ static_cast<int32_t>(std::floor(x * inv_resolution)), | ||
+ static_cast<int32_t>(std::floor(y * inv_resolution)), | ||
+ static_cast<int32_t>(std::floor(z * inv_resolution))}; | ||
} | ||
|
||
template <typename DataT> | ||
diff --git a/bonxai_core/include/bonxai/grid_coord.hpp b/bonxai_core/include/bonxai/grid_coord.hpp | ||
index da8fb7d..1f9727b 100644 | ||
--- a/bonxai_core/include/bonxai/grid_coord.hpp | ||
+++ b/bonxai_core/include/bonxai/grid_coord.hpp | ||
@@ -79,9 +79,9 @@ struct CoordT { | ||
|
||
[[nodiscard]] inline CoordT PosToCoord(const Point3D& point, double inv_resolution) { | ||
return { | ||
- static_cast<int32_t>(std::nearbyint(point.x * inv_resolution)), | ||
- static_cast<int32_t>(std::nearbyint(point.y * inv_resolution)), | ||
- static_cast<int32_t>(std::nearbyint(point.z * inv_resolution))}; | ||
+ static_cast<int32_t>(std::floor(point.x * inv_resolution)), | ||
+ static_cast<int32_t>(std::floor(point.y * inv_resolution)), | ||
+ static_cast<int32_t>(std::floor(point.z * inv_resolution))}; | ||
} | ||
|
||
[[nodiscard]] inline Point3D CoordToPos(const CoordT& coord, double resolution) { | ||
@@ -212,7 +212,7 @@ struct hash<Bonxai::CoordT> { | ||
std::size_t operator()(const Bonxai::CoordT& p) const { | ||
// same as OpenVDB | ||
return ((1 << 20) - 1) & (static_cast<int64_t>(p.x) * 73856093 ^ // | ||
- static_cast<int64_t>(p.y) * 19349663 ^ // | ||
+ static_cast<int64_t>(p.y) * 19349669 ^ // | ||
static_cast<int64_t>(p.z) * 83492791); | ||
} | ||
}; |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,2 @@ | ||
include(${CMAKE_CURRENT_LIST_DIR}/kiss_icp/kiss-icp.cmake) | ||
include(${CMAKE_CURRENT_LIST_DIR}/bonxai/bonxai.cmake) |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,26 @@ | ||
# MIT License | ||
# | ||
# Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill | ||
# Stachniss. | ||
# | ||
# Permission is hereby granted, free of charge, to any person obtaining a copy | ||
# of this software and associated documentation files (the "Software"), to deal | ||
# in the Software without restriction, including without limitation the rights | ||
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
# copies of the Software, and to permit persons to whom the Software is | ||
# furnished to do so, subject to the following conditions: | ||
# | ||
# The above copyright notice and this permission notice shall be included in all | ||
# copies or substantial portions of the Software. | ||
# | ||
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE | ||
# SOFTWARE. | ||
add_library(kinematic_icp_local_map STATIC) | ||
target_sources(kinematic_icp_local_map PRIVATE SparseVoxelGrid.cpp) | ||
target_link_libraries(kinematic_icp_local_map PUBLIC Eigen3::Eigen Sophus::Sophus bonxai_core) | ||
set_global_target_properties(kinematic_icp_local_map) |
Original file line number | Diff line number | Diff line change | ||||
---|---|---|---|---|---|---|
@@ -0,0 +1,148 @@ | ||||||
// MIT License | ||||||
|
||||||
// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill | ||||||
// Stachniss. | ||||||
|
||||||
// Permission is hereby granted, free of charge, to any person obtaining a copy | ||||||
// of this software and associated documentation files (the "Software"), to deal | ||||||
// in the Software without restriction, including without limitation the rights | ||||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||||||
// copies of the Software, and to permit persons to whom the Software is | ||||||
// furnished to do so, subject to the following conditions: | ||||||
|
||||||
// The above copyright notice and this permission notice shall be included in all | ||||||
// copies or substantial portions of the Software. | ||||||
|
||||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE | ||||||
// SOFTWARE. | ||||||
|
||||||
#include "SparseVoxelGrid.hpp" | ||||||
|
||||||
#include <Eigen/Core> | ||||||
#include <array> | ||||||
#include <bonxai/bonxai.hpp> | ||||||
#include <cstdint> | ||||||
#include <sophus/se3.hpp> | ||||||
|
||||||
#include "bonxai/grid_coord.hpp" | ||||||
|
||||||
namespace { | ||||||
static constexpr std::array<Bonxai::CoordT, 27> shifts{ | ||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. To be discussed after resolving PRBonn/kiss-icp#410 |
||||||
Bonxai::CoordT{-1, -1, -1}, Bonxai::CoordT{-1, -1, 0}, Bonxai::CoordT{-1, -1, 1}, | ||||||
Bonxai::CoordT{-1, 0, -1}, Bonxai::CoordT{-1, 0, 0}, Bonxai::CoordT{-1, 0, 1}, | ||||||
Bonxai::CoordT{-1, 1, -1}, Bonxai::CoordT{-1, 1, 0}, Bonxai::CoordT{-1, 1, 1}, | ||||||
|
||||||
Bonxai::CoordT{0, -1, -1}, Bonxai::CoordT{0, -1, 0}, Bonxai::CoordT{0, -1, 1}, | ||||||
Bonxai::CoordT{0, 0, -1}, Bonxai::CoordT{0, 0, 0}, Bonxai::CoordT{0, 0, 1}, | ||||||
Bonxai::CoordT{0, 1, -1}, Bonxai::CoordT{0, 1, 0}, Bonxai::CoordT{0, 1, 1}, | ||||||
|
||||||
Bonxai::CoordT{1, -1, -1}, Bonxai::CoordT{1, -1, 0}, Bonxai::CoordT{1, -1, 1}, | ||||||
Bonxai::CoordT{1, 0, -1}, Bonxai::CoordT{1, 0, 0}, Bonxai::CoordT{1, 0, 1}, | ||||||
Bonxai::CoordT{1, 1, -1}, Bonxai::CoordT{1, 1, 0}, Bonxai::CoordT{1, 1, 1}}; | ||||||
|
||||||
static constexpr uint8_t inner_grid_log2_dim = 2; | ||||||
static constexpr uint8_t leaf_grid_log2_dim = 3; | ||||||
} // namespace | ||||||
|
||||||
namespace kinematic_icp { | ||||||
|
||||||
SparseVoxelGrid::SparseVoxelGrid(const double voxel_size, | ||||||
const double clipping_distance, | ||||||
const unsigned int max_points_per_voxel) | ||||||
: voxel_size_(voxel_size), | ||||||
clipping_distance_(clipping_distance), | ||||||
max_points_per_voxel_(max_points_per_voxel), | ||||||
map_(voxel_size, inner_grid_log2_dim, leaf_grid_log2_dim), | ||||||
accessor_(map_.createAccessor()) {} | ||||||
|
||||||
std::tuple<Eigen::Vector3d, double> SparseVoxelGrid::GetClosestNeighbor( | ||||||
const Eigen::Vector3d &query) const { | ||||||
Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero(); | ||||||
double closest_distance = std::numeric_limits<double>::max(); | ||||||
const auto const_accessor = map_.createConstAccessor(); | ||||||
const Bonxai::CoordT voxel = map_.posToCoord(query); | ||||||
std::for_each(shifts.cbegin(), shifts.cend(), [&](const Bonxai::CoordT &voxel_shift) { | ||||||
const Bonxai::CoordT query_voxel = voxel + voxel_shift; | ||||||
const VoxelBlock *voxel_points = const_accessor.value(query_voxel); | ||||||
if (voxel_points != nullptr) { | ||||||
const Eigen::Vector3d &neighbor = | ||||||
*std::min_element(voxel_points->cbegin(), voxel_points->cend(), | ||||||
[&](const auto &lhs, const auto &rhs) { | ||||||
return (lhs - query).norm() < (rhs - query).norm(); | ||||||
}); | ||||||
double distance = (neighbor - query).norm(); | ||||||
if (distance < closest_distance) { | ||||||
closest_neighbor = neighbor; | ||||||
closest_distance = distance; | ||||||
} | ||||||
} | ||||||
}); | ||||||
return std::make_tuple(closest_neighbor, closest_distance); | ||||||
} | ||||||
|
||||||
void SparseVoxelGrid::AddPoints(const std::vector<Eigen::Vector3d> &points) { | ||||||
const double map_resolution = std::sqrt(voxel_size_ * voxel_size_ / max_points_per_voxel_); | ||||||
std::for_each(points.cbegin(), points.cend(), [&](const Eigen::Vector3d &p) { | ||||||
const auto voxel_coordinates = map_.posToCoord(p); | ||||||
VoxelBlock *voxel_points = accessor_.value(voxel_coordinates, /*create_if_missing=*/true); | ||||||
if (voxel_points->size() == max_points_per_voxel_ || | ||||||
std::any_of(voxel_points->cbegin(), voxel_points->cend(), [&](const auto &voxel_point) { | ||||||
return (voxel_point - p).norm() < map_resolution; | ||||||
})) { | ||||||
return; | ||||||
} | ||||||
voxel_points->reserve(max_points_per_voxel_); | ||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. There will be duplicated calls to |
||||||
voxel_points->emplace_back(p); | ||||||
}); | ||||||
} | ||||||
|
||||||
void SparseVoxelGrid::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) { | ||||||
auto is_too_far_away = [&](const VoxelBlock &block) { | ||||||
return (block.front() - origin).norm() > clipping_distance_; | ||||||
}; | ||||||
|
||||||
std::vector<Bonxai::CoordT> keys_to_delete; | ||||||
auto &root_map = map_.rootMap(); | ||||||
for (auto &[key, inner_grid] : root_map) { | ||||||
for (auto inner_it = inner_grid.mask().beginOn(); inner_it; ++inner_it) { | ||||||
const int32_t inner_index = *inner_it; | ||||||
auto &leaf_grid = inner_grid.cell(inner_index); | ||||||
const auto &voxel_block = leaf_grid->cell(leaf_grid->mask().findFirstOn()); | ||||||
if (is_too_far_away(voxel_block)) { | ||||||
inner_grid.mask().setOff(inner_index); | ||||||
leaf_grid.reset(); | ||||||
} | ||||||
} | ||||||
if (inner_grid.mask().isOff()) { | ||||||
keys_to_delete.push_back(key); | ||||||
} | ||||||
} | ||||||
for (const auto &key : keys_to_delete) { | ||||||
root_map.erase(key); | ||||||
} | ||||||
} | ||||||
|
||||||
void SparseVoxelGrid::Update(const std::vector<Eigen::Vector3d> &points, const Sophus::SE3d &pose) { | ||||||
std::vector<Eigen::Vector3d> points_transformed(points.size()); | ||||||
std::transform(points.cbegin(), points.cend(), points_transformed.begin(), | ||||||
[&](const auto &point) { return pose * point; }); | ||||||
const Eigen::Vector3d &origin = pose.translation(); | ||||||
AddPoints(points_transformed); | ||||||
RemovePointsFarFromLocation(origin); | ||||||
} | ||||||
|
||||||
std::vector<Eigen::Vector3d> SparseVoxelGrid::Pointcloud() const { | ||||||
std::vector<Eigen::Vector3d> point_cloud; | ||||||
point_cloud.reserve(map_.activeCellsCount() * max_points_per_voxel_); | ||||||
map_.forEachCell([&point_cloud, this](VoxelBlock &block, const auto &) { | ||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more.
Suggested change
|
||||||
point_cloud.insert(point_cloud.end(), block.cbegin(), block.cend()); | ||||||
}); | ||||||
return point_cloud; | ||||||
} | ||||||
|
||||||
} // namespace kinematic_icp |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,56 @@ | ||
// MIT License | ||
|
||
// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill | ||
// Stachniss. | ||
|
||
// Permission is hereby granted, free of charge, to any person obtaining a copy | ||
// of this software and associated documentation files (the "Software"), to deal | ||
// in the Software without restriction, including without limitation the rights | ||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
// copies of the Software, and to permit persons to whom the Software is | ||
// furnished to do so, subject to the following conditions: | ||
|
||
// The above copyright notice and this permission notice shall be included in all | ||
// copies or substantial portions of the Software. | ||
|
||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE | ||
// SOFTWARE. | ||
#pragma once | ||
#include <Eigen/Core> | ||
#include <array> | ||
#include <bonxai/bonxai.hpp> | ||
#include <sophus/se3.hpp> | ||
|
||
namespace kinematic_icp { | ||
|
||
using VoxelBlock = std::vector<Eigen::Vector3d>; | ||
|
||
struct SparseVoxelGrid { | ||
explicit SparseVoxelGrid(const double voxel_size, | ||
const double clipping_distance, | ||
const unsigned int max_points_per_voxel); | ||
|
||
inline void Clear() { map_.clear(Bonxai::ClearOption::CLEAR_MEMORY); } | ||
inline bool Empty() const { return map_.activeCellsCount() == 0; } | ||
void Update(const std::vector<Eigen::Vector3d> &points, const Sophus::SE3d &pose); | ||
void AddPoints(const std::vector<Eigen::Vector3d> &points); | ||
void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); | ||
std::vector<Eigen::Vector3d> Pointcloud() const; | ||
std::tuple<Eigen::Vector3d, double> GetClosestNeighbor(const Eigen::Vector3d &query) const; | ||
|
||
double voxel_size_; | ||
double clipping_distance_; | ||
unsigned int max_points_per_voxel_; | ||
Bonxai::VoxelGrid<VoxelBlock> map_; | ||
|
||
private: | ||
using AccessorType = typename Bonxai::VoxelGrid<VoxelBlock>::Accessor; | ||
AccessorType accessor_; | ||
}; | ||
|
||
} // namespace kinematic_icp |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Is this needed? I think it's coming from the
find_dependencies.cmake
from KISS, we could move it to the top-level.cmake