Skip to content
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

Open
wants to merge 25 commits into
base: main
Choose a base branch
from
Open
Changes from 1 commit
Commits
Show all changes
25 commits
Select commit Hold shift + click to select a range
7a19274
Add Bonxai and move kiss icp cmake
tizianoGuadagnino Nov 22, 2024
0084554
Start to work on the local map representation
tizianoGuadagnino Nov 22, 2024
4adc4e0
Add old API
tizianoGuadagnino Nov 22, 2024
01188b4
At least replicate the functionalities of kiss map, need to test and
tizianoGuadagnino Nov 22, 2024
be18fae
Just build bonxai_core
tizianoGuadagnino Nov 25, 2024
11b9c5a
Add patch
tizianoGuadagnino Nov 25, 2024
a7ddd20
To fix const correctness
tizianoGuadagnino Nov 25, 2024
7e9726f
Remove small vector
tizianoGuadagnino Nov 26, 2024
6ed4c01
Merge branch 'main' into tiziano/bonxai_minimal
tizianoGuadagnino Nov 29, 2024
c4c1d90
Some renaming and cleaning
tizianoGuadagnino Nov 29, 2024
2af1115
For more clarity
tizianoGuadagnino Nov 29, 2024
b76abda
Modify bonxai patch
tizianoGuadagnino Nov 29, 2024
f456e76
IWYU
tizianoGuadagnino Nov 30, 2024
bbb3ecc
Just reset the block
tizianoGuadagnino Dec 1, 2024
57977f1
Actually remove the keys when leafs are removed, a bit slower
tizianoGuadagnino Dec 2, 2024
45c1b2b
Control leaf and grid size
tizianoGuadagnino Dec 3, 2024
9859082
Forget to push the default config
tizianoGuadagnino Dec 3, 2024
9b84286
Update cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp
tizianoGuadagnino Jan 9, 2025
da104ce
Merge branch 'main' into tiziano/bonxai_minimal
tizianoGuadagnino Jan 9, 2025
db1e16c
Revert "Merge branch 'main' into tiziano/bonxai_minimal"
tizianoGuadagnino Jan 9, 2025
920fb8f
At least replicate the functionalities of kiss map, need to test and
tizianoGuadagnino Nov 22, 2024
475d08d
Revert "Merge branch 'main' into tiziano/bonxai_minimal"
tizianoGuadagnino Jan 9, 2025
18b102a
Cannot merge
tizianoGuadagnino Jan 9, 2025
e58c060
Merge Myself
tizianoGuadagnino Jan 9, 2025
1e14a4e
Merge me from the window
tizianoGuadagnino Jan 9, 2025
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Remove small vector
tizianoGuadagnino committed Nov 26, 2024

Verified

This commit was signed with the committer’s verified signature.
yeshan333 yeshan333
commit 7e9726f3b47f66b51e421d3283cfbf3be58a6913
11 changes: 3 additions & 8 deletions cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp
Original file line number Diff line number Diff line change
@@ -45,12 +45,6 @@ static std::array<Bonxai::CoordT, 27> shifts{
}

namespace kinematic_icp {
void VoxelBlock::addPoint(const Eigen::Vector3d &p) {
points_[size_++] = p;
if (size_ > VoxelBlock::MAX_SIZE) {
throw std::runtime_error("VoxelBlock| size is too big, want to fix somehow");
}
}

SparseVoxelGrid::SparseVoxelGrid(const double voxel_size,
const double clipping_distance,
@@ -96,7 +90,8 @@ void SparseVoxelGrid::AddPoints(const std::vector<Eigen::Vector3d> &points) {
})) {
return;
}
voxel_points->addPoint(p);
voxel_points->reserve(max_points_per_voxel_);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There will be duplicated calls to reserve, but since max_points_per_voxel_ is fixed, this should not matter, right?

voxel_points->emplace_back(p);
});
}

@@ -107,7 +102,7 @@ void SparseVoxelGrid::RemovePointsFarFromLocation(const Eigen::Vector3d &origin)
}
};
map_.forEachCell(remove_voxel);
// map_.releaseUnusedMemory();
map_.releaseUnusedMemory();
}

void SparseVoxelGrid::Update(const std::vector<Eigen::Vector3d> &points, const Sophus::SE3d &pose) {
30 changes: 1 addition & 29 deletions cpp/kinematic_icp/local_map/SparseVoxelGrid.hpp
Original file line number Diff line number Diff line change
@@ -27,36 +27,8 @@
#include <sophus/se3.hpp>

namespace kinematic_icp {
// Following the Cult of Faconti
struct VoxelBlock {
static constexpr size_t MAX_SIZE = 20;
using PointsContainerType = std::array<Eigen::Vector3d, MAX_SIZE>;
using IteratorType = PointsContainerType::iterator;
using ConstIteratorType = PointsContainerType::const_iterator;

VoxelBlock() = default;

VoxelBlock(const VoxelBlock &other) = delete;
VoxelBlock &operator=(const VoxelBlock &) = delete;

VoxelBlock(VoxelBlock &&other) = default;
VoxelBlock &operator=(VoxelBlock &&other) = default;

void addPoint(const Eigen::Vector3d &p);

inline std::size_t size() const { return std::distance(cbegin(), cend()); }

const Eigen::Vector3d &front() const { return points_.front(); }
inline IteratorType begin() { return points_.begin(); }
inline ConstIteratorType cbegin() const { return points_.cbegin(); }

inline IteratorType end() { return std::next(begin(), size_); }
inline ConstIteratorType cend() const { return std::next(cbegin(), size_); }

private:
std::array<Eigen::Vector3d, MAX_SIZE> points_;
uint16_t size_ = 0;
};
using VoxelBlock = std::vector<Eigen::Vector3d>;

struct SparseVoxelGrid {
explicit SparseVoxelGrid(const double voxel_size,