diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt index fc0cf38..2aba443 100755 --- a/cpp/CMakeLists.txt +++ b/cpp/CMakeLists.txt @@ -45,6 +45,7 @@ find_external_dependency("Eigen3" "Eigen3::Eigen" "${CMAKE_CURRENT_LIST_DIR}/cma set(PARENT_PROJECT_NAME ${PROJECT_NAME}) set(TARGET_NAME ground_seg_cores) +add_subdirectory(common) add_subdirectory(patchworkpp) add_subdirectory(patchwork) diff --git a/cpp/common/CMakeLists.txt b/cpp/common/CMakeLists.txt new file mode 100644 index 0000000..58e57f2 --- /dev/null +++ b/cpp/common/CMakeLists.txt @@ -0,0 +1,25 @@ +project(patchwork_common_src) + +include(GNUInstallDirs) + +set(COMMON_TARGET ground_seg_common) + +add_library(${COMMON_TARGET} STATIC src/plane_fit.cpp) +set_target_properties(${COMMON_TARGET} PROPERTIES POSITION_INDEPENDENT_CODE ON) + +target_include_directories(${COMMON_TARGET} PUBLIC + $ + $ + $ +) +target_link_libraries(${COMMON_TARGET} Eigen3::Eigen) +add_library(${PARENT_PROJECT_NAME}::${COMMON_TARGET} ALIAS ${COMMON_TARGET}) + +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ + DESTINATION include +) +install(TARGETS ${COMMON_TARGET} + EXPORT ${PARENT_PROJECT_NAME}Config + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} +) diff --git a/cpp/common/include/patchwork/plane_fit.h b/cpp/common/include/patchwork/plane_fit.h new file mode 100644 index 0000000..78279f1 --- /dev/null +++ b/cpp/common/include/patchwork/plane_fit.h @@ -0,0 +1,34 @@ +#ifndef PATCHWORK_COMMON_PLANE_FIT_H +#define PATCHWORK_COMMON_PLANE_FIT_H + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#include +#include + +#include + +#include "patchwork/types.h" + +namespace patchwork { + +/// SVD-based plane fit for the seed set. Fills every field of `out`, +/// including `d_ = -normal . mean` and `th_dist_d_ = th_dist - d_`. +/// No-op when `seeds` is empty (leaves `out` untouched). +void estimate_plane(const std::vector& seeds, PCAFeature& out, float th_dist); + +/// Polar angle in [0, 2*pi). Stable at `(0, 0)` (returns 0). +inline double xy2theta(double x, double y) { + const double a = std::atan2(y, x); + return (a >= 0.0) ? a : (a + 2.0 * M_PI); +} + +inline double xy2radius(double x, double y) { return std::hypot(x, y); } + +inline bool point_z_cmp(const PointXYZ& a, const PointXYZ& b) { return a.z < b.z; } + +} // namespace patchwork + +#endif // PATCHWORK_COMMON_PLANE_FIT_H diff --git a/cpp/common/include/patchwork/types.h b/cpp/common/include/patchwork/types.h new file mode 100644 index 0000000..59a8750 --- /dev/null +++ b/cpp/common/include/patchwork/types.h @@ -0,0 +1,50 @@ +#ifndef PATCHWORK_COMMON_TYPES_H +#define PATCHWORK_COMMON_TYPES_H + +#include + +#include + +namespace patchwork { + +/// Lightweight 3D point with an optional payload index used by the +/// concentric-zone bookkeeping. `idx` defaults to -1 for unrelated points. +struct PointXYZ { + float x; + float y; + float z; + int idx; + + PointXYZ() : x(0.f), y(0.f), z(0.f), idx(-1) {} + PointXYZ(float _x, float _y, float _z, int _idx = -1) : x(_x), y(_y), z(_z), idx(_idx) {} +}; + +/// PCA result for a fitted plane plus a couple of derived scalars used by +/// the GLE classifier. `principal_` is the largest singular vector and is +/// kept for parity with the original Patchwork repo even though the current +/// pipeline does not read it. +struct PCAFeature { + Eigen::Vector3f principal_; + Eigen::Vector3f normal_; + Eigen::Vector3f singular_values_; + Eigen::Vector3f mean_; + float d_; + float th_dist_d_; + float linearity_; + float planarity_; +}; + +/// GLE outcome for a single (zone, ring, sector) patch. +enum class PatchStatus { + NotAssigned = -2, + FewPoints = -1, + UprightEnough = 0, + FlatEnough = 1, + TooHighElevation = 2, + TooTilted = 3, + GloballyTooHighElevation = 4, +}; + +} // namespace patchwork + +#endif // PATCHWORK_COMMON_TYPES_H diff --git a/cpp/common/src/plane_fit.cpp b/cpp/common/src/plane_fit.cpp new file mode 100644 index 0000000..a5b9fd2 --- /dev/null +++ b/cpp/common/src/plane_fit.cpp @@ -0,0 +1,41 @@ +#include "patchwork/plane_fit.h" + +#include + +namespace patchwork { + +void estimate_plane(const std::vector& seeds, PCAFeature& out, float th_dist) { + if (seeds.empty()) return; + + Eigen::MatrixXf pts(static_cast(seeds.size()), 3); + for (size_t i = 0; i < seeds.size(); ++i) { + pts(static_cast(i), 0) = seeds[i].x; + pts(static_cast(i), 1) = seeds[i].y; + pts(static_cast(i), 2) = seeds[i].z; + } + + const Eigen::Vector3f mean = pts.colwise().mean(); + const Eigen::MatrixXf centered = pts.rowwise() - mean.transpose(); + const Eigen::Matrix3f cov = + (centered.adjoint() * centered) / std::max(1.0f, static_cast(pts.rows() - 1)); + + Eigen::JacobiSVD svd(cov, Eigen::ComputeFullU); + Eigen::Vector3f normal = svd.matrixU().col(2); + if (normal(2) < 0.0f) normal = -normal; + + out.normal_ = normal; + out.principal_ = svd.matrixU().col(0); + out.singular_values_ = svd.singularValues(); + out.mean_ = mean; + out.d_ = -normal.dot(mean); + out.th_dist_d_ = th_dist - out.d_; + + const float s0 = out.singular_values_(0); + const float s1 = out.singular_values_(1); + const float s2 = out.singular_values_(2); + const float eps = 1e-12f; + out.linearity_ = (s0 - s1) / std::max(s0, eps); + out.planarity_ = (s1 - s2) / std::max(s0, eps); +} + +} // namespace patchwork diff --git a/cpp/patchwork/CMakeLists.txt b/cpp/patchwork/CMakeLists.txt index daf5a3a..3008cbf 100644 --- a/cpp/patchwork/CMakeLists.txt +++ b/cpp/patchwork/CMakeLists.txt @@ -11,7 +11,7 @@ target_include_directories(${CLASSIC_TARGET} PUBLIC $ $ ) -target_link_libraries(${CLASSIC_TARGET} Eigen3::Eigen ground_seg_cores) +target_link_libraries(${CLASSIC_TARGET} Eigen3::Eigen ground_seg_common ground_seg_cores) add_library(${PARENT_PROJECT_NAME}::${CLASSIC_TARGET} ALIAS ${CLASSIC_TARGET}) install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ diff --git a/cpp/patchwork/include/patchwork/patchwork.h b/cpp/patchwork/include/patchwork/patchwork.h index bc4e293..65fffa2 100644 --- a/cpp/patchwork/include/patchwork/patchwork.h +++ b/cpp/patchwork/include/patchwork/patchwork.h @@ -5,30 +5,10 @@ #include -#include "patchwork/patchworkpp.h" // for patchwork::PointXYZ +#include "patchwork/types.h" // PointXYZ, PCAFeature, PatchStatus namespace patchwork { -struct PCAFeature { - Eigen::Vector3f normal_; - Eigen::Vector3f mean_; - Eigen::Vector3f singular_values_; - float d_; - float th_dist_d_; - float linearity_; - float planarity_; -}; - -enum class PatchStatus { - NotAssigned = -2, - FewPoints = -1, - UprightEnough = 0, - FlatEnough = 1, - TooHighElevation = 2, - TooTilted = 3, - GloballyTooHighElevation = 4, -}; - struct PatchworkParams { // Sensor / range double sensor_height = 1.723; @@ -84,13 +64,12 @@ class PatchWork { double getHeight() const; private: - // Helper functions (defined in patchwork.cpp in later tasks) + // Helper functions + // (xy2theta, xy2radius, point_z_cmp, estimate_plane live in + // patchwork/plane_fit.h and are shared with Patchwork++.) void initialize(); void flush(); - double xy2theta(double x, double y) const; - double xy2radius(double x, double y) const; void pc2regionwise_patches(const std::vector& src); - void estimate_plane(const std::vector& seeds, PCAFeature& out); void extract_initial_seeds(int zone_idx, const std::vector& sorted, std::vector& seeds); diff --git a/cpp/patchwork/src/patchwork.cpp b/cpp/patchwork/src/patchwork.cpp index 5afd99c..6819fd3 100644 --- a/cpp/patchwork/src/patchwork.cpp +++ b/cpp/patchwork/src/patchwork.cpp @@ -3,8 +3,11 @@ #include #include #include +#include #include +#include "patchwork/plane_fit.h" + namespace { inline Eigen::MatrixX3f to_matrix(const std::vector& pts) { Eigen::MatrixX3f m(pts.size(), 3); @@ -21,13 +24,6 @@ namespace patchwork { PatchWork::PatchWork(const PatchworkParams& params) : params_(params) {} -double PatchWork::xy2theta(double x, double y) const { - double a = std::atan2(y, x); - return (a >= 0) ? a : (a + 2 * M_PI); -} - -double PatchWork::xy2radius(double x, double y) const { return std::hypot(x, y); } - void PatchWork::initialize() { regionwise_patches_.clear(); regionwise_patches_.resize(params_.num_zones); @@ -80,36 +76,6 @@ void PatchWork::pc2regionwise_patches(const std::vector& src) { } } -void PatchWork::estimate_plane(const std::vector& seeds, PCAFeature& out) { - if (seeds.empty()) return; - - Eigen::MatrixXf pts(seeds.size(), 3); - for (size_t i = 0; i < seeds.size(); ++i) { - pts(i, 0) = seeds[i].x; - pts(i, 1) = seeds[i].y; - pts(i, 2) = seeds[i].z; - } - Eigen::Vector3f mean = pts.colwise().mean(); - Eigen::MatrixXf centered = pts.rowwise() - mean.transpose(); - Eigen::Matrix3f cov = - (centered.adjoint() * centered) / std::max(1.0f, static_cast(pts.rows() - 1)); - - Eigen::JacobiSVD svd(cov, Eigen::ComputeFullU); - out.normal_ = svd.matrixU().col(2); - if (out.normal_(2) < 0) out.normal_ = -out.normal_; - out.singular_values_ = svd.singularValues(); - out.mean_ = mean; - out.d_ = -out.normal_.dot(mean); - out.th_dist_d_ = static_cast(params_.th_dist) - out.d_; - - const float s0 = out.singular_values_(0); - const float s1 = out.singular_values_(1); - const float s2 = out.singular_values_(2); - const float eps = 1e-12f; - out.linearity_ = (s0 - s1) / std::max(s0, eps); - out.planarity_ = (s1 - s2) / std::max(s0, eps); -} - void PatchWork::extract_initial_seeds(int zone_idx, const std::vector& sorted, std::vector& seeds) { @@ -206,7 +172,7 @@ void PatchWork::perform_regionwise_segmentation(int zone_idx, PCAFeature feature{}; for (int it = 0; it < params_.num_iter; ++it) { if (ground.empty()) break; - estimate_plane(ground, feature); + estimate_plane(ground, feature, static_cast(params_.th_dist)); ground.clear(); std::vector nonground; @@ -251,7 +217,8 @@ void PatchWork::perform_regionwise_segmentation(int zone_idx, patch_nonground.push_back(p); } // Estimate plane on seeds so feature is valid for determine_gle_status. - if (!patch_ground.empty()) estimate_plane(patch_ground, feature); + if (!patch_ground.empty()) + estimate_plane(patch_ground, feature, static_cast(params_.th_dist)); } status_out = determine_gle_status(zone_idx, ring_idx, feature); diff --git a/cpp/patchworkpp/CMakeLists.txt b/cpp/patchworkpp/CMakeLists.txt index 64f1942..1fb091d 100644 --- a/cpp/patchworkpp/CMakeLists.txt +++ b/cpp/patchworkpp/CMakeLists.txt @@ -11,7 +11,7 @@ target_include_directories(${TARGET_NAME} PUBLIC $ $ ) -target_link_libraries(${TARGET_NAME} Eigen3::Eigen) +target_link_libraries(${TARGET_NAME} Eigen3::Eigen ground_seg_common) add_library(${PARENT_PROJECT_NAME}::${TARGET_NAME} ALIAS ${TARGET_NAME}) install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ @@ -23,7 +23,7 @@ install(TARGETS ${TARGET_NAME} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} ) -export(TARGETS ${TARGET_NAME} +export(TARGETS ${TARGET_NAME} ground_seg_common NAMESPACE ${PARENT_PROJECT_NAME}:: FILE "${CMAKE_CURRENT_BINARY_DIR}/${PARENT_PROJECT_NAME}Config.cmake" ) diff --git a/cpp/patchworkpp/include/patchwork/patchworkpp.h b/cpp/patchworkpp/include/patchwork/patchworkpp.h index 852af80..a48738a 100644 --- a/cpp/patchworkpp/include/patchwork/patchworkpp.h +++ b/cpp/patchworkpp/include/patchwork/patchworkpp.h @@ -13,21 +13,14 @@ #include +#include "patchwork/types.h" // PointXYZ, PCAFeature, PatchStatus + using namespace std; #define MAX_POINTS 5000 namespace patchwork { -struct PointXYZ { - float x; - float y; - float z; - int idx; - - PointXYZ(float _x, float _y, float _z, int _idx = -1) : x(_x), y(_y), z(_z), idx(_idx) {} -}; - struct RevertCandidate { int concentric_idx; int sector_idx; @@ -227,10 +220,8 @@ class PatchWorkpp { void update_elevation_thr(); void update_flatness_thr(); - double xy2theta(const double &x, const double &y); - - double xy2radius(const double &x, const double &y); - + // xy2theta / xy2radius / point_z_cmp now live in patchwork/plane_fit.h + // (shared with Patchwork classic). void estimate_plane(const vector &ground); void extract_piecewiseground(const int zone_idx, diff --git a/cpp/patchworkpp/src/patchworkpp.cpp b/cpp/patchworkpp/src/patchworkpp.cpp index a859f90..e33dbfe 100644 --- a/cpp/patchworkpp/src/patchworkpp.cpp +++ b/cpp/patchworkpp/src/patchworkpp.cpp @@ -1,10 +1,10 @@ #include "patchwork/patchworkpp.h" +#include "patchwork/plane_fit.h" // xy2theta, xy2radius, point_z_cmp + using namespace std; using namespace patchwork; -bool point_z_cmp(PointXYZ a, PointXYZ b) { return a.z < b.z; } - Eigen::MatrixX3f PatchWorkpp::toEigenCloud(const vector &cloud) { Eigen::MatrixX3f dst(cloud.size(), 3); int j = 0; @@ -573,16 +573,6 @@ void PatchWorkpp::calc_mean_stdev(std::vector vec, double &mean, double stdev = sqrt(stdev); } -double PatchWorkpp::xy2theta(const double &x, const double &y) { // 0 ~ 2 * PI - double angle = atan2(y, x); - return angle > 0 ? angle : 2 * M_PI + angle; -} - -double PatchWorkpp::xy2radius(const double &x, const double &y) { - // return sqrt(pow(x, 2) + pow(y, 2)); - return sqrt(x * x + y * y); -} - void PatchWorkpp::pc2czm(const Eigen::MatrixXf &src, std::vector &czm) { double max_range = params_.max_range, min_range = params_.min_range; double min_range_0 = min_ranges_[0], min_range_1 = min_ranges_[1], min_range_2 = min_ranges_[2],