From cd6761936971eef475d43e3af28dc50d3dcfa5af Mon Sep 17 00:00:00 2001 From: Yevgeniy Date: Sun, 5 May 2024 22:08:00 +0800 Subject: [PATCH 01/11] New branch From a6add3f898b49ca09c0b9d27d97ee921eb3b4f57 Mon Sep 17 00:00:00 2001 From: Yevgeniy Date: Tue, 7 May 2024 01:58:25 +0800 Subject: [PATCH 02/11] Added a new PointCloud data structure for future polygonization --- .vscode/settings.json | 1 + CMakeLists.txt | 10 +- src/pointcloud.hpp | 348 +++++++++++++++++++++++++++++++++++++++ test/test_pointcloud.cpp | 216 ++++++++++++++++++++++++ 4 files changed, 567 insertions(+), 8 deletions(-) create mode 100644 src/pointcloud.hpp create mode 100644 test/test_pointcloud.cpp diff --git a/.vscode/settings.json b/.vscode/settings.json index 1514aec..f9e50a9 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -92,6 +92,7 @@ "cSpell.words": [ "Clusterer", "KDTREE", + "pointcloud", "polygonize", "Polygonizer" ] diff --git a/CMakeLists.txt b/CMakeLists.txt index 20ec3d7..9d0d61e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,27 +69,21 @@ target_link_libraries(processor if(BUILD_TESTS) find_package(GTest REQUIRED) - enable_testing() - - set(TEST_NAME test_kdtree) - + set(TEST_NAME test_all) include(GoogleTest) - add_executable(${TEST_NAME} test/test_kdtree.cpp + test/test_pointcloud.cpp ) - target_include_directories(${TEST_NAME} PRIVATE include src ) - target_link_libraries(${TEST_NAME} PRIVATE GTest::gtest_main ) - gtest_discover_tests(${TEST_NAME}) endif() diff --git a/src/pointcloud.hpp b/src/pointcloud.hpp new file mode 100644 index 0000000..5e8acf0 --- /dev/null +++ b/src/pointcloud.hpp @@ -0,0 +1,348 @@ +#include +#include +#include +#include +#include +#include + +namespace lidar_processing +{ +template class PointCloud final +{ + public: + class PointsView final + { + using IteratorT = typename std::vector::iterator; + using ConstIteratorT = typename std::vector::const_iterator; + using ReverseIteratorT = typename std::vector::reverse_iterator; + using ConstReverseIteratorT = typename std::vector::const_reverse_iterator; + + public: + explicit PointsView(PointCloud *parent, std::size_t cluster_index); + + IteratorT begin() const noexcept; + IteratorT end() const noexcept; + ConstIteratorT cbegin() const noexcept; + ConstIteratorT cend() const noexcept; + ReverseIteratorT rbegin() const noexcept; + ReverseIteratorT rend() const noexcept; + ConstReverseIteratorT crbegin() const noexcept; + ConstReverseIteratorT crend() const noexcept; + + std::size_t size() const; + PointT &operator[](std::size_t index); + const PointT &operator[](std::size_t index) const; + PointT &at(std::size_t index); + const PointT &at(std::size_t index) const; + PointT &at_point(std::size_t index); + const PointT &at_point(std::size_t index) const; + + void push_back(const PointT &point); + void push_back(PointT &&point); + template void emplace_back(Args &&...args); + + private: + PointCloud *parent_; + std::size_t cluster_index_; + }; + + PointCloud() = default; + PointCloud(std::size_t initial_capacity); + ~PointCloud() = default; + + PointCloud(const PointCloud &) = default; + PointCloud &operator=(const PointCloud &) = default; + PointCloud(PointCloud &&) = default; + PointCloud &operator=(PointCloud &&) = default; + + void reserve_max_points(std::size_t capacity); + template void emplace_into_last_cluster(Args &&...args); + + void push_into_last_cluster(const PointT &point); + void push_into_last_cluster(PointT &&point); + void increment_cluster(); + void clear(); + + std::size_t total_clusters() const; + std::size_t total_points() const; + + PointT &operator()(std::size_t cluster_index, std::size_t point_index); + const PointT &operator()(std::size_t cluster_index, std::size_t point_index) const; + PointT &at(std::size_t cluster_index, std::size_t point_index); + const PointT &at(std::size_t cluster_index, std::size_t point_index) const; + + void add_cluster(const std::vector &new_cluster); + + PointsView at_cluster(std::size_t cluster_index); + const PointsView at_cluster(std::size_t cluster_index) const; + + PointsView last_cluster(); + const PointsView last_cluster() const; + + void pop_last_cluster() noexcept; + void print_clusters() const; + + private: + std::vector points_; // Main buffer for all points + std::vector> clusters_; // Start index and size for each cluster + + void ensure_capacity(std::size_t required_capacity); +}; + +template +PointCloud::PointsView::PointsView(PointCloud *parent, std::size_t cluster_index) + : parent_(parent), cluster_index_(cluster_index) +{ +} + +template +typename PointCloud::PointsView::IteratorT PointCloud::PointsView::begin() const noexcept +{ + return parent_->points_.begin() + parent_->clusters_[cluster_index_].first; +} + +template +typename PointCloud::PointsView::IteratorT PointCloud::PointsView::end() const noexcept +{ + return parent_->points_.begin() + parent_->clusters_[cluster_index_].first + + parent_->clusters_[cluster_index_].second; +} + +template +typename PointCloud::PointsView::ConstIteratorT PointCloud::PointsView::cbegin() const noexcept +{ + return parent_->points_.cbegin() + parent_->clusters_[cluster_index_].first; +} + +template +typename PointCloud::PointsView::ConstIteratorT PointCloud::PointsView::cend() const noexcept +{ + return parent_->points_.cbegin() + parent_->clusters_[cluster_index_].first + + parent_->clusters_[cluster_index_].second; +} + +template +typename PointCloud::PointsView::ReverseIteratorT PointCloud::PointsView::rbegin() const noexcept +{ + return ReverseIteratorT(end()); +} + +template +typename PointCloud::PointsView::ReverseIteratorT PointCloud::PointsView::rend() const noexcept +{ + return ReverseIteratorT(begin()); +} + +template +typename PointCloud::PointsView::ConstReverseIteratorT PointCloud::PointsView::crbegin() const noexcept +{ + return ConstReverseIteratorT(cend()); +} + +template +typename PointCloud::PointsView::ConstReverseIteratorT PointCloud::PointsView::crend() const noexcept +{ + return ConstReverseIteratorT(cbegin()); +} + +template std::size_t PointCloud::PointsView::size() const +{ + return parent_->clusters_[cluster_index_].second; +} + +template PointT &PointCloud::PointsView::operator[](std::size_t index) +{ + assert(index < size()); + return parent_->points_[parent_->clusters_[cluster_index_].first + index]; +} + +template const PointT &PointCloud::PointsView::operator[](std::size_t index) const +{ + assert(index < size()); + return parent_->points_[parent_->clusters_[cluster_index_].first + index]; +} + +template PointT &PointCloud::PointsView::at(std::size_t index) +{ + if (index >= size()) + { + throw std::runtime_error("PointCloud::PointsView::at(): Index out of bounds"); + } + return parent_->points_[parent_->clusters_[cluster_index_].first + index]; +} + +template const PointT &PointCloud::PointsView::at(std::size_t index) const +{ + if (index >= size()) + { + throw std::runtime_error("PointCloud::PointsView::at(): Index out of bounds"); + } + return parent_->points_[parent_->clusters_[cluster_index_].first + index]; +} + +template PointT &PointCloud::PointsView::at_point(std::size_t index) +{ + if (index >= size()) + { + throw std::runtime_error("PointCloud::PointsView::at_point(): Index out of bounds"); + } + return parent_->points_[parent_->clusters_[cluster_index_].first + index]; +} + +template const PointT &PointCloud::PointsView::at_point(std::size_t index) const +{ + if (index >= size()) + { + throw std::runtime_error("PointCloud::PointsView::at_point(): Index out of bounds"); + } + return parent_->points_[parent_->clusters_[cluster_index_].first + index]; +} + +template void PointCloud::PointsView::push_back(const PointT &point) +{ + parent_->push_into_last_cluster(point); +} + +template void PointCloud::PointsView::push_back(PointT &&point) +{ + parent_->push_into_last_cluster(std::move(point)); +} + +template template void PointCloud::PointsView::emplace_back(Args &&...args) +{ + parent_->emplace_into_last_cluster(std::forward(args)...); +} + +template PointCloud::PointCloud(std::size_t initial_capacity) +{ + points_.reserve(initial_capacity); + clusters_.reserve(initial_capacity); // Assume 1 pt per cluster worst case scenario +} + +template void PointCloud::reserve_max_points(std::size_t capacity) +{ + points_.reserve(capacity); + clusters_.reserve(capacity); // Same assumption as above +} + +template +template +void PointCloud::emplace_into_last_cluster(Args &&...args) +{ + if (clusters_.empty()) + { + clusters_.emplace_back(points_.size(), 0); // Start a new cluster if none exist + } + auto &last = clusters_.back(); + ensure_capacity(last.first + last.second + 1); + points_.emplace_back(std::forward(args)...); + ++last.second; +} + +template void PointCloud::push_into_last_cluster(const PointT &point) +{ + emplace_into_last_cluster(point); +} + +template void PointCloud::push_into_last_cluster(PointT &&point) +{ + emplace_into_last_cluster(std::move(point)); +} + +template void PointCloud::increment_cluster() +{ + if (!clusters_.empty()) + { + auto &last_cluster = clusters_.back(); + ensure_capacity(last_cluster.first + last_cluster.second + 1); + } + clusters_.emplace_back(points_.size(), 0); +} + +template void PointCloud::clear() +{ + points_.clear(); + clusters_.clear(); +} + +template std::size_t PointCloud::total_clusters() const +{ + return clusters_.size(); +} + +template std::size_t PointCloud::total_points() const +{ + return points_.size(); +} + +template void PointCloud::add_cluster(const std::vector &new_cluster) +{ + const std::size_t start_index = points_.size(); + points_.insert(points_.end(), new_cluster.begin(), new_cluster.end()); + clusters_.emplace_back(start_index, new_cluster.size()); +} + +template typename PointCloud::PointsView PointCloud::last_cluster() +{ + if (clusters_.empty()) + { + throw std::runtime_error("PointCloud::last_cluster(): No clusters"); + } + return PointsView(this, clusters_.size() - 1); +} + +template const typename PointCloud::PointsView PointCloud::last_cluster() const +{ + if (clusters_.empty()) + { + throw std::runtime_error("PointCloud::last_cluster(): No clusters"); + } + return PointsView(const_cast *>(this), clusters_.size() - 1); +} + +template +typename PointCloud::PointsView PointCloud::at_cluster(std::size_t cluster_index) +{ + if (cluster_index >= clusters_.size()) + { + throw std::runtime_error("PointCloud::at_cluster(): Index out of bounds"); + } + return PointsView(this, cluster_index); +} + +template +const typename PointCloud::PointsView PointCloud::at_cluster(std::size_t cluster_index) const +{ + if (cluster_index >= clusters_.size()) + { + throw std::runtime_error("PointCloud::at_cluster(): Index out of bounds"); + } + return PointsView(const_cast *>(this), cluster_index); +} + +template void PointCloud::pop_last_cluster() noexcept +{ + if (!clusters_.empty()) + { + auto &last = clusters_.back(); + points_.resize(last.first); + clusters_.pop_back(); + } +} + +template void PointCloud::print_clusters() const +{ + for (const auto &cluster : clusters_) + { + std::cout << "Cluster starting at index " << cluster.first << " with size " << cluster.second << std::endl; + } +} + +template void PointCloud::ensure_capacity(std::size_t required_capacity) +{ + if (required_capacity > points_.capacity()) + { + points_.reserve(required_capacity); + } +} +}; // namespace lidar_processing diff --git a/test/test_pointcloud.cpp b/test/test_pointcloud.cpp new file mode 100644 index 0000000..a561239 --- /dev/null +++ b/test/test_pointcloud.cpp @@ -0,0 +1,216 @@ +#include +#include + +#include + +using namespace lidar_processing; + +class PointCloudTest : public ::testing::Test +{ + protected: + struct PointXYZ final + { + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + PointXYZ() = default; + PointXYZ(float x, float y, float z = 0.0F) : x(x), y(y), z(z) + { + } + }; + + PointCloud pointcloud_; + + void add_sample_cluster() + { + pointcloud_.push_into_last_cluster({1.0f, 2.0f, 3.0f}); + pointcloud_.push_into_last_cluster({4.0f, 5.0f, 6.0f}); + pointcloud_.emplace_into_last_cluster(7.0f, 8.0f, 9.0f); + } + + std::vector get_sample_cluster() + { + return {{1.0f, 2.0f, 3.0f}, {4.0f, 5.0f, 6.0f}, {7.0f, 8.0f, 9.0f}}; + } + + public: + friend bool operator==(const PointXYZ &a, const PointXYZ &b) noexcept + { + static constexpr float EPSILON = 1e-6; + return (std::fabs(a.x - b.x) < EPSILON) && (std::fabs(a.y - b.y) < EPSILON) && (std::fabs(a.z - b.z) < EPSILON); + } +}; + +TEST_F(PointCloudTest, StartsEmpty) +{ + EXPECT_EQ(pointcloud_.total_points(), 0); +} + +TEST_F(PointCloudTest, AddPointsToCluster) +{ + PointXYZ point = {1.0f, 2.0f, 3.0f}; + pointcloud_.increment_cluster(); + pointcloud_.push_into_last_cluster(point); + EXPECT_EQ(pointcloud_.at_cluster(0).size(), 1); + EXPECT_TRUE(pointcloud_.at_cluster(0)[0] == point); +} + +TEST_F(PointCloudTest, MultipleClusters) +{ + PointXYZ point1 = {1.0f, 2.0f, 3.0f}; + PointXYZ point2 = {4.0f, 5.0f, 6.0f}; + pointcloud_.increment_cluster(); + pointcloud_.push_into_last_cluster(point1); + pointcloud_.increment_cluster(); + pointcloud_.push_into_last_cluster(point2); + EXPECT_EQ(pointcloud_.at_cluster(0).size(), 1); + EXPECT_EQ(pointcloud_.at_cluster(1).size(), 1); + EXPECT_TRUE(pointcloud_.at_cluster(0)[0] == point1); + EXPECT_TRUE(pointcloud_.at_cluster(1)[0] == point2); +} + +TEST_F(PointCloudTest, ClearPointCloud) +{ + PointXYZ point = {1.0f, 2.0f, 3.0f}; + pointcloud_.increment_cluster(); + pointcloud_.push_into_last_cluster(point); + pointcloud_.clear(); + EXPECT_EQ(pointcloud_.total_points(), 0); +} + +TEST_F(PointCloudTest, AccessOutOfBounds) +{ + pointcloud_.increment_cluster(); + EXPECT_THROW(pointcloud_.at_cluster(0).at(0), std::runtime_error); +} + +TEST_F(PointCloudTest, PopLastCluster) +{ + PointXYZ point1 = {1.0f, 2.0f, 3.0f}; + PointXYZ point2 = {4.0f, 5.0f, 6.0f}; + pointcloud_.increment_cluster(); + pointcloud_.push_into_last_cluster(point1); + pointcloud_.increment_cluster(); + pointcloud_.push_into_last_cluster(point2); + pointcloud_.pop_last_cluster(); + EXPECT_EQ(pointcloud_.total_points(), 1); + EXPECT_TRUE(pointcloud_.at_cluster(0).at_point(0) == point1); +} + +TEST_F(PointCloudTest, ForwardIterator) +{ + add_sample_cluster(); + auto cluster = pointcloud_.at_cluster(0); + auto it = cluster.begin(); + const auto sample_cluster = get_sample_cluster(); + EXPECT_EQ(*it, sample_cluster[0]); + ++it; + EXPECT_EQ(*it, sample_cluster[1]); + ++it; + EXPECT_EQ(*it, sample_cluster[2]); + ++it; + EXPECT_EQ(it, cluster.end()); +} + +TEST_F(PointCloudTest, ReverseIterator) +{ + add_sample_cluster(); + auto cluster = pointcloud_.at_cluster(0); + auto rit = cluster.rbegin(); + const auto sample_cluster = get_sample_cluster(); + EXPECT_EQ(*rit, sample_cluster[2]); + ++rit; + EXPECT_EQ(*rit, sample_cluster[1]); + ++rit; + EXPECT_EQ(*rit, sample_cluster[0]); + ++rit; + EXPECT_EQ(rit, cluster.rend()); +} + +TEST_F(PointCloudTest, ConstForwardIterator) +{ + add_sample_cluster(); + const auto &constCluster = pointcloud_.at_cluster(0); + auto cit = constCluster.cbegin(); + const auto sample_cluster = get_sample_cluster(); + EXPECT_EQ(*cit, sample_cluster[0]); + ++cit; + EXPECT_EQ(*cit, sample_cluster[1]); + ++cit; + EXPECT_EQ(*cit, sample_cluster[2]); + ++cit; + EXPECT_EQ(cit, constCluster.cend()); +} + +TEST_F(PointCloudTest, ConstReverseIterator) +{ + add_sample_cluster(); + const auto &constCluster = pointcloud_.at_cluster(0); + auto crit = constCluster.crbegin(); + const auto sample_cluster = get_sample_cluster(); + EXPECT_EQ(*crit, sample_cluster[2]); + ++crit; + EXPECT_EQ(*crit, sample_cluster[1]); + ++crit; + EXPECT_EQ(*crit, sample_cluster[0]); + ++crit; + EXPECT_EQ(crit, constCluster.crend()); +} + +TEST_F(PointCloudTest, DynamicResizing) +{ + for (int i = 0; i < 1000; i++) + { + pointcloud_.push_into_last_cluster({1.0f * i, 2.0f * i, 3.0f * i}); + } + EXPECT_EQ(pointcloud_.total_points(), 1000); + const PointXYZ expected_last_point{999.0f, 1998.0f, 2997.0f}; + EXPECT_TRUE(pointcloud_.at_cluster(0)[999] == expected_last_point); +} + +TEST_F(PointCloudTest, CorrectClusterIncrementing) +{ + pointcloud_.increment_cluster(); + pointcloud_.push_into_last_cluster({1.0f, 2.0f, 3.0f}); + pointcloud_.increment_cluster(); + pointcloud_.push_into_last_cluster({4.0f, 5.0f, 6.0f}); + const auto point_0 = PointXYZ{1.0f, 2.0f, 3.0f}; + EXPECT_EQ(pointcloud_.at_cluster(0)[0], point_0); + const auto point_1 = PointXYZ{4.0f, 5.0f, 6.0f}; + EXPECT_EQ(pointcloud_.at_cluster(1)[0], point_1); + EXPECT_EQ(pointcloud_.total_clusters(), 2); +} + +TEST_F(PointCloudTest, EmptyClusterHandling) +{ + pointcloud_.increment_cluster(); + EXPECT_NO_THROW({ + auto cluster = pointcloud_.at_cluster(0); + EXPECT_EQ(cluster.size(), 0); + }); +} + +TEST_F(PointCloudTest, ExceptionSafety) +{ + EXPECT_THROW(pointcloud_.at_cluster(1), std::runtime_error); + pointcloud_.increment_cluster(); + pointcloud_.push_into_last_cluster({1.0f, 2.0f, 3.0f}); + EXPECT_EQ(pointcloud_.total_points(), 1); +} + +TEST_F(PointCloudTest, CopyConstructor) +{ + add_sample_cluster(); + const PointCloud copied_pointcloud = pointcloud_; + const auto sample_cluster = get_sample_cluster(); + EXPECT_TRUE(copied_pointcloud.at_cluster(0)[0] == sample_cluster[0]); +} + +TEST_F(PointCloudTest, MoveConstructor) +{ + add_sample_cluster(); + const PointCloud moved_pointcloud = std::move(pointcloud_); + const auto sample_cluster = get_sample_cluster(); + EXPECT_TRUE(moved_pointcloud.at_cluster(0)[0] == sample_cluster[0]); + EXPECT_EQ(pointcloud_.total_points(), 0); +} From 56dbac3f518411c9b8354c84ba4b06615efa14c5 Mon Sep 17 00:00:00 2001 From: Yevgeniy Date: Tue, 7 May 2024 02:05:23 +0800 Subject: [PATCH 03/11] Minor refactoring --- src/pointcloud.hpp | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/pointcloud.hpp b/src/pointcloud.hpp index 5e8acf0..543fdaf 100644 --- a/src/pointcloud.hpp +++ b/src/pointcloud.hpp @@ -29,9 +29,9 @@ template class PointCloud final ConstReverseIteratorT crbegin() const noexcept; ConstReverseIteratorT crend() const noexcept; - std::size_t size() const; - PointT &operator[](std::size_t index); - const PointT &operator[](std::size_t index) const; + std::size_t size() const noexcept; + PointT &operator[](std::size_t index) noexcept; + const PointT &operator[](std::size_t index) const noexcept; PointT &at(std::size_t index); const PointT &at(std::size_t index) const; PointT &at_point(std::size_t index); @@ -42,8 +42,8 @@ template class PointCloud final template void emplace_back(Args &&...args); private: - PointCloud *parent_; - std::size_t cluster_index_; + PointCloud *parent_{nullptr}; + std::size_t cluster_index_{std::size_t(-1)}; }; PointCloud() = default; @@ -61,10 +61,10 @@ template class PointCloud final void push_into_last_cluster(const PointT &point); void push_into_last_cluster(PointT &&point); void increment_cluster(); - void clear(); + void clear() noexcept; - std::size_t total_clusters() const; - std::size_t total_points() const; + std::size_t total_clusters() const noexcept; + std::size_t total_points() const noexcept; PointT &operator()(std::size_t cluster_index, std::size_t point_index); const PointT &operator()(std::size_t cluster_index, std::size_t point_index) const; @@ -145,18 +145,18 @@ typename PointCloud::PointsView::ConstReverseIteratorT PointCloud std::size_t PointCloud::PointsView::size() const +template std::size_t PointCloud::PointsView::size() const noexcept { return parent_->clusters_[cluster_index_].second; } -template PointT &PointCloud::PointsView::operator[](std::size_t index) +template PointT &PointCloud::PointsView::operator[](std::size_t index) noexcept { assert(index < size()); return parent_->points_[parent_->clusters_[cluster_index_].first + index]; } -template const PointT &PointCloud::PointsView::operator[](std::size_t index) const +template const PointT &PointCloud::PointsView::operator[](std::size_t index) const noexcept { assert(index < size()); return parent_->points_[parent_->clusters_[cluster_index_].first + index]; @@ -259,18 +259,18 @@ template void PointCloud::increment_cluster() clusters_.emplace_back(points_.size(), 0); } -template void PointCloud::clear() +template void PointCloud::clear() noexcept { points_.clear(); clusters_.clear(); } -template std::size_t PointCloud::total_clusters() const +template std::size_t PointCloud::total_clusters() const noexcept { return clusters_.size(); } -template std::size_t PointCloud::total_points() const +template std::size_t PointCloud::total_points() const noexcept { return points_.size(); } @@ -278,7 +278,7 @@ template std::size_t PointCloud::total_points() const template void PointCloud::add_cluster(const std::vector &new_cluster) { const std::size_t start_index = points_.size(); - points_.insert(points_.end(), new_cluster.begin(), new_cluster.end()); + points_.insert(points_.end(), new_cluster.cbegin(), new_cluster.cend()); clusters_.emplace_back(start_index, new_cluster.size()); } From 7398d4ba5c1fdc0ccc53004144d522d636737b6b Mon Sep 17 00:00:00 2001 From: Yevgeniy Date: Tue, 7 May 2024 02:09:53 +0800 Subject: [PATCH 04/11] Removed non-const version of at_cluster because it is unsafe --- src/pointcloud.hpp | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/src/pointcloud.hpp b/src/pointcloud.hpp index 543fdaf..d6ff679 100644 --- a/src/pointcloud.hpp +++ b/src/pointcloud.hpp @@ -73,7 +73,6 @@ template class PointCloud final void add_cluster(const std::vector &new_cluster); - PointsView at_cluster(std::size_t cluster_index); const PointsView at_cluster(std::size_t cluster_index) const; PointsView last_cluster(); @@ -300,16 +299,6 @@ template const typename PointCloud::PointsView PointCl return PointsView(const_cast *>(this), clusters_.size() - 1); } -template -typename PointCloud::PointsView PointCloud::at_cluster(std::size_t cluster_index) -{ - if (cluster_index >= clusters_.size()) - { - throw std::runtime_error("PointCloud::at_cluster(): Index out of bounds"); - } - return PointsView(this, cluster_index); -} - template const typename PointCloud::PointsView PointCloud::at_cluster(std::size_t cluster_index) const { From fc7d08a4428bfc486cefb6cdc68003522fa0163a Mon Sep 17 00:00:00 2001 From: Yevgeniy Date: Fri, 10 May 2024 00:44:44 +0800 Subject: [PATCH 05/11] Added some cartesian vector adapters --- .vscode/settings.json | 2 + CMakeLists.txt | 2 + src/cartesian_vector_2d_adapter.hpp | 248 ++++++++++++++++++++ src/cartesian_vector_3d_adapter.hpp | 269 ++++++++++++++++++++++ src/delaunator.cpp | 0 src/delaunator.hpp | 105 +++++++++ src/kdtree.hpp | 6 - src/kdtree_3d.hpp | 151 ++++++++++++ src/pointcloud.hpp | 22 ++ test/test_cartesian_vector_2d_adapter.cpp | 107 +++++++++ test/test_cartesian_vector_3d_adapter.cpp | 114 +++++++++ 11 files changed, 1020 insertions(+), 6 deletions(-) create mode 100644 src/cartesian_vector_2d_adapter.hpp create mode 100644 src/cartesian_vector_3d_adapter.hpp create mode 100644 src/delaunator.cpp create mode 100644 src/delaunator.hpp create mode 100644 src/kdtree_3d.hpp create mode 100644 test/test_cartesian_vector_2d_adapter.cpp create mode 100644 test/test_cartesian_vector_3d_adapter.cpp diff --git a/.vscode/settings.json b/.vscode/settings.json index f9e50a9..c3e0a81 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -90,7 +90,9 @@ }, "cmake.configureOnOpen": true, "cSpell.words": [ + "circumradius", "Clusterer", + "DELAUNATOR", "KDTREE", "pointcloud", "polygonize", diff --git a/CMakeLists.txt b/CMakeLists.txt index 9d0d61e..0113d8d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,6 +75,8 @@ if(BUILD_TESTS) add_executable(${TEST_NAME} test/test_kdtree.cpp test/test_pointcloud.cpp + test/test_cartesian_vector_2d_adapter.cpp + test/test_cartesian_vector_3d_adapter.cpp ) target_include_directories(${TEST_NAME} PRIVATE diff --git a/src/cartesian_vector_2d_adapter.hpp b/src/cartesian_vector_2d_adapter.hpp new file mode 100644 index 0000000..5412612 --- /dev/null +++ b/src/cartesian_vector_2d_adapter.hpp @@ -0,0 +1,248 @@ +#ifndef LIDAR_PROCESSING__CARTESIAN_VECTOR_2D_ADAPTER_HPP +#define LIDAR_PROCESSING__CARTESIAN_VECTOR_2D_ADAPTER_HPP + +#include +#include +#include +#include +#include + +namespace containers +{ +class CartesianVector2DAdapter final +{ + public: + class Point2D final + { + public: + void x(float x) noexcept; + void y(float y) noexcept; + + float &x() noexcept; + float &y() noexcept; + + float x() const noexcept; + float y() const noexcept; + + void set(float x, float y) noexcept; + + private: + friend class CartesianVector2DAdapter; + friend class KDTree2D; + + explicit Point2D(float *ptr) noexcept; + + float *ptr_; + }; + + CartesianVector2DAdapter(const std::shared_ptr> &data); + ~CartesianVector2DAdapter() noexcept; + + Point2D operator[](std::size_t index) noexcept; + const Point2D operator[](std::size_t index) const; + + Point2D at(std::size_t index); + const Point2D at(std::size_t index) const; + + float &operator()(std::size_t index, std::size_t dim) noexcept; + float operator()(std::size_t index, std::size_t dim) const noexcept; + + float &at(std::size_t index, std::size_t dim); + float at(std::size_t index, std::size_t dim) const; + + void clear() noexcept; + bool empty() const noexcept; + std::size_t size() const noexcept; + std::size_t capacity() const noexcept; + void resize(std::size_t new_point_count); + void reserve(std::size_t point_count); + void emplace_back(float x, float y); + void assign(std::size_t new_point_count, float x, float y); + void pop_back() noexcept; + + private: + // Stores coordinates: x1, y1, z1, x2, y2, z2, x3, y3, z3, ... , xN, yN, zN + std::shared_ptr> data_; +}; + +inline void CartesianVector2DAdapter::Point2D::x(float x) noexcept +{ + ptr_[0] = x; +} + +inline void CartesianVector2DAdapter::Point2D::y(float y) noexcept +{ + ptr_[1] = y; +} + +inline float &CartesianVector2DAdapter::Point2D::x() noexcept +{ + return ptr_[0]; +} + +inline float &CartesianVector2DAdapter::Point2D::y() noexcept +{ + return ptr_[1]; +} + +inline float CartesianVector2DAdapter::Point2D::x() const noexcept +{ + return ptr_[0]; +} + +inline float CartesianVector2DAdapter::Point2D::y() const noexcept +{ + return ptr_[1]; +} + +inline void CartesianVector2DAdapter::Point2D::set(float x, float y) noexcept +{ + ptr_[0] = x; + ptr_[1] = y; +} + +inline CartesianVector2DAdapter::Point2D::Point2D(float *ptr) noexcept : ptr_(ptr) +{ +} + +inline CartesianVector2DAdapter::CartesianVector2DAdapter(const std::shared_ptr> &data) : data_(data) +{ + if (!data_) + { + throw std::invalid_argument("Data vector cannot be null"); + } +} + +inline CartesianVector2DAdapter::~CartesianVector2DAdapter() noexcept +{ + data_.reset(); +} + +inline CartesianVector2DAdapter::Point2D CartesianVector2DAdapter::operator[](std::size_t index) noexcept +{ + assert(index < size() && "Index out of bounds"); + return Point2D(&(*data_)[index * 2]); +} + +inline const CartesianVector2DAdapter::Point2D CartesianVector2DAdapter::operator[](std::size_t index) const +{ + assert(index < size() && "Index out of bounds"); + return Point2D(&(*data_)[index * 2]); +} + +inline CartesianVector2DAdapter::Point2D CartesianVector2DAdapter::at(std::size_t index) +{ + if (index >= size()) + { + throw std::out_of_range("Index out of bounds"); + } + return Point2D(&(*data_)[index * 2]); +} + +inline const CartesianVector2DAdapter::Point2D CartesianVector2DAdapter::at(std::size_t index) const +{ + if (index >= size()) + { + throw std::out_of_range("Index out of bounds"); + } + return Point2D(&(*data_)[index * 2]); +} + +inline float &CartesianVector2DAdapter::operator()(std::size_t index, std::size_t dim) noexcept +{ + assert((dim < 2) && "Dimension out of bounds"); + assert((index * 2 + dim < data_->size()) && "Index out of bounds"); + return data_->operator[](index * 2 + dim); +} + +inline float CartesianVector2DAdapter::operator()(std::size_t index, std::size_t dim) const noexcept +{ + assert((dim < 2) && "Dimension out of bounds"); + assert((index * 2 + dim < data_->size()) && "Index out of bounds"); + return data_->operator[](index * 2 + dim); +} + +inline float &CartesianVector2DAdapter::at(std::size_t index, std::size_t dim) +{ + if (dim > 1) + { + throw std::out_of_range("Dimension out of bounds"); + } + if (index * 2 + dim >= data_->size()) + { + throw std::out_of_range("Index out of bounds"); + } + return data_->operator[](index * 3 + dim); +} + +inline float CartesianVector2DAdapter::at(std::size_t index, std::size_t dim) const +{ + if (dim > 1) + { + throw std::out_of_range("Dimension out of bounds"); + } + if (index * 2 + dim >= data_->size()) + { + throw std::out_of_range("Index out of bounds"); + } + return data_->operator[](index * 3 + dim); +} + +inline void CartesianVector2DAdapter::clear() noexcept +{ + return data_->clear(); +} + +inline bool CartesianVector2DAdapter::empty() const noexcept +{ + return data_->empty(); +} + +inline std::size_t CartesianVector2DAdapter::size() const noexcept +{ + return data_->size() / 2; +} + +inline std::size_t CartesianVector2DAdapter::capacity() const noexcept +{ + return data_->capacity() / 2; +} + +inline void CartesianVector2DAdapter::resize(std::size_t new_point_count) +{ + data_->resize(new_point_count * 2); +} + +inline void CartesianVector2DAdapter::reserve(std::size_t point_count) +{ + data_->reserve(point_count * 2); +} + +inline void CartesianVector2DAdapter::emplace_back(float x, float y) +{ + data_->push_back(x); + data_->push_back(y); +} + +inline void CartesianVector2DAdapter::assign(std::size_t new_point_count, float x, float y) +{ + data_->resize(new_point_count * 2); + + for (std::size_t i = 0; i < data_->size(); i += 2) + { + (*data_)[i] = x; + (*data_)[i + 1] = y; + } +} + +inline void CartesianVector2DAdapter::pop_back() noexcept +{ + if (empty()) + { + return; + } + data_->resize(data_->size() - 2); +} +} // namespace containers + +#endif // LIDAR_PROCESSING__CARTESIAN_VECTOR_2D_ADAPTER_HPP diff --git a/src/cartesian_vector_3d_adapter.hpp b/src/cartesian_vector_3d_adapter.hpp new file mode 100644 index 0000000..5a06e8b --- /dev/null +++ b/src/cartesian_vector_3d_adapter.hpp @@ -0,0 +1,269 @@ +#ifndef LIDAR_PROCESSING__CARTESIAN_VECTOR_3D_ADAPTER_HPP +#define LIDAR_PROCESSING__CARTESIAN_VECTOR_3D_ADAPTER_HPP + +#include +#include +#include +#include +#include + +namespace containers +{ +class CartesianVector3DAdapter final +{ + public: + class Point3D final + { + public: + void x(float x) noexcept; + void y(float y) noexcept; + void z(float z) noexcept; + + float &x() noexcept; + float &y() noexcept; + float &z() noexcept; + + float x() const noexcept; + float y() const noexcept; + float z() const noexcept; + + void set(float x, float y, float z) noexcept; + + private: + friend class CartesianVector3DAdapter; + friend class KDTree3D; + + explicit Point3D(float *ptr) noexcept; + + float *ptr_; + }; + + CartesianVector3DAdapter(const std::shared_ptr> &data); + ~CartesianVector3DAdapter() noexcept; + + Point3D operator[](std::size_t index) noexcept; + const Point3D operator[](std::size_t index) const; + + Point3D at(std::size_t index); + const Point3D at(std::size_t index) const; + + float &operator()(std::size_t index, std::size_t dim) noexcept; + float operator()(std::size_t index, std::size_t dim) const noexcept; + + float &at(std::size_t index, std::size_t dim); + float at(std::size_t index, std::size_t dim) const; + + void clear() noexcept; + bool empty() const noexcept; + std::size_t size() const noexcept; + std::size_t capacity() const noexcept; + void resize(std::size_t new_point_count); + void reserve(std::size_t point_count); + void emplace_back(float x, float y, float z); + void assign(std::size_t new_point_count, float x, float y, float z); + void pop_back() noexcept; + + private: + // Stores coordinates: x1, y1, z1, x2, y2, z2, x3, y3, z3, ... , xN, yN, zN + std::shared_ptr> data_; +}; + +inline void CartesianVector3DAdapter::Point3D::x(float x) noexcept +{ + ptr_[0] = x; +} + +inline void CartesianVector3DAdapter::Point3D::y(float y) noexcept +{ + ptr_[1] = y; +} + +inline void CartesianVector3DAdapter::Point3D::z(float z) noexcept +{ + ptr_[2] = z; +} + +inline float &CartesianVector3DAdapter::Point3D::x() noexcept +{ + return ptr_[0]; +} + +inline float &CartesianVector3DAdapter::Point3D::y() noexcept +{ + return ptr_[1]; +} + +inline float &CartesianVector3DAdapter::Point3D::z() noexcept +{ + return ptr_[2]; +} + +inline float CartesianVector3DAdapter::Point3D::x() const noexcept +{ + return ptr_[0]; +} + +inline float CartesianVector3DAdapter::Point3D::y() const noexcept +{ + return ptr_[1]; +} + +inline float CartesianVector3DAdapter::Point3D::z() const noexcept +{ + return ptr_[2]; +} + +inline void CartesianVector3DAdapter::Point3D::set(float x, float y, float z) noexcept +{ + ptr_[0] = x; + ptr_[1] = y; + ptr_[2] = z; +} + +inline CartesianVector3DAdapter::Point3D::Point3D(float *ptr) noexcept : ptr_(ptr) +{ +} + +inline CartesianVector3DAdapter::CartesianVector3DAdapter(const std::shared_ptr> &data) : data_(data) +{ + if (!data_) + { + throw std::invalid_argument("Data vector cannot be null"); + } +} + +inline CartesianVector3DAdapter::~CartesianVector3DAdapter() noexcept +{ + data_.reset(); +} + +inline CartesianVector3DAdapter::Point3D CartesianVector3DAdapter::operator[](std::size_t index) noexcept +{ + assert(index < size() && "Index out of bounds"); + return Point3D(&(*data_)[index * 3]); +} + +inline const CartesianVector3DAdapter::Point3D CartesianVector3DAdapter::operator[](std::size_t index) const +{ + assert(index < size() && "Index out of bounds"); + return Point3D(&(*data_)[index * 3]); +} + +inline CartesianVector3DAdapter::Point3D CartesianVector3DAdapter::at(std::size_t index) +{ + if (index >= size()) + { + throw std::out_of_range("Index out of bounds"); + } + return Point3D(&(*data_)[index * 3]); +} + +inline const CartesianVector3DAdapter::Point3D CartesianVector3DAdapter::at(std::size_t index) const +{ + if (index >= size()) + { + throw std::out_of_range("Index out of bounds"); + } + return Point3D(&(*data_)[index * 3]); +} + +inline float &CartesianVector3DAdapter::operator()(std::size_t index, std::size_t dim) noexcept +{ + assert((dim < 3) && "Dimension out of bounds"); + assert((index * 3 + dim < data_->size()) && "Index out of bounds"); + return data_->operator[](index * 3 + dim); +} + +inline float CartesianVector3DAdapter::operator()(std::size_t index, std::size_t dim) const noexcept +{ + assert((dim < 3) && "Dimension out of bounds"); + assert((index * 3 + dim < data_->size()) && "Index out of bounds"); + return data_->operator[](index * 3 + dim); +} + +inline float &CartesianVector3DAdapter::at(std::size_t index, std::size_t dim) +{ + if (dim > 2) + { + throw std::out_of_range("Dimension out of bounds"); + } + if (index * 3 + dim >= data_->size()) + { + throw std::out_of_range("Index out of bounds"); + } + return data_->operator[](index * 3 + dim); +} + +inline float CartesianVector3DAdapter::at(std::size_t index, std::size_t dim) const +{ + if (dim > 2) + { + throw std::out_of_range("Dimension out of bounds"); + } + if (index * 3 + dim >= data_->size()) + { + throw std::out_of_range("Index out of bounds"); + } + return data_->operator[](index * 3 + dim); +} + +inline void CartesianVector3DAdapter::clear() noexcept +{ + return data_->clear(); +} + +inline bool CartesianVector3DAdapter::empty() const noexcept +{ + return data_->empty(); +} + +inline std::size_t CartesianVector3DAdapter::size() const noexcept +{ + return data_->size() / 3; +} + +inline std::size_t CartesianVector3DAdapter::capacity() const noexcept +{ + return data_->capacity() / 3; +} + +inline void CartesianVector3DAdapter::resize(std::size_t new_point_count) +{ + data_->resize(new_point_count * 3); +} + +inline void CartesianVector3DAdapter::reserve(std::size_t point_count) +{ + data_->reserve(point_count * 3); +} + +inline void CartesianVector3DAdapter::emplace_back(float x, float y, float z) +{ + data_->push_back(x); + data_->push_back(y); + data_->push_back(z); +} + +inline void CartesianVector3DAdapter::assign(std::size_t new_point_count, float x, float y, float z) +{ + data_->resize(new_point_count * 3); + + for (std::size_t i = 0; i < data_->size(); i += 3) + { + (*data_)[i] = x; + (*data_)[i + 1] = y; + (*data_)[i + 2] = z; + } +} + +inline void CartesianVector3DAdapter::pop_back() noexcept +{ + if (empty()) + { + return; + } + data_->resize(data_->size() - 3); +} +} // namespace containers + +#endif // LIDAR_PROCESSING__CARTESIAN_VECTOR_3D_ADAPTER_HPP diff --git a/src/delaunator.cpp b/src/delaunator.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/delaunator.hpp b/src/delaunator.hpp new file mode 100644 index 0000000..8d7d756 --- /dev/null +++ b/src/delaunator.hpp @@ -0,0 +1,105 @@ +#ifndef LIDAR_PROCESSING__DELAUNATOR_HPP +#define LIDAR_PROCESSING__DELAUNATOR_HPP + +#include +#include +#include +#include + +namespace lidar_processing +{ +struct PointXY final +{ + double x{0.0F}; + double y{0.0F}; + + PointXY() noexcept = default; + + PointXY(double x, double y) noexcept : x(x), y(y) + { + } + + inline double magnitude_squared() const noexcept + { + return (x * x) + (y * y); + } + + inline double magnitude() const noexcept + { + return std::sqrt(magnitude_squared()); + } + + static double determinant(const PointXY &p1, const PointXY &p2) noexcept + { + return (p1.x * p2.y) - (p1.y * p2.x); + } + + static PointXY vector(const PointXY &v1, const PointXY &v2) noexcept + { + return {v2.x - v1.x, v2.y - v1.y}; + } + + static double distance_squared(const PointXY &v1, const PointXY &v2) noexcept + { + const auto vec = vector(v1, v2); + return (vec.x * vec.x) + (vec.y * vec.y); + } + + static bool equal(const PointXY &p1, const PointXY &p2, double span) noexcept + { + const double dist = distance_squared(p1, p2) / span; + return (dist < std::numeric_limits::epsilon()); + } +}; + +constexpr inline std::size_t fast_modulus(std::size_t i, std::size_t c) noexcept +{ + return (i >= c) ? (i % c) : i; +} + +constexpr inline double sum(const std::vector &x) noexcept +{ + double sum = 0.0; + double err = 0.0; + + for (std::size_t i = 1; i < x.size(); ++i) + { + const double k = x[i]; + const double m = sum + k; + + err += (std::fabs(sum) >= std::fabs(k)) ? (sum - m + k) : (k - m + sum); + sum = m; + } + + return sum + err; +} + +constexpr inline double distance_squared(const double ax, const double ay, const double bx, const double by) noexcept +{ + const double dx = ax - bx; + const double dy = ay - by; + + return dx * dx + dy * dy; +} + +// constexpr inline double circumradius(const Point) + +constexpr inline std::size_t next_half_edge(std::size_t e) noexcept +{ + return (e % 3 == 2) ? (e - 2) : (e + 1); +} + +constexpr inline std::size_t prev_half_edge(std::size_t e) noexcept +{ + return (e % 3 == 0) ? (e + 2) : (e - 1); +} + +class Delaunator final +{ + public: + private: +}; + +} // namespace lidar_processing + +#endif // LIDAR_PROCESSING__DELAUNATOR_HPP diff --git a/src/kdtree.hpp b/src/kdtree.hpp index 104dbd3..fe99f03 100644 --- a/src/kdtree.hpp +++ b/src/kdtree.hpp @@ -31,7 +31,6 @@ #include // std::sort, std::nth_element #include // std::array #include // std::size_t -#include // std::runtime_error #include // std::pair namespace lidar_processing @@ -217,11 +216,6 @@ template void KDTree::rebuild(const conta rebuild_stack_.emplace(mid + 1, end, &((*node_ref)->right), depth + 1); } } - - if (root_ == nullptr) - { - throw std::runtime_error("KDTree is empty"); - } } template diff --git a/src/kdtree_3d.hpp b/src/kdtree_3d.hpp new file mode 100644 index 0000000..4667604 --- /dev/null +++ b/src/kdtree_3d.hpp @@ -0,0 +1,151 @@ + + +#ifndef LIDAR_PROCESSING__KDTREE_3D_HPP +#define LIDAR_PROCESSING__KDTREE_3D_HPP + +// Internal +#include "cartesian_vector_3d_adapter.hpp" +#include "priority_queue.hpp" +#include "stack.hpp" +#include "vector.hpp" + +// STL +#include +#include +#include + +namespace lidar_processing +{ +using namespace containers; + +class KDTree3D final +{ + using Point3D = CartesianVector3DAdapter::Point3D; + + struct Node final + { + Point3D point{nullptr}; + std::uint32_t index{0U}; + Node *left{nullptr}; + Node *right{nullptr}; + + Node() = default; + explicit Node(float *ptr, std::uint32_t index) : point(ptr), index(index) + { + } + }; + + public: + using RetT = std::pair; + + struct RetTCompare final + { + bool operator()(const RetT &a, const RetT &b) const noexcept + { + return a.second < b.second; + } + }; + + KDTree3D(bool sort = false, std::uint8_t num_threads = 1); + + static constexpr float dist_sqr(const Point3D &a, const Point3D &b) noexcept; + + void reserve(std::uint32_t num_pts = 200'000U); + void rebuild(const float *coordinates, std::uint32_t num_pts); // x1, y1, z1, x2, y2, z2, ... + void k_nearest(const float *target, std::uint32_t num_neigh, Vector &neigh); + void radius_search(const float *target, float proximity_sqr, Vector &neigh); + + private: + struct RebuildStack final + { + typename Vector::iterator begin{nullptr}; + typename Vector::iterator end{nullptr}; + Node **node_ref{nullptr}; + std::uint32_t depth{0U}; + + RebuildStack() = default; + explicit RebuildStack(typename Vector::iterator begin, typename Vector::iterator end, + Node **node_ref, std::uint32_t depth) + : begin(begin), end(end), node_ref(node_ref), depth(depth) + { + } + }; + + struct KNearestStack final + { + const Node *node_ptr{nullptr}; + std::uint32_t index{0U}; + bool first_visit{true}; + + KNearestStack() = default; + KNearestStack(const Node *node_ptr, std::uint32_t index, bool first_visit) + : node_ptr(node_ptr), index(index), first_visit(first_visit) + { + } + }; + + struct RadiusSearchStack final + { + const Node *node_ptr{nullptr}; + std::uint32_t index{0U}; + + RadiusSearchStack() = default; + RadiusSearchStack(const Node *node_ptr, std::uint32_t index) : node_ptr(node_ptr), index(index) + { + } + }; + + bool sort_; + bool threaded_; + std::uint8_t num_threads_; + + Node *root_{nullptr}; + Vector nodes_; + Stack rebuild_stack_; + Stack k_nearest_stack_; + Stack radius_search_stack_; + PriorityQueue min_heap_; +}; + +inline KDTree3D::KDTree3D(bool sort, std::uint8_t num_threads) + : sort_(sort), threaded_(num_threads > 1), num_threads_(num_threads) +{ + reserve(); +} + +inline constexpr float KDTree3D::dist_sqr(const Point3D &a, const Point3D &b) noexcept +{ + const float dx = b.x() - a.x(); + const float dy = b.y() - a.y(); + const float dz = b.z() - a.z(); + + return dx * dx + dy * dy + dz * dz; +} + +inline void KDTree3D::reserve(std::uint32_t num_pts) +{ + nodes_.reserve(num_pts); + rebuild_stack_.reserve(num_pts); + k_nearest_stack_.reserve(num_pts); + radius_search_stack_.reserve(num_pts); + min_heap_.reserve(num_pts); +} + +inline void KDTree3D::rebuild(const float *coordinates, std::uint32_t num_pts) +{ + // TODO +} + +inline void KDTree3D::k_nearest(const float *target, std::uint32_t num_neigh, Vector &neigh) +{ + // TODO +} + +inline void KDTree3D::radius_search(const float *target, float proximity_sqr, Vector &neigh) +{ + // TODO +} + +} // namespace lidar_processing + +#endif // LIDAR_PROCESSING__KDTREE_3D_HPP diff --git a/src/pointcloud.hpp b/src/pointcloud.hpp index d6ff679..f8647ee 100644 --- a/src/pointcloud.hpp +++ b/src/pointcloud.hpp @@ -1,3 +1,25 @@ +/* + * Copyright (c) 2024 Yevgeniy Simonov + * + * 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 #include #include diff --git a/test/test_cartesian_vector_2d_adapter.cpp b/test/test_cartesian_vector_2d_adapter.cpp new file mode 100644 index 0000000..0cabe36 --- /dev/null +++ b/test/test_cartesian_vector_2d_adapter.cpp @@ -0,0 +1,107 @@ +#include "cartesian_vector_2d_adapter.hpp" +#include +#include + +using namespace containers; + +TEST(CartesianVector2DAdapterTest, ConstructEmpty) +{ + auto data = std::make_shared>(); + CartesianVector2DAdapter adapter(data); + EXPECT_EQ(adapter.size(), 0); +} + +TEST(CartesianVector2DAdapterTest, AddPointsAndAccess) +{ + auto data = std::make_shared>(); + CartesianVector2DAdapter adapter(data); + adapter.emplace_back(1.0f, 2.0f); + adapter.emplace_back(4.0f, 5.0f); + + EXPECT_EQ(adapter.size(), 2); + + auto point = adapter[0]; + EXPECT_FLOAT_EQ(point.x(), 1.0f); + EXPECT_FLOAT_EQ(point.y(), 2.0f); + + adapter[0].set(10.0f, 20.0f); + EXPECT_FLOAT_EQ(adapter[0].x(), 10.0f); + EXPECT_FLOAT_EQ(adapter[0].y(), 20.0f); +} + +TEST(CartesianVector2DAdapterTest, ResizeAndAccessOutOfBounds) +{ + auto data = std::make_shared>(); + CartesianVector2DAdapter adapter(data); + adapter.reserve(100); + EXPECT_EQ(adapter.capacity(), 100); + adapter.resize(2); + EXPECT_EQ(adapter.size(), 2); + + EXPECT_THROW(adapter.at(2), std::out_of_range); +} + +TEST(CartesianVector2DAdapterTest, DirectDimensionAccess) +{ + auto data = std::make_shared>(std::initializer_list{1.0f, 2.0f, 4.0f, 5.0f}); + CartesianVector2DAdapter adapter(data); + + EXPECT_FLOAT_EQ(adapter(0, 0), 1.0f); + EXPECT_FLOAT_EQ(adapter(0, 1), 2.0f); + + adapter(1, 0) = 10.0f; + adapter(1, 1) = 20.0f; + + EXPECT_FLOAT_EQ(adapter(1, 0), 10.0f); + EXPECT_FLOAT_EQ(adapter(1, 1), 20.0f); +} + +TEST(CartesianVector2DAdapterTest, BoundsCheckedAccess) +{ + auto data = std::make_shared>(std::initializer_list{1.0f, 2.0f, 4.0f, 5.0f}); + CartesianVector2DAdapter adapter(data); + + EXPECT_FLOAT_EQ(adapter.at(0, 0), 1.0f); + EXPECT_FLOAT_EQ(adapter.at(0, 1), 2.0f); + + EXPECT_THROW(adapter.at(2, 0), std::out_of_range); // Should throw because index 5 is out of bounds + EXPECT_THROW(adapter.at(0, 2), std::out_of_range); // Should throw because dim 2 is out of data range + EXPECT_NO_THROW(adapter.at(1, 1)); +} + +TEST(CartesianVector2DAdapterTest, AssignFunctionality) +{ + auto data = std::make_shared>(); + CartesianVector2DAdapter adapter(data); + adapter.assign(3, 1.0f, 2.0f); + + ASSERT_EQ(adapter.size(), 3); + EXPECT_FLOAT_EQ(adapter(0, 0), 1.0f); +} + +TEST(CartesianVector2DAdapterTest, ClearFunctionality) +{ + auto data = std::make_shared>(std::initializer_list{1.0f, 2.0f}); + CartesianVector2DAdapter adapter(data); + adapter.clear(); + EXPECT_EQ(adapter.size(), 0); + EXPECT_TRUE(adapter.empty()); + EXPECT_EQ(data->size(), 0); +} + +TEST(CartesianVector2DAdapterTest, PopBackFunctionality) +{ + auto data = std::make_shared>(std::initializer_list{1.0f, 2.0f, 4.0f, 5.0f}); + CartesianVector2DAdapter adapter(data); + adapter.pop_back(); + EXPECT_EQ(adapter.size(), 1); +} + +TEST(CartesianVector2DAdapterTest, CapacityTest) +{ + auto data = std::make_shared>(7); + CartesianVector2DAdapter adapter(data); + EXPECT_EQ(adapter.capacity(), 3); // Capacity is the number of complete points the vector can hold + adapter.resize(2); + EXPECT_GE(adapter.capacity(), 2); +} diff --git a/test/test_cartesian_vector_3d_adapter.cpp b/test/test_cartesian_vector_3d_adapter.cpp new file mode 100644 index 0000000..3aad2c8 --- /dev/null +++ b/test/test_cartesian_vector_3d_adapter.cpp @@ -0,0 +1,114 @@ +#include "cartesian_vector_3d_adapter.hpp" +#include +#include + +using namespace containers; + +TEST(CartesianVector3DAdapterTest, ConstructEmpty) +{ + auto data = std::make_shared>(); + CartesianVector3DAdapter adapter(data); + EXPECT_EQ(adapter.size(), 0); +} + +TEST(CartesianVector3DAdapterTest, AddPointsAndAccess) +{ + auto data = std::make_shared>(); + CartesianVector3DAdapter adapter(data); + adapter.emplace_back(1.0f, 2.0f, 3.0f); + adapter.emplace_back(4.0f, 5.0f, 6.0f); + + EXPECT_EQ(adapter.size(), 2); + + auto point = adapter[0]; + EXPECT_FLOAT_EQ(point.x(), 1.0f); + EXPECT_FLOAT_EQ(point.y(), 2.0f); + EXPECT_FLOAT_EQ(point.z(), 3.0f); + + adapter[0].set(10.0f, 20.0f, 30.0f); + EXPECT_FLOAT_EQ(adapter[0].x(), 10.0f); + EXPECT_FLOAT_EQ(adapter[0].y(), 20.0f); + EXPECT_FLOAT_EQ(adapter[0].z(), 30.0f); +} + +TEST(CartesianVector3DAdapterTest, ResizeAndAccessOutOfBounds) +{ + auto data = std::make_shared>(); + CartesianVector3DAdapter adapter(data); + adapter.reserve(100); + EXPECT_EQ(adapter.capacity(), 100); + adapter.resize(2); + EXPECT_EQ(adapter.size(), 2); + + EXPECT_THROW(adapter.at(2), std::out_of_range); +} + +TEST(CartesianVector3DAdapterTest, DirectDimensionAccess) +{ + auto data = std::make_shared>(std::initializer_list{1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f}); + CartesianVector3DAdapter adapter(data); + + EXPECT_FLOAT_EQ(adapter(0, 0), 1.0f); + EXPECT_FLOAT_EQ(adapter(0, 1), 2.0f); + EXPECT_FLOAT_EQ(adapter(0, 2), 3.0f); + + adapter(1, 0) = 10.0f; + adapter(1, 1) = 20.0f; + adapter(1, 2) = 30.0f; + + EXPECT_FLOAT_EQ(adapter(1, 0), 10.0f); + EXPECT_FLOAT_EQ(adapter(1, 1), 20.0f); + EXPECT_FLOAT_EQ(adapter(1, 2), 30.0f); +} + +TEST(CartesianVector3DAdapterTest, BoundsCheckedAccess) +{ + auto data = std::make_shared>(std::initializer_list{1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f}); + CartesianVector3DAdapter adapter(data); + + EXPECT_FLOAT_EQ(adapter.at(0, 0), 1.0f); + EXPECT_FLOAT_EQ(adapter.at(0, 1), 2.0f); + EXPECT_FLOAT_EQ(adapter.at(0, 2), 3.0f); + + EXPECT_THROW(adapter.at(2, 0), std::out_of_range); // Should throw because index 6 is out of bounds + EXPECT_THROW(adapter.at(0, 3), std::out_of_range); // Should throw because dim 3 is out of data range + EXPECT_NO_THROW(adapter.at(1, 2)); +} + +TEST(CartesianVector3DAdapterTest, AssignFunctionality) +{ + auto data = std::make_shared>(); + CartesianVector3DAdapter adapter(data); + adapter.assign(3, 1.0f, 2.0f, 3.0f); + + ASSERT_EQ(adapter.size(), 3); + EXPECT_FLOAT_EQ(adapter(0, 0), 1.0f); + EXPECT_FLOAT_EQ(adapter(2, 2), 3.0f); +} + +TEST(CartesianVector3DAdapterTest, ClearFunctionality) +{ + auto data = std::make_shared>(std::initializer_list{1.0f, 2.0f, 3.0f}); + CartesianVector3DAdapter adapter(data); + adapter.clear(); + EXPECT_EQ(adapter.size(), 0); + EXPECT_TRUE(adapter.empty()); + EXPECT_EQ(data->size(), 0); +} + +TEST(CartesianVector3DAdapterTest, PopBackFunctionality) +{ + auto data = std::make_shared>(std::initializer_list{1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f}); + CartesianVector3DAdapter adapter(data); + adapter.pop_back(); + EXPECT_EQ(adapter.size(), 1); +} + +TEST(CartesianVector3DAdapterTest, CapacityTest) +{ + auto data = std::make_shared>(10); + CartesianVector3DAdapter adapter(data); + EXPECT_EQ(adapter.capacity(), 3); // Capacity is the number of complete points the vector can hold + adapter.resize(2); + EXPECT_GE(adapter.capacity(), 2); +} From be72e4dccf9a00265eb9d3017a8eb005c4879c69 Mon Sep 17 00:00:00 2001 From: Yevgeniy Date: Fri, 10 May 2024 00:50:22 +0800 Subject: [PATCH 06/11] Minor fixes --- src/cartesian_vector_2d_adapter.hpp | 4 ++-- src/cartesian_vector_3d_adapter.hpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/cartesian_vector_2d_adapter.hpp b/src/cartesian_vector_2d_adapter.hpp index 5412612..5aebaae 100644 --- a/src/cartesian_vector_2d_adapter.hpp +++ b/src/cartesian_vector_2d_adapter.hpp @@ -172,7 +172,7 @@ inline float &CartesianVector2DAdapter::at(std::size_t index, std::size_t dim) { throw std::out_of_range("Index out of bounds"); } - return data_->operator[](index * 3 + dim); + return (*data_)[index * 2 + dim]; } inline float CartesianVector2DAdapter::at(std::size_t index, std::size_t dim) const @@ -185,7 +185,7 @@ inline float CartesianVector2DAdapter::at(std::size_t index, std::size_t dim) co { throw std::out_of_range("Index out of bounds"); } - return data_->operator[](index * 3 + dim); + return (*data_)[index * 2 + dim]; } inline void CartesianVector2DAdapter::clear() noexcept diff --git a/src/cartesian_vector_3d_adapter.hpp b/src/cartesian_vector_3d_adapter.hpp index 5a06e8b..b372acc 100644 --- a/src/cartesian_vector_3d_adapter.hpp +++ b/src/cartesian_vector_3d_adapter.hpp @@ -171,14 +171,14 @@ inline float &CartesianVector3DAdapter::operator()(std::size_t index, std::size_ { assert((dim < 3) && "Dimension out of bounds"); assert((index * 3 + dim < data_->size()) && "Index out of bounds"); - return data_->operator[](index * 3 + dim); + return (*data_)[index * 3 + dim]; } inline float CartesianVector3DAdapter::operator()(std::size_t index, std::size_t dim) const noexcept { assert((dim < 3) && "Dimension out of bounds"); assert((index * 3 + dim < data_->size()) && "Index out of bounds"); - return data_->operator[](index * 3 + dim); + return (*data_)[index * 3 + dim]; } inline float &CartesianVector3DAdapter::at(std::size_t index, std::size_t dim) @@ -191,7 +191,7 @@ inline float &CartesianVector3DAdapter::at(std::size_t index, std::size_t dim) { throw std::out_of_range("Index out of bounds"); } - return data_->operator[](index * 3 + dim); + return (*data_)[index * 3 + dim]; } inline float CartesianVector3DAdapter::at(std::size_t index, std::size_t dim) const @@ -204,7 +204,7 @@ inline float CartesianVector3DAdapter::at(std::size_t index, std::size_t dim) co { throw std::out_of_range("Index out of bounds"); } - return data_->operator[](index * 3 + dim); + return (*data_)[index * 3 + dim]; } inline void CartesianVector3DAdapter::clear() noexcept From a4525a1e7b927c4037c165b7c21aacefc5988f2c Mon Sep 17 00:00:00 2001 From: Yevgeniy Date: Fri, 10 May 2024 00:52:44 +0800 Subject: [PATCH 07/11] Fixed namespaces and added copyright statements --- src/cartesian_vector_2d_adapter.hpp | 28 +++++++++++++++++++++++++--- src/cartesian_vector_3d_adapter.hpp | 28 +++++++++++++++++++++++++--- 2 files changed, 50 insertions(+), 6 deletions(-) diff --git a/src/cartesian_vector_2d_adapter.hpp b/src/cartesian_vector_2d_adapter.hpp index 5aebaae..9530d90 100644 --- a/src/cartesian_vector_2d_adapter.hpp +++ b/src/cartesian_vector_2d_adapter.hpp @@ -1,5 +1,27 @@ -#ifndef LIDAR_PROCESSING__CARTESIAN_VECTOR_2D_ADAPTER_HPP -#define LIDAR_PROCESSING__CARTESIAN_VECTOR_2D_ADAPTER_HPP +/* + * Copyright (c) 2024 Yevgeniy Simonov + * + * 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. + */ + +#ifndef CONTAINERS__CARTESIAN_VECTOR_2D_ADAPTER_HPP +#define CONTAINERS__CARTESIAN_VECTOR_2D_ADAPTER_HPP #include #include @@ -245,4 +267,4 @@ inline void CartesianVector2DAdapter::pop_back() noexcept } } // namespace containers -#endif // LIDAR_PROCESSING__CARTESIAN_VECTOR_2D_ADAPTER_HPP +#endif // CONTAINERS__CARTESIAN_VECTOR_2D_ADAPTER_HPP diff --git a/src/cartesian_vector_3d_adapter.hpp b/src/cartesian_vector_3d_adapter.hpp index b372acc..0017d68 100644 --- a/src/cartesian_vector_3d_adapter.hpp +++ b/src/cartesian_vector_3d_adapter.hpp @@ -1,5 +1,27 @@ -#ifndef LIDAR_PROCESSING__CARTESIAN_VECTOR_3D_ADAPTER_HPP -#define LIDAR_PROCESSING__CARTESIAN_VECTOR_3D_ADAPTER_HPP +/* + * Copyright (c) 2024 Yevgeniy Simonov + * + * 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. + */ + +#ifndef CONTAINERS__CARTESIAN_VECTOR_3D_ADAPTER_HPP +#define CONTAINERS__CARTESIAN_VECTOR_3D_ADAPTER_HPP #include #include @@ -266,4 +288,4 @@ inline void CartesianVector3DAdapter::pop_back() noexcept } } // namespace containers -#endif // LIDAR_PROCESSING__CARTESIAN_VECTOR_3D_ADAPTER_HPP +#endif // CONTAINERS__CARTESIAN_VECTOR_3D_ADAPTER_HPP From 972e3bb55b1610a2541f6a133ce21811b651191d Mon Sep 17 00:00:00 2001 From: Yevgeniy Date: Fri, 10 May 2024 01:02:09 +0800 Subject: [PATCH 08/11] Added swap() methods --- src/cartesian_vector_2d_adapter.hpp | 14 ++++++++++++++ src/cartesian_vector_3d_adapter.hpp | 14 ++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/src/cartesian_vector_2d_adapter.hpp b/src/cartesian_vector_2d_adapter.hpp index 9530d90..fbee4c2 100644 --- a/src/cartesian_vector_2d_adapter.hpp +++ b/src/cartesian_vector_2d_adapter.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include namespace containers @@ -48,6 +49,9 @@ class CartesianVector2DAdapter final void set(float x, float y) noexcept; + void swap(Point2D &other) noexcept; + friend void swap(Point2D &a, Point2D &b) noexcept; + private: friend class CartesianVector2DAdapter; friend class KDTree2D; @@ -123,6 +127,16 @@ inline void CartesianVector2DAdapter::Point2D::set(float x, float y) noexcept ptr_[1] = y; } +inline void CartesianVector2DAdapter::Point2D::swap(Point2D &other) noexcept +{ + std::swap(this->ptr_, other.ptr_); +} + +inline void swap(CartesianVector2DAdapter::Point2D &a, CartesianVector2DAdapter::Point2D &b) noexcept +{ + a.swap(b); +} + inline CartesianVector2DAdapter::Point2D::Point2D(float *ptr) noexcept : ptr_(ptr) { } diff --git a/src/cartesian_vector_3d_adapter.hpp b/src/cartesian_vector_3d_adapter.hpp index 0017d68..8067062 100644 --- a/src/cartesian_vector_3d_adapter.hpp +++ b/src/cartesian_vector_3d_adapter.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include namespace containers @@ -51,6 +52,9 @@ class CartesianVector3DAdapter final void set(float x, float y, float z) noexcept; + void swap(Point3D &other) noexcept; + void swap(Point3D &a, Point3D &b) noexcept; + private: friend class CartesianVector3DAdapter; friend class KDTree3D; @@ -142,6 +146,16 @@ inline void CartesianVector3DAdapter::Point3D::set(float x, float y, float z) no ptr_[2] = z; } +inline void CartesianVector3DAdapter::Point3D::swap(Point3D &other) noexcept +{ + std::swap(this->ptr_, other.ptr_); +} + +inline void swap(CartesianVector3DAdapter::Point3D &a, CartesianVector3DAdapter::Point3D &b) noexcept +{ + a.swap(b); +} + inline CartesianVector3DAdapter::Point3D::Point3D(float *ptr) noexcept : ptr_(ptr) { } From 5e3d690bc9433760fd15c012912befd41e2b23e3 Mon Sep 17 00:00:00 2001 From: Yevgeniy Date: Fri, 10 May 2024 01:02:46 +0800 Subject: [PATCH 09/11] Minor --- src/cartesian_vector_3d_adapter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/cartesian_vector_3d_adapter.hpp b/src/cartesian_vector_3d_adapter.hpp index 8067062..5a63eb2 100644 --- a/src/cartesian_vector_3d_adapter.hpp +++ b/src/cartesian_vector_3d_adapter.hpp @@ -53,7 +53,7 @@ class CartesianVector3DAdapter final void set(float x, float y, float z) noexcept; void swap(Point3D &other) noexcept; - void swap(Point3D &a, Point3D &b) noexcept; + friend void swap(Point3D &a, Point3D &b) noexcept; private: friend class CartesianVector3DAdapter; From 2e9ec2290fe369038ac79ae0f2777304dff2df21 Mon Sep 17 00:00:00 2001 From: Yevgeniy Date: Fri, 31 May 2024 11:46:27 +0800 Subject: [PATCH 10/11] Minor --- src/dataloader.cpp | 2 +- src/processor.cpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/dataloader.cpp b/src/dataloader.cpp index 7e43650..aad9a5d 100644 --- a/src/dataloader.cpp +++ b/src/dataloader.cpp @@ -170,7 +170,7 @@ void Dataloader::publish_next_sample() publisher_->publish(msg); last_timestamp_ += PUBLICATION_RATE; - RCLCPP_INFO(this->get_logger(), "Published at [%ds, %uns]", msg.header.stamp.sec, msg.header.stamp.nanosec); + // RCLCPP_INFO(this->get_logger(), "Published at [%ds, %uns]", msg.header.stamp.sec, msg.header.stamp.nanosec); ++current_cloud_; } diff --git a/src/processor.cpp b/src/processor.cpp index 7688233..fabb3f5 100644 --- a/src/processor.cpp +++ b/src/processor.cpp @@ -98,6 +98,8 @@ class Processor : public rclcpp::Node publisher_obstacle_cloud_ = this->create_publisher("obstacle_pointcloud", qos); publisher_clustered_cloud_ = this->create_publisher("clustered_pointcloud", qos); publisher_polygonization_ = this->create_publisher("polygonization", qos); + + RCLCPP_INFO(this->get_logger(), "Processor node constructed."); } ~Processor() = default; From ba9cd1424bb34d373d84446dee85f30e4ec09b9f Mon Sep 17 00:00:00 2001 From: Yevgeniy Date: Fri, 31 May 2024 11:57:28 +0800 Subject: [PATCH 11/11] Removed kdtree_3d.hpp --- src/kdtree_3d.hpp | 151 ---------------------------------------------- 1 file changed, 151 deletions(-) delete mode 100644 src/kdtree_3d.hpp diff --git a/src/kdtree_3d.hpp b/src/kdtree_3d.hpp deleted file mode 100644 index 4667604..0000000 --- a/src/kdtree_3d.hpp +++ /dev/null @@ -1,151 +0,0 @@ - - -#ifndef LIDAR_PROCESSING__KDTREE_3D_HPP -#define LIDAR_PROCESSING__KDTREE_3D_HPP - -// Internal -#include "cartesian_vector_3d_adapter.hpp" -#include "priority_queue.hpp" -#include "stack.hpp" -#include "vector.hpp" - -// STL -#include -#include -#include - -namespace lidar_processing -{ -using namespace containers; - -class KDTree3D final -{ - using Point3D = CartesianVector3DAdapter::Point3D; - - struct Node final - { - Point3D point{nullptr}; - std::uint32_t index{0U}; - Node *left{nullptr}; - Node *right{nullptr}; - - Node() = default; - explicit Node(float *ptr, std::uint32_t index) : point(ptr), index(index) - { - } - }; - - public: - using RetT = std::pair; - - struct RetTCompare final - { - bool operator()(const RetT &a, const RetT &b) const noexcept - { - return a.second < b.second; - } - }; - - KDTree3D(bool sort = false, std::uint8_t num_threads = 1); - - static constexpr float dist_sqr(const Point3D &a, const Point3D &b) noexcept; - - void reserve(std::uint32_t num_pts = 200'000U); - void rebuild(const float *coordinates, std::uint32_t num_pts); // x1, y1, z1, x2, y2, z2, ... - void k_nearest(const float *target, std::uint32_t num_neigh, Vector &neigh); - void radius_search(const float *target, float proximity_sqr, Vector &neigh); - - private: - struct RebuildStack final - { - typename Vector::iterator begin{nullptr}; - typename Vector::iterator end{nullptr}; - Node **node_ref{nullptr}; - std::uint32_t depth{0U}; - - RebuildStack() = default; - explicit RebuildStack(typename Vector::iterator begin, typename Vector::iterator end, - Node **node_ref, std::uint32_t depth) - : begin(begin), end(end), node_ref(node_ref), depth(depth) - { - } - }; - - struct KNearestStack final - { - const Node *node_ptr{nullptr}; - std::uint32_t index{0U}; - bool first_visit{true}; - - KNearestStack() = default; - KNearestStack(const Node *node_ptr, std::uint32_t index, bool first_visit) - : node_ptr(node_ptr), index(index), first_visit(first_visit) - { - } - }; - - struct RadiusSearchStack final - { - const Node *node_ptr{nullptr}; - std::uint32_t index{0U}; - - RadiusSearchStack() = default; - RadiusSearchStack(const Node *node_ptr, std::uint32_t index) : node_ptr(node_ptr), index(index) - { - } - }; - - bool sort_; - bool threaded_; - std::uint8_t num_threads_; - - Node *root_{nullptr}; - Vector nodes_; - Stack rebuild_stack_; - Stack k_nearest_stack_; - Stack radius_search_stack_; - PriorityQueue min_heap_; -}; - -inline KDTree3D::KDTree3D(bool sort, std::uint8_t num_threads) - : sort_(sort), threaded_(num_threads > 1), num_threads_(num_threads) -{ - reserve(); -} - -inline constexpr float KDTree3D::dist_sqr(const Point3D &a, const Point3D &b) noexcept -{ - const float dx = b.x() - a.x(); - const float dy = b.y() - a.y(); - const float dz = b.z() - a.z(); - - return dx * dx + dy * dy + dz * dz; -} - -inline void KDTree3D::reserve(std::uint32_t num_pts) -{ - nodes_.reserve(num_pts); - rebuild_stack_.reserve(num_pts); - k_nearest_stack_.reserve(num_pts); - radius_search_stack_.reserve(num_pts); - min_heap_.reserve(num_pts); -} - -inline void KDTree3D::rebuild(const float *coordinates, std::uint32_t num_pts) -{ - // TODO -} - -inline void KDTree3D::k_nearest(const float *target, std::uint32_t num_neigh, Vector &neigh) -{ - // TODO -} - -inline void KDTree3D::radius_search(const float *target, float proximity_sqr, Vector &neigh) -{ - // TODO -} - -} // namespace lidar_processing - -#endif // LIDAR_PROCESSING__KDTREE_3D_HPP