feat: add Patchwork (classic) algorithm alongside Patchwork++#79
Merged
Conversation
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.
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).
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_.
…lize 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 <chrono> include.
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.
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.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
Adds the original Patchwork ground segmentation algorithm as a parallel library inside this repo so users can A/B compare against Patchwork++ from the same Python module and ROS2 node.
The maintainer suspected ground-plane noise removal is more aggressive in upstream Patchwork (z-cutoff at sensor_height + 2 m, reject-too-small-patches policy). Rather than try to merge two genuinely different algorithms, we expose both behind a runtime switch and let users pick what fits their data.
What you get
patchworkpp::ground_seg_classic(cpp/patchwork/)pypatchworkpp.patchwork(PatchworkParams)with the same I/O signature as the existingpypatchworkpp.patchworkppros2 launch patchworkpp patchworkpp.launch.py algorithm:=patchworkdata/000000.binDeliberate deviations from a literal port
The upstream
/Users/fudxo/git/patchworkis ROS-coupled (PCL, TBB, Boost.Format, rclcpp). We strip all of that and adapt the I/O surface:num_zones,num_sectors_each_zone,num_rings_each_zone,min_ranges) instead of the upstream sensor-string-driven CZM. Nosensor_configs.hpp/zone_models.hpp.tbb::parallel_forover patches becomes a sequentialforloop. Patchwork++ is also sequential and fast enough for 100 k-point scans.pcl::PointCloud<PointXYZI>becomesstd::vector<patchwork::PointXYZ>. ThePointXYZPOD is reused fromcpp/patchworkpp/include/patchwork/patchworkpp.h.boost::format+RCLCPP_INFObecomesstd::coutguarded byparams_.verbose.The semantics of the algorithm (seed selection, plane fit, GLE classifier, ATAT) are byte-for-byte logically equivalent.
Bit of design context
Spec:
docs/superpowers/specs/2026-05-09-add-patchwork-classic-algorithm-design.mdPlan:
docs/superpowers/plans/2026-05-09-patchwork-classic.mdPlan was decomposed into 19 commits across 7 phases (skeleton → algorithm port → Python bindings → ROS wiring → docs/version). Each commit independently builds and keeps the smoke test green.
Test plan
patchwork_smoke)data/000000.bin) producing non-empty ground/nonground partitionsOut of scope
cpp/example_of_find_package/for the classic algorithm