11#ifndef PATCHWORK_CLASSIC_H
22#define PATCHWORK_CLASSIC_H
33
4- #include < Eigen/Dense>
54#include < vector>
65
6+ #include < Eigen/Dense>
7+
78#include " patchwork/patchworkpp.h" // for patchwork::PointXYZ
89
910namespace patchwork {
@@ -19,12 +20,12 @@ struct PCAFeature {
1920};
2021
2122enum class PatchStatus {
22- NotAssigned = -2 ,
23- FewPoints = -1 ,
24- UprightEnough = 0 ,
25- FlatEnough = 1 ,
26- TooHighElevation = 2 ,
27- TooTilted = 3 ,
23+ NotAssigned = -2 ,
24+ FewPoints = -1 ,
25+ UprightEnough = 0 ,
26+ FlatEnough = 1 ,
27+ TooHighElevation = 2 ,
28+ TooTilted = 3 ,
2829 GloballyTooHighElevation = 4 ,
2930};
3031
@@ -35,35 +36,35 @@ struct PatchworkParams {
3536 double min_range = 2.7 ;
3637
3738 // Concentric Zone Model (parametric)
38- int num_zones = 4 ;
39- std::vector<int > num_sectors_each_zone = {16 , 32 , 54 , 32 };
40- std::vector<int > num_rings_each_zone = {2 , 4 , 4 , 4 };
41- std::vector<double > min_ranges = {2.7 , 12.3625 , 22.025 , 41.35 };
39+ int num_zones = 4 ;
40+ std::vector<int > num_sectors_each_zone = {16 , 32 , 54 , 32 };
41+ std::vector<int > num_rings_each_zone = {2 , 4 , 4 , 4 };
42+ std::vector<double > min_ranges = {2.7 , 12.3625 , 22.025 , 41.35 };
4243
4344 // Plane fit
44- int num_iter = 3 ;
45- int num_lpr = 20 ;
46- int num_min_pts = 10 ;
47- double th_seeds = 0.5 ;
48- double th_dist = 0.125 ;
45+ int num_iter = 3 ;
46+ int num_lpr = 20 ;
47+ int num_min_pts = 10 ;
48+ double th_seeds = 0.5 ;
49+ double th_dist = 0.125 ;
4950
5051 // Ground likelihood thresholds (fixed, the Patchwork classic style)
51- double uprightness_thr = 0.5 ;
52- std::vector<double > elevation_thr = {0.523 , 0.746 , 0.879 , 1.125 };
53- std::vector<double > flatness_thr = {0.0005 , 0.000725 , 0.001 , 0.001 };
52+ double uprightness_thr = 0.5 ;
53+ std::vector<double > elevation_thr = {0.523 , 0.746 , 0.879 , 1.125 };
54+ std::vector<double > flatness_thr = {0.0005 , 0.000725 , 0.001 , 0.001 };
5455
5556 // Adaptive seed selection margin for highly tilted ground
5657 double adaptive_seed_selection_margin = -1.1 ;
5758
5859 // Global elevation guard
59- bool using_global_thr = true ;
60+ bool using_global_thr = true ;
6061 double global_elevation_thr = 0.0 ;
6162
6263 // ATAT (default ON)
63- bool ATAT_ON = true ;
64- double max_h_for_ATAT = 0.3 ;
65- int num_sectors_for_ATAT = 20 ;
66- double noise_bound = 0.2 ;
64+ bool ATAT_ON = true ;
65+ double max_h_for_ATAT = 0.3 ;
66+ int num_sectors_for_ATAT = 20 ;
67+ double noise_bound = 0.2 ;
6768
6869 bool verbose = false ;
6970};
@@ -75,41 +76,41 @@ class PatchWork {
7576
7677 void estimateGround (const Eigen::MatrixXf& cloud);
7778
78- Eigen::MatrixX3f getGround () const ;
79- Eigen::MatrixX3f getNonground () const ;
80- std::vector<int > getGroundIndices () const ;
79+ Eigen::MatrixX3f getGround () const ;
80+ Eigen::MatrixX3f getNonground () const ;
81+ std::vector<int > getGroundIndices () const ;
8182 std::vector<int > getNongroundIndices () const ;
82- double getTimeTaken () const ;
83- double getHeight () const ;
83+ double getTimeTaken () const ;
84+ double getHeight () const ;
8485
8586 private:
8687 // Helper functions (defined in patchwork.cpp in later tasks)
87- void initialize ();
88- void flush ();
88+ void initialize ();
89+ void flush ();
8990 double xy2theta (double x, double y) const ;
9091 double xy2radius (double x, double y) const ;
91- void pc2regionwise_patches (const std::vector<PointXYZ>& src);
92- void estimate_plane (const std::vector<PointXYZ>& seeds, PCAFeature& out);
93- void extract_initial_seeds (int zone_idx,
94- const std::vector<PointXYZ>& sorted,
95- std::vector<PointXYZ>& seeds);
96- PatchStatus determine_gle_status (int zone_idx, int ring_idx,
97- const PCAFeature& feature) const ;
98- void perform_regionwise_segmentation ( int zone_idx, int ring_idx,
99- const std::vector<PointXYZ>& patch,
100- std::vector<PointXYZ>& patch_ground,
101- std::vector<PointXYZ>& patch_nonground,
102- PatchStatus& status_out);
103- void estimate_sensor_height (std::vector<PointXYZ>& cloud);
92+ void pc2regionwise_patches (const std::vector<PointXYZ>& src);
93+ void estimate_plane (const std::vector<PointXYZ>& seeds, PCAFeature& out);
94+ void extract_initial_seeds (int zone_idx,
95+ const std::vector<PointXYZ>& sorted,
96+ std::vector<PointXYZ>& seeds);
97+ PatchStatus determine_gle_status (int zone_idx, int ring_idx, const PCAFeature& feature) const ;
98+ void perform_regionwise_segmentation ( int zone_idx,
99+ int ring_idx,
100+ const std::vector<PointXYZ>& patch,
101+ std::vector<PointXYZ>& patch_ground,
102+ std::vector<PointXYZ>& patch_nonground,
103+ PatchStatus& status_out);
104+ void estimate_sensor_height (std::vector<PointXYZ>& cloud);
104105 double consensus_set_based_height_estimation (const std::vector<double >& candidate_heights);
105- void materialize () const ;
106+ void materialize () const ;
106107
107108 PatchworkParams params_;
108109
109110 // Per-iteration scratch (cleared each estimateGround call)
110- using Sector = std::vector<PointXYZ>;
111- using Ring = std::vector<Sector>; // sectors within one ring
112- using Zone = std::vector<Ring>; // rings within one zone
111+ using Sector = std::vector<PointXYZ>;
112+ using Ring = std::vector<Sector>; // sectors within one ring
113+ using Zone = std::vector<Ring>; // rings within one zone
113114 using RegionwisePatches = std::vector<Zone>;
114115 RegionwisePatches regionwise_patches_;
115116
@@ -122,7 +123,7 @@ class PatchWork {
122123 mutable Eigen::MatrixX3f nonground_mat_;
123124 mutable std::vector<int > ground_idx_;
124125 mutable std::vector<int > nonground_idx_;
125- mutable bool outputs_dirty_ = true ;
126+ mutable bool outputs_dirty_ = true ;
126127
127128 double time_taken_ = 0.0 ;
128129 double sensor_height_ = 1.723 ; // updated by ATAT if enabled
0 commit comments