Skip to content

Commit 74147d8

Browse files
LimHyungTaeLimHyungTae
committed
refactor(common): extract shared PointXYZ / PCAFeature / plane-fit math
cpp/patchwork/ and cpp/patchworkpp/ each carried their own copies of PointXYZ, PCAFeature, and the small geometry helpers (xy2theta, xy2radius, point_z_cmp). The plane-fit SVD itself lived in three slightly-different forms across cpp/patchwork/src/patchwork.cpp, cpp/patchworkpp/src/patchworkpp.cpp, and the upstream ~/git/patchwork. PR #90 (Fix 2) was a concrete bug caused by that drift. This commit factors them into a new tiny static library: cpp/common/ include/patchwork/types.h — PointXYZ, PCAFeature, PatchStatus include/patchwork/plane_fit.h — xy2theta, xy2radius, point_z_cmp, estimate_plane(seeds, out, th_dist) src/plane_fit.cpp — SVD-based plane fit (single canonical implementation) PCAFeature now carries the `principal_` (largest singular vector) field that exists in the original Patchwork repo; the classic Patchwork's PCAFeature was missing it. The current pipeline does not read `principal_` yet, but adding it costs ~12 bytes per patch and keeps the type signature aligned with the original. Dependents updated: - cpp/patchwork/include/patchwork/patchwork.h: delete duplicated PCAFeature/PatchStatus, include patchwork/types.h. - cpp/patchwork/src/patchwork.cpp: delete in-class xy2theta / xy2radius / estimate_plane, route call sites to the namespace-level helpers in patchwork/plane_fit.h. - cpp/patchworkpp/include/patchwork/patchworkpp.h: delete duplicated PointXYZ + xy2theta / xy2radius member decls, include patchwork/types.h. - cpp/patchworkpp/src/patchworkpp.cpp: delete its file-scope point_z_cmp and the PatchWorkpp::xy2theta / xy2radius definitions, include patchwork/plane_fit.h. `estimate_plane` is left untouched in patchworkpp.cpp (it still writes to instance-level pc_mean_/normal_/singular_values_) because refactoring it requires the thread-safety changes that will arrive with the TBB work in the next PR. CMake: new cpp/common/CMakeLists.txt builds ground_seg_common (static, PIC) linked against Eigen3::Eigen. Both classic and patchworkpp link to it. The patchworkpp build-tree export() set now also lists ground_seg_common to keep find_package(patchworkpp) usable from a build directory. Numerical equivalence verified on KITTI 00-10 full sweep (23,201 frames) for all four method × protocol combinations against the v1.3.1 baseline: every macro-average P / R / F1 cell within 0.0013 of the v1.3.1 number (two combinations byte-identical), well under the ±0.05 budget set in the refactor plan. Sets up the TBB parallelisation that will land in the follow-up PR.
1 parent 2f28cbe commit 74147d8

11 files changed

Lines changed: 170 additions & 92 deletions

File tree

‎cpp/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,7 @@ find_external_dependency("Eigen3" "Eigen3::Eigen" "${CMAKE_CURRENT_LIST_DIR}/cma
4545
set(PARENT_PROJECT_NAME ${PROJECT_NAME})
4646
set(TARGET_NAME ground_seg_cores)
4747

48+
add_subdirectory(common)
4849
add_subdirectory(patchworkpp)
4950
add_subdirectory(patchwork)
5051

‎cpp/common/CMakeLists.txt‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
project(patchwork_common_src)
2+
3+
include(GNUInstallDirs)
4+
5+
set(COMMON_TARGET ground_seg_common)
6+
7+
add_library(${COMMON_TARGET} STATIC src/plane_fit.cpp)
8+
set_target_properties(${COMMON_TARGET} PROPERTIES POSITION_INDEPENDENT_CODE ON)
9+
10+
target_include_directories(${COMMON_TARGET} PUBLIC
11+
$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>
12+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
13+
$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
14+
)
15+
target_link_libraries(${COMMON_TARGET} Eigen3::Eigen)
16+
add_library(${PARENT_PROJECT_NAME}::${COMMON_TARGET} ALIAS ${COMMON_TARGET})
17+
18+
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/
19+
DESTINATION include
20+
)
21+
install(TARGETS ${COMMON_TARGET}
22+
EXPORT ${PARENT_PROJECT_NAME}Config
23+
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
24+
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
25+
)

‎cpp/common/include/patchwork/plane_fit.h‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
#ifndef PATCHWORK_COMMON_PLANE_FIT_H
2+
#define PATCHWORK_COMMON_PLANE_FIT_H
3+
4+
#ifndef M_PI
5+
#define M_PI 3.14159265358979323846
6+
#endif
7+
8+
#include <cmath>
9+
#include <vector>
10+
11+
#include <Eigen/Dense>
12+
13+
#include "patchwork/types.h"
14+
15+
namespace patchwork {
16+
17+
/// SVD-based plane fit for the seed set. Fills every field of `out`,
18+
/// including `d_ = -normal . mean` and `th_dist_d_ = th_dist - d_`.
19+
/// No-op when `seeds` is empty (leaves `out` untouched).
20+
void estimate_plane(const std::vector<PointXYZ>& seeds, PCAFeature& out, float th_dist);
21+
22+
/// Polar angle in [0, 2*pi). Stable at `(0, 0)` (returns 0).
23+
inline double xy2theta(double x, double y) {
24+
const double a = std::atan2(y, x);
25+
return (a >= 0.0) ? a : (a + 2.0 * M_PI);
26+
}
27+
28+
inline double xy2radius(double x, double y) { return std::hypot(x, y); }
29+
30+
inline bool point_z_cmp(const PointXYZ& a, const PointXYZ& b) { return a.z < b.z; }
31+
32+
} // namespace patchwork
33+
34+
#endif // PATCHWORK_COMMON_PLANE_FIT_H

‎cpp/common/include/patchwork/types.h‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
#ifndef PATCHWORK_COMMON_TYPES_H
2+
#define PATCHWORK_COMMON_TYPES_H
3+
4+
#include <vector>
5+
6+
#include <Eigen/Dense>
7+
8+
namespace patchwork {
9+
10+
/// Lightweight 3D point with an optional payload index used by the
11+
/// concentric-zone bookkeeping. `idx` defaults to -1 for unrelated points.
12+
struct PointXYZ {
13+
float x;
14+
float y;
15+
float z;
16+
int idx;
17+
18+
PointXYZ() : x(0.f), y(0.f), z(0.f), idx(-1) {}
19+
PointXYZ(float _x, float _y, float _z, int _idx = -1) : x(_x), y(_y), z(_z), idx(_idx) {}
20+
};
21+
22+
/// PCA result for a fitted plane plus a couple of derived scalars used by
23+
/// the GLE classifier. `principal_` is the largest singular vector and is
24+
/// kept for parity with the original Patchwork repo even though the current
25+
/// pipeline does not read it.
26+
struct PCAFeature {
27+
Eigen::Vector3f principal_;
28+
Eigen::Vector3f normal_;
29+
Eigen::Vector3f singular_values_;
30+
Eigen::Vector3f mean_;
31+
float d_;
32+
float th_dist_d_;
33+
float linearity_;
34+
float planarity_;
35+
};
36+
37+
/// GLE outcome for a single (zone, ring, sector) patch.
38+
enum class PatchStatus {
39+
NotAssigned = -2,
40+
FewPoints = -1,
41+
UprightEnough = 0,
42+
FlatEnough = 1,
43+
TooHighElevation = 2,
44+
TooTilted = 3,
45+
GloballyTooHighElevation = 4,
46+
};
47+
48+
} // namespace patchwork
49+
50+
#endif // PATCHWORK_COMMON_TYPES_H

‎cpp/common/src/plane_fit.cpp‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
#include "patchwork/plane_fit.h"
2+
3+
#include <algorithm>
4+
5+
namespace patchwork {
6+
7+
void estimate_plane(const std::vector<PointXYZ>& seeds, PCAFeature& out, float th_dist) {
8+
if (seeds.empty()) return;
9+
10+
Eigen::MatrixXf pts(static_cast<Eigen::Index>(seeds.size()), 3);
11+
for (size_t i = 0; i < seeds.size(); ++i) {
12+
pts(static_cast<Eigen::Index>(i), 0) = seeds[i].x;
13+
pts(static_cast<Eigen::Index>(i), 1) = seeds[i].y;
14+
pts(static_cast<Eigen::Index>(i), 2) = seeds[i].z;
15+
}
16+
17+
const Eigen::Vector3f mean = pts.colwise().mean();
18+
const Eigen::MatrixXf centered = pts.rowwise() - mean.transpose();
19+
const Eigen::Matrix3f cov =
20+
(centered.adjoint() * centered) / std::max<float>(1.0f, static_cast<float>(pts.rows() - 1));
21+
22+
Eigen::JacobiSVD<Eigen::Matrix3f> svd(cov, Eigen::ComputeFullU);
23+
Eigen::Vector3f normal = svd.matrixU().col(2);
24+
if (normal(2) < 0.0f) normal = -normal;
25+
26+
out.normal_ = normal;
27+
out.principal_ = svd.matrixU().col(0);
28+
out.singular_values_ = svd.singularValues();
29+
out.mean_ = mean;
30+
out.d_ = -normal.dot(mean);
31+
out.th_dist_d_ = th_dist - out.d_;
32+
33+
const float s0 = out.singular_values_(0);
34+
const float s1 = out.singular_values_(1);
35+
const float s2 = out.singular_values_(2);
36+
const float eps = 1e-12f;
37+
out.linearity_ = (s0 - s1) / std::max(s0, eps);
38+
out.planarity_ = (s1 - s2) / std::max(s0, eps);
39+
}
40+
41+
} // namespace patchwork

‎cpp/patchwork/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@ target_include_directories(${CLASSIC_TARGET} PUBLIC
1111
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
1212
$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
1313
)
14-
target_link_libraries(${CLASSIC_TARGET} Eigen3::Eigen ground_seg_cores)
14+
target_link_libraries(${CLASSIC_TARGET} Eigen3::Eigen ground_seg_common ground_seg_cores)
1515
add_library(${PARENT_PROJECT_NAME}::${CLASSIC_TARGET} ALIAS ${CLASSIC_TARGET})
1616

1717
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/

‎cpp/patchwork/include/patchwork/patchwork.h‎

Lines changed: 4 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -5,30 +5,10 @@
55

66
#include <Eigen/Dense>
77

8-
#include "patchwork/patchworkpp.h" // for patchwork::PointXYZ
8+
#include "patchwork/types.h" // PointXYZ, PCAFeature, PatchStatus
99

1010
namespace patchwork {
1111

12-
struct PCAFeature {
13-
Eigen::Vector3f normal_;
14-
Eigen::Vector3f mean_;
15-
Eigen::Vector3f singular_values_;
16-
float d_;
17-
float th_dist_d_;
18-
float linearity_;
19-
float planarity_;
20-
};
21-
22-
enum class PatchStatus {
23-
NotAssigned = -2,
24-
FewPoints = -1,
25-
UprightEnough = 0,
26-
FlatEnough = 1,
27-
TooHighElevation = 2,
28-
TooTilted = 3,
29-
GloballyTooHighElevation = 4,
30-
};
31-
3212
struct PatchworkParams {
3313
// Sensor / range
3414
double sensor_height = 1.723;
@@ -84,13 +64,12 @@ class PatchWork {
8464
double getHeight() const;
8565

8666
private:
87-
// Helper functions (defined in patchwork.cpp in later tasks)
67+
// Helper functions
68+
// (xy2theta, xy2radius, point_z_cmp, estimate_plane live in
69+
// patchwork/plane_fit.h and are shared with Patchwork++.)
8870
void initialize();
8971
void flush();
90-
double xy2theta(double x, double y) const;
91-
double xy2radius(double x, double y) const;
9272
void pc2regionwise_patches(const std::vector<PointXYZ>& src);
93-
void estimate_plane(const std::vector<PointXYZ>& seeds, PCAFeature& out);
9473
void extract_initial_seeds(int zone_idx,
9574
const std::vector<PointXYZ>& sorted,
9675
std::vector<PointXYZ>& seeds);

‎cpp/patchwork/src/patchwork.cpp‎

Lines changed: 6 additions & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,11 @@
33
#include <algorithm>
44
#include <chrono>
55
#include <cmath>
6+
#include <iostream>
67
#include <limits>
78

9+
#include "patchwork/plane_fit.h"
10+
811
namespace {
912
inline Eigen::MatrixX3f to_matrix(const std::vector<patchwork::PointXYZ>& pts) {
1013
Eigen::MatrixX3f m(pts.size(), 3);
@@ -21,13 +24,6 @@ namespace patchwork {
2124

2225
PatchWork::PatchWork(const PatchworkParams& params) : params_(params) {}
2326

24-
double PatchWork::xy2theta(double x, double y) const {
25-
double a = std::atan2(y, x);
26-
return (a >= 0) ? a : (a + 2 * M_PI);
27-
}
28-
29-
double PatchWork::xy2radius(double x, double y) const { return std::hypot(x, y); }
30-
3127
void PatchWork::initialize() {
3228
regionwise_patches_.clear();
3329
regionwise_patches_.resize(params_.num_zones);
@@ -80,36 +76,6 @@ void PatchWork::pc2regionwise_patches(const std::vector<PointXYZ>& src) {
8076
}
8177
}
8278

83-
void PatchWork::estimate_plane(const std::vector<PointXYZ>& seeds, PCAFeature& out) {
84-
if (seeds.empty()) return;
85-
86-
Eigen::MatrixXf pts(seeds.size(), 3);
87-
for (size_t i = 0; i < seeds.size(); ++i) {
88-
pts(i, 0) = seeds[i].x;
89-
pts(i, 1) = seeds[i].y;
90-
pts(i, 2) = seeds[i].z;
91-
}
92-
Eigen::Vector3f mean = pts.colwise().mean();
93-
Eigen::MatrixXf centered = pts.rowwise() - mean.transpose();
94-
Eigen::Matrix3f cov =
95-
(centered.adjoint() * centered) / std::max<float>(1.0f, static_cast<float>(pts.rows() - 1));
96-
97-
Eigen::JacobiSVD<Eigen::Matrix3f> svd(cov, Eigen::ComputeFullU);
98-
out.normal_ = svd.matrixU().col(2);
99-
if (out.normal_(2) < 0) out.normal_ = -out.normal_;
100-
out.singular_values_ = svd.singularValues();
101-
out.mean_ = mean;
102-
out.d_ = -out.normal_.dot(mean);
103-
out.th_dist_d_ = static_cast<float>(params_.th_dist) - out.d_;
104-
105-
const float s0 = out.singular_values_(0);
106-
const float s1 = out.singular_values_(1);
107-
const float s2 = out.singular_values_(2);
108-
const float eps = 1e-12f;
109-
out.linearity_ = (s0 - s1) / std::max(s0, eps);
110-
out.planarity_ = (s1 - s2) / std::max(s0, eps);
111-
}
112-
11379
void PatchWork::extract_initial_seeds(int zone_idx,
11480
const std::vector<PointXYZ>& sorted,
11581
std::vector<PointXYZ>& seeds) {
@@ -206,7 +172,7 @@ void PatchWork::perform_regionwise_segmentation(int zone_idx,
206172
PCAFeature feature{};
207173
for (int it = 0; it < params_.num_iter; ++it) {
208174
if (ground.empty()) break;
209-
estimate_plane(ground, feature);
175+
estimate_plane(ground, feature, static_cast<float>(params_.th_dist));
210176

211177
ground.clear();
212178
std::vector<PointXYZ> nonground;
@@ -251,7 +217,8 @@ void PatchWork::perform_regionwise_segmentation(int zone_idx,
251217
patch_nonground.push_back(p);
252218
}
253219
// Estimate plane on seeds so feature is valid for determine_gle_status.
254-
if (!patch_ground.empty()) estimate_plane(patch_ground, feature);
220+
if (!patch_ground.empty())
221+
estimate_plane(patch_ground, feature, static_cast<float>(params_.th_dist));
255222
}
256223

257224
status_out = determine_gle_status(zone_idx, ring_idx, feature);

‎cpp/patchworkpp/CMakeLists.txt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@ target_include_directories(${TARGET_NAME} PUBLIC
1111
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
1212
$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
1313
)
14-
target_link_libraries(${TARGET_NAME} Eigen3::Eigen)
14+
target_link_libraries(${TARGET_NAME} Eigen3::Eigen ground_seg_common)
1515
add_library(${PARENT_PROJECT_NAME}::${TARGET_NAME} ALIAS ${TARGET_NAME})
1616

1717
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/
@@ -23,7 +23,7 @@ install(TARGETS ${TARGET_NAME}
2323
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
2424
)
2525

26-
export(TARGETS ${TARGET_NAME}
26+
export(TARGETS ${TARGET_NAME} ground_seg_common
2727
NAMESPACE ${PARENT_PROJECT_NAME}::
2828
FILE "${CMAKE_CURRENT_BINARY_DIR}/${PARENT_PROJECT_NAME}Config.cmake"
2929
)

‎cpp/patchworkpp/include/patchwork/patchworkpp.h‎

Lines changed: 4 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -13,21 +13,14 @@
1313

1414
#include <Eigen/Dense>
1515

16+
#include "patchwork/types.h" // PointXYZ, PCAFeature, PatchStatus
17+
1618
using namespace std;
1719

1820
#define MAX_POINTS 5000
1921

2022
namespace patchwork {
2123

22-
struct PointXYZ {
23-
float x;
24-
float y;
25-
float z;
26-
int idx;
27-
28-
PointXYZ(float _x, float _y, float _z, int _idx = -1) : x(_x), y(_y), z(_z), idx(_idx) {}
29-
};
30-
3124
struct RevertCandidate {
3225
int concentric_idx;
3326
int sector_idx;
@@ -227,10 +220,8 @@ class PatchWorkpp {
227220
void update_elevation_thr();
228221
void update_flatness_thr();
229222

230-
double xy2theta(const double &x, const double &y);
231-
232-
double xy2radius(const double &x, const double &y);
233-
223+
// xy2theta / xy2radius / point_z_cmp now live in patchwork/plane_fit.h
224+
// (shared with Patchwork classic).
234225
void estimate_plane(const vector<PointXYZ> &ground);
235226

236227
void extract_piecewiseground(const int zone_idx,

0 commit comments

Comments
 (0)