refactor(common): extract shared PointXYZ / PCAFeature / plane-fit math (Part A of TBB work)

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)
 

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
+)
+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}
+)

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 <cmath>
+#include <vector>
+
+#include <Eigen/Dense>
+
+#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<PointXYZ>& 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

cpp/common/include/patchwork/types.h

@@ -0,0 +1,50 @@
+#ifndef PATCHWORK_COMMON_TYPES_H
+#define PATCHWORK_COMMON_TYPES_H
+
+#include <vector>
+
+#include <Eigen/Dense>
+
+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

cpp/common/src/plane_fit.cpp

@@ -0,0 +1,41 @@
+#include "patchwork/plane_fit.h"
+
+#include <algorithm>
+
+namespace patchwork {
+
+void estimate_plane(const std::vector<PointXYZ>& seeds, PCAFeature& out, float th_dist) {
+  if (seeds.empty()) return;
+
+  Eigen::MatrixXf pts(static_cast<Eigen::Index>(seeds.size()), 3);
+  for (size_t i = 0; i < seeds.size(); ++i) {
+    pts(static_cast<Eigen::Index>(i), 0) = seeds[i].x;
+    pts(static_cast<Eigen::Index>(i), 1) = seeds[i].y;
+    pts(static_cast<Eigen::Index>(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<float>(1.0f, static_cast<float>(pts.rows() - 1));
+
+  Eigen::JacobiSVD<Eigen::Matrix3f> 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

cpp/patchwork/CMakeLists.txt

@@ -11,7 +11,7 @@ target_include_directories(${CLASSIC_TARGET} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
 )
-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/

cpp/patchwork/include/patchwork/patchwork.h

@@ -5,30 +5,10 @@
 
 #include <Eigen/Dense>
 
-#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<PointXYZ>& src);
-  void estimate_plane(const std::vector<PointXYZ>& seeds, PCAFeature& out);
   void extract_initial_seeds(int zone_idx,
                              const std::vector<PointXYZ>& sorted,
                              std::vector<PointXYZ>& seeds);

cpp/patchwork/src/patchwork.cpp

@@ -3,8 +3,11 @@
 #include <algorithm>
 #include <chrono>
 #include <cmath>
+#include <iostream>
 #include <limits>
 
+#include "patchwork/plane_fit.h"
+
 namespace {
 inline Eigen::MatrixX3f to_matrix(const std::vector<patchwork::PointXYZ>& 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<PointXYZ>& src) {
   }
 }
 
-void PatchWork::estimate_plane(const std::vector<PointXYZ>& 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<float>(1.0f, static_cast<float>(pts.rows() - 1));
-
-  Eigen::JacobiSVD<Eigen::Matrix3f> 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<float>(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<PointXYZ>& sorted,
                                       std::vector<PointXYZ>& 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<float>(params_.th_dist));
 
     ground.clear();
     std::vector<PointXYZ> 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<float>(params_.th_dist));
   }
 
   status_out = determine_gle_status(zone_idx, ring_idx, feature);

cpp/patchworkpp/CMakeLists.txt

@@ -11,7 +11,7 @@ target_include_directories(${TARGET_NAME} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
 )
-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"
 )

cpp/patchworkpp/include/patchwork/patchworkpp.h

@@ -13,21 +13,14 @@
 
 #include <Eigen/Dense>
 
+#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<PointXYZ> &ground);
 
   void extract_piecewiseground(const int zone_idx,

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<PointXYZ> &cloud) {
   Eigen::MatrixX3f dst(cloud.size(), 3);
   int j = 0;
@@ -573,16 +573,6 @@ void PatchWorkpp::calc_mean_stdev(std::vector<double> 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<Zone> &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],