From e1cfe97df40a1312a396aaf980fa940a3cf76046 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 10:59:36 +0900 Subject: [PATCH 01/21] docs: add design spec for Patchwork (classic) algorithm support Documents the planned addition of the original Patchwork algorithm as a parallel library target alongside Patchwork++, exposed through the same Python module and ROS2 node via a runtime switch. Captures the API surface, the strategy for stripping PCL/TBB/ROS dependencies from the upstream code, testing approach, and out-of-scope items. --- ...-add-patchwork-classic-algorithm-design.md | 239 ++++++++++++++++++ 1 file changed, 239 insertions(+) create mode 100644 docs/superpowers/specs/2026-05-09-add-patchwork-classic-algorithm-design.md 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..2ad74de --- /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). +2. **Output conversion** — `Eigen::MatrixX3f` built from accumulated ground/nonground vectors, identical to Patchwork++'s helpers. +3. **PCL → Eigen/std** — `pcl::PointCloud` becomes `std::vector`. `pcl::compute3DCentroid` and `pcl::getMinMax3D` become Eigen mean / min/max reductions. +4. **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. +5. **Boost.Format → std::cout** — verbose logging only. +6. **rclcpp** — every reference is in the ROS publishing path, not the algorithm itself; it does not appear in our adapter. +7. **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. +2. **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. +3. **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. +4. **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. +2. Port the upstream algorithm body, removing PCL types incrementally; cross-check with the smoke test. +3. Wire the Python binding, add the new smoke test. +4. Wire the ROS variant dispatch and the launch parameter. +5. README + version bump. From 6a0df57b330c7d4c5558ce18bb2b1d3fc938a0fc Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 11:07:51 +0900 Subject: [PATCH 02/21] docs: add implementation plan for Patchwork (classic) algorithm Decomposes the spec into 7 phases (skeleton, C++ verify, algorithm port, Python bindings, ROS wiring, docs/version, PR) with bite-sized tasks per phase. Each task ends in a commit. Documents the deliberate deviations from a literal upstream port (parametric CZM, sequential patch processing, Eigen-only types). --- .../plans/2026-05-09-patchwork-classic.md | 1327 +++++++++++++++++ 1 file changed, 1327 insertions(+) create mode 100644 docs/superpowers/plans/2026-05-09-patchwork-classic.md 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..aada587 --- /dev/null +++ b/docs/superpowers/plans/2026-05-09-patchwork-classic.md @@ -0,0 +1,1327 @@ +# 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`). +2. `tbb::parallel_for` over patches becomes a sequential `for` loop. +3. Verbose logging via `std::cout` instead of `boost::format` + `RCLCPP_INFO`. +4. 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 +2. Translate the body, replacing the patterns in the table below +3. 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`. +2. Sort patch by z. +3. Call `extract_initial_seeds`. +4. Loop `num_iter` times: estimate plane, partition points by `(p - mean) · normal < th_dist_d_` into ground/nonground. +5. 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. From 58a0c668b9f69ab197f17efc1e1d4957b55e36e1 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 11:55:47 +0900 Subject: [PATCH 03/21] feat(patchwork): scaffold ground_seg_classic target with empty PatchWork class --- cpp/CMakeLists.txt | 1 + cpp/patchwork/CMakeLists.txt | 24 +++++++++++++ cpp/patchwork/include/patchwork/patchwork.h | 39 +++++++++++++++++++++ cpp/patchwork/src/patchwork.cpp | 16 +++++++++ 4 files changed, 80 insertions(+) create mode 100644 cpp/patchwork/CMakeLists.txt create mode 100644 cpp/patchwork/include/patchwork/patchwork.h create mode 100644 cpp/patchwork/src/patchwork.cpp diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt index ae0fefd..c7adaeb 100755 --- a/cpp/CMakeLists.txt +++ b/cpp/CMakeLists.txt @@ -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..17c8623 --- /dev/null +++ b/cpp/patchwork/CMakeLists.txt @@ -0,0 +1,24 @@ +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} +) diff --git a/cpp/patchwork/include/patchwork/patchwork.h b/cpp/patchwork/include/patchwork/patchwork.h new file mode 100644 index 0000000..afdbfc7 --- /dev/null +++ b/cpp/patchwork/include/patchwork/patchwork.h @@ -0,0 +1,39 @@ +#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 diff --git a/cpp/patchwork/src/patchwork.cpp b/cpp/patchwork/src/patchwork.cpp new file mode 100644 index 0000000..76951c6 --- /dev/null +++ b/cpp/patchwork/src/patchwork.cpp @@ -0,0 +1,16 @@ +#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 From 8b9f64cee287eb60c05b5600a490d4680816f931 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 11:57:51 +0900 Subject: [PATCH 04/21] test(patchwork): add C++ smoke test for empty input handling --- cpp/patchwork/CMakeLists.txt | 6 ++++++ cpp/patchwork/tests/smoke_test.cpp | 18 ++++++++++++++++++ 2 files changed, 24 insertions(+) create mode 100644 cpp/patchwork/tests/smoke_test.cpp diff --git a/cpp/patchwork/CMakeLists.txt b/cpp/patchwork/CMakeLists.txt index 17c8623..ad5c26a 100644 --- a/cpp/patchwork/CMakeLists.txt +++ b/cpp/patchwork/CMakeLists.txt @@ -22,3 +22,9 @@ install(TARGETS ${CLASSIC_TARGET} 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/tests/smoke_test.cpp b/cpp/patchwork/tests/smoke_test.cpp new file mode 100644 index 0000000..623ba12 --- /dev/null +++ b/cpp/patchwork/tests/smoke_test.cpp @@ -0,0 +1,18 @@ +#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; +} From 0235a313cf7f2180f86fe6af0f3bbc805d92f9cb Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 11:59:46 +0900 Subject: [PATCH 05/21] feat(patchwork): port full PatchworkParams and private state Replace stub header with full PCAFeature, PatchStatus, PatchworkParams (CZM, plane-fit, GLE thresholds, ATAT) and PatchWork private state (regionwise_patches_, lazy materialize pattern). Link ground_seg_cores so the classic target can include patchworkpp.h for PointXYZ reuse. Also fixes stub patchwork.cpp to use renamed ground_mat_/nonground_mat_. --- cpp/patchwork/CMakeLists.txt | 2 +- cpp/patchwork/include/patchwork/patchwork.h | 116 ++++++++++++++++++-- cpp/patchwork/src/patchwork.cpp | 4 +- 3 files changed, 108 insertions(+), 14 deletions(-) diff --git a/cpp/patchwork/CMakeLists.txt b/cpp/patchwork/CMakeLists.txt index ad5c26a..daf5a3a 100644 --- a/cpp/patchwork/CMakeLists.txt +++ b/cpp/patchwork/CMakeLists.txt @@ -11,7 +11,7 @@ target_include_directories(${CLASSIC_TARGET} PUBLIC $ $ ) -target_link_libraries(${CLASSIC_TARGET} Eigen3::Eigen) +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/ diff --git a/cpp/patchwork/include/patchwork/patchwork.h b/cpp/patchwork/include/patchwork/patchwork.h index afdbfc7..0cbf875 100644 --- a/cpp/patchwork/include/patchwork/patchwork.h +++ b/cpp/patchwork/include/patchwork/patchwork.h @@ -4,9 +4,67 @@ #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; }; @@ -17,21 +75,57 @@ class PatchWork { void estimateGround(const Eigen::MatrixXf& cloud); - Eigen::MatrixX3f getGround() const; - Eigen::MatrixX3f getNonground() const; - std::vector getGroundIndices() const; + Eigen::MatrixX3f getGround() const; + Eigen::MatrixX3f getNonground() const; + std::vector getGroundIndices() const; std::vector getNongroundIndices() const; - double getTimeTaken() const; - double getHeight() 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_; - Eigen::MatrixX3f ground_; - Eigen::MatrixX3f nonground_; - std::vector ground_idx_; - std::vector nonground_idx_; - double time_taken_ = 0.0; - double sensor_height_ = 1.723; + + // 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 diff --git a/cpp/patchwork/src/patchwork.cpp b/cpp/patchwork/src/patchwork.cpp index 76951c6..9a31dab 100644 --- a/cpp/patchwork/src/patchwork.cpp +++ b/cpp/patchwork/src/patchwork.cpp @@ -6,8 +6,8 @@ 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_; } +Eigen::MatrixX3f PatchWork::getGround() const { return ground_mat_; } +Eigen::MatrixX3f PatchWork::getNonground() const { return nonground_mat_; } std::vector PatchWork::getGroundIndices() const { return ground_idx_; } std::vector PatchWork::getNongroundIndices() const { return nonground_idx_; } double PatchWork::getTimeTaken() const { return time_taken_; } From 7d053608f84ed39c66b15807b2f43b7e40b6eadd Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 12:00:48 +0900 Subject: [PATCH 06/21] feat(patchwork): port CZM init, flush, and polar coord helpers --- cpp/patchwork/src/patchwork.cpp | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/cpp/patchwork/src/patchwork.cpp b/cpp/patchwork/src/patchwork.cpp index 9a31dab..ce943c8 100644 --- a/cpp/patchwork/src/patchwork.cpp +++ b/cpp/patchwork/src/patchwork.cpp @@ -1,9 +1,38 @@ +#include + #include "patchwork/patchwork.h" 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::estimateGround(const Eigen::MatrixXf& /*cloud*/) {} Eigen::MatrixX3f PatchWork::getGround() const { return ground_mat_; } From cdd94a0164d3ad6f4e6cc643055d1ef394caae08 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 12:01:52 +0900 Subject: [PATCH 07/21] feat(patchwork): port pc2regionwise_patches with parametric CZM --- cpp/patchwork/src/patchwork.cpp | 34 +++++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/cpp/patchwork/src/patchwork.cpp b/cpp/patchwork/src/patchwork.cpp index ce943c8..2c44f3f 100644 --- a/cpp/patchwork/src/patchwork.cpp +++ b/cpp/patchwork/src/patchwork.cpp @@ -1,3 +1,4 @@ +#include #include #include "patchwork/patchwork.h" @@ -33,6 +34,39 @@ void PatchWork::flush() { 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::estimateGround(const Eigen::MatrixXf& /*cloud*/) {} Eigen::MatrixX3f PatchWork::getGround() const { return ground_mat_; } From 1655293d452ab124283dcfc3d433f0821121bec0 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 12:02:40 +0900 Subject: [PATCH 08/21] feat(patchwork): port estimate_plane PCA fit --- cpp/patchwork/src/patchwork.cpp | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/cpp/patchwork/src/patchwork.cpp b/cpp/patchwork/src/patchwork.cpp index 2c44f3f..b341be7 100644 --- a/cpp/patchwork/src/patchwork.cpp +++ b/cpp/patchwork/src/patchwork.cpp @@ -67,6 +67,36 @@ void PatchWork::pc2regionwise_patches(const std::vector& src) { } } +void PatchWork::estimate_plane(const std::vector& seeds, PCAFeature& out) { + if (seeds.empty()) return; + + Eigen::MatrixXf pts(seeds.size(), 3); + for (size_t i = 0; i < seeds.size(); ++i) { + pts(i, 0) = seeds[i].x; + pts(i, 1) = seeds[i].y; + pts(i, 2) = seeds[i].z; + } + Eigen::Vector3f mean = pts.colwise().mean(); + Eigen::MatrixXf centered = pts.rowwise() - mean.transpose(); + Eigen::Matrix3f cov = (centered.adjoint() * centered) / + std::max(1.0f, static_cast(pts.rows() - 1)); + + Eigen::JacobiSVD svd(cov, Eigen::ComputeFullU); + out.normal_ = svd.matrixU().col(2); + if (out.normal_(2) < 0) out.normal_ = -out.normal_; + out.singular_values_ = svd.singularValues(); + out.mean_ = mean; + out.d_ = -out.normal_.dot(mean); + out.th_dist_d_ = static_cast(params_.th_dist) - out.d_; + + const float s0 = out.singular_values_(0); + const float s1 = out.singular_values_(1); + const float s2 = out.singular_values_(2); + const float eps = 1e-12f; + out.linearity_ = (s0 - s1) / std::max(s0, eps); + out.planarity_ = (s1 - s2) / std::max(s0, eps); +} + void PatchWork::estimateGround(const Eigen::MatrixXf& /*cloud*/) {} Eigen::MatrixX3f PatchWork::getGround() const { return ground_mat_; } From 7714a1aee478d9bdff14238b1633292debfe9509 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 12:03:26 +0900 Subject: [PATCH 09/21] feat(patchwork): port extract_initial_seeds with adaptive margin --- cpp/patchwork/src/patchwork.cpp | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/cpp/patchwork/src/patchwork.cpp b/cpp/patchwork/src/patchwork.cpp index b341be7..4c65a6b 100644 --- a/cpp/patchwork/src/patchwork.cpp +++ b/cpp/patchwork/src/patchwork.cpp @@ -97,6 +97,38 @@ void PatchWork::estimate_plane(const std::vector& seeds, PCAFeature& o 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); + } +} + void PatchWork::estimateGround(const Eigen::MatrixXf& /*cloud*/) {} Eigen::MatrixX3f PatchWork::getGround() const { return ground_mat_; } From 8a20ee326fddd093e950145ba77630317358bf09 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 12:04:24 +0900 Subject: [PATCH 10/21] feat(patchwork): port GLE status classifier --- cpp/patchwork/src/patchwork.cpp | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/cpp/patchwork/src/patchwork.cpp b/cpp/patchwork/src/patchwork.cpp index 4c65a6b..a71971c 100644 --- a/cpp/patchwork/src/patchwork.cpp +++ b/cpp/patchwork/src/patchwork.cpp @@ -129,6 +129,34 @@ void PatchWork::extract_initial_seeds(int zone_idx, } } +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::estimateGround(const Eigen::MatrixXf& /*cloud*/) {} Eigen::MatrixX3f PatchWork::getGround() const { return ground_mat_; } From 2e14f9e05aa2272752fcc205a6f991581a02e69a Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 12:05:40 +0900 Subject: [PATCH 11/21] feat(patchwork): port per-patch ground segmentation loop --- cpp/patchwork/src/patchwork.cpp | 67 +++++++++++++++++++++++++++++++++ 1 file changed, 67 insertions(+) diff --git a/cpp/patchwork/src/patchwork.cpp b/cpp/patchwork/src/patchwork.cpp index a71971c..fb5aed5 100644 --- a/cpp/patchwork/src/patchwork.cpp +++ b/cpp/patchwork/src/patchwork.cpp @@ -157,6 +157,73 @@ PatchStatus PatchWork::determine_gle_status(int zone_idx, int ring_idx, 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); +} + void PatchWork::estimateGround(const Eigen::MatrixXf& /*cloud*/) {} Eigen::MatrixX3f PatchWork::getGround() const { return ground_mat_; } From 4a92f23b725f0aaffbb77f8abf838ebfa3798244 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 13:56:01 +0900 Subject: [PATCH 12/21] feat(patchwork): port estimate_ground main pipeline with lazy materialize Implement estimateGround (CZM init on first call, point conversion, pre-filter, ATAT stub, flush+redistribute, per-patch segmentation loop, lazy outputs_dirty flag), materialize() with to_matrix helper, and rewritten getters. Add no-op stubs for consensus_set_based_height_estimation and estimate_sensor_height (filled in C9). Add include. --- cpp/patchwork/src/patchwork.cpp | 121 ++++++++++++++++++++++++++++++-- 1 file changed, 114 insertions(+), 7 deletions(-) diff --git a/cpp/patchwork/src/patchwork.cpp b/cpp/patchwork/src/patchwork.cpp index fb5aed5..f48db15 100644 --- a/cpp/patchwork/src/patchwork.cpp +++ b/cpp/patchwork/src/patchwork.cpp @@ -1,8 +1,21 @@ #include +#include #include #include "patchwork/patchwork.h" +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) {} @@ -224,13 +237,107 @@ void PatchWork::perform_regionwise_segmentation(int zone_idx, int ring_idx, status_out = determine_gle_status(zone_idx, ring_idx, feature); } -void PatchWork::estimateGround(const Eigen::MatrixXf& /*cloud*/) {} +// --------------------------------------------------------------------------- +// ATAT stubs — bodies filled in C9 +// --------------------------------------------------------------------------- + +double PatchWork::consensus_set_based_height_estimation( + const std::vector& /*candidate_heights*/) { + // Filled in C9. + return sensor_height_; +} + +void PatchWork::estimate_sensor_height(std::vector& /*cloud*/) { + // Filled in C9. +} + +// --------------------------------------------------------------------------- +// 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; +} -Eigen::MatrixX3f PatchWork::getGround() const { return ground_mat_; } -Eigen::MatrixX3f PatchWork::getNonground() const { return nonground_mat_; } -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_; } +// --------------------------------------------------------------------------- +// 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 From 02cbfda268c685ee463dbd0d3785be5c0968d8bb Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 15:26:39 +0900 Subject: [PATCH 13/21] feat(patchwork): port ATAT auto-tuning sensor height Implement consensus_set_based_height_estimation and estimate_sensor_height (Task C9). Uses per-sector lowest-z bucketing in a 5 m near-field window, filters candidates within max_h_for_ATAT of the current sensor_height_, and selects the largest noise_bound-coherent cluster via pairwise counting. --- cpp/patchwork/src/patchwork.cpp | 85 ++++++++++++++++++++++++++++++--- 1 file changed, 79 insertions(+), 6 deletions(-) diff --git a/cpp/patchwork/src/patchwork.cpp b/cpp/patchwork/src/patchwork.cpp index f48db15..1973161 100644 --- a/cpp/patchwork/src/patchwork.cpp +++ b/cpp/patchwork/src/patchwork.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include "patchwork/patchwork.h" @@ -238,17 +239,89 @@ void PatchWork::perform_regionwise_segmentation(int zone_idx, int ring_idx, } // --------------------------------------------------------------------------- -// ATAT stubs — bodies filled in C9 +// ATAT — All-Terrain Automatic sensor-height estimator (C9) // --------------------------------------------------------------------------- double PatchWork::consensus_set_based_height_estimation( - const std::vector& /*candidate_heights*/) { - // Filled in C9. - return sensor_height_; + 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*/) { - // Filled in C9. +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; } // --------------------------------------------------------------------------- From 93f89441124a5ac0b785f626eca29f043ab089d1 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 15:27:45 +0900 Subject: [PATCH 14/21] build(python): link ground_seg_classic into pypatchworkpp module --- python/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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) From 83dce796f4698cba6ae6e8ca1dcdc56816d08b29 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 15:29:31 +0900 Subject: [PATCH 15/21] feat(python): expose PatchworkParams and patchwork class via pybind11 --- python/patchworkpp/pybinding.cpp | 38 ++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/python/patchworkpp/pybinding.cpp b/python/patchworkpp/pybinding.cpp index 3c22bd4..aaea8cd 100644 --- a/python/patchworkpp/pybinding.cpp +++ b/python/patchworkpp/pybinding.cpp @@ -3,6 +3,7 @@ #include #include "patchwork/patchworkpp.h" +#include "patchwork/patchwork.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); } From 9763d9a4270bbdabb583fd0ec94f6b3f47e036fa Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 15:30:43 +0900 Subject: [PATCH 16/21] test(python): smoke test for patchwork classic against data/000000.bin --- python/tests/test_patchwork_smoke.py | 32 ++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 python/tests/test_patchwork_smoke.py diff --git a/python/tests/test_patchwork_smoke.py b/python/tests/test_patchwork_smoke.py new file mode 100644 index 0000000..9dc7dd9 --- /dev/null +++ b/python/tests/test_patchwork_smoke.py @@ -0,0 +1,32 @@ +# 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] From d32aec3eb28db0dc89c962747d357a761784c8d5 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 15:36:09 +0900 Subject: [PATCH 17/21] feat(ros): dispatch on algorithm parameter (patchwork vs patchworkpp) Replace the single PatchWorkpp unique_ptr with an ImplVariant (std::variant) and branch on the 'algorithm' ROS parameter at node startup. Adds loadPlusplusParamsFromROS() and loadClassicParamsFromROS() helpers so only the selected algorithm's parameters are declared. All call-sites use std::visit for uniform dispatch. Links ground_seg_classic into gseg_component so patchwork/patchwork.h is visible; verified clean build on ROS Humble in Docker. --- ros/CMakeLists.txt | 4 +- ros/src/GroundSegmentationServer.cpp | 81 ++++++++++++++++++++++++---- ros/src/GroundSegmentationServer.hpp | 17 ++++-- 3 files changed, 88 insertions(+), 14 deletions(-) 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/src/GroundSegmentationServer.cpp b/ros/src/GroundSegmentationServer.cpp index a460c35..f39f300 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,63 @@ 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 +130,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..7f211aa 100644 --- a/ros/src/GroundSegmentationServer.hpp +++ b/ros/src/GroundSegmentationServer.hpp @@ -1,7 +1,9 @@ -// Patchwork++ +// Patchwork++ and Patchwork classic #include "patchwork/patchworkpp.h" +#include "patchwork/patchwork.h" -// ROS 2 +// Standard library +#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,11 @@ 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, + std::unique_ptr>; + ImplVariant impl_; std::string base_frame_{"base_link"}; }; From 5d2042d7e70e0aa039f029e4aaa4c905c2f6476c Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 15:37:15 +0900 Subject: [PATCH 18/21] feat(ros): expose 'algorithm' launch argument --- ros/launch/patchworkpp.launch.py | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) 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, From f6b38dbd7e028bfbcc465536fc5f937e322449ac Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 15:38:07 +0900 Subject: [PATCH 19/21] docs(readme): add 'Choosing an algorithm' subsection for patchwork vs patchwork++ --- README.md | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) 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]) From 9d14a5edbaed5d565c796a10f19f66a2fe0e981b Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 15:43:24 +0900 Subject: [PATCH 20/21] chore: bump version to 1.1.0 (adds Patchwork classic algorithm) --- cpp/CMakeLists.txt | 2 +- python/pyproject.toml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt index c7adaeb..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) 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 = [ From 96c9170a4377fbe214ac1f09afbce74746f594c6 Mon Sep 17 00:00:00 2001 From: Hyungtae Lim Date: Sat, 9 May 2026 15:55:52 +0900 Subject: [PATCH 21/21] style: apply pre-commit auto-fixes (clang-format, isort, mdformat) --- cpp/patchwork/include/patchwork/patchwork.h | 99 ++++----- cpp/patchwork/src/patchwork.cpp | 136 +++++++------ cpp/patchwork/tests/smoke_test.cpp | 3 +- .../plans/2026-05-09-patchwork-classic.md | 189 ++++++++++-------- ...-add-patchwork-classic-algorithm-design.md | 26 +-- python/patchworkpp/pybinding.cpp | 56 +++--- python/tests/test_patchwork_smoke.py | 1 - ros/src/GroundSegmentationServer.cpp | 34 ++-- ros/src/GroundSegmentationServer.hpp | 11 +- 9 files changed, 295 insertions(+), 260 deletions(-) diff --git a/cpp/patchwork/include/patchwork/patchwork.h b/cpp/patchwork/include/patchwork/patchwork.h index 0cbf875..bc4e293 100644 --- a/cpp/patchwork/include/patchwork/patchwork.h +++ b/cpp/patchwork/include/patchwork/patchwork.h @@ -1,9 +1,10 @@ #ifndef PATCHWORK_CLASSIC_H #define PATCHWORK_CLASSIC_H -#include #include +#include + #include "patchwork/patchworkpp.h" // for patchwork::PointXYZ namespace patchwork { @@ -19,12 +20,12 @@ struct PCAFeature { }; enum class PatchStatus { - NotAssigned = -2, - FewPoints = -1, - UprightEnough = 0, - FlatEnough = 1, - TooHighElevation = 2, - TooTilted = 3, + NotAssigned = -2, + FewPoints = -1, + UprightEnough = 0, + FlatEnough = 1, + TooHighElevation = 2, + TooTilted = 3, GloballyTooHighElevation = 4, }; @@ -35,35 +36,35 @@ struct PatchworkParams { 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}; + 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; + 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}; + 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; + 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 ATAT_ON = true; + double max_h_for_ATAT = 0.3; + int num_sectors_for_ATAT = 20; + double noise_bound = 0.2; bool verbose = false; }; @@ -75,41 +76,41 @@ class PatchWork { void estimateGround(const Eigen::MatrixXf& cloud); - Eigen::MatrixX3f getGround() const; - Eigen::MatrixX3f getNonground() const; - std::vector getGroundIndices() const; + Eigen::MatrixX3f getGround() const; + Eigen::MatrixX3f getNonground() const; + std::vector getGroundIndices() const; std::vector getNongroundIndices() const; - double getTimeTaken() const; - double getHeight() const; + double getTimeTaken() const; + double getHeight() const; private: // Helper functions (defined in patchwork.cpp in later tasks) - void initialize(); - void flush(); + 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); + 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; + 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 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_; @@ -122,7 +123,7 @@ class PatchWork { mutable Eigen::MatrixX3f nonground_mat_; mutable std::vector ground_idx_; mutable std::vector nonground_idx_; - mutable bool outputs_dirty_ = true; + mutable bool outputs_dirty_ = true; double time_taken_ = 0.0; double sensor_height_ = 1.723; // updated by ATAT if enabled diff --git a/cpp/patchwork/src/patchwork.cpp b/cpp/patchwork/src/patchwork.cpp index 1973161..fafb31e 100644 --- a/cpp/patchwork/src/patchwork.cpp +++ b/cpp/patchwork/src/patchwork.cpp @@ -1,10 +1,10 @@ +#include "patchwork/patchwork.h" + #include #include #include #include -#include "patchwork/patchwork.h" - namespace { inline Eigen::MatrixX3f to_matrix(const std::vector& pts) { Eigen::MatrixX3f m(pts.size(), 3); @@ -26,9 +26,7 @@ double PatchWork::xy2theta(double x, double y) const { return (a >= 0) ? a : (a + 2 * M_PI); } -double PatchWork::xy2radius(double x, double y) const { - return std::hypot(x, y); -} +double PatchWork::xy2radius(double x, double y) const { return std::hypot(x, y); } void PatchWork::initialize() { regionwise_patches_.clear(); @@ -44,38 +42,39 @@ void PatchWork::initialize() { void PatchWork::flush() { for (auto& zone : regionwise_patches_) for (auto& ring : zone) - for (auto& sector : ring) - sector.clear(); + 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); + 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 (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); + 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); } @@ -90,25 +89,25 @@ void PatchWork::estimate_plane(const std::vector& seeds, PCAFeature& o pts(i, 1) = seeds[i].y; pts(i, 2) = seeds[i].z; } - Eigen::Vector3f mean = pts.colwise().mean(); + 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::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_; + 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 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); + out.linearity_ = (s0 - s1) / std::max(s0, eps); + out.planarity_ = (s1 - s2) / std::max(s0, eps); } void PatchWork::extract_initial_seeds(int zone_idx, @@ -131,7 +130,7 @@ void PatchWork::extract_initial_seeds(int zone_idx, } double sum = 0.0; - int cnt = 0; + int cnt = 0; for (int i = init_idx; i < static_cast(sorted.size()) && cnt < params_.num_lpr; ++i) { sum += sorted[i].z; ++cnt; @@ -143,7 +142,8 @@ void PatchWork::extract_initial_seeds(int zone_idx, } } -PatchStatus PatchWork::determine_gle_status(int zone_idx, int ring_idx, +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) { @@ -171,7 +171,8 @@ PatchStatus PatchWork::determine_gle_status(int zone_idx, int ring_idx, return PatchStatus::UprightEnough; } -void PatchWork::perform_regionwise_segmentation(int zone_idx, int ring_idx, +void PatchWork::perform_regionwise_segmentation(int zone_idx, + int ring_idx, const std::vector& patch, std::vector& patch_ground, std::vector& patch_nonground, @@ -181,14 +182,14 @@ void PatchWork::perform_regionwise_segmentation(int zone_idx, int ring_idx, if (static_cast(patch.size()) < params_.num_min_pts) { patch_nonground = patch; - status_out = PatchStatus::FewPoints; + 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; }); + std::sort( + sorted.begin(), sorted.end(), [](const PointXYZ& a, const PointXYZ& b) { return a.z < b.z; }); // Extract initial seeds (LPR) std::vector ground; @@ -226,10 +227,15 @@ void PatchWork::perform_regionwise_segmentation(int zone_idx, int ring_idx, 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 (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); + 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); @@ -279,18 +285,16 @@ 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()); + 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); + 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; } @@ -310,16 +314,14 @@ void PatchWork::estimate_sensor_height(std::vector& cloud) { if (candidates.empty()) { if (params_.verbose) { - std::cout << "[ATAT] no candidates; keeping sensor_height = " - << sensor_height_ << std::endl; + 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; + std::cout << "[ATAT] sensor_height: " << sensor_height_ << " -> " << new_height << std::endl; } sensor_height_ = new_height; } @@ -334,7 +336,7 @@ void PatchWork::materialize() const { 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 : ground_pts_) ground_idx_.push_back(p.idx); for (const auto& p : nonground_pts_) nonground_idx_.push_back(p.idx); outputs_dirty_ = false; } @@ -343,19 +345,31 @@ void PatchWork::materialize() const { // 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_; } +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; + using clock = std::chrono::high_resolution_clock; auto t_start = clock::now(); // Initialize CZM on first call @@ -409,7 +423,7 @@ void PatchWork::estimateGround(const Eigen::MatrixXf& cloud) { // 6) Mark outputs dirty (actual matrix materialization is lazy) outputs_dirty_ = true; - auto t_end = clock::now(); + auto t_end = clock::now(); time_taken_ = std::chrono::duration(t_end - t_start).count(); } diff --git a/cpp/patchwork/tests/smoke_test.cpp b/cpp/patchwork/tests/smoke_test.cpp index 623ba12..a546815 100644 --- a/cpp/patchwork/tests/smoke_test.cpp +++ b/cpp/patchwork/tests/smoke_test.cpp @@ -1,7 +1,8 @@ -#include #include #include +#include + #include "patchwork/patchwork.h" int main() { diff --git a/docs/superpowers/plans/2026-05-09-patchwork-classic.md b/docs/superpowers/plans/2026-05-09-patchwork-classic.md index aada587..0b5810d 100644 --- a/docs/superpowers/plans/2026-05-09-patchwork-classic.md +++ b/docs/superpowers/plans/2026-05-09-patchwork-classic.md @@ -11,12 +11,13 @@ **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`). -2. `tbb::parallel_for` over patches becomes a sequential `for` loop. -3. Verbose logging via `std::cout` instead of `boost::format` + `RCLCPP_INFO`. -4. Input is `Eigen::MatrixXf` (N×4), not `pcl::PointCloud`. +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 @@ -36,24 +37,27 @@ | `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** +- \[ \] **Step 1: Write the CMakeLists.txt** ```cmake project(patchwork_classic_src) @@ -82,7 +86,7 @@ install(TARGETS ${CLASSIC_TARGET} ) ``` -- [ ] **Step 2: Write the minimal header** +- \[ \] **Step 2: Write the minimal header** ```cpp // cpp/patchwork/include/patchwork/patchwork.h @@ -127,7 +131,7 @@ class PatchWork { #endif ``` -- [ ] **Step 3: Write a stub `.cpp`** +- \[ \] **Step 3: Write a stub `.cpp`** ```cpp // cpp/patchwork/src/patchwork.cpp @@ -149,7 +153,7 @@ double PatchWork::getHeight() const { return sensor_height_; } } // namespace patchwork ``` -- [ ] **Step 4: Hook into top-level cpp/CMakeLists.txt** +- \[ \] **Step 4: Hook into top-level cpp/CMakeLists.txt** Modify `cpp/CMakeLists.txt`. After the existing `add_subdirectory(patchworkpp)` line (currently line 48), add: @@ -172,7 +176,7 @@ set(CLASSIC_TARGET ground_seg_classic) (already done in Step 1 above). -- [ ] **Step 5: Configure and build** +- \[ \] **Step 5: Configure and build** ```bash rm -rf cpp/build @@ -182,26 +186,27 @@ cmake --build cpp/build -j Expected: builds both `libground_seg_cores.a` and `libground_seg_classic.a` without errors. -- [ ] **Step 6: Commit** +- \[ \] **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** +- \[ \] **Step 1: Add CMake test option** Append to `cpp/patchwork/CMakeLists.txt`: @@ -213,7 +218,7 @@ if (BUILD_PATCHWORK_TESTS) endif() ``` -- [ ] **Step 2: Write the smoke test** +- \[ \] **Step 2: Write the smoke test** ```cpp // cpp/patchwork/tests/smoke_test.cpp @@ -237,7 +242,7 @@ int main() { } ``` -- [ ] **Step 3: Build with tests on, run** +- \[ \] **Step 3: Build with tests on, run** ```bash cmake -Bcpp/build cpp/ -DBUILD_PATCHWORK_TESTS=ON @@ -247,23 +252,24 @@ cmake --build cpp/build -j Expected: `patchwork smoke: ok`. -- [ ] **Step 4: Commit** +- \[ \] **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 -2. Translate the body, replacing the patterns in the table below -3. Compile after each function +1. Translate the body, replacing the patterns in the table below +1. Compile after each function ### Translation cheat sheet @@ -301,9 +307,10 @@ target_link_libraries(${CLASSIC_TARGET} Eigen3::Eigen ground_seg_cores) ### 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** +- \[ \] **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: @@ -349,7 +356,7 @@ struct PatchworkParams { }; ``` -- [ ] **Step 2: Add private state members to PatchWork** +- \[ \] **Step 2: Add private state members to PatchWork** Mirror upstream `up:196-248` selectively. Drop ROS-only fields. Add: @@ -386,7 +393,7 @@ Mirror upstream `up:196-248` selectively. Drop ROS-only fields. Add: // ... more added in subsequent tasks ``` -- [ ] **Step 3: Build, run smoke** +- \[ \] **Step 3: Build, run smoke** ```bash cmake --build cpp/build -j && ./cpp/build/patchwork/patchwork_smoke @@ -394,7 +401,7 @@ cmake --build cpp/build -j && ./cpp/build/patchwork/patchwork_smoke Expected: PASS. -- [ ] **Step 4: Commit** +- \[ \] **Step 4: Commit** ```bash git add cpp/patchwork/include/patchwork/patchwork.h @@ -404,11 +411,12 @@ 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** +- \[ \] **Step 1: Implement helpers** ```cpp double PatchWork::xy2theta(double x, double y) const { @@ -421,7 +429,7 @@ double PatchWork::xy2radius(double x, double y) const { } ``` -- [ ] **Step 2: Implement `initialize()`** +- \[ \] **Step 2: Implement `initialize()`** ```cpp void PatchWork::initialize() { @@ -447,7 +455,7 @@ using RegionwisePatches = std::vector; (Update Task C1's typedef block accordingly.) -- [ ] **Step 3: Implement `flush()`** +- \[ \] **Step 3: Implement `flush()`** ```cpp void PatchWork::flush() { @@ -458,7 +466,7 @@ void PatchWork::flush() { } ``` -- [ ] **Step 4: Build** +- \[ \] **Step 4: Build** ```bash cmake --build cpp/build -j @@ -466,7 +474,7 @@ cmake --build cpp/build -j Expected: success. -- [ ] **Step 5: Commit** +- \[ \] **Step 5: Commit** ```bash git add cpp/patchwork/src/patchwork.cpp cpp/patchwork/include/patchwork/patchwork.h @@ -476,11 +484,12 @@ 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** +- \[ \] **Step 1: Implement** ```cpp void PatchWork::pc2regionwise_patches(const std::vector& src) { @@ -517,13 +526,13 @@ void PatchWork::pc2regionwise_patches(const std::vector& src) { } ``` -- [ ] **Step 2: Build** +- \[ \] **Step 2: Build** ```bash cmake --build cpp/build -j ``` -- [ ] **Step 3: Commit** +- \[ \] **Step 3: Commit** ```bash git add cpp/patchwork/src/patchwork.cpp @@ -533,11 +542,12 @@ 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** +- \[ \] **Step 1: Add PCAFeature struct to header** In `cpp/patchwork/include/patchwork/patchwork.h`, before `class PatchWork`, add: @@ -555,7 +565,7 @@ struct PCAFeature { And declare `void estimate_plane(const std::vector& seeds, PCAFeature& out);` as a private method. -- [ ] **Step 2: Implement** +- \[ \] **Step 2: Implement** Translate `up:325-351`. Pseudocode: @@ -582,7 +592,7 @@ void PatchWork::estimate_plane(const std::vector& seeds, PCAFeature& o } ``` -- [ ] **Step 3: Build, commit** +- \[ \] **Step 3: Build, commit** ```bash cmake --build cpp/build -j @@ -593,11 +603,12 @@ 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** +- \[ \] **Step 1: Declare and implement** Header (private): @@ -642,7 +653,7 @@ void PatchWork::extract_initial_seeds(int zone_idx, } ``` -- [ ] **Step 2: Build, commit** +- \[ \] **Step 2: Build, commit** ```bash cmake --build cpp/build -j @@ -653,11 +664,12 @@ 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** +- \[ \] **Step 1: Add status enum to header** ```cpp enum class PatchStatus { @@ -671,7 +683,7 @@ enum class PatchStatus { }; ``` -- [ ] **Step 2: Declare and implement** +- \[ \] **Step 2: Declare and implement** Header (private): @@ -709,7 +721,7 @@ PatchStatus PatchWork::determine_gle_status(int zone_idx, int ring_idx, } ``` -- [ ] **Step 3: Build, commit** +- \[ \] **Step 3: Build, commit** ```bash cmake --build cpp/build -j @@ -720,11 +732,12 @@ 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** +- \[ \] **Step 1: Declare and implement** Header (private): @@ -739,14 +752,14 @@ void perform_regionwise_segmentation(int zone_idx, int ring_idx, 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`. -2. Sort patch by z. -3. Call `extract_initial_seeds`. -4. Loop `num_iter` times: estimate plane, partition points by `(p - mean) · normal < th_dist_d_` into ground/nonground. -5. After final iteration, call `determine_gle_status`. +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** +- \[ \] **Step 2: Build, commit** ```bash cmake --build cpp/build -j @@ -757,11 +770,12 @@ 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`** +- \[ \] **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. @@ -828,7 +842,7 @@ void PatchWork::estimateGround(const Eigen::MatrixXf& cloud) { } ``` -- [ ] **Step 2: Implement getters with lazy materialization** +- \[ \] **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: @@ -879,13 +893,13 @@ 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)** +- \[ \] **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** +- \[ \] **Step 4: Commit** ```bash git add cpp/patchwork/ @@ -895,24 +909,25 @@ 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)** +- \[ \] **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** +- \[ \] **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** +- \[ \] **Step 3: Build, commit** ```bash cmake --build cpp/build -j @@ -920,16 +935,17 @@ 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** +- \[ \] **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: @@ -939,7 +955,7 @@ target_link_libraries(pypatchworkpp PUBLIC ${PARENT_PROJECT_NAME}::ground_seg_classic) ``` -- [ ] **Step 2: Verify pip install still works** +- \[ \] **Step 2: Verify pip install still works** ```bash cd /tmp && rm -rf pkgtest_d1 && python3 -m venv pkgtest_d1 @@ -951,7 +967,7 @@ python -c "import pypatchworkpp; print(dir(pypatchworkpp))" Expected: prints module symbols (still only the patchworkpp class until Task D2). -- [ ] **Step 3: Commit** +- \[ \] **Step 3: Commit** ```bash git add python/CMakeLists.txt @@ -961,9 +977,10 @@ git commit -m "build(python): link classic ground_seg target into pypatchworkpp ### Task D2: Add pybind11 bindings for PatchworkParams + patchwork **Files:** + - Modify: `python/patchworkpp/pybinding.cpp` -- [ ] **Step 1: Add include and bindings** +- \[ \] **Step 1: Add include and bindings** Edit `python/patchworkpp/pybinding.cpp`. Add at the top: @@ -1012,7 +1029,7 @@ py::class_(m, "patchwork") .def("getHeight", &patchwork::PatchWork::getHeight); ``` -- [ ] **Step 2: Reinstall and probe** +- \[ \] **Step 2: Reinstall and probe** ```bash pip install --force-reinstall --no-deps /Users/fudxo/git/patchwork-plusplus/python/ @@ -1021,7 +1038,7 @@ python -c "import pypatchworkpp; pp=pypatchworkpp.patchwork(pypatchworkpp.Patchw Expected: `ok`. -- [ ] **Step 3: Commit** +- \[ \] **Step 3: Commit** ```bash git add python/patchworkpp/pybinding.cpp @@ -1031,9 +1048,10 @@ git commit -m "feat(python): expose PatchworkParams and patchwork class via pybi ### Task D3: Add Python smoke test for patchwork class **Files:** + - Create: `python/tests/test_patchwork_smoke.py` -- [ ] **Step 1: Write the test** +- \[ \] **Step 1: Write the test** ```python # python/tests/test_patchwork_smoke.py @@ -1070,7 +1088,7 @@ def test_classic_estimate_ground_partitions_all_points(): assert ground.shape[0] + nonground.shape[0] <= scan.shape[0] ``` -- [ ] **Step 2: Run all python tests** +- \[ \] **Step 2: Run all python tests** ```bash pip install --force-reinstall --no-deps '/Users/fudxo/git/patchwork-plusplus/python[test]' @@ -1079,26 +1097,27 @@ pytest -rA --verbose /Users/fudxo/git/patchwork-plusplus/python/ Expected: all 4 tests pass (2 patchworkpp + 2 patchwork). -- [ ] **Step 3: Commit** +- \[ \] **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** +- \[ \] **Step 1: In .hpp, replace the impl member** Old: @@ -1119,7 +1138,7 @@ using ImplVariant = std::variant< ImplVariant impl_; ``` -- [ ] **Step 2: In .cpp constructor, branch on `algorithm` parameter** +- \[ \] **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): @@ -1144,7 +1163,7 @@ if (algorithm == "patchwork") { Extract `loadPlusplusParamsFromROS()` and `loadClassicParamsFromROS()` as private helpers. -- [ ] **Step 3: Replace direct calls with std::visit** +- \[ \] **Step 3: Replace direct calls with std::visit** Anywhere the code calls `patchwork_->estimateGround(...)` or `patchwork_->getGround()`, rewrite as: @@ -1158,7 +1177,7 @@ For the getters, the variant alternatives have identical method names so `std::v auto ground = std::visit([](auto& impl) { return impl->getGround(); }, impl_); ``` -- [ ] **Step 4: Build the ROS package** +- \[ \] **Step 4: Build the ROS package** ```bash cd /Users/fudxo/git/patchwork-plusplus @@ -1168,7 +1187,7 @@ docker run --rm -v $(pwd):/src -w /src osrf/ros:humble-desktop \ Expected: build succeeds. (If Docker is unavailable, just build via colcon on a Linux host with ROS humble.) -- [ ] **Step 5: Commit** +- \[ \] **Step 5: Commit** ```bash git add ros/src/GroundSegmentationServer.hpp ros/src/GroundSegmentationServer.cpp @@ -1178,15 +1197,16 @@ git commit -m "feat(ros): dispatch on algorithm parameter (patchwork vs patchwor ### Task E2: Expose `algorithm` launch arg **Files:** + - Modify: `ros/launch/patchworkpp.launch.py` -- [ ] **Step 1: Read the launch file** +- \[ \] **Step 1: Read the launch file** ```bash cat ros/launch/patchworkpp.launch.py ``` -- [ ] **Step 2: Add a DeclareLaunchArgument and pass it through** +- \[ \] **Step 2: Add a DeclareLaunchArgument and pass it through** Add to the `LaunchDescription` items, before the `Node(...)`: @@ -1206,23 +1226,24 @@ In the `Node(...)` parameters list, append: (Adjust imports: add `from launch.actions import DeclareLaunchArgument` and `from launch.substitutions import LaunchConfiguration` if not already present.) -- [ ] **Step 3: Commit** +- \[ \] **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** +- \[ \] **Step 1: Add the subsection** Find the `## :gear: How to build & Run` section. After the existing Python/C++/ROS2 subsections, before `## :pencil: Citation`, insert: @@ -1252,7 +1273,7 @@ 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** +- \[ \] **Step 2: Commit** ```bash git add README.md @@ -1262,18 +1283,20 @@ git commit -m "docs(readme): add 'Choosing an algorithm' subsection for patchwor ### 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** +- \[ \] **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** +- \[ \] **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** +- \[ \] **Step 3: Reinstall and verify version** ```bash pip install --force-reinstall --no-deps /Users/fudxo/git/patchwork-plusplus/python/ @@ -1282,20 +1305,20 @@ python -c "import pypatchworkpp; print(pypatchworkpp.patchwork.__init__.__doc__) (Sanity, not a hard assertion.) -- [ ] **Step 4: Commit** +- \[ \] **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** +- \[ \] **Step 1: Final local CI run** Build cpp + run python tests once more: @@ -1313,7 +1336,7 @@ pytest -rA --verbose /Users/fudxo/git/patchwork-plusplus/python/ All green. -- [ ] **Step 2: Push and open PR** +- \[ \] **Step 2: Push and open PR** ```bash git push -u origin feat/patchwork-classic @@ -1322,6 +1345,6 @@ gh pr create --base master --head feat/patchwork-classic \ --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** +- \[ \] **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 index 2ad74de..e8f9946 100644 --- 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 @@ -138,12 +138,12 @@ The upstream `/Users/fudxo/git/patchwork` algorithm (`include/patchwork/patchwor 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). -2. **Output conversion** — `Eigen::MatrixX3f` built from accumulated ground/nonground vectors, identical to Patchwork++'s helpers. -3. **PCL → Eigen/std** — `pcl::PointCloud` becomes `std::vector`. `pcl::compute3DCentroid` and `pcl::getMinMax3D` become Eigen mean / min/max reductions. -4. **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. -5. **Boost.Format → std::cout** — verbose logging only. -6. **rclcpp** — every reference is in the ROS publishing path, not the algorithm itself; it does not appear in our adapter. -7. **ATAT** — ported as-is (sort patches by mean z, average the lowest N, set sensor_height). +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. @@ -216,9 +216,9 @@ Add a "Choosing an algorithm" subsection under "How to build & Run": ## 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. -2. **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. -3. **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. -4. **Build-target naming** — `ground_seg_classic` is the chosen identifier, deliberately neutral and tied to the build system, not user-facing. +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) @@ -233,7 +233,7 @@ Add a "Choosing an algorithm" subsection under "How to build & Run": 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. -2. Port the upstream algorithm body, removing PCL types incrementally; cross-check with the smoke test. -3. Wire the Python binding, add the new smoke test. -4. Wire the ROS variant dispatch and the launch parameter. -5. README + version bump. +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/patchworkpp/pybinding.cpp b/python/patchworkpp/pybinding.cpp index aaea8cd..b7e7ad5 100644 --- a/python/patchworkpp/pybinding.cpp +++ b/python/patchworkpp/pybinding.cpp @@ -2,8 +2,8 @@ #include #include -#include "patchwork/patchworkpp.h" #include "patchwork/patchwork.h" +#include "patchwork/patchworkpp.h" namespace py = pybind11; @@ -58,38 +58,38 @@ PYBIND11_MODULE(pypatchworkpp, m) { 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("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("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); + .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("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); + .def("getTimeTaken", &patchwork::PatchWork::getTimeTaken) + .def("getHeight", &patchwork::PatchWork::getHeight); } diff --git a/python/tests/test_patchwork_smoke.py b/python/tests/test_patchwork_smoke.py index 9dc7dd9..e94e5a9 100644 --- a/python/tests/test_patchwork_smoke.py +++ b/python/tests/test_patchwork_smoke.py @@ -4,7 +4,6 @@ import numpy as np import pypatchworkpp - DATA_DIR = os.path.join(os.path.dirname(__file__), "..", "..", "data") diff --git a/ros/src/GroundSegmentationServer.cpp b/ros/src/GroundSegmentationServer.cpp index f39f300..78d44f1 100644 --- a/ros/src/GroundSegmentationServer.cpp +++ b/ros/src/GroundSegmentationServer.cpp @@ -66,19 +66,18 @@ patchwork::PatchworkParams GroundSegmentationServer::loadClassicParamsFromROS() 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.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.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.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); @@ -93,8 +92,7 @@ GroundSegmentationServer::GroundSegmentationServer(const rclcpp::NodeOptions &op : rclcpp::Node("patchworkpp_node", options) { base_frame_ = declare_parameter("base_frame", base_frame_); - const std::string algorithm = - declare_parameter("algorithm", "patchworkpp"); + const std::string algorithm = declare_parameter("algorithm", "patchworkpp"); if (algorithm == "patchwork") { patchwork::PatchworkParams classic_params = loadClassicParamsFromROS(); @@ -102,7 +100,7 @@ GroundSegmentationServer::GroundSegmentationServer(const rclcpp::NodeOptions &op RCLCPP_INFO(get_logger(), "Algorithm: patchwork (classic)"); } else { patchwork::Params plusplus_params = loadPlusplusParamsFromROS(); - impl_ = std::make_unique(plusplus_params); + impl_ = std::make_unique(plusplus_params); RCLCPP_INFO(get_logger(), "Algorithm: patchworkpp (default)"); } @@ -139,13 +137,13 @@ void GroundSegmentationServer::EstimateGround( const auto &cloud = patchworkpp_ros::utils::PointCloud2ToEigenMat(msg); // Estimate ground - std::visit([&](auto& impl) { impl->estimateGround(cloud); }, impl_); + 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 = 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_); + 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 7f211aa..6fb946c 100644 --- a/ros/src/GroundSegmentationServer.hpp +++ b/ros/src/GroundSegmentationServer.hpp @@ -1,10 +1,10 @@ // Patchwork++ and Patchwork classic -#include "patchwork/patchworkpp.h" #include "patchwork/patchwork.h" +#include "patchwork/patchworkpp.h" // Standard library -#include #include +#include #include #include @@ -28,7 +28,7 @@ class GroundSegmentationServer : public rclcpp::Node { const std_msgs::msg::Header header_msg); /// Parameter loaders — only the selected algorithm's loader is called - patchwork::Params loadPlusplusParamsFromROS(); + patchwork::Params loadPlusplusParamsFromROS(); patchwork::PatchworkParams loadClassicParamsFromROS(); private: @@ -41,9 +41,8 @@ class GroundSegmentationServer : public rclcpp::Node { rclcpp::Publisher::SharedPtr nonground_publisher_; /// Algorithm implementation (patchworkpp or patchwork classic) - using ImplVariant = std::variant< - std::unique_ptr, - std::unique_ptr>; + using ImplVariant = + std::variant, std::unique_ptr>; ImplVariant impl_; std::string base_frame_{"base_link"};