diff --git a/README.md b/README.md index 4065c2a..f5b0aa4 100644 --- a/README.md +++ b/README.md @@ -107,6 +107,28 @@ source ./install/setup.bash How to launch ROS2 nodes is explained [here][rosexamplelink]. +## :compass: Choosing an algorithm + +This repository ships two ground segmentation algorithms with the same input/output API. Pick the one that fits your data: + +- **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. +- **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. + +**Python:** + +```python +import pypatchworkpp as p + +pp_default = p.patchworkpp(p.Parameters()) # Patchwork++ +pp_classic = p.patchwork(p.PatchworkParams()) # Patchwork (classic) +``` + +**ROS2:** + +```bash +ros2 launch patchworkpp patchworkpp.launch.py algorithm:=patchwork +``` + ## :pencil: Citation If you use our codes, please cite our paper ([arXiv][arxivlink], [IEEE *Xplore*][patchworkppieeelink]) diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt index ae0fefd..81e9e69 100755 --- a/cpp/CMakeLists.txt +++ b/cpp/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.11) -project(patchworkpp VERSION 1.0.4) +project(patchworkpp VERSION 1.1.0) option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" OFF) option(INCLUDE_CPP_EXAMPLES "Include C++ example codes, which require Open3D for visualization" OFF) @@ -46,6 +46,7 @@ set(PARENT_PROJECT_NAME ${PROJECT_NAME}) set(TARGET_NAME ground_seg_cores) add_subdirectory(patchworkpp) +add_subdirectory(patchwork) if (INCLUDE_CPP_EXAMPLES) if(CMAKE_VERSION VERSION_LESS "3.15") diff --git a/cpp/patchwork/CMakeLists.txt b/cpp/patchwork/CMakeLists.txt new file mode 100644 index 0000000..daf5a3a --- /dev/null +++ b/cpp/patchwork/CMakeLists.txt @@ -0,0 +1,30 @@ +project(patchwork_classic_src) + +include(GNUInstallDirs) + +set(CLASSIC_TARGET ground_seg_classic) + +add_library(${CLASSIC_TARGET} STATIC src/patchwork.cpp) +set_target_properties(${CLASSIC_TARGET} PROPERTIES POSITION_INDEPENDENT_CODE ON) + +target_include_directories(${CLASSIC_TARGET} PUBLIC + $ + $ +) +target_link_libraries(${CLASSIC_TARGET} Eigen3::Eigen ground_seg_cores) +add_library(${PARENT_PROJECT_NAME}::${CLASSIC_TARGET} ALIAS ${CLASSIC_TARGET}) + +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ + DESTINATION include +) +install(TARGETS ${CLASSIC_TARGET} + EXPORT ${PARENT_PROJECT_NAME}Config + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} +) + +option(BUILD_PATCHWORK_TESTS "Build C++ smoke test for PatchWork" OFF) +if (BUILD_PATCHWORK_TESTS) + add_executable(patchwork_smoke tests/smoke_test.cpp) + target_link_libraries(patchwork_smoke PRIVATE ${CLASSIC_TARGET}) +endif() diff --git a/cpp/patchwork/include/patchwork/patchwork.h b/cpp/patchwork/include/patchwork/patchwork.h new file mode 100644 index 0000000..bc4e293 --- /dev/null +++ b/cpp/patchwork/include/patchwork/patchwork.h @@ -0,0 +1,134 @@ +#ifndef PATCHWORK_CLASSIC_H +#define PATCHWORK_CLASSIC_H + +#include + +#include + +#include "patchwork/patchworkpp.h" // for patchwork::PointXYZ + +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; + double max_range = 80.0; + double min_range = 2.7; + + // Concentric Zone Model (parametric) + int num_zones = 4; + std::vector num_sectors_each_zone = {16, 32, 54, 32}; + std::vector num_rings_each_zone = {2, 4, 4, 4}; + std::vector min_ranges = {2.7, 12.3625, 22.025, 41.35}; + + // Plane fit + int num_iter = 3; + int num_lpr = 20; + int num_min_pts = 10; + double th_seeds = 0.5; + double th_dist = 0.125; + + // Ground likelihood thresholds (fixed, the Patchwork classic style) + double uprightness_thr = 0.5; + std::vector elevation_thr = {0.523, 0.746, 0.879, 1.125}; + std::vector flatness_thr = {0.0005, 0.000725, 0.001, 0.001}; + + // Adaptive seed selection margin for highly tilted ground + double adaptive_seed_selection_margin = -1.1; + + // Global elevation guard + bool using_global_thr = true; + double global_elevation_thr = 0.0; + + // ATAT (default ON) + bool ATAT_ON = true; + double max_h_for_ATAT = 0.3; + int num_sectors_for_ATAT = 20; + double noise_bound = 0.2; + + bool verbose = false; +}; + +class PatchWork { + public: + PatchWork() = default; + explicit PatchWork(const PatchworkParams& params); + + void estimateGround(const Eigen::MatrixXf& cloud); + + Eigen::MatrixX3f getGround() const; + Eigen::MatrixX3f getNonground() const; + std::vector getGroundIndices() const; + std::vector getNongroundIndices() const; + double getTimeTaken() const; + double getHeight() const; + + private: + // Helper functions (defined in patchwork.cpp in later tasks) + 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); + PatchStatus determine_gle_status(int zone_idx, int ring_idx, const PCAFeature& feature) const; + void perform_regionwise_segmentation(int zone_idx, + int ring_idx, + const std::vector& patch, + std::vector& patch_ground, + std::vector& patch_nonground, + PatchStatus& status_out); + void estimate_sensor_height(std::vector& cloud); + double consensus_set_based_height_estimation(const std::vector& candidate_heights); + void materialize() const; + + PatchworkParams params_; + + // Per-iteration scratch (cleared each estimateGround call) + using Sector = std::vector; + using Ring = std::vector; // sectors within one ring + using Zone = std::vector; // rings within one zone + using RegionwisePatches = std::vector; + RegionwisePatches regionwise_patches_; + + // Materialized output points (per-call) + std::vector ground_pts_; + std::vector nonground_pts_; + + // Cached output matrices/indices (lazy via materialize()) + mutable Eigen::MatrixX3f ground_mat_; + mutable Eigen::MatrixX3f nonground_mat_; + mutable std::vector ground_idx_; + mutable std::vector nonground_idx_; + mutable bool outputs_dirty_ = true; + + double time_taken_ = 0.0; + double sensor_height_ = 1.723; // updated by ATAT if enabled +}; + +} // namespace patchwork + +#endif diff --git a/cpp/patchwork/src/patchwork.cpp b/cpp/patchwork/src/patchwork.cpp new file mode 100644 index 0000000..fafb31e --- /dev/null +++ b/cpp/patchwork/src/patchwork.cpp @@ -0,0 +1,430 @@ +#include "patchwork/patchwork.h" + +#include +#include +#include +#include + +namespace { +inline Eigen::MatrixX3f to_matrix(const std::vector& pts) { + Eigen::MatrixX3f m(pts.size(), 3); + for (size_t i = 0; i < pts.size(); ++i) { + m(i, 0) = pts[i].x; + m(i, 1) = pts[i].y; + m(i, 2) = pts[i].z; + } + return m; +} +} // namespace + +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); + for (int z = 0; z < params_.num_zones; ++z) { + regionwise_patches_[z].resize(params_.num_rings_each_zone[z]); + for (int r = 0; r < params_.num_rings_each_zone[z]; ++r) { + regionwise_patches_[z][r].resize(params_.num_sectors_each_zone[z]); + } + } +} + +void PatchWork::flush() { + for (auto& zone : regionwise_patches_) + for (auto& ring : zone) + for (auto& sector : ring) sector.clear(); +} + +void PatchWork::pc2regionwise_patches(const std::vector& src) { + for (int idx = 0; idx < static_cast(src.size()); ++idx) { + const auto& p = src[idx]; + double r = xy2radius(p.x, p.y); + double theta = xy2theta(p.x, p.y); + + if (r < params_.min_range || r > params_.max_range) continue; + + // Determine zone by min_ranges (last index whose min_range <= r) + int zone = 0; + for (int z = params_.num_zones - 1; z >= 0; --z) { + if (r >= params_.min_ranges[z]) { + zone = z; + break; + } + } + if (zone < 0 || zone >= params_.num_zones) continue; + + // Within zone, ring index proportional to (r - min_ranges[z]) / ring_width + double ring_width = + (zone + 1 < params_.num_zones) + ? (params_.min_ranges[zone + 1] - params_.min_ranges[zone]) / + params_.num_rings_each_zone[zone] + : (params_.max_range - params_.min_ranges[zone]) / params_.num_rings_each_zone[zone]; + int ring = std::min(static_cast((r - params_.min_ranges[zone]) / ring_width), + params_.num_rings_each_zone[zone] - 1); + + int sector = + std::min(static_cast(theta / (2 * M_PI / params_.num_sectors_each_zone[zone])), + params_.num_sectors_each_zone[zone] - 1); + + regionwise_patches_[zone][ring][sector].push_back(p); + } +} + +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) { + seeds.clear(); + if (sorted.empty()) return; + + // Patchwork uses adaptive_seed_selection_margin in the first zone (innermost) + // to skip points that are likely from the sensor body / car roof. + int init_idx = 0; + if (zone_idx == 0) { + for (int i = 0; i < static_cast(sorted.size()); ++i) { + if (sorted[i].z < params_.adaptive_seed_selection_margin * params_.sensor_height) { + ++init_idx; + } else { + break; + } + } + } + + double sum = 0.0; + int cnt = 0; + for (int i = init_idx; i < static_cast(sorted.size()) && cnt < params_.num_lpr; ++i) { + sum += sorted[i].z; + ++cnt; + } + double lpr_height = (cnt != 0) ? (sum / cnt) : 0.0; + + for (const auto& p : sorted) { + if (p.z < lpr_height + params_.th_seeds) seeds.push_back(p); + } +} + +PatchStatus PatchWork::determine_gle_status(int zone_idx, + int ring_idx, + const PCAFeature& feature) const { + // Uprightness check + if (std::abs(feature.normal_(2)) < params_.uprightness_thr) { + return PatchStatus::TooTilted; + } + + // The first elevation_thr.size() tiers get tier-specific elevation/flatness thresholds. + const int tier = (zone_idx == 0) ? ring_idx : zone_idx; + if (tier < static_cast(params_.elevation_thr.size())) { + const double mean_z = feature.mean_(2); + if (mean_z > params_.elevation_thr[tier]) { + // Recoverable if the patch is very flat + if (feature.singular_values_(2) < params_.flatness_thr[tier]) { + return PatchStatus::FlatEnough; + } + return PatchStatus::TooHighElevation; + } + return PatchStatus::UprightEnough; + } + + // Beyond tier coverage: optional global elevation guard + if (params_.using_global_thr && feature.mean_(2) > params_.global_elevation_thr) { + return PatchStatus::GloballyTooHighElevation; + } + return PatchStatus::UprightEnough; +} + +void PatchWork::perform_regionwise_segmentation(int zone_idx, + int ring_idx, + const std::vector& patch, + std::vector& patch_ground, + std::vector& patch_nonground, + PatchStatus& status_out) { + patch_ground.clear(); + patch_nonground.clear(); + + if (static_cast(patch.size()) < params_.num_min_pts) { + patch_nonground = patch; + status_out = PatchStatus::FewPoints; + return; + } + + // Sort ascending by z + std::vector sorted = patch; + std::sort( + sorted.begin(), sorted.end(), [](const PointXYZ& a, const PointXYZ& b) { return a.z < b.z; }); + + // Extract initial seeds (LPR) + std::vector ground; + extract_initial_seeds(zone_idx, sorted, ground); + + PCAFeature feature{}; + for (int it = 0; it < params_.num_iter; ++it) { + if (ground.empty()) break; + estimate_plane(ground, feature); + + ground.clear(); + std::vector nonground; + for (const auto& p : sorted) { + Eigen::Vector3f v(p.x, p.y, p.z); + const float distance = feature.normal_.dot(v - feature.mean_); + if (distance < feature.th_dist_d_) { + ground.push_back(p); + } else { + nonground.push_back(p); + } + } + if (it == params_.num_iter - 1) { + patch_ground = std::move(ground); + patch_nonground = std::move(nonground); + } + // For non-final iterations: ground becomes seeds for the next plane fit. + } + + // Fall-back: num_iter == 0 or the loop body was never entered (ground was empty + // on the first iteration). Mirror upstream defensive coding. + if (patch_ground.empty() && patch_nonground.empty()) { + // Re-extract seeds since ground may have been emptied inside the loop. + std::vector seeds; + extract_initial_seeds(zone_idx, sorted, seeds); + for (const auto& p : sorted) { + bool in_seeds = false; + for (const auto& s : seeds) { + if (s.x == p.x && s.y == p.y && s.z == p.z) { + in_seeds = true; + break; + } + } + if (in_seeds) + patch_ground.push_back(p); + else + 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); + } + + status_out = determine_gle_status(zone_idx, ring_idx, feature); +} + +// --------------------------------------------------------------------------- +// ATAT — All-Terrain Automatic sensor-height estimator (C9) +// --------------------------------------------------------------------------- + +double PatchWork::consensus_set_based_height_estimation( + const std::vector& candidate_heights) { + if (candidate_heights.empty()) return sensor_height_; + + // For each candidate, count how many other candidates lie within noise_bound; + // pick the cluster with the highest count and average those. + size_t best_idx = 0; + size_t best_count = 0; + for (size_t i = 0; i < candidate_heights.size(); ++i) { + size_t count = 0; + for (size_t j = 0; j < candidate_heights.size(); ++j) { + if (std::abs(candidate_heights[i] - candidate_heights[j]) < params_.noise_bound) { + ++count; + } + } + if (count > best_count) { + best_count = count; + best_idx = i; + } + } + + // Average the cluster around best_idx + double sum = 0.0; + size_t cnt = 0; + for (double h : candidate_heights) { + if (std::abs(h - candidate_heights[best_idx]) < params_.noise_bound) { + sum += h; + ++cnt; + } + } + return (cnt != 0) ? (sum / cnt) : sensor_height_; +} + +void PatchWork::estimate_sensor_height(std::vector& cloud) { + if (cloud.empty()) return; + + const int num_sectors = std::max(1, params_.num_sectors_for_ATAT); + std::vector sector_min_z(num_sectors, std::numeric_limits::infinity()); + + // Bucket points into angular sectors; track the lowest-z per sector. + // Restrict to a near-field radius window (<=5 m) where ground is reliable. + for (const auto& p : cloud) { + const double r = xy2radius(p.x, p.y); + if (r > 5.0) continue; + const double theta = xy2theta(p.x, p.y); + const int sector = + std::min(static_cast(theta / (2 * M_PI / num_sectors)), num_sectors - 1); + if (p.z < sector_min_z[sector]) sector_min_z[sector] = p.z; + } + + // Candidate sensor heights = negation of lowest-z per sector, + // filtered to within max_h_for_ATAT of the current sensor_height_. + std::vector candidates; + candidates.reserve(num_sectors); + for (double z : sector_min_z) { + if (std::isfinite(z)) { + const double h = -z; // estimated sensor height from this sector + if (h < sensor_height_ + params_.max_h_for_ATAT && + h > sensor_height_ - params_.max_h_for_ATAT) { + candidates.push_back(h); + } + } + } + + if (candidates.empty()) { + if (params_.verbose) { + std::cout << "[ATAT] no candidates; keeping sensor_height = " << sensor_height_ << std::endl; + } + return; + } + + const double new_height = consensus_set_based_height_estimation(candidates); + if (params_.verbose) { + std::cout << "[ATAT] sensor_height: " << sensor_height_ << " -> " << new_height << std::endl; + } + sensor_height_ = new_height; +} + +// --------------------------------------------------------------------------- +// materialize() — lazy output matrix/index population +// --------------------------------------------------------------------------- + +void PatchWork::materialize() const { + if (!outputs_dirty_) return; + ground_mat_ = to_matrix(ground_pts_); + nonground_mat_ = to_matrix(nonground_pts_); + ground_idx_.clear(); + nonground_idx_.clear(); + for (const auto& p : ground_pts_) ground_idx_.push_back(p.idx); + for (const auto& p : nonground_pts_) nonground_idx_.push_back(p.idx); + outputs_dirty_ = false; +} + +// --------------------------------------------------------------------------- +// Public getters +// --------------------------------------------------------------------------- + +Eigen::MatrixX3f PatchWork::getGround() const { + materialize(); + return ground_mat_; +} +Eigen::MatrixX3f PatchWork::getNonground() const { + materialize(); + return nonground_mat_; +} +std::vector PatchWork::getGroundIndices() const { + materialize(); + return ground_idx_; +} +std::vector PatchWork::getNongroundIndices() const { + materialize(); + return nonground_idx_; +} +double PatchWork::getTimeTaken() const { return time_taken_; } +double PatchWork::getHeight() const { return sensor_height_; } + +// --------------------------------------------------------------------------- +// estimateGround — main public entry point +// --------------------------------------------------------------------------- + +void PatchWork::estimateGround(const Eigen::MatrixXf& cloud) { + using clock = std::chrono::high_resolution_clock; + auto t_start = clock::now(); + + // Initialize CZM on first call + if (regionwise_patches_.empty()) initialize(); + + // 1) Convert input + std::vector all_points; + all_points.reserve(cloud.rows()); + for (int i = 0; i < cloud.rows(); ++i) { + all_points.emplace_back(cloud(i, 0), cloud(i, 1), cloud(i, 2), i); + } + + // 2) Quick pre-filter: drop points far below sensor (upstream's noise cutoff) + std::vector kept; + kept.reserve(all_points.size()); + for (const auto& p : all_points) { + if (p.z >= -sensor_height_ - 2.0) kept.push_back(p); + } + + // 3) ATAT (auto-tuning sensor height) — implemented in Task C9 + if (params_.ATAT_ON) estimate_sensor_height(kept); + + // 4) Reset patch buckets, redistribute + flush(); + pc2regionwise_patches(kept); + + // 5) Per-patch segmentation (sequential — was tbb::parallel_for upstream) + ground_pts_.clear(); + nonground_pts_.clear(); + for (int z = 0; z < params_.num_zones; ++z) { + for (int r = 0; r < params_.num_rings_each_zone[z]; ++r) { + for (int s = 0; s < params_.num_sectors_each_zone[z]; ++s) { + const auto& patch = regionwise_patches_[z][r][s]; + std::vector pg, png; + PatchStatus status; + perform_regionwise_segmentation(z, r, patch, pg, png, status); + switch (status) { + case PatchStatus::UprightEnough: + case PatchStatus::FlatEnough: + ground_pts_.insert(ground_pts_.end(), pg.begin(), pg.end()); + nonground_pts_.insert(nonground_pts_.end(), png.begin(), png.end()); + break; + default: + // Reject the whole patch as nonground + nonground_pts_.insert(nonground_pts_.end(), patch.begin(), patch.end()); + } + } + } + } + + // 6) Mark outputs dirty (actual matrix materialization is lazy) + outputs_dirty_ = true; + + auto t_end = clock::now(); + time_taken_ = std::chrono::duration(t_end - t_start).count(); +} + +} // namespace patchwork diff --git a/cpp/patchwork/tests/smoke_test.cpp b/cpp/patchwork/tests/smoke_test.cpp new file mode 100644 index 0000000..a546815 --- /dev/null +++ b/cpp/patchwork/tests/smoke_test.cpp @@ -0,0 +1,19 @@ +#include +#include + +#include + +#include "patchwork/patchwork.h" + +int main() { + patchwork::PatchworkParams params; + patchwork::PatchWork pw(params); + + Eigen::MatrixXf cloud(0, 4); + pw.estimateGround(cloud); + + assert(pw.getGround().rows() == 0); + assert(pw.getNonground().rows() == 0); + std::cout << "patchwork smoke: ok" << std::endl; + return 0; +} diff --git a/docs/superpowers/plans/2026-05-09-patchwork-classic.md b/docs/superpowers/plans/2026-05-09-patchwork-classic.md new file mode 100644 index 0000000..0b5810d --- /dev/null +++ b/docs/superpowers/plans/2026-05-09-patchwork-classic.md @@ -0,0 +1,1350 @@ +# Patchwork (Classic) Implementation Plan + +> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** Add the original Patchwork algorithm as a parallel library inside this repo so users can A/B compare against Patchwork++ from the same Python module and ROS2 node. + +**Architecture:** New CMake target `ground_seg_classic` under `cpp/patchwork/`, exposed via the existing `pypatchworkpp` Python module as a sibling class `pypatchworkpp.patchwork`. ROS2 node uses `std::variant` to dispatch on a launch parameter `algorithm`. Algorithm body is ported from `/Users/fudxo/git/patchwork` with PCL/TBB/ROS dependencies stripped; we reuse Patchwork++'s parametric Concentric Zone Model (`num_zones`, `num_sectors_each_zone`, `num_rings_each_zone`, `min_ranges`) instead of upstream's sensor-string-driven CZM. + +**Tech Stack:** C++20, Eigen 3.4 (FetchContent), pybind11, scikit-build-core, CMake ≥ 3.18, ROS2 Humble/Jazzy. + +**Reference:** Spec at `docs/superpowers/specs/2026-05-09-add-patchwork-classic-algorithm-design.md`. Upstream source at `/Users/fudxo/git/patchwork/include/patchwork/patchwork.hpp` (lines referenced by `up:NNN`). + +**Key deliberate deviations from a literal port:** + +1. CZM uses Patchwork++'s parametric form (no `sensor_configs.hpp`, no `zone_models.hpp`). +1. `tbb::parallel_for` over patches becomes a sequential `for` loop. +1. Verbose logging via `std::cout` instead of `boost::format` + `RCLCPP_INFO`. +1. Input is `Eigen::MatrixXf` (N×4), not `pcl::PointCloud`. + +______________________________________________________________________ + +## File Structure + +| Path | Status | Responsibility | +|---|---|---| +| `cpp/patchwork/CMakeLists.txt` | NEW | Builds `ground_seg_classic` static lib, exports config | +| `cpp/patchwork/include/patchwork/patchwork.h` | NEW | `PatchworkParams` struct + `PatchWork` class declaration | +| `cpp/patchwork/src/patchwork.cpp` | NEW | Algorithm body | +| `cpp/CMakeLists.txt` | MODIFY | `add_subdirectory(patchwork)` | +| `python/CMakeLists.txt` | MODIFY | Link both targets | +| `python/patchworkpp/pybinding.cpp` | MODIFY | Bind `PatchworkParams` and `patchwork` class | +| `python/tests/test_patchwork_smoke.py` | NEW | Smoke test for the new class | +| `ros/src/GroundSegmentationServer.hpp` | MODIFY | `std::variant` member | +| `ros/src/GroundSegmentationServer.cpp` | MODIFY | Dispatch via `std::visit` | +| `ros/launch/patchworkpp.launch.py` | MODIFY | Expose `algorithm` launch arg | +| `python/pyproject.toml` | MODIFY | Version bump 1.0.4 → 1.1.0 | +| `cpp/CMakeLists.txt` | MODIFY | Project version bump | +| `README.md` | MODIFY | "Choosing an algorithm" subsection | + +______________________________________________________________________ + +## Branch + +All work happens on `feat/patchwork-classic` (already created from current `master` at 306da05). One PR at the end. + +______________________________________________________________________ + +## Phase A — Skeleton & build wiring + +### Task A1: Create empty `cpp/patchwork/` skeleton + +**Files:** + +- Create: `cpp/patchwork/CMakeLists.txt` + +- Create: `cpp/patchwork/include/patchwork/patchwork.h` + +- Create: `cpp/patchwork/src/patchwork.cpp` + +- \[ \] **Step 1: Write the CMakeLists.txt** + +```cmake +project(patchwork_classic_src) + +include(GNUInstallDirs) + +set(CLASSIC_TARGET ground_seg_classic) + +add_library(${CLASSIC_TARGET} STATIC src/patchwork.cpp) +set_target_properties(${CLASSIC_TARGET} PROPERTIES POSITION_INDEPENDENT_CODE ON) + +target_include_directories(${CLASSIC_TARGET} PUBLIC + $ + $ +) +target_link_libraries(${CLASSIC_TARGET} Eigen3::Eigen) +add_library(${PARENT_PROJECT_NAME}::${CLASSIC_TARGET} ALIAS ${CLASSIC_TARGET}) + +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ + DESTINATION include +) +install(TARGETS ${CLASSIC_TARGET} + EXPORT ${PARENT_PROJECT_NAME}Config + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} +) +``` + +- \[ \] **Step 2: Write the minimal header** + +```cpp +// cpp/patchwork/include/patchwork/patchwork.h +#ifndef PATCHWORK_CLASSIC_H +#define PATCHWORK_CLASSIC_H + +#include +#include + +namespace patchwork { + +struct PatchworkParams { + bool verbose = false; +}; + +class PatchWork { + public: + PatchWork() = default; + explicit PatchWork(const PatchworkParams& params); + + void estimateGround(const Eigen::MatrixXf& cloud); + + Eigen::MatrixX3f getGround() const; + Eigen::MatrixX3f getNonground() const; + std::vector getGroundIndices() const; + std::vector getNongroundIndices() const; + double getTimeTaken() const; + double getHeight() const; + + private: + PatchworkParams params_; + Eigen::MatrixX3f ground_; + Eigen::MatrixX3f nonground_; + std::vector ground_idx_; + std::vector nonground_idx_; + double time_taken_ = 0.0; + double sensor_height_ = 1.723; +}; + +} // namespace patchwork + +#endif +``` + +- \[ \] **Step 3: Write a stub `.cpp`** + +```cpp +// cpp/patchwork/src/patchwork.cpp +#include "patchwork/patchwork.h" + +namespace patchwork { + +PatchWork::PatchWork(const PatchworkParams& params) : params_(params) {} + +void PatchWork::estimateGround(const Eigen::MatrixXf& /*cloud*/) {} + +Eigen::MatrixX3f PatchWork::getGround() const { return ground_; } +Eigen::MatrixX3f PatchWork::getNonground() const { return nonground_; } +std::vector PatchWork::getGroundIndices() const { return ground_idx_; } +std::vector PatchWork::getNongroundIndices() const { return nonground_idx_; } +double PatchWork::getTimeTaken() const { return time_taken_; } +double PatchWork::getHeight() const { return sensor_height_; } + +} // namespace patchwork +``` + +- \[ \] **Step 4: Hook into top-level cpp/CMakeLists.txt** + +Modify `cpp/CMakeLists.txt`. After the existing `add_subdirectory(patchworkpp)` line (currently line 48), add: + +```cmake +set(TARGET_NAME ground_seg_classic) +add_subdirectory(patchwork) +``` + +The intermediate `set(TARGET_NAME ...)` is required because `patchwork/CMakeLists.txt` reads `${TARGET_NAME}` (mirrors how `patchworkpp/CMakeLists.txt` reads it). Restore the previous value if any subsequent code needs it; in this codebase there is none. + +Wait — actually `patchworkpp/CMakeLists.txt` reads `TARGET_NAME` as `ground_seg_cores` set at line 46 of the parent. Read both files before modifying to confirm the pattern, then either: +(a) inline the target name inside `patchwork/CMakeLists.txt` (simpler — recommended), or +(b) set/unset around the `add_subdirectory` call. + +Pick (a). Update Task A1 Step 1's CMakeLists.txt to use a literal `ground_seg_classic` instead of `${TARGET_NAME}`: + +```cmake +set(CLASSIC_TARGET ground_seg_classic) +``` + +(already done in Step 1 above). + +- \[ \] **Step 5: Configure and build** + +```bash +rm -rf cpp/build +cmake -Bcpp/build cpp/ +cmake --build cpp/build -j +``` + +Expected: builds both `libground_seg_cores.a` and `libground_seg_classic.a` without errors. + +- \[ \] **Step 6: Commit** + +```bash +git add cpp/patchwork/ cpp/CMakeLists.txt +git commit -m "feat(patchwork): scaffold ground_seg_classic target with empty PatchWork class" +``` + +______________________________________________________________________ + +## Phase B — C++ unit verification + +### Task B1: Add a C++ smoke test that constructs PatchWork + +**Files:** + +- Create: `cpp/patchwork/tests/smoke_test.cpp` +- Modify: `cpp/patchwork/CMakeLists.txt` + +The cpp side has no test framework currently. We add a minimal `main()` test compiled only when `BUILD_PATCHWORK_TESTS` is ON. This serves as a compile-time assurance plus a runtime assertion. + +- \[ \] **Step 1: Add CMake test option** + +Append to `cpp/patchwork/CMakeLists.txt`: + +```cmake +option(BUILD_PATCHWORK_TESTS "Build C++ smoke test for PatchWork" OFF) +if (BUILD_PATCHWORK_TESTS) + add_executable(patchwork_smoke tests/smoke_test.cpp) + target_link_libraries(patchwork_smoke PRIVATE ${CLASSIC_TARGET}) +endif() +``` + +- \[ \] **Step 2: Write the smoke test** + +```cpp +// cpp/patchwork/tests/smoke_test.cpp +#include +#include +#include + +#include "patchwork/patchwork.h" + +int main() { + patchwork::PatchworkParams params; + patchwork::PatchWork pw(params); + + Eigen::MatrixXf cloud(0, 4); + pw.estimateGround(cloud); + + assert(pw.getGround().rows() == 0); + assert(pw.getNonground().rows() == 0); + std::cout << "patchwork smoke: ok" << std::endl; + return 0; +} +``` + +- \[ \] **Step 3: Build with tests on, run** + +```bash +cmake -Bcpp/build cpp/ -DBUILD_PATCHWORK_TESTS=ON +cmake --build cpp/build -j +./cpp/build/patchwork/patchwork_smoke +``` + +Expected: `patchwork smoke: ok`. + +- \[ \] **Step 4: Commit** + +```bash +git add cpp/patchwork/CMakeLists.txt cpp/patchwork/tests/smoke_test.cpp +git commit -m "test(patchwork): add C++ smoke test for empty input handling" +``` + +______________________________________________________________________ + +## Phase C — Algorithm port + +The algorithm body comes from `/Users/fudxo/git/patchwork/include/patchwork/patchwork.hpp`. Each task ports one logical group of functions and verifies the build still succeeds. The runtime smoke test (`patchwork_smoke`) keeps passing because we feed empty input until Phase D. + +When this plan says "port lines X–Y from upstream", the engineer should: + +1. Open `/Users/fudxo/git/patchwork/include/patchwork/patchwork.hpp` at those lines +1. Translate the body, replacing the patterns in the table below +1. Compile after each function + +### Translation cheat sheet + +| Upstream | Replacement | +|---|---| +| `pcl::PointCloud` | `std::vector` | +| `pcl::PointCloud::Ptr` | `std::shared_ptr>` (rare in core algo) | +| `pcl::compute3DCentroid(cloud, centroid)` | `Eigen::Vector3f centroid = mean_xyz(points);` (helper we add) | +| `pcl::getMinMax3D(cloud, min, max)` | manual loop over xyz | +| `tbb::parallel_for(blocked_range(0, N), [&](auto r) { for (int i = r.begin(); i != r.end(); ++i) ... })` | `for (int i = 0; i < N; ++i) ...` | +| `boost::format("...") % a % b` | `std::cout << "..." << a << " " << b << ...` | +| `RCLCPP_INFO(node->get_logger(), fmt, args)` | `if (verbose_) std::cout << fmt << ...` | +| `cloud.points[i]` | `cloud[i]` | +| `cloud.size()` | `cloud.size()` (same) | +| `cloud.push_back(p)` | `cloud.push_back(p)` (same) | + +### Reuse `patchwork::PointXYZ` + +Already declared in `cpp/patchworkpp/include/patchwork/patchworkpp.h:22-29`. Do **not** duplicate. In `cpp/patchwork/include/patchwork/patchwork.h`, add a forward-declared shared header or `#include ` directly. Recommended: include patchworkpp.h's POD struct only. Since the patchworkpp header is a single file, include it; the linker will dedupe. + +Add at the top of `cpp/patchwork/include/patchwork/patchwork.h`: + +```cpp +#include "patchwork/patchworkpp.h" // for patchwork::PointXYZ +``` + +This adds Patchwork++ as a public-include compile dependency of the classic target. Update `cpp/patchwork/CMakeLists.txt` to declare it: + +```cmake +target_link_libraries(${CLASSIC_TARGET} Eigen3::Eigen ground_seg_cores) +``` + +(yes, the classic target gains a transitive header dep on the cores target. This is the smallest-scope way to share the POD.) + +### Task C1: Port full PatchworkParams + private state members + +**Files:** + +- Modify: `cpp/patchwork/include/patchwork/patchwork.h` + +- \[ \] **Step 1: Replace the minimal PatchworkParams with the full struct** + +Source: `up:119-148` (declare_parameter calls show every field with type and default). Translate to: + +```cpp +struct PatchworkParams { + // Sensor / range + double sensor_height = 1.723; + double max_range = 80.0; + double min_range = 2.7; + + // Concentric Zone Model (parametric, mirrors patchwork++ style) + int num_zones = 4; + std::vector num_sectors_each_zone = {16, 32, 54, 32}; + std::vector num_rings_each_zone = {2, 4, 4, 4}; + std::vector min_ranges = {2.7, 12.3625, 22.025, 41.35}; + + // Plane fit + int num_iter = 3; + int num_lpr = 20; + int num_min_pts = 10; + double th_seeds = 0.5; + double th_dist = 0.125; + + // Ground likelihood thresholds (fixed) + double uprightness_thr = 0.5; + std::vector elevation_thr = {0.523, 0.746, 0.879, 1.125}; + std::vector flatness_thr = {0.0005, 0.000725, 0.001, 0.001}; + + // Adaptive seed selection margin for highly tilted ground + double adaptive_seed_selection_margin = -1.1; + + // Global elevation guard + bool using_global_thr = true; + double global_elevation_thr = 0.0; + + // ATAT (default ON per spec) + bool ATAT_ON = true; + double max_h_for_ATAT = 0.3; + int num_sectors_for_ATAT = 20; + double noise_bound = 0.2; + + bool verbose = false; +}; +``` + +- \[ \] **Step 2: Add private state members to PatchWork** + +Mirror upstream `up:196-248` selectively. Drop ROS-only fields. Add: + +```cpp + private: + PatchworkParams params_; + + // Materialized outputs + std::vector ground_pts_; + std::vector nonground_pts_; + + // Cached results + Eigen::MatrixX3f ground_mat_; + Eigen::MatrixX3f nonground_mat_; + std::vector ground_idx_; + std::vector nonground_idx_; + bool outputs_dirty_ = true; + + double time_taken_ = 0.0; + double sensor_height_ = 1.723; // updated by ATAT + + // Per-iteration scratch (cleared each estimateGround call) + using Patch = std::vector; + using Ring = std::vector; + using RegionwisePatches = std::vector; + RegionwisePatches regionwise_patches_; + + // Helper functions (impl in .cpp) + 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); + // ... more added in subsequent tasks +``` + +- \[ \] **Step 3: Build, run smoke** + +```bash +cmake --build cpp/build -j && ./cpp/build/patchwork/patchwork_smoke +``` + +Expected: PASS. + +- \[ \] **Step 4: Commit** + +```bash +git add cpp/patchwork/include/patchwork/patchwork.h +git commit -m "feat(patchwork): port full PatchworkParams and private state" +``` + +### Task C2: Port `initialize()`, `flush()`, polar coord helpers + +**Files:** + +- Modify: `cpp/patchwork/src/patchwork.cpp` + +Source: `up:282-323` for initialize/flush; `up:683-700` for xy2theta/xy2radius. + +- \[ \] **Step 1: Implement helpers** + +```cpp +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); +} +``` + +- \[ \] **Step 2: Implement `initialize()`** + +```cpp +void PatchWork::initialize() { + regionwise_patches_.clear(); + regionwise_patches_.resize(params_.num_zones); + for (int z = 0; z < params_.num_zones; ++z) { + regionwise_patches_[z].resize(params_.num_rings_each_zone[z]); + for (int r = 0; r < params_.num_rings_each_zone[z]; ++r) { + regionwise_patches_[z][r].resize(params_.num_sectors_each_zone[z]); + } + } +} +``` + +Wait — original CZM uses `Ring = vector` per zone (rings × sectors flat). Patchwork++ flattens by `num_rings_each_zone[z] × num_sectors_each_zone[z]` per zone. We follow patchwork++'s shape for consistency. Patches stored as `regionwise_patches_[zone][ring*num_sectors + sector]` is one option; another is `[zone][ring][sector]`. Pick the 3D form for readability: + +```cpp +using Sector = std::vector; +using Ring = std::vector; // sectors within one ring +using Zone = std::vector; // rings within one zone +using RegionwisePatches = std::vector; +``` + +(Update Task C1's typedef block accordingly.) + +- \[ \] **Step 3: Implement `flush()`** + +```cpp +void PatchWork::flush() { + for (auto& zone : regionwise_patches_) + for (auto& ring : zone) + for (auto& sector : ring) + sector.clear(); +} +``` + +- \[ \] **Step 4: Build** + +```bash +cmake --build cpp/build -j +``` + +Expected: success. + +- \[ \] **Step 5: Commit** + +```bash +git add cpp/patchwork/src/patchwork.cpp cpp/patchwork/include/patchwork/patchwork.h +git commit -m "feat(patchwork): port CZM init, flush, and polar coord helpers" +``` + +### Task C3: Port `pc2regionwise_patches` + +**Files:** + +- Modify: `cpp/patchwork/src/patchwork.cpp` + +Source: `up:702-712`. The upstream uses sensor-config-derived ring assignment (`zone_model_.ring_idx(...)`). We rewrite to use parametric ranges. + +- \[ \] **Step 1: Implement** + +```cpp +void PatchWork::pc2regionwise_patches(const std::vector& src) { + for (int idx = 0; idx < static_cast(src.size()); ++idx) { + const auto& p = src[idx]; + double r = xy2radius(p.x, p.y); + double theta = xy2theta(p.x, p.y); + + if (r < params_.min_range || r > params_.max_range) continue; + + // Determine zone by min_ranges + int zone = 0; + for (int z = params_.num_zones - 1; z >= 0; --z) { + if (r >= params_.min_ranges[z]) { zone = z; break; } + } + if (zone < 0 || zone >= params_.num_zones) continue; + + // Within zone, ring index proportional to (r - min_ranges[z]) / ring_width + double ring_width = (zone + 1 < params_.num_zones) + ? (params_.min_ranges[zone + 1] - params_.min_ranges[zone]) + / params_.num_rings_each_zone[zone] + : (params_.max_range - params_.min_ranges[zone]) + / params_.num_rings_each_zone[zone]; + int ring = std::min( + static_cast((r - params_.min_ranges[zone]) / ring_width), + params_.num_rings_each_zone[zone] - 1); + + int sector = std::min( + static_cast(theta / (2 * M_PI / params_.num_sectors_each_zone[zone])), + params_.num_sectors_each_zone[zone] - 1); + + regionwise_patches_[zone][ring][sector].push_back(p); + } +} +``` + +- \[ \] **Step 2: Build** + +```bash +cmake --build cpp/build -j +``` + +- \[ \] **Step 3: Commit** + +```bash +git add cpp/patchwork/src/patchwork.cpp +git commit -m "feat(patchwork): port pc2regionwise_patches with parametric CZM" +``` + +### Task C4: Port `estimate_plane_` (PCA fit) + +**Files:** + +- Modify: `cpp/patchwork/src/patchwork.cpp`, `cpp/patchwork/include/patchwork/patchwork.h` + +Source: `up:325-351`. PCA on a patch's seed points. Return PCAFeature struct. + +- \[ \] **Step 1: Add PCAFeature struct to header** + +In `cpp/patchwork/include/patchwork/patchwork.h`, before `class PatchWork`, add: + +```cpp +struct PCAFeature { + Eigen::Vector3f normal_; + Eigen::Vector3f mean_; + Eigen::Vector3f singular_values_; + float d_; + float th_dist_d_; + float linearity_; + float planarity_; +}; +``` + +And declare `void estimate_plane(const std::vector& seeds, PCAFeature& out);` as a private method. + +- \[ \] **Step 2: Implement** + +Translate `up:325-351`. Pseudocode: + +```cpp +void PatchWork::estimate_plane(const std::vector& seeds, PCAFeature& out) { + 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) / float(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_ = params_.th_dist - out.d_; + out.linearity_ = (out.singular_values_(0) - out.singular_values_(1)) / out.singular_values_(0); + out.planarity_ = (out.singular_values_(1) - out.singular_values_(2)) / out.singular_values_(0); +} +``` + +- \[ \] **Step 3: Build, commit** + +```bash +cmake --build cpp/build -j +git add cpp/patchwork/ +git commit -m "feat(patchwork): port estimate_plane PCA fit" +``` + +### Task C5: Port `extract_initial_seeds_` + +**Files:** + +- Modify: `cpp/patchwork/src/patchwork.cpp`, `cpp/patchwork/include/patchwork/patchwork.h` + +Source: `up:353-393`. LPR-based seed selection. + +- \[ \] **Step 1: Declare and implement** + +Header (private): + +```cpp +void extract_initial_seeds(int zone_idx, + const std::vector& sorted, + std::vector& seeds); +``` + +Body (translate `up:353-393`): + +```cpp +void PatchWork::extract_initial_seeds(int zone_idx, + const std::vector& sorted, + std::vector& seeds) { + seeds.clear(); + double sum = 0.0; + int cnt = 0; + + // Patchwork uses adaptive_seed_selection_margin in the first zone (innermost) + // to skip points that are likely from the sensor body / car roof. + int init_idx = 0; + if (zone_idx == 0) { + for (int i = 0; i < static_cast(sorted.size()); ++i) { + if (sorted[i].z < params_.adaptive_seed_selection_margin * params_.sensor_height) { + ++init_idx; + } else { + break; + } + } + } + + for (int i = init_idx; i < static_cast(sorted.size()) && cnt < params_.num_lpr; ++i) { + sum += sorted[i].z; + ++cnt; + } + double lpr_height = (cnt != 0) ? (sum / cnt) : 0.0; + + for (const auto& p : sorted) { + if (p.z < lpr_height + params_.th_seeds) seeds.push_back(p); + } +} +``` + +- \[ \] **Step 2: Build, commit** + +```bash +cmake --build cpp/build -j +git add cpp/patchwork/ +git commit -m "feat(patchwork): port extract_initial_seeds with adaptive margin" +``` + +### Task C6: Port `determine_ground_likelihood_estimation_status` (GLE check) + +**Files:** + +- Modify: `cpp/patchwork/src/patchwork.cpp`, `cpp/patchwork/include/patchwork/patchwork.h` + +Source: `up:809-836`. Classifies a patch as ground/nonground using uprightness + elevation + flatness. + +- \[ \] **Step 1: Add status enum to header** + +```cpp +enum class PatchStatus { + NotAssigned = -2, + FewPoints = -1, + UprightEnough = 0, + FlatEnough = 1, + TooHighElevation = 2, + TooTilted = 3, + GloballyTooHighElevation = 4, +}; +``` + +- \[ \] **Step 2: Declare and implement** + +Header (private): + +```cpp +PatchStatus determine_gle_status(int zone_idx, int ring_idx, + const PCAFeature& feature) const; +``` + +Body (translate `up:810-836`, replacing magic-number returns with the enum): + +```cpp +PatchStatus PatchWork::determine_gle_status(int zone_idx, int ring_idx, + const PCAFeature& feature) const { + // Uprightness check (normal_ z-component) + if (std::abs(feature.normal_(2)) < params_.uprightness_thr) return PatchStatus::TooTilted; + + // The first num_rings_of_interest rings get tier-specific elevation/flatness thresholds. + int tier = (zone_idx == 0) ? ring_idx : zone_idx; + if (tier < static_cast(params_.elevation_thr.size())) { + double mean_z = feature.mean_(2); + if (mean_z > params_.elevation_thr[tier]) { + // Recoverable if patch is very flat + if (feature.singular_values_(2) < params_.flatness_thr[tier]) + return PatchStatus::FlatEnough; + return PatchStatus::TooHighElevation; + } + return PatchStatus::UprightEnough; + } + + // Beyond tier coverage: optional global elevation guard + if (params_.using_global_thr && feature.mean_(2) > params_.global_elevation_thr) { + return PatchStatus::GloballyTooHighElevation; + } + return PatchStatus::UprightEnough; +} +``` + +- \[ \] **Step 3: Build, commit** + +```bash +cmake --build cpp/build -j +git add cpp/patchwork/ +git commit -m "feat(patchwork): port GLE status classifier" +``` + +### Task C7: Port `perform_regionwise_ground_segmentation` (per-patch loop) + +**Files:** + +- Modify: `cpp/patchwork/src/patchwork.cpp`, `cpp/patchwork/include/patchwork/patchwork.h` + +Source: `up:715-806`. Per-patch: sort by z, extract seeds, fit plane, refit num_iter times, classify. + +- \[ \] **Step 1: Declare and implement** + +Header (private): + +```cpp +void perform_regionwise_segmentation(int zone_idx, int ring_idx, + const std::vector& patch, + std::vector& patch_ground, + std::vector& patch_nonground, + PatchStatus& status_out); +``` + +Body — translate the upstream function structure: + +1. Skip if `patch.size() < num_min_pts`. Set `status_out = FewPoints` and put the entire patch into `patch_nonground`. +1. Sort patch by z. +1. Call `extract_initial_seeds`. +1. Loop `num_iter` times: estimate plane, partition points by `(p - mean) · normal < th_dist_d_` into ground/nonground. +1. After final iteration, call `determine_gle_status`. + +Skip the upstream's `pcl::PointCloud` membership games; we're operating on `std::vector`. + +- \[ \] **Step 2: Build, commit** + +```bash +cmake --build cpp/build -j +git add cpp/patchwork/ +git commit -m "feat(patchwork): port per-patch ground segmentation loop" +``` + +### Task C8: Port `estimate_ground` (main entry) + +**Files:** + +- Modify: `cpp/patchwork/src/patchwork.cpp`, `cpp/patchwork/include/patchwork/patchwork.h` + +Source: `up:528-680`. Main entry that ties everything together. + +- \[ \] **Step 1: Implement `estimateGround`** + +The upstream signature is `estimate_ground(in, ground, nonground)`. Ours is `estimateGround(const Eigen::MatrixXf&)` and getters. Rewrite so that `estimateGround` populates `ground_pts_` / `nonground_pts_` / index vectors and starts a timer. + +```cpp +void PatchWork::estimateGround(const Eigen::MatrixXf& cloud) { + using clock = std::chrono::high_resolution_clock; + auto t_start = clock::now(); + + // Initialize CZM on first call + if (regionwise_patches_.empty()) initialize(); + + // 1) Convert input + std::vector all_points; + all_points.reserve(cloud.rows()); + for (int i = 0; i < cloud.rows(); ++i) { + all_points.emplace_back(cloud(i, 0), cloud(i, 1), cloud(i, 2), i); + } + + // 2) Quick pre-filter: drop points more than 2.0 m below sensor height + // (this is the noise filter the maintainer was interested in) + std::vector kept; + kept.reserve(all_points.size()); + for (const auto& p : all_points) { + if (p.z >= -sensor_height_ - 2.0) kept.push_back(p); + } + + // 3) ATAT (optional) + if (params_.ATAT_ON) estimate_sensor_height(kept); + + // 4) Reset patch buckets, redistribute + flush(); + pc2regionwise_patches(kept); + + // 5) Per-patch segmentation (sequential — was tbb::parallel_for upstream) + ground_pts_.clear(); + nonground_pts_.clear(); + for (int z = 0; z < params_.num_zones; ++z) { + for (int r = 0; r < params_.num_rings_each_zone[z]; ++r) { + for (int s = 0; s < params_.num_sectors_each_zone[z]; ++s) { + const auto& patch = regionwise_patches_[z][r][s]; + std::vector pg, png; + PatchStatus status; + perform_regionwise_segmentation(z, r, patch, pg, png, status); + // Aggregate per-patch outputs + switch (status) { + case PatchStatus::UprightEnough: + case PatchStatus::FlatEnough: + ground_pts_.insert(ground_pts_.end(), pg.begin(), pg.end()); + nonground_pts_.insert(nonground_pts_.end(), png.begin(), png.end()); + break; + default: + // Reject the whole patch as nonground + nonground_pts_.insert(nonground_pts_.end(), patch.begin(), patch.end()); + } + } + } + } + + // 6) Materialize outputs (lazy via outputs_dirty_; here we just mark dirty) + outputs_dirty_ = true; + + auto t_end = clock::now(); + time_taken_ = std::chrono::duration(t_end - t_start).count(); +} +``` + +- \[ \] **Step 2: Implement getters with lazy materialization** + +In the header, mark the cached output members `mutable`, plus `mutable bool outputs_dirty_`. Then the materialize helper can be `const`. Updated header excerpt: + +```cpp + private: + // ... params_, ground_pts_, nonground_pts_, time_taken_, sensor_height_, regionwise_patches_ ... + + mutable Eigen::MatrixX3f ground_mat_; + mutable Eigen::MatrixX3f nonground_mat_; + mutable std::vector ground_idx_; + mutable std::vector nonground_idx_; + mutable bool outputs_dirty_ = true; + + void materialize() const; +``` + +Implementation in `.cpp`: + +```cpp +namespace { +inline Eigen::MatrixX3f to_matrix(const std::vector& pts) { + Eigen::MatrixX3f m(pts.size(), 3); + for (size_t i = 0; i < pts.size(); ++i) { + m(i, 0) = pts[i].x; + m(i, 1) = pts[i].y; + m(i, 2) = pts[i].z; + } + return m; +} +} // namespace + +void PatchWork::materialize() const { + if (!outputs_dirty_) return; + ground_mat_ = to_matrix(ground_pts_); + nonground_mat_ = to_matrix(nonground_pts_); + ground_idx_.clear(); + nonground_idx_.clear(); + for (const auto& p : ground_pts_) ground_idx_.push_back(p.idx); + for (const auto& p : nonground_pts_) nonground_idx_.push_back(p.idx); + outputs_dirty_ = false; +} + +Eigen::MatrixX3f PatchWork::getGround() const { materialize(); return ground_mat_; } +Eigen::MatrixX3f PatchWork::getNonground() const { materialize(); return nonground_mat_; } +std::vector PatchWork::getGroundIndices() const { materialize(); return ground_idx_; } +std::vector PatchWork::getNongroundIndices() const { materialize(); return nonground_idx_; } +double PatchWork::getTimeTaken() const { return time_taken_; } +double PatchWork::getHeight() const { return sensor_height_; } +``` + +- \[ \] **Step 3: Build, run smoke test (still empty input — should succeed)** + +```bash +cmake --build cpp/build -j && ./cpp/build/patchwork/patchwork_smoke +``` + +- \[ \] **Step 4: Commit** + +```bash +git add cpp/patchwork/ +git commit -m "feat(patchwork): port estimate_ground main pipeline" +``` + +### Task C9: Port `estimate_sensor_height` (ATAT) + +**Files:** + +- Modify: `cpp/patchwork/src/patchwork.cpp`, `cpp/patchwork/include/patchwork/patchwork.h` + +Source: `up:453-525` for `estimate_sensor_height` and `up:394-451` for `consensus_set_based_height_estimation`. + +- \[ \] **Step 1: Declare both methods (private)** + +```cpp +void estimate_sensor_height(std::vector& cloud); +double consensus_set_based_height_estimation(const std::vector& candidate_heights); +``` + +- \[ \] **Step 2: Implement** + +The consensus algorithm picks heights within `noise_bound` of each other and averages the largest cluster. Translate `up:394-451` literally — it's pure math, no PCL. + +`estimate_sensor_height` (`up:453-525`) sorts points by xy distance, picks ones within `max_h_for_ATAT` of expected ground (using current `sensor_height_`), buckets them by sector, computes per-sector lowest-z, runs consensus on those candidates, and updates `sensor_height_`. + +- \[ \] **Step 3: Build, commit** + +```bash +cmake --build cpp/build -j +git add cpp/patchwork/ +git commit -m "feat(patchwork): port ATAT (auto-tuning sensor height)" +``` + +______________________________________________________________________ + +## Phase D — Python bindings + +### Task D1: Wire ground_seg_classic into the python module + +**Files:** + +- Modify: `python/CMakeLists.txt` + +- \[ \] **Step 1: Add classic to add_subdirectory and link** + +`python/CMakeLists.txt` already includes `cpp/` via `add_subdirectory(.../../cpp ...)`. That subdir now adds both targets. Update the `target_link_libraries` line: + +```cmake +target_link_libraries(pypatchworkpp PUBLIC + ${PARENT_PROJECT_NAME}::ground_seg_cores + ${PARENT_PROJECT_NAME}::ground_seg_classic) +``` + +- \[ \] **Step 2: Verify pip install still works** + +```bash +cd /tmp && rm -rf pkgtest_d1 && python3 -m venv pkgtest_d1 +source pkgtest_d1/bin/activate +pip install --upgrade pip --quiet +pip install /Users/fudxo/git/patchwork-plusplus/python/ +python -c "import pypatchworkpp; print(dir(pypatchworkpp))" +``` + +Expected: prints module symbols (still only the patchworkpp class until Task D2). + +- \[ \] **Step 3: Commit** + +```bash +git add python/CMakeLists.txt +git commit -m "build(python): link classic ground_seg target into pypatchworkpp module" +``` + +### Task D2: Add pybind11 bindings for PatchworkParams + patchwork + +**Files:** + +- Modify: `python/patchworkpp/pybinding.cpp` + +- \[ \] **Step 1: Add include and bindings** + +Edit `python/patchworkpp/pybinding.cpp`. Add at the top: + +```cpp +#include "patchwork/patchwork.h" +``` + +Inside `PYBIND11_MODULE(pypatchworkpp, m)`, after the existing patchworkpp bindings, add: + +```cpp +py::class_(m, "PatchworkParams") + .def(py::init<>()) + .def_readwrite("sensor_height", &patchwork::PatchworkParams::sensor_height) + .def_readwrite("max_range", &patchwork::PatchworkParams::max_range) + .def_readwrite("min_range", &patchwork::PatchworkParams::min_range) + .def_readwrite("num_zones", &patchwork::PatchworkParams::num_zones) + .def_readwrite("num_sectors_each_zone", &patchwork::PatchworkParams::num_sectors_each_zone) + .def_readwrite("num_rings_each_zone", &patchwork::PatchworkParams::num_rings_each_zone) + .def_readwrite("min_ranges", &patchwork::PatchworkParams::min_ranges) + .def_readwrite("num_iter", &patchwork::PatchworkParams::num_iter) + .def_readwrite("num_lpr", &patchwork::PatchworkParams::num_lpr) + .def_readwrite("num_min_pts", &patchwork::PatchworkParams::num_min_pts) + .def_readwrite("th_seeds", &patchwork::PatchworkParams::th_seeds) + .def_readwrite("th_dist", &patchwork::PatchworkParams::th_dist) + .def_readwrite("uprightness_thr", &patchwork::PatchworkParams::uprightness_thr) + .def_readwrite("elevation_thr", &patchwork::PatchworkParams::elevation_thr) + .def_readwrite("flatness_thr", &patchwork::PatchworkParams::flatness_thr) + .def_readwrite("adaptive_seed_selection_margin", + &patchwork::PatchworkParams::adaptive_seed_selection_margin) + .def_readwrite("using_global_thr", &patchwork::PatchworkParams::using_global_thr) + .def_readwrite("global_elevation_thr", &patchwork::PatchworkParams::global_elevation_thr) + .def_readwrite("ATAT_ON", &patchwork::PatchworkParams::ATAT_ON) + .def_readwrite("max_h_for_ATAT", &patchwork::PatchworkParams::max_h_for_ATAT) + .def_readwrite("num_sectors_for_ATAT", &patchwork::PatchworkParams::num_sectors_for_ATAT) + .def_readwrite("noise_bound", &patchwork::PatchworkParams::noise_bound) + .def_readwrite("verbose", &patchwork::PatchworkParams::verbose); + +py::class_(m, "patchwork") + .def(py::init()) + .def("estimateGround", &patchwork::PatchWork::estimateGround) + .def("getGround", &patchwork::PatchWork::getGround) + .def("getNonground", &patchwork::PatchWork::getNonground) + .def("getGroundIndices", &patchwork::PatchWork::getGroundIndices) + .def("getNongroundIndices",&patchwork::PatchWork::getNongroundIndices) + .def("getTimeTaken", &patchwork::PatchWork::getTimeTaken) + .def("getHeight", &patchwork::PatchWork::getHeight); +``` + +- \[ \] **Step 2: Reinstall and probe** + +```bash +pip install --force-reinstall --no-deps /Users/fudxo/git/patchwork-plusplus/python/ +python -c "import pypatchworkpp; pp=pypatchworkpp.patchwork(pypatchworkpp.PatchworkParams()); print('ok')" +``` + +Expected: `ok`. + +- \[ \] **Step 3: Commit** + +```bash +git add python/patchworkpp/pybinding.cpp +git commit -m "feat(python): expose PatchworkParams and patchwork class via pybind11" +``` + +### Task D3: Add Python smoke test for patchwork class + +**Files:** + +- Create: `python/tests/test_patchwork_smoke.py` + +- \[ \] **Step 1: Write the test** + +```python +# python/tests/test_patchwork_smoke.py +import os + +import numpy as np +import pypatchworkpp + + +DATA_DIR = os.path.join(os.path.dirname(__file__), "..", "..", "data") + + +def _read_bin(path): + return np.fromfile(path, dtype=np.float32).reshape(-1, 4) + + +def test_classic_module_exposes_api(): + assert hasattr(pypatchworkpp, "PatchworkParams") + assert hasattr(pypatchworkpp, "patchwork") + + +def test_classic_estimate_ground_partitions_all_points(): + params = pypatchworkpp.PatchworkParams() + pw = pypatchworkpp.patchwork(params) + + scan = _read_bin(os.path.join(DATA_DIR, "000000.bin")) + pw.estimateGround(scan) + + ground = pw.getGround() + nonground = pw.getNonground() + + assert ground.shape[0] > 0 + assert nonground.shape[0] > 0 + assert ground.shape[0] + nonground.shape[0] <= scan.shape[0] +``` + +- \[ \] **Step 2: Run all python tests** + +```bash +pip install --force-reinstall --no-deps '/Users/fudxo/git/patchwork-plusplus/python[test]' +pytest -rA --verbose /Users/fudxo/git/patchwork-plusplus/python/ +``` + +Expected: all 4 tests pass (2 patchworkpp + 2 patchwork). + +- \[ \] **Step 3: Commit** + +```bash +git add python/tests/test_patchwork_smoke.py +git commit -m "test(python): smoke test for patchwork classic against data/000000.bin" +``` + +______________________________________________________________________ + +## Phase E — ROS2 wiring + +### Task E1: Add algorithm dispatch to GroundSegmentationServer + +**Files:** + +- Modify: `ros/src/GroundSegmentationServer.hpp` +- Modify: `ros/src/GroundSegmentationServer.cpp` + +Read `ros/src/GroundSegmentationServer.hpp` and `.cpp` first. The current code holds a `std::unique_ptr`. We change it to a `std::variant`. + +- \[ \] **Step 1: In .hpp, replace the impl member** + +Old: + +```cpp +std::unique_ptr patchwork_; +``` + +New: + +```cpp +#include +#include "patchwork/patchwork.h" +// existing includes for patchworkpp.h stay + +using ImplVariant = std::variant< + std::unique_ptr, + std::unique_ptr>; +ImplVariant impl_; +``` + +- \[ \] **Step 2: In .cpp constructor, branch on `algorithm` parameter** + +Add at the top of the constructor (after node init, before any param reads that go into the patchworkpp Params): + +```cpp +const std::string algorithm = + declare_parameter("algorithm", "patchworkpp"); +``` + +Then load both Params structs from declared parameters (patchworkpp.h existing parameters keep working; add a parallel block for `PatchworkParams`). Branch: + +```cpp +if (algorithm == "patchwork") { + patchwork::PatchworkParams classic_params = loadClassicParamsFromROS(); + impl_ = std::make_unique(classic_params); + RCLCPP_INFO(get_logger(), "Algorithm: patchwork (classic)"); +} else { + patchwork::Params plusplus_params = loadPlusplusParamsFromROS(); + impl_ = std::make_unique(plusplus_params); + RCLCPP_INFO(get_logger(), "Algorithm: patchworkpp (default)"); +} +``` + +Extract `loadPlusplusParamsFromROS()` and `loadClassicParamsFromROS()` as private helpers. + +- \[ \] **Step 3: Replace direct calls with std::visit** + +Anywhere the code calls `patchwork_->estimateGround(...)` or `patchwork_->getGround()`, rewrite as: + +```cpp +std::visit([&](auto& impl) { impl->estimateGround(cloud); }, impl_); +``` + +For the getters, the variant alternatives have identical method names so `std::visit` lambdas work uniformly: + +```cpp +auto ground = std::visit([](auto& impl) { return impl->getGround(); }, impl_); +``` + +- \[ \] **Step 4: Build the ROS package** + +```bash +cd /Users/fudxo/git/patchwork-plusplus +docker run --rm -v $(pwd):/src -w /src osrf/ros:humble-desktop \ + bash -lc 'source /opt/ros/humble/setup.bash && colcon build --packages-select patchworkpp --event-handlers console_direct+' +``` + +Expected: build succeeds. (If Docker is unavailable, just build via colcon on a Linux host with ROS humble.) + +- \[ \] **Step 5: Commit** + +```bash +git add ros/src/GroundSegmentationServer.hpp ros/src/GroundSegmentationServer.cpp +git commit -m "feat(ros): dispatch on algorithm parameter (patchwork vs patchworkpp)" +``` + +### Task E2: Expose `algorithm` launch arg + +**Files:** + +- Modify: `ros/launch/patchworkpp.launch.py` + +- \[ \] **Step 1: Read the launch file** + +```bash +cat ros/launch/patchworkpp.launch.py +``` + +- \[ \] **Step 2: Add a DeclareLaunchArgument and pass it through** + +Add to the `LaunchDescription` items, before the `Node(...)`: + +```python +DeclareLaunchArgument( + "algorithm", + default_value="patchworkpp", + description="Ground segmentation algorithm: 'patchwork' or 'patchworkpp'", +), +``` + +In the `Node(...)` parameters list, append: + +```python +{"algorithm": LaunchConfiguration("algorithm")}, +``` + +(Adjust imports: add `from launch.actions import DeclareLaunchArgument` and `from launch.substitutions import LaunchConfiguration` if not already present.) + +- \[ \] **Step 3: Commit** + +```bash +git add ros/launch/patchworkpp.launch.py +git commit -m "feat(ros): expose 'algorithm' launch argument" +``` + +______________________________________________________________________ + +## Phase F — Documentation and version bump + +### Task F1: Add "Choosing an algorithm" subsection to README + +**Files:** + +- Modify: `README.md` + +- \[ \] **Step 1: Add the subsection** + +Find the `## :gear: How to build & Run` section. After the existing Python/C++/ROS2 subsections, before `## :pencil: Citation`, insert: + +```markdown +## :compass: Choosing an algorithm + +This repository ships two ground segmentation algorithms with the same input/output API. Pick the one that fits your data: + +- **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. +- **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. + +Python: + +\`\`\`python +import pypatchworkpp as p + +pp_default = p.patchworkpp(p.Parameters()) # Patchwork++ +pp_classic = p.patchwork(p.PatchworkParams()) # Patchwork (classic) +\`\`\` + +ROS2: + +\`\`\`bash +ros2 launch patchworkpp patchworkpp.launch.py algorithm:=patchwork +\`\`\` +``` + +(Use real backticks for the code fences; the example here escapes them so the markdown plan parses cleanly.) + +- \[ \] **Step 2: Commit** + +```bash +git add README.md +git commit -m "docs(readme): add 'Choosing an algorithm' subsection for patchwork vs patchwork++" +``` + +### Task F2: Bump version 1.0.4 → 1.1.0 + +**Files:** + +- Modify: `python/pyproject.toml` + +- Modify: `cpp/CMakeLists.txt` + +- \[ \] **Step 1: Bump in pyproject.toml** + +Change `version = "1.0.4"` to `version = "1.1.0"` (line 7 of `python/pyproject.toml`). + +- \[ \] **Step 2: Bump in cpp/CMakeLists.txt** + +Change `project(patchworkpp VERSION 1.0.4)` to `project(patchworkpp VERSION 1.1.0)` (line 2). + +- \[ \] **Step 3: Reinstall and verify version** + +```bash +pip install --force-reinstall --no-deps /Users/fudxo/git/patchwork-plusplus/python/ +python -c "import pypatchworkpp; print(pypatchworkpp.patchwork.__init__.__doc__)" +``` + +(Sanity, not a hard assertion.) + +- \[ \] **Step 4: Commit** + +```bash +git add python/pyproject.toml cpp/CMakeLists.txt +git commit -m "chore: bump version to 1.1.0 (adds Patchwork classic algorithm)" +``` + +______________________________________________________________________ + +## Phase G — PR + +### Task G1: Push branch and create PR + +- \[ \] **Step 1: Final local CI run** + +Build cpp + run python tests once more: + +```bash +rm -rf cpp/build +cmake -Bcpp/build cpp/ -DBUILD_PATCHWORK_TESTS=ON +cmake --build cpp/build -j +./cpp/build/patchwork/patchwork_smoke + +cd /tmp && python3 -m venv finalpkg && source finalpkg/bin/activate +pip install --upgrade pip --quiet +pip install '/Users/fudxo/git/patchwork-plusplus/python[test]' +pytest -rA --verbose /Users/fudxo/git/patchwork-plusplus/python/ +``` + +All green. + +- \[ \] **Step 2: Push and open PR** + +```bash +git push -u origin feat/patchwork-classic +gh pr create --base master --head feat/patchwork-classic \ + --title "feat: add Patchwork (classic) algorithm alongside Patchwork++" \ + --body "$(cat docs/superpowers/specs/2026-05-09-add-patchwork-classic-algorithm-design.md | sed -n '/^## Background/,/^## Out of scope/p')" +``` + +- \[ \] **Step 3: Wait for CI, address feedback, merge** + +Use `gh pr checks --watch` or the same background-watcher pattern from PR #78. diff --git a/docs/superpowers/specs/2026-05-09-add-patchwork-classic-algorithm-design.md b/docs/superpowers/specs/2026-05-09-add-patchwork-classic-algorithm-design.md new file mode 100644 index 0000000..e8f9946 --- /dev/null +++ b/docs/superpowers/specs/2026-05-09-add-patchwork-classic-algorithm-design.md @@ -0,0 +1,239 @@ +# Add Patchwork (Classic) Algorithm Alongside Patchwork++ + +Date: 2026-05-09 +Status: Draft, awaiting user approval + +## Background + +The maintainer suspects that the original Patchwork algorithm (predecessor of Patchwork++) handles ground-plane noise more aggressively in some scenes, particularly via the simple `z < -sensor_height - 2.0m` cutoff and per-patch reject-if-too-few-points policy. Rather than try to merge two genuinely different algorithms (Patchwork uses fixed elevation/flatness thresholds, Patchwork++ uses adaptive ones plus RNR/RVPF/TGR), we expose **both** in this repository so users can A/B compare on their own data and pick what fits. + +## Goals + +- Add the original Patchwork algorithm as a self-contained library target inside this repo, with the **identical I/O signature** as Patchwork++ (Eigen::MatrixXf in, Eigen::MatrixX3f out). +- Expose it through the same Python module as a parallel class. +- Wire a runtime `algorithm` switch into the ROS2 node so a single launch can run either. +- Preserve the existing Patchwork++ public API and behavior bit-for-bit. Adding the classic algorithm is purely additive. + +## Non-goals + +- No port-and-merge: we are not selectively grafting Patchwork's noise removal onto Patchwork++. Both implementations stay independent. +- No PCL or ROS dependency in the algorithm core. ROS support stays in `ros/`. +- No PyPI release in this PR. Version bump and release are a follow-up. + +## Architecture + +### Source layout + +``` +cpp/ +├── patchworkpp/ # unchanged +│ ├── include/patchwork/patchworkpp.h +│ └── src/patchworkpp.cpp +├── patchwork/ # NEW +│ ├── include/patchwork/patchwork.h +│ └── src/patchwork.cpp +└── CMakeLists.txt # adds add_subdirectory(patchwork) + +python/ +├── patchworkpp/pybinding.cpp # extended to bind both classes +└── tests/ + ├── test_smoke.py # unchanged (Patchwork++) + └── test_patchwork_smoke.py # NEW + +ros/ +├── src/GroundSegmentationServer.cpp # adds algorithm switch +├── src/GroundSegmentationServer.hpp # adds variant member +└── launch/patchworkpp.launch.py # exposes algorithm parameter +``` + +### CMake targets + +- `patchworkpp::ground_seg_cores` — unchanged. Builds the Patchwork++ algorithm. +- `patchworkpp::ground_seg_classic` — new. Builds the Patchwork (classic) algorithm. Static archive, Eigen-only dependency. + +The two targets share the `patchworkpp::` namespace prefix so an installed `find_package(patchworkpp)` pulls both. The CMake target identifier (`ground_seg_classic`) is independent of the C++ class names; the C++ namespace and class names below are not affected by the target name choice. + +## C++ API + +Both algorithms live under `namespace patchwork`. Two parameter structs, two classes, no virtual base (kept simple and zero-cost; users pick concretely). + +```cpp +namespace patchwork { + +// Existing — unchanged +struct Params { /* Patchwork++ parameters */ }; +class PatchWorkpp { /* Patchwork++ implementation */ }; + +// New +struct PatchworkParams { + // Sensor / range + double sensor_height = 1.723; + double max_range = 80.0; + double min_range = 2.7; + + // Concentric Zone Model + int num_zones = 4; + std::vector num_sectors_each_zone = {16, 32, 54, 32}; + std::vector num_rings_each_zone = {2, 4, 4, 4}; + std::vector min_ranges = {2.7, 12.3625, 22.025, 41.35}; + + // Plane fit + int num_iter = 3; + int num_lpr = 20; + int num_min_pts = 10; + double th_seeds = 0.5; + double th_dist = 0.125; + + // Ground likelihood thresholds (fixed, the Patchwork classic style) + double uprightness_thr = 0.5; + std::vector elevation_thr = {0.523, 0.746, 0.879, 1.125}; + std::vector flatness_thr = {0.0005, 0.000725, 0.001, 0.001}; + + // ATAT (All-Terrain Automatic sensor-height Estimator) + bool ATAT_ON = true; // default ON per maintainer preference + double max_h_for_ATAT = 0.3; + int num_sectors_for_ATAT = 20; + + bool verbose = false; +}; + +class PatchWork { + public: + PatchWork() = default; + explicit PatchWork(const PatchworkParams& params); + + // Identical signature to PatchWorkpp::estimateGround + void estimateGround(const Eigen::MatrixXf& cloud); // N x 4 (x, y, z, intensity) + + Eigen::MatrixX3f getGround() const; + Eigen::MatrixX3f getNonground() const; + std::vector getGroundIndices() const; + std::vector getNongroundIndices() const; + double getTimeTaken() const; + double getHeight() const; // Updated by ATAT if enabled + + private: + // Implementation (PCL/ROS-free port from /Users/fudxo/git/patchwork) +}; + +} // namespace patchwork +``` + +The class deliberately has no virtual base. Users that want polymorphism can wrap with `std::variant` or function pointers, but the default API is concrete. Both classes have identical method names so generic templates over the algorithm are trivial. + +## Adapting the original Patchwork code + +The upstream `/Users/fudxo/git/patchwork` algorithm (`include/patchwork/patchwork.hpp`) is ROS-coupled and depends on PCL, TBB, Boost.Format, and rclcpp. Verified by header inspection: + +``` +#include # centroid math +#include # PCD I/O (test only) +#include +#include # ROS 2 QoS +#include +#include # patch parallelism +#include # verbose logging +``` + +Our adapter strips all of these: + +1. **Input conversion** — a private helper `convertToPointVec(const Eigen::MatrixXf&)` produces `std::vector` (the existing POD struct from `cpp/patchworkpp/include/patchwork/patchworkpp.h`, reused so both algorithms share point representation). +1. **Output conversion** — `Eigen::MatrixX3f` built from accumulated ground/nonground vectors, identical to Patchwork++'s helpers. +1. **PCL → Eigen/std** — `pcl::PointCloud` becomes `std::vector`. `pcl::compute3DCentroid` and `pcl::getMinMax3D` become Eigen mean / min/max reductions. +1. **TBB → sequential** — the `tbb::parallel_for` over patches (patchwork.hpp:578) is replaced with a plain `for` loop. Patchwork++'s implementation is also sequential and fast enough for typical 100k-point scans, so the perf delta is acceptable. We do not add an optional TBB toggle in this PR. +1. **Boost.Format → std::cout** — verbose logging only. +1. **rclcpp** — every reference is in the ROS publishing path, not the algorithm itself; it does not appear in our adapter. +1. **ATAT** — ported as-is (sort patches by mean z, average the lowest N, set sensor_height). + +The semantics of the original algorithm (seed selection, plane fitting, GLE thresholds) stay byte-for-byte logically equivalent. We do not "improve" the upstream algorithm; we adapt only the I/O surface. + +## Python API + +`python/patchworkpp/pybinding.cpp` is extended to bind the new class and parameter struct. The existing bindings stay untouched. + +```python +import pypatchworkpp as p + +# Existing (unchanged) +pp = p.patchworkpp(p.Parameters()) +pp.estimateGround(scan) +ground = pp.getGround() + +# New +classic = p.patchwork(p.PatchworkParams()) +classic.estimateGround(scan) +ground = classic.getGround() +``` + +The Python class is named `patchwork` (lowercase) to match the existing `patchworkpp` casing convention. Method names are identical so the same downstream code can swap between them. + +## ROS2 API + +The ROS node gains a string parameter `algorithm` (default `"patchworkpp"`). At construction: + +```cpp +class GroundSegmentationServer : public rclcpp::Node { + // ... + std::variant, + std::unique_ptr> impl_; +}; +``` + +`EstimateGround` and the getters dispatch via `std::visit`. The ROS launch file exposes `algorithm` as a launch argument so users can flip without rebuilding: + +```bash +ros2 launch patchworkpp patchworkpp.launch.py algorithm:=patchwork +``` + +ROS CI runs both `humble` and `jazzy` builds, which already exercise the C++ build. We do not add a runtime launch test in this PR — that scope belongs to a separate testing PR. + +## Testing + +- `python/tests/test_smoke.py` — unchanged. Asserts Patchwork++ still partitions all input points on `data/000000.bin`. +- `python/tests/test_patchwork_smoke.py` — new. Same assertions but instantiates `pypatchworkpp.patchwork(PatchworkParams())`. Confirms the new binding loads and produces non-empty ground/nonground arrays. + +No correctness comparison test (e.g., asserting one algorithm matches the other) — they are deliberately different. The smoke tests only verify that both pipelines run end to end on real data. + +## Build / CI impact + +- `cpp/CMakeLists.txt` adds `add_subdirectory(patchwork)`. The existing `cpp_api` matrix job (Ubuntu 22/24, Windows 2022, macOS 14) builds it automatically. +- `python/CMakeLists.txt` links the pybind module against both `patchworkpp::ground_seg_cores` and `patchworkpp::ground_seg_classic`. +- No new third-party dependency. Only Eigen3 (already FetchContent'd) and pybind11. +- `cpp/example_of_find_package/` is **not** updated in this PR — that example demonstrates `find_package(patchworkpp)` and continues to work. A separate example for the classic algorithm is out of scope. + +## Versioning + +Bump `python/pyproject.toml` and `cpp/CMakeLists.txt` from `1.0.4` to `1.1.0` (minor — additive feature). Actual PyPI release is a follow-up: cut a GitHub Release after this PR merges and the `pypa/gh-action-pypi-publish` step uploads. + +## README updates + +Add a "Choosing an algorithm" subsection under "How to build & Run": + +- When to prefer Patchwork++ (default, adaptive thresholds, RNR/RVPF/TGR for noisy LiDAR). +- When to prefer Patchwork (fixed thresholds, simpler behavior, ATAT for unknown sensor heights). +- Example snippets for both. + +## Risks and open questions + +1. **Code volume** — the upstream `patchwork.hpp` is roughly 1k lines including PCL plumbing. After stripping PCL/ROS we expect 600–800 lines of pure algorithm. Reviewable but not trivial. +1. **Numerical equivalence** — we do not have a regression dataset to assert that our PCL-free port yields identical ground/nonground partitions for the same input as upstream Patchwork. We accept this: the upstream is the reference, ours is a faithful adaptation, and behavior may drift in floating-point edge cases. +1. **Eigen vector resize semantics** — original Patchwork uses pcl point cloud `push_back`; we use `std::vector` then materialize to `Eigen::MatrixX3f` once. This shifts the cost profile slightly but keeps allocations predictable. +1. **Build-target naming** — `ground_seg_classic` is the chosen identifier, deliberately neutral and tied to the build system, not user-facing. + +## Out of scope (explicit) + +- Cross-algorithm benchmark suite or paper-style evaluation. +- Refactoring or "improving" the original Patchwork algorithm. +- Running Patchwork via Patchwork++'s adaptive threshold loop or vice versa. +- C# / Rust bindings, GPU offload, threading changes. +- Updating `cpp/example_of_find_package/` to demonstrate the classic algorithm. + +## Implementation phases (preview, not part of this spec) + +The detailed plan is the next step (writing-plans skill). Expected sequence: + +1. Bring in `cpp/patchwork/` skeleton with placeholder algorithm; verify CMake hooks compile. +1. Port the upstream algorithm body, removing PCL types incrementally; cross-check with the smoke test. +1. Wire the Python binding, add the new smoke test. +1. Wire the ROS variant dispatch and the launch parameter. +1. README + version bump. diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 5cb7f57..5609053 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -35,7 +35,9 @@ endif() pybind11_add_module(pypatchworkpp patchworkpp/pybinding.cpp) -target_link_libraries(pypatchworkpp PUBLIC ${PARENT_PROJECT_NAME}::${TARGET_NAME}) +target_link_libraries(pypatchworkpp PUBLIC + ${PARENT_PROJECT_NAME}::${TARGET_NAME} + ${PARENT_PROJECT_NAME}::ground_seg_classic) if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") target_compile_options(pypatchworkpp PUBLIC -fsized-deallocation) diff --git a/python/patchworkpp/pybinding.cpp b/python/patchworkpp/pybinding.cpp index 3c22bd4..b7e7ad5 100644 --- a/python/patchworkpp/pybinding.cpp +++ b/python/patchworkpp/pybinding.cpp @@ -2,6 +2,7 @@ #include #include +#include "patchwork/patchwork.h" #include "patchwork/patchworkpp.h" namespace py = pybind11; @@ -54,4 +55,41 @@ PYBIND11_MODULE(pypatchworkpp, m) { .def("getNormals", &patchwork::PatchWorkpp::getNormals) .def("estimateGround", &patchwork::PatchWorkpp::estimateGround); // .def_readwrite("sensor_height_", &PatchWorkpp::sensor_height_); + + py::class_(m, "PatchworkParams") + .def(py::init<>()) + .def_readwrite("sensor_height", &patchwork::PatchworkParams::sensor_height) + .def_readwrite("max_range", &patchwork::PatchworkParams::max_range) + .def_readwrite("min_range", &patchwork::PatchworkParams::min_range) + .def_readwrite("num_zones", &patchwork::PatchworkParams::num_zones) + .def_readwrite("num_sectors_each_zone", &patchwork::PatchworkParams::num_sectors_each_zone) + .def_readwrite("num_rings_each_zone", &patchwork::PatchworkParams::num_rings_each_zone) + .def_readwrite("min_ranges", &patchwork::PatchworkParams::min_ranges) + .def_readwrite("num_iter", &patchwork::PatchworkParams::num_iter) + .def_readwrite("num_lpr", &patchwork::PatchworkParams::num_lpr) + .def_readwrite("num_min_pts", &patchwork::PatchworkParams::num_min_pts) + .def_readwrite("th_seeds", &patchwork::PatchworkParams::th_seeds) + .def_readwrite("th_dist", &patchwork::PatchworkParams::th_dist) + .def_readwrite("uprightness_thr", &patchwork::PatchworkParams::uprightness_thr) + .def_readwrite("elevation_thr", &patchwork::PatchworkParams::elevation_thr) + .def_readwrite("flatness_thr", &patchwork::PatchworkParams::flatness_thr) + .def_readwrite("adaptive_seed_selection_margin", + &patchwork::PatchworkParams::adaptive_seed_selection_margin) + .def_readwrite("using_global_thr", &patchwork::PatchworkParams::using_global_thr) + .def_readwrite("global_elevation_thr", &patchwork::PatchworkParams::global_elevation_thr) + .def_readwrite("ATAT_ON", &patchwork::PatchworkParams::ATAT_ON) + .def_readwrite("max_h_for_ATAT", &patchwork::PatchworkParams::max_h_for_ATAT) + .def_readwrite("num_sectors_for_ATAT", &patchwork::PatchworkParams::num_sectors_for_ATAT) + .def_readwrite("noise_bound", &patchwork::PatchworkParams::noise_bound) + .def_readwrite("verbose", &patchwork::PatchworkParams::verbose); + + py::class_(m, "patchwork") + .def(py::init()) + .def("estimateGround", &patchwork::PatchWork::estimateGround) + .def("getGround", &patchwork::PatchWork::getGround) + .def("getNonground", &patchwork::PatchWork::getNonground) + .def("getGroundIndices", &patchwork::PatchWork::getGroundIndices) + .def("getNongroundIndices", &patchwork::PatchWork::getNongroundIndices) + .def("getTimeTaken", &patchwork::PatchWork::getTimeTaken) + .def("getHeight", &patchwork::PatchWork::getHeight); } diff --git a/python/pyproject.toml b/python/pyproject.toml index b9fd219..d747a96 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "scikit_build_core.build" [project] name = "pypatchworkpp" -version = "1.0.4" +version = "1.1.0" requires-python = ">=3.8" description = "ground segmentation" dependencies = [ diff --git a/python/tests/test_patchwork_smoke.py b/python/tests/test_patchwork_smoke.py new file mode 100644 index 0000000..e94e5a9 --- /dev/null +++ b/python/tests/test_patchwork_smoke.py @@ -0,0 +1,31 @@ +# python/tests/test_patchwork_smoke.py +import os + +import numpy as np +import pypatchworkpp + +DATA_DIR = os.path.join(os.path.dirname(__file__), "..", "..", "data") + + +def _read_bin(path): + return np.fromfile(path, dtype=np.float32).reshape(-1, 4) + + +def test_classic_module_exposes_api(): + assert hasattr(pypatchworkpp, "PatchworkParams") + assert hasattr(pypatchworkpp, "patchwork") + + +def test_classic_estimate_ground_partitions_all_points(): + params = pypatchworkpp.PatchworkParams() + pw = pypatchworkpp.patchwork(params) + + scan = _read_bin(os.path.join(DATA_DIR, "000000.bin")) + pw.estimateGround(scan) + + ground = pw.getGround() + nonground = pw.getNonground() + + assert ground.shape[0] > 0 + assert nonground.shape[0] > 0 + assert ground.shape[0] + nonground.shape[0] <= scan.shape[0] diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index f788a6b..fc17bd4 100644 --- a/ros/CMakeLists.txt +++ b/ros/CMakeLists.txt @@ -33,7 +33,9 @@ find_package(tf2_ros REQUIRED) add_library(gseg_component SHARED src/GroundSegmentationServer.cpp) target_compile_features(gseg_component PUBLIC cxx_std_20) target_include_directories(gseg_component PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) -target_link_libraries(gseg_component ${PARENT_PROJECT_NAME}::${TARGET_NAME}) +target_link_libraries(gseg_component + ${PARENT_PROJECT_NAME}::${TARGET_NAME} + ${PARENT_PROJECT_NAME}::ground_seg_classic) ament_target_dependencies( gseg_component rcutils diff --git a/ros/launch/patchworkpp.launch.py b/ros/launch/patchworkpp.launch.py index 562a31c..8f68c98 100644 --- a/ros/launch/patchworkpp.launch.py +++ b/ros/launch/patchworkpp.launch.py @@ -1,5 +1,5 @@ from launch import LaunchDescription -from launch.actions import ExecuteProcess +from launch.actions import DeclareLaunchArgument, ExecuteProcess from launch.conditions import IfCondition from launch.substitutions import (LaunchConfiguration, PathJoinSubstitution, PythonExpression) @@ -17,6 +17,8 @@ class config: def generate_launch_description(): + algorithm = LaunchConfiguration("algorithm", default="patchworkpp") + use_sim_time = LaunchConfiguration("use_sim_time", default="true") # tf tree configuration, these are the likely 3 parameters to change and nothing else @@ -59,7 +61,8 @@ def generate_launch_description(): "uprightness_thr": 0.101, # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. "verbose": True, # display verbose info - } + }, + {"algorithm": algorithm}, ], ) rviz_node = Node( @@ -82,6 +85,11 @@ def generate_launch_description(): ) return LaunchDescription( [ + DeclareLaunchArgument( + "algorithm", + default_value="patchworkpp", + description="Ground segmentation algorithm: 'patchwork' or 'patchworkpp'", + ), patchworkpp_node, rviz_node, bagfile_play, diff --git a/ros/src/GroundSegmentationServer.cpp b/ros/src/GroundSegmentationServer.cpp index a460c35..78d44f1 100644 --- a/ros/src/GroundSegmentationServer.cpp +++ b/ros/src/GroundSegmentationServer.cpp @@ -22,10 +22,12 @@ using utils::EigenToPointCloud2; using utils::GetTimestamps; using utils::PointCloud2ToEigen; -GroundSegmentationServer::GroundSegmentationServer(const rclcpp::NodeOptions &options) - : rclcpp::Node("patchworkpp_node", options) { +// --------------------------------------------------------------------------- +// Parameter loaders +// --------------------------------------------------------------------------- + +patchwork::Params GroundSegmentationServer::loadPlusplusParamsFromROS() { patchwork::Params params; - base_frame_ = declare_parameter("base_frame", base_frame_); params.sensor_height = declare_parameter("sensor_height", params.sensor_height); params.num_iter = declare_parameter("num_iter", params.num_iter); @@ -46,8 +48,61 @@ GroundSegmentationServer::GroundSegmentationServer(const rclcpp::NodeOptions &op // ToDo. Support intensity params.enable_RNR = false; - // Construct the main Patchwork++ node - Patchworkpp_ = std::make_unique(params); + return params; +} + +patchwork::PatchworkParams GroundSegmentationServer::loadClassicParamsFromROS() { + patchwork::PatchworkParams params; + + params.sensor_height = declare_parameter("sensor_height", params.sensor_height); + params.max_range = declare_parameter("max_range", params.max_range); + params.min_range = declare_parameter("min_range", params.min_range); + + params.num_iter = declare_parameter("num_iter", params.num_iter); + params.num_lpr = declare_parameter("num_lpr", params.num_lpr); + params.num_min_pts = declare_parameter("num_min_pts", params.num_min_pts); + params.th_seeds = declare_parameter("th_seeds", params.th_seeds); + params.th_dist = declare_parameter("th_dist", params.th_dist); + + params.uprightness_thr = declare_parameter("uprightness_thr", params.uprightness_thr); + + params.adaptive_seed_selection_margin = declare_parameter( + "adaptive_seed_selection_margin", params.adaptive_seed_selection_margin); + + params.using_global_thr = declare_parameter("using_global_thr", params.using_global_thr); + params.global_elevation_thr = + declare_parameter("global_elevation_thr", params.global_elevation_thr); + + params.ATAT_ON = declare_parameter("ATAT_ON", params.ATAT_ON); + params.max_h_for_ATAT = declare_parameter("max_h_for_ATAT", params.max_h_for_ATAT); + params.num_sectors_for_ATAT = + declare_parameter("num_sectors_for_ATAT", params.num_sectors_for_ATAT); + params.noise_bound = declare_parameter("noise_bound", params.noise_bound); + + params.verbose = declare_parameter("verbose", params.verbose); + + return params; +} + +// --------------------------------------------------------------------------- +// Constructor +// --------------------------------------------------------------------------- + +GroundSegmentationServer::GroundSegmentationServer(const rclcpp::NodeOptions &options) + : rclcpp::Node("patchworkpp_node", options) { + base_frame_ = declare_parameter("base_frame", base_frame_); + + const std::string algorithm = declare_parameter("algorithm", "patchworkpp"); + + if (algorithm == "patchwork") { + patchwork::PatchworkParams classic_params = loadClassicParamsFromROS(); + impl_ = std::make_unique(classic_params); + RCLCPP_INFO(get_logger(), "Algorithm: patchwork (classic)"); + } else { + patchwork::Params plusplus_params = loadPlusplusParamsFromROS(); + impl_ = std::make_unique(plusplus_params); + RCLCPP_INFO(get_logger(), "Algorithm: patchworkpp (default)"); + } // Initialize subscribers pointcloud_sub_ = create_subscription( @@ -73,17 +128,23 @@ GroundSegmentationServer::GroundSegmentationServer(const rclcpp::NodeOptions &op RCLCPP_INFO(this->get_logger(), "Patchwork++ ROS 2 node initialized"); } +// --------------------------------------------------------------------------- +// Callbacks +// --------------------------------------------------------------------------- + void GroundSegmentationServer::EstimateGround( const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { const auto &cloud = patchworkpp_ros::utils::PointCloud2ToEigenMat(msg); // Estimate ground - Patchworkpp_->estimateGround(cloud); + std::visit([&](auto &impl) { impl->estimateGround(cloud); }, impl_); cloud_publisher_->publish(patchworkpp_ros::utils::EigenMatToPointCloud2(cloud, msg->header)); + // Get ground and nonground - Eigen::MatrixX3f ground = Patchworkpp_->getGround(); - Eigen::MatrixX3f nonground = Patchworkpp_->getNonground(); - double time_taken = Patchworkpp_->getTimeTaken(); + Eigen::MatrixX3f ground = std::visit([](auto &impl) { return impl->getGround(); }, impl_); + Eigen::MatrixX3f nonground = std::visit([](auto &impl) { return impl->getNonground(); }, impl_); + double time_taken = std::visit([](auto &impl) { return impl->getTimeTaken(); }, impl_); + (void)time_taken; // available for debug logging if needed PublishClouds(ground, nonground, msg->header); } diff --git a/ros/src/GroundSegmentationServer.hpp b/ros/src/GroundSegmentationServer.hpp index 90e8b03..6fb946c 100644 --- a/ros/src/GroundSegmentationServer.hpp +++ b/ros/src/GroundSegmentationServer.hpp @@ -1,8 +1,10 @@ -// Patchwork++ +// Patchwork++ and Patchwork classic +#include "patchwork/patchwork.h" #include "patchwork/patchworkpp.h" -// ROS 2 +// Standard library #include +#include #include #include @@ -25,6 +27,10 @@ class GroundSegmentationServer : public rclcpp::Node { const Eigen::MatrixX3f &est_nonground, const std_msgs::msg::Header header_msg); + /// Parameter loaders — only the selected algorithm's loader is called + patchwork::Params loadPlusplusParamsFromROS(); + patchwork::PatchworkParams loadClassicParamsFromROS(); + private: /// Data subscribers. rclcpp::Subscription::SharedPtr pointcloud_sub_; @@ -34,8 +40,10 @@ class GroundSegmentationServer : public rclcpp::Node { rclcpp::Publisher::SharedPtr ground_publisher_; rclcpp::Publisher::SharedPtr nonground_publisher_; - /// Patchwork++ - std::unique_ptr Patchworkpp_; + /// Algorithm implementation (patchworkpp or patchwork classic) + using ImplVariant = + std::variant, std::unique_ptr>; + ImplVariant impl_; std::string base_frame_{"base_link"}; };