diff --git a/.vscode/settings.json b/.vscode/settings.json index 1514aec..c3e0a81 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -90,8 +90,11 @@ }, "cmake.configureOnOpen": true, "cSpell.words": [ + "circumradius", "Clusterer", + "DELAUNATOR", "KDTREE", + "pointcloud", "polygonize", "Polygonizer" ] diff --git a/CMakeLists.txt b/CMakeLists.txt index 20ec3d7..0113d8d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,27 +69,23 @@ 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 + test/test_cartesian_vector_2d_adapter.cpp + test/test_cartesian_vector_3d_adapter.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/cartesian_vector_2d_adapter.hpp b/src/cartesian_vector_2d_adapter.hpp new file mode 100644 index 0000000..fbee4c2 --- /dev/null +++ b/src/cartesian_vector_2d_adapter.hpp @@ -0,0 +1,284 @@ +/* + * 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 +#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; + + void swap(Point2D &other) noexcept; + friend void swap(Point2D &a, Point2D &b) 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 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) +{ +} + +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_)[index * 2 + 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_)[index * 2 + 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 // CONTAINERS__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..5a63eb2 --- /dev/null +++ b/src/cartesian_vector_3d_adapter.hpp @@ -0,0 +1,305 @@ +/* + * 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 +#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; + + void swap(Point3D &other) noexcept; + friend void swap(Point3D &a, Point3D &b) 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 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) +{ +} + +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_)[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_)[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_)[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_)[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 // CONTAINERS__CARTESIAN_VECTOR_3D_ADAPTER_HPP 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/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/pointcloud.hpp b/src/pointcloud.hpp new file mode 100644 index 0000000..f8647ee --- /dev/null +++ b/src/pointcloud.hpp @@ -0,0 +1,359 @@ +/* + * 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 +#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 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); + 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_{nullptr}; + std::size_t cluster_index_{std::size_t(-1)}; + }; + + 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() noexcept; + + 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; + 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); + + 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 noexcept +{ + return parent_->clusters_[cluster_index_].second; +} + +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 noexcept +{ + 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() noexcept +{ + points_.clear(); + clusters_.clear(); +} + +template std::size_t PointCloud::total_clusters() const noexcept +{ + return clusters_.size(); +} + +template std::size_t PointCloud::total_points() const noexcept +{ + 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.cbegin(), new_cluster.cend()); + 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 +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/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; 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); +} 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); +}