Skip to content

Commit 099a413

Browse files
LimHyungTaeLimHyungTae
authored
Merge pull request #79 from url-kaist/feat/patchwork-classic
feat: add Patchwork (classic) algorithm alongside Patchwork++
2 parents 306da05 + 96c9170 commit 099a413

16 files changed

Lines changed: 2394 additions & 19 deletions

‎README.md‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -107,6 +107,28 @@ source ./install/setup.bash
107107

108108
How to launch ROS2 nodes is explained [here][rosexamplelink].
109109

110+
## :compass: Choosing an algorithm
111+
112+
This repository ships two ground segmentation algorithms with the same input/output API. Pick the one that fits your data:
113+
114+
- **Patchwork++** (default): adaptive elevation/flatness thresholds, RNR (intensity-based reflected noise removal), RVPF (vertical structure suppression), and TGR (probability-based ground revert). Best when the LiDAR has reflection artefacts or you want self-tuning thresholds.
115+
- **Patchwork** (classic, since 1.1.0): fixed elevation/flatness thresholds with explicit `z < -sensor_height - 2.0m` cutoff and few-points reject, plus optional ATAT for unknown sensor heights. Often more aggressive on ground-plane noise in heavily cluttered scenes.
116+
117+
**Python:**
118+
119+
```python
120+
import pypatchworkpp as p
121+
122+
pp_default = p.patchworkpp(p.Parameters()) # Patchwork++
123+
pp_classic = p.patchwork(p.PatchworkParams()) # Patchwork (classic)
124+
```
125+
126+
**ROS2:**
127+
128+
```bash
129+
ros2 launch patchworkpp patchworkpp.launch.py algorithm:=patchwork
130+
```
131+
110132
## :pencil: Citation
111133

112134
If you use our codes, please cite our paper ([arXiv][arxivlink], [IEEE *Xplore*][patchworkppieeelink])

‎cpp/CMakeLists.txt‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
cmake_minimum_required(VERSION 3.11)
2-
project(patchworkpp VERSION 1.0.4)
2+
project(patchworkpp VERSION 1.1.0)
33

44
option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" OFF)
55
option(INCLUDE_CPP_EXAMPLES "Include C++ example codes, which require Open3D for visualization" OFF)
@@ -46,6 +46,7 @@ set(PARENT_PROJECT_NAME ${PROJECT_NAME})
4646
set(TARGET_NAME ground_seg_cores)
4747

4848
add_subdirectory(patchworkpp)
49+
add_subdirectory(patchwork)
4950

5051
if (INCLUDE_CPP_EXAMPLES)
5152
if(CMAKE_VERSION VERSION_LESS "3.15")

‎cpp/patchwork/CMakeLists.txt‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
project(patchwork_classic_src)
2+
3+
include(GNUInstallDirs)
4+
5+
set(CLASSIC_TARGET ground_seg_classic)
6+
7+
add_library(${CLASSIC_TARGET} STATIC src/patchwork.cpp)
8+
set_target_properties(${CLASSIC_TARGET} PROPERTIES POSITION_INDEPENDENT_CODE ON)
9+
10+
target_include_directories(${CLASSIC_TARGET} PUBLIC
11+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
12+
$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
13+
)
14+
target_link_libraries(${CLASSIC_TARGET} Eigen3::Eigen ground_seg_cores)
15+
add_library(${PARENT_PROJECT_NAME}::${CLASSIC_TARGET} ALIAS ${CLASSIC_TARGET})
16+
17+
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/
18+
DESTINATION include
19+
)
20+
install(TARGETS ${CLASSIC_TARGET}
21+
EXPORT ${PARENT_PROJECT_NAME}Config
22+
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
23+
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
24+
)
25+
26+
option(BUILD_PATCHWORK_TESTS "Build C++ smoke test for PatchWork" OFF)
27+
if (BUILD_PATCHWORK_TESTS)
28+
add_executable(patchwork_smoke tests/smoke_test.cpp)
29+
target_link_libraries(patchwork_smoke PRIVATE ${CLASSIC_TARGET})
30+
endif()

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

Lines changed: 134 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,134 @@
1+
#ifndef PATCHWORK_CLASSIC_H
2+
#define PATCHWORK_CLASSIC_H
3+
4+
#include <vector>
5+
6+
#include <Eigen/Dense>
7+
8+
#include "patchwork/patchworkpp.h" // for patchwork::PointXYZ
9+
10+
namespace patchwork {
11+
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+
32+
struct PatchworkParams {
33+
// Sensor / range
34+
double sensor_height = 1.723;
35+
double max_range = 80.0;
36+
double min_range = 2.7;
37+
38+
// Concentric Zone Model (parametric)
39+
int num_zones = 4;
40+
std::vector<int> num_sectors_each_zone = {16, 32, 54, 32};
41+
std::vector<int> num_rings_each_zone = {2, 4, 4, 4};
42+
std::vector<double> min_ranges = {2.7, 12.3625, 22.025, 41.35};
43+
44+
// Plane fit
45+
int num_iter = 3;
46+
int num_lpr = 20;
47+
int num_min_pts = 10;
48+
double th_seeds = 0.5;
49+
double th_dist = 0.125;
50+
51+
// Ground likelihood thresholds (fixed, the Patchwork classic style)
52+
double uprightness_thr = 0.5;
53+
std::vector<double> elevation_thr = {0.523, 0.746, 0.879, 1.125};
54+
std::vector<double> flatness_thr = {0.0005, 0.000725, 0.001, 0.001};
55+
56+
// Adaptive seed selection margin for highly tilted ground
57+
double adaptive_seed_selection_margin = -1.1;
58+
59+
// Global elevation guard
60+
bool using_global_thr = true;
61+
double global_elevation_thr = 0.0;
62+
63+
// ATAT (default ON)
64+
bool ATAT_ON = true;
65+
double max_h_for_ATAT = 0.3;
66+
int num_sectors_for_ATAT = 20;
67+
double noise_bound = 0.2;
68+
69+
bool verbose = false;
70+
};
71+
72+
class PatchWork {
73+
public:
74+
PatchWork() = default;
75+
explicit PatchWork(const PatchworkParams& params);
76+
77+
void estimateGround(const Eigen::MatrixXf& cloud);
78+
79+
Eigen::MatrixX3f getGround() const;
80+
Eigen::MatrixX3f getNonground() const;
81+
std::vector<int> getGroundIndices() const;
82+
std::vector<int> getNongroundIndices() const;
83+
double getTimeTaken() const;
84+
double getHeight() const;
85+
86+
private:
87+
// Helper functions (defined in patchwork.cpp in later tasks)
88+
void initialize();
89+
void flush();
90+
double xy2theta(double x, double y) const;
91+
double xy2radius(double x, double y) const;
92+
void pc2regionwise_patches(const std::vector<PointXYZ>& src);
93+
void estimate_plane(const std::vector<PointXYZ>& seeds, PCAFeature& out);
94+
void extract_initial_seeds(int zone_idx,
95+
const std::vector<PointXYZ>& sorted,
96+
std::vector<PointXYZ>& seeds);
97+
PatchStatus determine_gle_status(int zone_idx, int ring_idx, const PCAFeature& feature) const;
98+
void perform_regionwise_segmentation(int zone_idx,
99+
int ring_idx,
100+
const std::vector<PointXYZ>& patch,
101+
std::vector<PointXYZ>& patch_ground,
102+
std::vector<PointXYZ>& patch_nonground,
103+
PatchStatus& status_out);
104+
void estimate_sensor_height(std::vector<PointXYZ>& cloud);
105+
double consensus_set_based_height_estimation(const std::vector<double>& candidate_heights);
106+
void materialize() const;
107+
108+
PatchworkParams params_;
109+
110+
// Per-iteration scratch (cleared each estimateGround call)
111+
using Sector = std::vector<PointXYZ>;
112+
using Ring = std::vector<Sector>; // sectors within one ring
113+
using Zone = std::vector<Ring>; // rings within one zone
114+
using RegionwisePatches = std::vector<Zone>;
115+
RegionwisePatches regionwise_patches_;
116+
117+
// Materialized output points (per-call)
118+
std::vector<PointXYZ> ground_pts_;
119+
std::vector<PointXYZ> nonground_pts_;
120+
121+
// Cached output matrices/indices (lazy via materialize())
122+
mutable Eigen::MatrixX3f ground_mat_;
123+
mutable Eigen::MatrixX3f nonground_mat_;
124+
mutable std::vector<int> ground_idx_;
125+
mutable std::vector<int> nonground_idx_;
126+
mutable bool outputs_dirty_ = true;
127+
128+
double time_taken_ = 0.0;
129+
double sensor_height_ = 1.723; // updated by ATAT if enabled
130+
};
131+
132+
} // namespace patchwork
133+
134+
#endif

0 commit comments

Comments
 (0)