nils/simple_slam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

orthtree wip

Browse Source
This commit is contained in:
Nils Schulte 2024-05-30 21:33:10 +02:00
parent bccf38e984
commit 338933f12d
5 changed files with 274 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
CMakeLists.txt
View File

@ -26,10 +26,12 @@ find_package(Ceres REQUIRED)
# find_package(<dependency> REQUIRED)
find_package(Eigen3 REQUIRED NO_MODULE)
find_package(TBB REQUIRED)
find_package(Sophus REQUIRED)
add_executable(simpleslam src/simpleslam_node.cpp )
ament_target_dependencies(simpleslam rclcpp sensor_msgs nav_msgs)
target_link_libraries(simpleslam Ceres::ceres Eigen3::Eigen)
target_link_libraries(simpleslam Ceres::ceres Eigen3::Eigen TBB::tbb Sophus::Sophus)
# include_directories(include lib/karto_sdk/include
@ -43,15 +45,13 @@ install(TARGETS
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(simpleslam_test test/orthtree_test.cpp)
target_include_directories(simpleslam_test PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
target_link_libraries(simpleslam_test Ceres::ceres Eigen3::Eigen TBB::tbb Sophus::Sophus)
endif()
ament_package()

4
package.xml
View File

@ -22,11 +22,13 @@
<build_depend>liblapack-dev</build_depend>
<build_depend>libceres-dev</build_depend>
<build_depend>tbb</build_depend>b
<build_depend>tbb</build_depend>
<build_depend>sophus</build_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_cmake_gtest</test_depend>
<export>
<build_type>ament_cmake</build_type>

171
src/orthtree.hpp Normal file
View File

@ -0,0 +1,171 @@
#include <Eigen/Dense>
#include <algorithm>
#include <array>
#include <bitset>
#include <execution>
#include <functional>
#include <iostream>
#include <map>
#include <memory>
#include <mutex>
template <unsigned int dim, typename number, typename T> class Orthtree {
public:
typedef Eigen::Vector<number, dim> Point;
typedef std::function<bool(std::unique_ptr<T> &, const Point &)>
data_access_f;
typedef std::function<bool(const std::unique_ptr<T> &, const Point &)>
cdata_access_f;
class BBox {
public:
enum Relation {
NO_OVERLAP,
OVERLAP,
CONTAINS,
CONTAINED,
};
Point min, max;
Point center() { return (min + max) / 2; }
Relation relation(const BBox &b) {
if ((b.min.array() > max.array()).any() ||
(b.max.array() < min.array()).any())
return NO_OVERLAP;
if ((b.min.array() > min.array()).all() &&
(b.max.array() < max.array()).all())
return CONTAINS;
if ((b.min.array() < min.array()).all() &&
(b.max.array() > max.array()).all())
return CONTAINED;
return OVERLAP;
};
BBox(const Point &min, const Point &max) : min(min), max(max) {
assert((min.array() < max.array()).all());
}
};
class Node {
public:
typedef std::bitset<dim> coord;
Point center;
std::unique_ptr<std::array<Node, (2 << dim)>> children;
std::unique_ptr<T> data;
mutable std::mutex mtx;
void create_children(const Point &parent_center, number parent_width) {
children = std::make_unique<std::array<Node, (2 << dim)>>();
for (unsigned int i = 0; i < (2 << dim); ++i) {
Point offset = Point::Ones() * (parent_width / 4);
for (unsigned int j = 0; j < dim; ++j)
if (!coord(i)[j])
offset(j) *= -1;
(*children)[i].center = parent_center + offset;
}
};
unsigned int get_child_coord(const Point &p) const {
coord child_coord;
for (unsigned int i = 0; i < dim; ++i)
child_coord[i] = center[i] < p[i];
return child_coord.to_ulong();
};
void depth(const Point &point, int depth, data_access_f f, number width) {
if (depth == 0) {
std::lock_guard<std::mutex> guard(mtx);
f(data, center);
} else {
{
std::lock_guard<std::mutex> guard(mtx);
if (!children)
create_children(center, width);
}
(*children)[get_child_coord(point)].depth(point, depth - 1, f,
width / 2);
}
};
BBox bb(number width) const {
Point offset = Point::Ones() * (width / 2);
return BBox(center - offset, center + offset);
};
void access_data(cdata_access_f f) const {
std::lock_guard<std::mutex> guard(mtx);
if (data) {
f(data, center);
}
}
void all_data(cdata_access_f f) const {
std::lock_guard<std::mutex> guard(mtx);
if (data) {
f(data, center);
}
if (children) {
std::for_each(std::execution::par_unseq, std::begin(*children),
std::end(*children),
[f](const auto &c) { c.all_data(f); });
}
}
bool is_leaf() const {
std::lock_guard<std::mutex> guard(mtx);
return !bool(children);
}
};
std::unique_ptr<Node> root;
number root_width;
mutable std::mutex mtx;
Orthtree(const Point &root_center, number root_width)
: root(new Node()), root_width(root_width) {
root->center = root_center;
};
void all_data(cdata_access_f f) const { root->all_data(f); };
void depth(const Point &point, int depth, data_access_f f) {
root->depth(point, depth, f, root_width);
}
void data_in_bb(const BBox &bb, cdata_access_f f) const {
number curr_width = root_width;
std::vector<const Node *> to_visit{root.get()};
std::vector<const Node *> to_visit_children;
bool stop = false;
while (!to_visit.empty() && !stop) {
auto cbb = to_visit.back()->bb(curr_width);
auto bbrel = cbb.relation(bb);
// std::cout << to_visit.back()->center.transpose() << " - "
// << cbb.min.transpose() << " - " << bbrel << std::endl;
switch (bbrel) {
case (Orthtree::BBox::CONTAINED):
to_visit.back()->all_data(f);
break;
case (Orthtree::BBox::OVERLAP):
case Orthtree::BBox::CONTAINS:
to_visit.back()->access_data([&](auto &p, auto c) {
if (!f(p, c))
stop = true;
return false;
});
if (!to_visit.back()->is_leaf()) {
// std::transform(std::execution::par_unseq,to_visit.back()->children->cbegin(),
// to_visit.back()->children->cend(),
// to_visit_children.begin(), std::addressof<const
// Node>);
for (const auto &n : *to_visit.back()->children) {
to_visit_children.push_back(&n);
}
// std::cout << "leaf "<<to_visit_children.size() << std::endl;
}
break;
default:
break;
}
to_visit.pop_back();
if (to_visit.empty()) {
to_visit.swap(to_visit_children);
to_visit_children.clear();
curr_width /= 2;
}
}
}
};

1
src/simpleslam_node.cpp
View File

@ -13,6 +13,7 @@
#include "simpleslam.hpp"
#include "utils.hpp"
#include "orthtree.hpp"
class MinimalSubscriber : public rclcpp::Node {
public:

89
test/orthtree_test.cpp Normal file
View File

@ -0,0 +1,89 @@
#include <Eigen/Dense>
#include <gtest/gtest.h>
#include <memory>
#include "../src/orthtree.hpp"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
TEST(orthtree, insert1) {
typedef Orthtree<2, float, bool> O;
Orthtree<2, float, bool> t(O::Point::Zero(), 16);
auto p = O::Point(1, -7);
t.depth(p, 3, [&](auto &tp, auto c) {
tp = std::make_unique<bool>(true);
return true;
});
t.all_data([&](auto &tp, auto c) -> bool {
EXPECT_TRUE(tp && *tp) << c.transpose();
EXPECT_TRUE(c == p) << c.transpose();
return true;
});
}
TEST(orthtree, bbox1) {
typedef Orthtree<2, float, bool> O;
EXPECT_EQ(O::BBox(O::Point(0, 0), O::Point(10, 10))
.relation(O::BBox(O::Point(0, 0), O::Point(10, 11))),
O::BBox::OVERLAP);
EXPECT_EQ(O::BBox(O::Point(-1, -1), O::Point(10, 10))
.relation(O::BBox(O::Point(0, 0), O::Point(10, 11))),
O::BBox::OVERLAP);
EXPECT_EQ(O::BBox(O::Point(-1, -1), O::Point(1, 1))
.relation(O::BBox(O::Point(2, 2), O::Point(10, 11))),
O::BBox::NO_OVERLAP);
EXPECT_EQ(O::BBox(O::Point(-1, -1), O::Point(1, 1))
.relation(O::BBox(O::Point(-2, -2), O::Point(10, 11))),
O::BBox::CONTAINED);
EXPECT_EQ(O::BBox(O::Point(-100, -100), O::Point(100, 100))
.relation(O::BBox(O::Point(-2, -2), O::Point(10, 11))),
O::BBox::CONTAINS);
EXPECT_EQ(O::BBox(O::Point(-100, -100), O::Point(-2, -2))
.relation(O::BBox(O::Point(-2, -2), O::Point(10, 11))),
O::BBox::OVERLAP);
EXPECT_EQ(O::BBox(O::Point(-100, -100), O::Point(-2, -2))
.relation(O::BBox(O::Point(-10, -2), O::Point(10, 11))),
O::BBox::OVERLAP);
EXPECT_EQ(O::BBox(O::Point(-100, -100), O::Point(-2, -2))
.relation(O::BBox(O::Point(-10, -1), O::Point(10, 11))),
O::BBox::NO_OVERLAP);
}
TEST(orthtree, insert2) {
typedef Orthtree<2, float, Eigen::Vector2f> O;
O t(O::Point::Zero(), 16);
for (int i = 0; i <= 360; i += 90) {
Eigen::Rotation2D<float> rot(float(i) / 180.0 * M_PI);
O::Point p = O::Point(-4, 4) + (rot * O::Point(1.9, 0));
t.depth(p, 3, [&](auto &tp, auto c) {
tp = std::make_unique<O::Point>(p);
return true;
});
}
// t.all_data([&](auto &tp, auto c) -> bool {
// EXPECT_TRUE(false) << c.transpose() << " - " << tp->transpose();
// return true;
// });
t.data_in_bb(O::BBox(O::Point(-2, 0), O::Point(0, 32)),
[&](auto &tp, auto c) -> bool {
EXPECT_TRUE(tp) << c.transpose();
EXPECT_TRUE(false) << c.transpose();
return true;
});
}
int main(int argc, char **argv) {
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}