feat: add Patchwork (classic) algorithm alongside Patchwork++

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])

cpp/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.11)
-project(patchworkpp VERSION 1.0.4)
+project(patchworkpp VERSION 1.1.0)
 
 option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" OFF)
 option(INCLUDE_CPP_EXAMPLES "Include C++ example codes, which require Open3D for visualization" OFF)
@@ -46,6 +46,7 @@ set(PARENT_PROJECT_NAME ${PROJECT_NAME})
 set(TARGET_NAME ground_seg_cores)
 
 add_subdirectory(patchworkpp)
+add_subdirectory(patchwork)
 
 if (INCLUDE_CPP_EXAMPLES)
   if(CMAKE_VERSION VERSION_LESS "3.15")

cpp/patchwork/CMakeLists.txt

@@ -0,0 +1,30 @@
+project(patchwork_classic_src)
+
+include(GNUInstallDirs)
+
+set(CLASSIC_TARGET ground_seg_classic)
+
+add_library(${CLASSIC_TARGET} STATIC src/patchwork.cpp)
+set_target_properties(${CLASSIC_TARGET} PROPERTIES POSITION_INDEPENDENT_CODE ON)
+
+target_include_directories(${CLASSIC_TARGET} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
+)
+target_link_libraries(${CLASSIC_TARGET} Eigen3::Eigen ground_seg_cores)
+add_library(${PARENT_PROJECT_NAME}::${CLASSIC_TARGET} ALIAS ${CLASSIC_TARGET})
+
+install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/
+  DESTINATION include
+)
+install(TARGETS ${CLASSIC_TARGET}
+  EXPORT ${PARENT_PROJECT_NAME}Config
+  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+)
+
+option(BUILD_PATCHWORK_TESTS "Build C++ smoke test for PatchWork" OFF)
+if (BUILD_PATCHWORK_TESTS)
+  add_executable(patchwork_smoke tests/smoke_test.cpp)
+  target_link_libraries(patchwork_smoke PRIVATE ${CLASSIC_TARGET})
+endif()

cpp/patchwork/include/patchwork/patchwork.h

@@ -0,0 +1,134 @@
+#ifndef PATCHWORK_CLASSIC_H
+#define PATCHWORK_CLASSIC_H
+
+#include <vector>
+
+#include <Eigen/Dense>
+
+#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<int> num_sectors_each_zone = {16, 32, 54, 32};
+  std::vector<int> num_rings_each_zone   = {2, 4, 4, 4};
+  std::vector<double> 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<double> elevation_thr = {0.523, 0.746, 0.879, 1.125};
+  std::vector<double> flatness_thr  = {0.0005, 0.000725, 0.001, 0.001};
+
+  // Adaptive seed selection margin for highly tilted ground
+  double adaptive_seed_selection_margin = -1.1;
+
+  // Global elevation guard
+  bool using_global_thr       = true;
+  double global_elevation_thr = 0.0;
+
+  // ATAT (default ON)
+  bool ATAT_ON             = true;
+  double max_h_for_ATAT    = 0.3;
+  int num_sectors_for_ATAT = 20;
+  double noise_bound       = 0.2;
+
+  bool verbose = false;
+};
+
+class PatchWork {
+ public:
+  PatchWork() = default;
+  explicit PatchWork(const PatchworkParams& params);
+
+  void estimateGround(const Eigen::MatrixXf& cloud);
+
+  Eigen::MatrixX3f getGround() const;
+  Eigen::MatrixX3f getNonground() const;
+  std::vector<int> getGroundIndices() const;
+  std::vector<int> getNongroundIndices() const;
+  double getTimeTaken() const;
+  double getHeight() const;
+
+ private:
+  // Helper functions (defined in patchwork.cpp in later tasks)
+  void initialize();
+  void flush();
+  double xy2theta(double x, double y) const;
+  double xy2radius(double x, double y) const;
+  void pc2regionwise_patches(const std::vector<PointXYZ>& src);
+  void estimate_plane(const std::vector<PointXYZ>& seeds, PCAFeature& out);
+  void extract_initial_seeds(int zone_idx,
+                             const std::vector<PointXYZ>& sorted,
+                             std::vector<PointXYZ>& 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<PointXYZ>& patch,
+                                       std::vector<PointXYZ>& patch_ground,
+                                       std::vector<PointXYZ>& patch_nonground,
+                                       PatchStatus& status_out);
+  void estimate_sensor_height(std::vector<PointXYZ>& cloud);
+  double consensus_set_based_height_estimation(const std::vector<double>& candidate_heights);
+  void materialize() const;
+
+  PatchworkParams params_;
+
+  // Per-iteration scratch (cleared each estimateGround call)
+  using Sector            = std::vector<PointXYZ>;
+  using Ring              = std::vector<Sector>;  // sectors within one ring
+  using Zone              = std::vector<Ring>;    // rings within one zone
+  using RegionwisePatches = std::vector<Zone>;
+  RegionwisePatches regionwise_patches_;
+
+  // Materialized output points (per-call)
+  std::vector<PointXYZ> ground_pts_;
+  std::vector<PointXYZ> nonground_pts_;
+
+  // Cached output matrices/indices (lazy via materialize())
+  mutable Eigen::MatrixX3f ground_mat_;
+  mutable Eigen::MatrixX3f nonground_mat_;
+  mutable std::vector<int> ground_idx_;
+  mutable std::vector<int> nonground_idx_;
+  mutable bool outputs_dirty_ = true;
+
+  double time_taken_    = 0.0;
+  double sensor_height_ = 1.723;  // updated by ATAT if enabled
+};
+
+}  // namespace patchwork
+
+#endif