├── data
    └── testdata.pcd
├── README.md
├── CMakeLists.txt
├── main.cpp
├── include
    └── patchworkpp.h
└── src
    └── patchworkpp.cpp


/data/testdata.pcd:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ChenChengkai/patchwork-PCL/HEAD/data/testdata.pcd


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # patchwork-PCL
 2 | 
 3 | git clone https://github.com/ChenChengkai/patchwork-PCL.git  
 4 | cd patchworkPCL  
 5 | mkdir build  
 6 | cd build  
 7 | cmake ..  
 8 | make  
 9 | ./main
10 | 
11 | 
12 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.0)    
 2 | project(test)
 3 | 
 4 | set(CMAKE_CXX_STANDARD 14)
 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON)
 6 | add_definitions(-D_GLIBCXX_USE_C99=1)
 7 | 
 8 | #boost
 9 | find_package(Boost REQUIRED COMPONENTS thread filesystem)
10 | 
11 | 
12 | # PCL
13 | find_package(PCL 1.8 REQUIRED)
14 | include_directories(${PCL_INCLUDE_DIRS})
15 | link_directories(${PCL_LIBRARY_DIRS})
16 | add_definitions(${PCL_DEFINITIONS})
17 | 
18 | # user define
19 | aux_source_directory(src DIR_SRCS)
20 | add_library(SRC ${DIR_SRCS})
21 | 
22 | find_package( OpenCV 3 REQUIRED )
23 | include_directories(include ${OpenCV_INCLUDE_DIRS} )
24 | 
25 | 
26 | # main function
27 | add_executable(main main.cpp)
28 | target_link_libraries (main  SRC -lboost_thread ${Boost_LIBRARIES}  ${PCL_LIBRARIES}  )
29 | 


--------------------------------------------------------------------------------
/main.cpp:
--------------------------------------------------------------------------------
 1 | #include <iostream>
 2 | #include "patchworkpp.h"
 3 | 
 4 | #include <pcl/io/pcd_io.h>
 5 | #include <pcl/point_types.h>
 6 | #include <pcl/point_cloud.h>
 7 | #include <pcl/visualization/pcl_visualizer.h>
 8 | #include <boost/thread/thread.hpp>
 9 | 
10 | 
11 | void viewTwoPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud1,const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud2 ,int pointssize=1)
12 | {
13 |     boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("test"));
14 |     viewer->setBackgroundColor(0,0,0); // rgb
15 |     viewer->addCoordinateSystem();
16 | 
17 |     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color1(cloud1, 255, 0, 0); // rgb
18 |     viewer->addPointCloud<pcl::PointXYZ>(cloud1, color1, "cloud1"); 
19 |     viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointssize, "cloud1"); 
20 | 
21 |     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color2(cloud2, 0, 255, 0); // rgb
22 |     viewer->addPointCloud<pcl::PointXYZ>(cloud2, color2, "cloud2"); 
23 |     viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointssize, "cloud2"); 
24 | 
25 |     while (!viewer->wasStopped())
26 |     {
27 |         viewer->spinOnce(100);
28 |     }
29 |     viewer->close();
30 | }
31 | 
32 | void pcl2eigen(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in,Eigen::MatrixX3f &cloud)
33 | {
34 |     int num=cloud_in->points.size();
35 |     cloud.resize(num,3);
36 |     for (int i = 0; i < num; i++)
37 |     {
38 |         cloud.row(i)<<cloud_in->points[i].x,cloud_in->points[i].y,cloud_in->points[i].z;
39 |     }
40 | }
41 | 
42 | void eigen2pcl(const Eigen::MatrixX3f &cloud_in,pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud)
43 | {
44 |         int num=cloud_in.rows();
45 |         for (int i = 0; i < num; i++)
46 |         {
47 |             pcl::PointXYZ p;
48 |             p.x=cloud_in(i,0);        p.y=cloud_in(i,1);        p.z=cloud_in(i,2);
49 |             cloud->push_back(p);
50 |         }
51 | }
52 | 
53 | int main(int argc, char const *argv[])
54 | {
55 |     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
56 |     pcl::io::loadPCDFile<pcl::PointXYZ>("../data/testdata.pcd",*cloud);
57 | 
58 |     // Patchwork++ initialization
59 |     patchwork::Params patchwork_parameters;
60 |     patchwork::PatchWorkpp Patchworkpp(patchwork_parameters);
61 | 
62 |     // Load point cloud
63 |     Eigen::MatrixX3f cloudEigen;
64 |         pcl2eigen(cloud,cloudEigen);
65 |     // Estimate Ground
66 |     Patchworkpp.estimateGround(cloudEigen);
67 |         Eigen::MatrixX3f ground     = Patchworkpp.getGround();
68 |         Eigen::MatrixX3f nonground  = Patchworkpp.getNonground();
69 | 
70 | 
71 |     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_NoGround(new pcl::PointCloud<pcl::PointXYZ>);
72 |     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Ground(new pcl::PointCloud<pcl::PointXYZ>);
73 |         eigen2pcl(ground,cloud_Ground);
74 |         eigen2pcl(nonground,cloud_NoGround);
75 | 
76 |     std::cout << "Origianl Points  #: " << cloud->points.size() << std::endl;
77 |     std::cout << "Ground Points    #: " << ground.rows() << std::endl;
78 |     std::cout << "Nonground Points #: " << nonground.rows() << std::endl;
79 | 
80 |     std::cout<<"***If this project is useful, please give a star!***"<<std::endl;
81 |     viewTwoPointCloud(cloud_NoGround,cloud_Ground);
82 | 
83 | 
84 |     return 0;
85 | }
86 | 


--------------------------------------------------------------------------------
/include/patchworkpp.h:
--------------------------------------------------------------------------------
  1 | #ifndef PATCHWORKPP_H
  2 | #define PATCHWORKPP_H
  3 | 
  4 | #include <iostream>
  5 | #include <vector>
  6 | #include <Eigen/Dense>
  7 | #include <numeric>
  8 | #include <time.h>
  9 | 
 10 | using namespace std;
 11 | 
 12 | #define MAX_POINTS 5000
 13 | 
 14 | namespace patchwork {
 15 | 
 16 | struct PointXYZ {
 17 |     float x;
 18 |     float y;
 19 |     float z;
 20 | 
 21 |     PointXYZ(float _x, float _y, float _z) : x(_x), y(_y), z(_z) {}
 22 | };
 23 | 
 24 | struct RevertCandidate 
 25 | {   
 26 |     int concentric_idx;
 27 |     int sector_idx;
 28 |     double ground_flatness;
 29 |     double line_variable;
 30 |     Eigen::VectorXf pc_mean;
 31 |     vector<PointXYZ> regionwise_ground;
 32 |     
 33 |     RevertCandidate(int _c_idx, int _s_idx, double _flatness, double _line_var, Eigen::VectorXf _pc_mean, vector<PointXYZ> _ground)
 34 |      : concentric_idx(_c_idx), sector_idx(_s_idx), ground_flatness(_flatness), line_variable(_line_var), pc_mean(_pc_mean), regionwise_ground(_ground) {}
 35 | };
 36 | 
 37 | struct Params 
 38 | {
 39 |     bool verbose;
 40 |     bool enable_RNR;
 41 |     bool enable_RVPF;
 42 |     bool enable_TGR;
 43 | 
 44 |     int num_iter;
 45 |     int num_lpr;
 46 |     int num_min_pts;
 47 |     int num_zones;
 48 |     int num_rings_of_interest;
 49 |     int noise_filter_channel_num;
 50 |     int pc_num_channel;
 51 | 
 52 |     double sensor_height;
 53 |     double th_seeds;
 54 |     double th_dist;
 55 |     double th_seeds_v;
 56 |     double th_dist_v;
 57 |     double max_range;
 58 |     double min_range;
 59 |     double uprightness_thr;
 60 |     double adaptive_seed_selection_margin;
 61 |     double intensity_thr;
 62 | 
 63 |     vector<int> num_sectors_each_zone;
 64 |     vector<int> num_rings_each_zone;
 65 |     
 66 |     int max_flatness_storage;
 67 |     int max_elevation_storage;
 68 | 
 69 |     vector<double> elevation_thr;
 70 |     vector<double> flatness_thr;
 71 | 
 72 |     
 73 |     Params() {
 74 |         verbose     = false;
 75 |         enable_RNR  = false;
 76 |         enable_RVPF = true;
 77 |         enable_TGR  = true;
 78 | 
 79 |         num_iter = 3;
 80 |         num_lpr = 20;
 81 |         num_min_pts = 10;
 82 |         num_zones = 4;
 83 |         num_rings_of_interest = 4;
 84 |         noise_filter_channel_num = 20;
 85 |         pc_num_channel = 2048;
 86 | 
 87 |         sensor_height = 0.723;
 88 |         th_seeds = 0.125;
 89 |         th_dist = 0.125;
 90 |         th_seeds_v = 0.25;
 91 |         th_dist_v = 0.1;
 92 | 
 93 |         max_range = 180.0;
 94 |         min_range = 2.0;
 95 | 
 96 |         uprightness_thr = 0.707;
 97 |         adaptive_seed_selection_margin = -1.2;
 98 |         intensity_thr = 0.2;
 99 | 
100 |         num_sectors_each_zone = {16, 32, 54, 32};
101 |         num_rings_each_zone = {2, 4, 4, 4};
102 |         max_flatness_storage = 1000;
103 |         max_elevation_storage = 1000;
104 |         elevation_thr = {0, 0, 0, 0};
105 |         flatness_thr = {0, 0, 0, 0};
106 |     }
107 | };
108 | 
109 | class PatchWorkpp {
110 | 
111 | public:
112 |     typedef std::vector<vector<PointXYZ>> Ring;
113 |     typedef std::vector<Ring> Zone;
114 | 
115 |     PatchWorkpp(patchwork::Params _params) : params_(_params) {
116 | 
117 |         double min_range_z2_ = (7 * params_.min_range + params_.max_range) / 8.0;
118 |         double min_range_z3_ = (3 * params_.min_range + params_.max_range) / 4.0;
119 |         double min_range_z4_ = (params_.min_range + params_.max_range) / 2.0;
120 |         min_ranges_ = {params_.min_range, min_range_z2_, min_range_z3_, min_range_z4_};
121 | 
122 |         ring_sizes_ = {(min_range_z2_ - params_.min_range) / params_.num_rings_each_zone.at(0),
123 |                       (min_range_z3_ - min_range_z2_) / params_.num_rings_each_zone.at(1),
124 |                       (min_range_z4_ - min_range_z3_) / params_.num_rings_each_zone.at(2),
125 |                       (params_.max_range - min_range_z4_) / params_.num_rings_each_zone.at(3)};
126 |         sector_sizes_ = {2 * M_PI / params_.num_sectors_each_zone.at(0),
127 |                          2 * M_PI / params_.num_sectors_each_zone.at(1),
128 |                          2 * M_PI / params_.num_sectors_each_zone.at(2),
129 |                          2 * M_PI / params_.num_sectors_each_zone.at(3)};
130 | 
131 |         for (int k = 0; k < params_.num_zones; k++) {
132 |             
133 |             Ring empty_ring;
134 |             empty_ring.resize(params_.num_sectors_each_zone[k]);
135 | 
136 |             Zone z;
137 |             for (int i = 0; i < params_.num_rings_each_zone[k]; i++) {
138 |                 z.push_back(empty_ring);
139 |             }
140 | 
141 |             ConcentricZoneModel_.push_back(z);
142 |         }
143 | 
144 |         std::cout << "PatchWorkpp::PatchWorkpp() - INITIALIZATION COMPLETE" << std::endl;
145 |     }
146 | 
147 |     void estimateGround(Eigen::MatrixX3f cloud_in);
148 | 
149 |     double getHeight() { return params_.sensor_height; }
150 |     double getTimeTaken() { return time_taken_; }
151 | 
152 |     Eigen::MatrixX3f getGround() { return toEigenCloud(cloud_ground_); }
153 |     Eigen::MatrixX3f getNonground() { return toEigenCloud(cloud_nonground_); }
154 |     
155 |     Eigen::MatrixX3f getCenters() { return toEigenCloud(centers_); }
156 |     Eigen::MatrixX3f getNormals() { return toEigenCloud(normals_); }
157 | 
158 | private:
159 | 
160 |     // Every private member variable is written with the undescore("_") in its end.
161 | 
162 |     patchwork::Params params_;
163 | 
164 |     time_t timer_;
165 |     long time_taken_;
166 | 
167 |     std::vector<double> update_flatness_[4];
168 |     std::vector<double> update_elevation_[4];
169 | 
170 |     double d_;
171 | 
172 |     Eigen::VectorXf normal_;
173 |     Eigen::VectorXf singular_values_;
174 |     Eigen::Matrix3f cov_;
175 |     Eigen::VectorXf pc_mean_;
176 | 
177 |     vector<double> min_ranges_;
178 |     vector<double> sector_sizes_;
179 |     vector<double> ring_sizes_;
180 | 
181 |     vector<Zone> ConcentricZoneModel_;
182 | 
183 |     vector<PointXYZ> ground_pc_, non_ground_pc_;
184 |     vector<PointXYZ> regionwise_ground_, regionwise_nonground_;
185 | 
186 |     vector<PointXYZ> cloud_ground_, cloud_nonground_;
187 | 
188 |     vector<PointXYZ> centers_, normals_;
189 | 
190 |     Eigen::MatrixX3f toEigenCloud(vector<PointXYZ> cloud);
191 | 
192 |     void addCloud(vector<PointXYZ> &cloud, vector<PointXYZ> &add);
193 |     
194 |     void flush_patches(std::vector<Zone> &czm);
195 | 
196 |     void pc2czm(const Eigen::MatrixX3f &src, std::vector<Zone> &czm);
197 | 
198 |     void reflected_noise_removal(Eigen::MatrixX3f &cloud_in, vector<PointXYZ> &nonground);
199 |     
200 |     void temporal_ground_revert(vector<PointXYZ> &cloud_ground, vector<PointXYZ> &cloud_nonground,
201 |                                 std::vector<double> ring_flatness, std::vector<patchwork::RevertCandidate> candidates,
202 |                                 int concentric_idx);
203 |     
204 |     double calc_point_to_plane_d(PointXYZ p, Eigen::VectorXf normal, double d);
205 |     void calc_mean_stdev(std::vector<double> vec, double &mean, double &stdev);
206 | 
207 |     void update_elevation_thr();
208 |     void update_flatness_thr();
209 | 
210 |     double xy2theta(const double &x, const double &y);
211 | 
212 |     double xy2radius(const double &x, const double &y);
213 | 
214 |     void estimate_plane(const vector<PointXYZ> &ground, double th_dist);
215 | 
216 |     void extract_piecewiseground(
217 |             const int zone_idx, const vector<PointXYZ> &src,
218 |             vector<PointXYZ> &dst,
219 |             vector<PointXYZ> &non_ground_dst);
220 | 
221 |     void extract_initial_seeds(
222 |             const int zone_idx, const vector<PointXYZ> &p_sorted,
223 |             vector<PointXYZ> &init_seeds);
224 | 
225 |     void extract_initial_seeds(
226 |             const int zone_idx, const vector<PointXYZ> &p_sorted,
227 |             vector<PointXYZ> &init_seeds, double th_seed);
228 | 
229 | };
230 | 
231 | };
232 | 
233 | #endif
234 | 


--------------------------------------------------------------------------------
/src/patchworkpp.cpp:
--------------------------------------------------------------------------------
  1 | #include "patchworkpp.h"
  2 | 
  3 | using namespace std;
  4 | using namespace patchwork;
  5 | 
  6 | bool point_z_cmp(PointXYZ a, PointXYZ b) { return a.z < b.z; }
  7 | 
  8 | Eigen::MatrixX3f PatchWorkpp::toEigenCloud(vector<PointXYZ> cloud)
  9 | {
 10 |     Eigen::MatrixX3f dst(cloud.size(), 3);
 11 |     int j=0;
 12 |     for (auto &p: cloud) {
 13 |         dst.row(j++) << p.x, p.y, p.z;
 14 |     }
 15 |     return dst;
 16 | }
 17 | 
 18 | void PatchWorkpp::addCloud(vector<PointXYZ> &cloud, vector<PointXYZ> &add)
 19 | {
 20 |     cloud.insert(cloud.end(), add.begin(), add.end());
 21 | }
 22 | 
 23 | void PatchWorkpp::flush_patches(vector<Zone> &czm) {
 24 | 
 25 |     // if (params_.verbose)
 26 |     // {
 27 |     //     cout << "Zone #: " << czm.size() << endl;
 28 |     //     for( auto zone : czm ) {
 29 |     //         cout << "   Ring #: " << zone.size() << endl;
 30 |     //         cout << "      Sectors #: ";
 31 |     //         for ( auto ring : zone ) cout << ring.size() << ", ";
 32 |     //         cout << endl;
 33 |     //     }
 34 |     // }    
 35 |     
 36 |     for (int k = 0; k < params_.num_zones; k++) {
 37 |         for (int i = 0; i < params_.num_rings_each_zone[k]; i++) {
 38 |             for (int j = 0; j < params_.num_sectors_each_zone[k]; j++) {
 39 |                 // czm[k][i][j].resize(MAX_POINTS, 3);
 40 |                 czm[k][i][j].clear();
 41 |             }
 42 |         }
 43 |     }
 44 | 
 45 |     if( params_.verbose ) cout << "\033[1;31m" << "PatchWorkpp::flush_patches() - Flushed patches successfully!" << "\033[0m" << endl;
 46 | }
 47 | 
 48 | void PatchWorkpp::estimate_plane(const vector<PointXYZ> &ground, double th_dist) {
 49 | 
 50 |     if (ground.empty()) return;
 51 | 
 52 |     Eigen::MatrixX3f eigen_ground(ground.size(), 3);
 53 |     int j=0;
 54 |     for (auto &p: ground) {
 55 |         eigen_ground.row(j++) << p.x, p.y, p.z;
 56 |     }
 57 | 
 58 |     Eigen::MatrixX3f centered = eigen_ground.rowwise() - eigen_ground.colwise().mean();
 59 |     Eigen::MatrixX3f cov = (centered.adjoint() * centered) / double(eigen_ground.rows() - 1);
 60 |     
 61 |     pc_mean_.resize(3);
 62 |     // pc_mean_ << eigen_ground.colwise().mean()(0), eigen_ground.colwise().mean()(1), eigen_ground.colwise().mean()(2);
 63 |     float a=0,b=0,c=0;
 64 |     for (int i = 0; i < eigen_ground.rows(); i++)
 65 |     {
 66 |         a+=eigen_ground(i,0);
 67 |         b+=eigen_ground(i,1);
 68 |         c+=eigen_ground(i,2);
 69 |     }
 70 |     a=a/eigen_ground.rows();
 71 |     b=b/eigen_ground.rows();
 72 |     c=c/eigen_ground.rows();
 73 |     pc_mean_ <<a,b,c;
 74 | 
 75 | 
 76 |     Eigen::JacobiSVD<Eigen::MatrixX3f> svd(cov, Eigen::DecompositionOptions::ComputeFullU);
 77 |     singular_values_ = svd.singularValues();
 78 |        
 79 |     // use the least singular vector as normal
 80 |     normal_ = (svd.matrixU().col(2));
 81 |     
 82 |     if (normal_(2) < 0) { for(int i=0; i<3; i++) normal_(i) *= -1; }
 83 | 
 84 |     // mean ground seeds value
 85 |     Eigen::Vector3f seeds_mean = pc_mean_.head<3>();
 86 | 
 87 |     // according to normal.T*[x,y,z] = -d
 88 |     d_ = -(normal_.transpose() * seeds_mean)(0, 0);
 89 | }
 90 | 
 91 | void PatchWorkpp::extract_initial_seeds(
 92 |         const int zone_idx, const vector<PointXYZ> &p_sorted,
 93 |         vector<PointXYZ> &init_seeds, double th_seed) {
 94 |     
 95 |     init_seeds.clear();
 96 | 
 97 |     // LPR is the mean of low point representative
 98 |     double sum = 0;
 99 |     int cnt = 0;
100 | 
101 |     int init_idx = 0;
102 |     if (zone_idx == 0) {
103 |         for (int i = 0; i < p_sorted.size(); i++) {
104 |             if (p_sorted[i].z < params_.adaptive_seed_selection_margin * params_.sensor_height) {
105 |                 ++init_idx;
106 |             } else {
107 |                 break;
108 |             }
109 |         }
110 |     }
111 |     
112 |     // Calculate the mean height value.
113 |     for (int i = init_idx; i < p_sorted.size() && cnt < params_.num_lpr; i++) {
114 |         sum += p_sorted[i].z;
115 |         cnt++;
116 |     }
117 |     double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0
118 | 
119 | 
120 |     int init_seeds_num = 0;    // iterate pointcloud, filter those height is less than lpr.height+params_.th_seeds
121 |     for (int i = 0; i < p_sorted.size(); i++) {
122 |         if (p_sorted[i].z < lpr_height + th_seed) {
123 |             init_seeds.push_back(p_sorted[i]);
124 |         }
125 |     }
126 | }
127 | 
128 | void PatchWorkpp::extract_initial_seeds(
129 |         const int zone_idx, const vector<PointXYZ> &p_sorted,
130 |         vector<PointXYZ> &init_seeds) {
131 | 
132 |     init_seeds.clear();
133 | 
134 |     // LPR is the mean of low point representative
135 |     double sum = 0;
136 |     int cnt = 0;
137 | 
138 |     int init_idx = 0;
139 |     if (zone_idx == 0) {
140 |         for (int i = 0; i < p_sorted.size(); i++) {
141 |             if (p_sorted[i].z < params_.adaptive_seed_selection_margin * params_.sensor_height) {
142 |                 ++init_idx;
143 |             } else {
144 |                 break;
145 |             }
146 |         }
147 |     }
148 |     
149 |     // Calculate the mean height value.
150 |     for (int i = init_idx; i < p_sorted.size() && cnt < params_.num_lpr; i++) {
151 |         sum += p_sorted[i].z;
152 |         cnt++;
153 |     }
154 |     double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0
155 | 
156 |     int init_seeds_num = 0;
157 |     // iterate pointcloud, filter those height is less than lpr.height+params_.th_seeds
158 |     for (int i = 0; i < p_sorted.size(); i++) {
159 |         if (p_sorted[i].z < lpr_height + params_.th_seeds) {
160 |             init_seeds.push_back(p_sorted[i]);
161 |         }
162 |     }
163 | }
164 | 
165 | void PatchWorkpp::estimateGround(Eigen::MatrixX3f cloud_in) {
166 | 
167 |     if (params_.verbose) cout << "\033[1;32m" << "PatchWorkpp::estimateGround() - Estimation starts !" << "\033[0m" << endl;
168 | 
169 |     clock_t beg = clock();
170 |     
171 |     // 1. Reflected Noise Removal (RNR)
172 |     if (params_.enable_RNR) 
173 |         reflected_noise_removal(cloud_in, cloud_nonground_);
174 | 
175 |     clock_t t1 = clock();
176 | 
177 |     // 2. Concentric Zone Model (CZM)
178 |     flush_patches(ConcentricZoneModel_);
179 | 
180 |     clock_t t1_1 = clock();
181 | 
182 |     pc2czm(cloud_in, ConcentricZoneModel_);
183 | 
184 |     clock_t t2 = clock();
185 | 
186 |     cloud_ground_.clear();
187 |     cloud_nonground_.clear();
188 | 
189 |     int concentric_idx = 0;
190 | 
191 |     centers_.clear();
192 |     normals_.clear();
193 | 
194 |     double t_flush = t1_1-t1, t_czm = t2-t1_1, t_sort = 0.0, t_pca = 0.0, t_gle = 0.0, t_revert = 0.0, t_update = 0.0;
195 | 
196 |     std::vector<patchwork::RevertCandidate> candidates;
197 |     std::vector<double> ringwise_flatness;
198 |     
199 |     for (int zone_idx = 0; zone_idx < params_.num_zones; ++zone_idx) {
200 |         
201 |         auto zone = ConcentricZoneModel_[zone_idx];
202 | 
203 |         for (int ring_idx = 0; ring_idx < params_.num_rings_each_zone[zone_idx]; ++ring_idx) {
204 |             for (int sector_idx = 0; sector_idx < params_.num_sectors_each_zone[zone_idx]; ++sector_idx) {
205 |                 
206 |                 if (zone[ring_idx][sector_idx].size() < params_.num_min_pts)
207 |                 {
208 |                     addCloud(cloud_nonground_, zone[ring_idx][sector_idx]);
209 |                     continue;
210 |                 }
211 | 
212 |                 // --------- region-wise sorting (faster than global sorting method) ---------------- //
213 |                 clock_t t_bef_sort = clock();
214 |                 sort(zone[ring_idx][sector_idx].begin(), zone[ring_idx][sector_idx].end(), point_z_cmp);
215 |                 if (params_.enable_RNR)
216 |                 {
217 |                     auto it = zone[ring_idx][sector_idx].begin();
218 |                     for ( auto &p : zone[ring_idx][sector_idx] ) {
219 |                         if (p.z == std::numeric_limits<float>::min()) it++;
220 |                         else break;
221 |                     }
222 |                     zone[ring_idx][sector_idx].erase(zone[ring_idx][sector_idx].begin(), it);
223 |                 }
224 |                 clock_t t_aft_sort = clock();
225 | 
226 |                 t_sort += t_aft_sort - t_bef_sort;
227 |                 // ---------------------------------------------------------------------------------- //
228 | 
229 |                 clock_t t_bef_pca = clock();
230 |                 extract_piecewiseground(zone_idx, zone[ring_idx][sector_idx], regionwise_ground_, regionwise_nonground_);
231 |                 clock_t t_aft_pca = clock();
232 | 
233 |                 t_pca += t_aft_pca - t_bef_pca;
234 | 
235 |                 centers_.push_back(PointXYZ(pc_mean_(0), pc_mean_(1), pc_mean_(2)));
236 |                 normals_.push_back(PointXYZ(normal_(0), normal_(1), normal_(2)));
237 | 
238 |                 clock_t t_bef_gle = clock();
239 |                 // Status of each patch
240 |                 // used in checking uprightness, elevation, and flatness, respectively
241 |                 const double ground_uprightness = normal_(2);
242 |                 const double ground_elevation   = pc_mean_(2);
243 |                 const double ground_flatness    = singular_values_.minCoeff();
244 |                 const double line_variable      = singular_values_(1) != 0 ? singular_values_(0)/singular_values_(1) : std::numeric_limits<double>::max();
245 |                 
246 |                 double heading = 0.0;
247 |                 for(int i=0; i<3; i++) heading += pc_mean_(i)*normal_(i);
248 | 
249 |                 /*  
250 |                     About 'is_heading_outside' condition, heading should be smaller than 0 theoretically.
251 |                     ( Imagine the geometric relationship between the surface normal vector on the ground plane and 
252 |                         the vector connecting the sensor origin and the mean point of the ground plane )
253 | 
254 |                     However, when the patch is far awaw from the sensor origin, 
255 |                     heading could be larger than 0 even if it's ground due to lack of amount of ground plane points.
256 |                     
257 |                     Therefore, we only check this value when concentric_idx < num_rings_of_interest ( near condition )
258 |                 */
259 |                 bool is_upright         = ground_uprightness > params_.uprightness_thr;
260 |                 bool is_not_elevated    = ground_elevation < params_.elevation_thr[concentric_idx];
261 |                 bool is_flat            = ground_flatness < params_.flatness_thr[concentric_idx];
262 |                 bool is_near_zone       = concentric_idx < params_.num_rings_of_interest;
263 |                 bool is_heading_outside = heading < 0.0;
264 | 
265 |                 /*
266 |                     Store the elevation & flatness variables
267 |                     for A-GLE (Adaptive Ground Likelihood Estimation)
268 |                     and TGR (Temporal Ground Revert). More information in the paper Patchwork++.
269 |                 */
270 |                 if (is_upright && is_not_elevated && is_near_zone)
271 |                 {
272 |                     update_elevation_[concentric_idx].push_back(ground_elevation);
273 |                     update_flatness_[concentric_idx].push_back(ground_flatness);
274 | 
275 |                     ringwise_flatness.push_back(ground_flatness);
276 |                 }
277 | 
278 |                 // Ground estimation based on conditions
279 |                 if (!is_upright)
280 |                 {
281 |                     addCloud(cloud_nonground_, regionwise_ground_);
282 |                 }
283 |                 else if (!is_near_zone)
284 |                 {
285 |                     addCloud(cloud_ground_, regionwise_ground_);
286 |                 }
287 |                 else if (!is_heading_outside)
288 |                 {
289 |                     addCloud(cloud_nonground_, regionwise_ground_);
290 |                 }
291 |                 else if (is_not_elevated || is_flat)
292 |                 {
293 |                     addCloud(cloud_ground_, regionwise_ground_);
294 |                 }
295 |                 else
296 |                 {
297 |                     patchwork::RevertCandidate candidate(concentric_idx, sector_idx, ground_flatness, line_variable, pc_mean_, regionwise_ground_);
298 |                     candidates.push_back(candidate);
299 |                 }
300 |                 // Every regionwise_nonground is considered nonground.
301 |                 addCloud(cloud_nonground_, regionwise_nonground_);
302 | 
303 |                 clock_t t_aft_gle = clock();
304 | 
305 |                 t_gle += t_aft_gle - t_bef_gle;
306 |             }
307 | 
308 |             clock_t t_bef_revert = clock();
309 |             if (!candidates.empty())
310 |             {
311 |                 if (params_.enable_TGR) {
312 |                     temporal_ground_revert(cloud_ground_, cloud_nonground_, ringwise_flatness, candidates, concentric_idx);
313 |                 }
314 |                 else {
315 |                     for ( auto candidate : candidates ) {
316 |                         addCloud(cloud_nonground_, candidate.regionwise_ground);
317 |                     }
318 |                 }
319 | 
320 |                 candidates.clear();
321 |                 ringwise_flatness.clear();
322 |             }
323 |             clock_t t_aft_revert = clock();
324 |             
325 |             t_revert += t_aft_revert - t_bef_revert;
326 | 
327 |             concentric_idx++;
328 |         }
329 |     }
330 | 
331 |     clock_t t_bef_update = clock();
332 |     update_elevation_thr();
333 |     update_flatness_thr();
334 |     clock_t t_aft_update = clock();
335 | 
336 |     t_update = t_aft_update - t_bef_update;
337 | 
338 |     clock_t end = clock();
339 |     time_taken_ = end - beg;
340 | 
341 |     if (params_.verbose)
342 |     {
343 |         cout << "Time taken : " << time_taken_ / double(1000000) << "(sec) ~ "
344 |             //  << t_flush / double(1000000)  << "(flush) + " 
345 |              << t_czm / double(1000000)  << "(czm) + " 
346 |              << t_sort / double(1000000)  << "(sort) + " 
347 |              << t_pca / double(1000000) << "(pca) + "
348 |              << t_gle / double(1000000) << "(estimate)" << endl;
349 |             //  << t_revert / double(1000000) << "(revert) + "
350 |             //  << t_update / double(1000000) << "(update)" << endl;
351 |     }
352 | 
353 |     if (params_.verbose) cout << "\033[1;32m" << "PatchWorkpp::estimateGround() - Estimation is finished !" << "\033[0m" << endl;
354 | }
355 | 
356 | void PatchWorkpp::update_elevation_thr(void)
357 | {
358 |     for (int i=0; i<params_.num_rings_of_interest; i++)
359 |     {
360 |         if (update_elevation_[i].empty()) continue;
361 | 
362 |         double update_mean = 0.0, update_stdev = 0.0;
363 |         calc_mean_stdev(update_elevation_[i], update_mean, update_stdev);
364 |         if (i==0) {
365 |             params_.elevation_thr[i] = update_mean + 3*update_stdev;
366 |             params_.sensor_height = -update_mean;
367 |         }
368 |         else params_.elevation_thr[i] = update_mean + 2*update_stdev;
369 | 
370 |         // if (params_.verbose) cout << "elevation threshold [" << i << "]: " << params_.elevation_thr[i] << endl;
371 | 
372 |         int exceed_num = update_elevation_[i].size() - params_.max_elevation_storage;
373 |         if (exceed_num > 0) update_elevation_[i].erase(update_elevation_[i].begin(), update_elevation_[i].begin() + exceed_num);
374 |     }
375 | }
376 | 
377 | void PatchWorkpp::update_flatness_thr(void)
378 | {
379 |     for (int i=0; i<params_.num_rings_of_interest; i++)
380 |     {
381 |         if (update_flatness_[i].empty()) break;
382 |         if (update_flatness_[i].size() <= 1) break;
383 | 
384 |         double update_mean = 0.0, update_stdev = 0.0;
385 |         calc_mean_stdev(update_flatness_[i], update_mean, update_stdev);
386 |         params_.flatness_thr[i] = update_mean+update_stdev;
387 | 
388 |         // if (params_.verbose) { cout << "flatness threshold [" << i << "]: " << params_.flatness_thr[i] << endl; }
389 | 
390 |         int exceed_num = update_flatness_[i].size() - params_.max_flatness_storage;
391 |         if (exceed_num > 0) update_flatness_[i].erase(update_flatness_[i].begin(), update_flatness_[i].begin() + exceed_num);
392 |     }
393 | }
394 | 
395 | void PatchWorkpp::reflected_noise_removal(Eigen::MatrixX3f &cloud_in, vector<PointXYZ> &nonground)
396 | {
397 |     // RNR can be used when the pointcloud is sorted in the order of ring information.
398 |     // The following code works when the ring information of points are sorted with a decreasing order...
399 |     // TODO : Change codes when it is available to get the ring information of points.
400 |     for (int i=cloud_in.rows()-1; i>cloud_in.rows()-params_.pc_num_channel*params_.noise_filter_channel_num; i--) 
401 |     {
402 |         if (cloud_in.row(i)(2) > -params_.sensor_height-0.8) continue;
403 | 
404 |         // double theta = fabs(atan2(cloud_in[i].y, cloud_in[i].x));
405 |         // if ( min(theta, M_PI-theta) > 45.0*(M_PI/180) ) cout << "outside intensity: " << cloud_in[i].intensity << endl;
406 |         // if ( min(theta, M_PI-theta) < 45.0*(M_PI/180) ) cout << "inside  intensity: " << cloud_in[i].intensity << endl;
407 |         // if ( min(theta, M_PI-theta) > 45.0*(M_PI/180) && cloud_in[i].intensity > 0.1) continue;
408 |         // noise_pc_.points.emplace_back(cloud_in[i]);
409 | 
410 |         cloud_in.row(i)(2) = std::numeric_limits<float>::min();
411 |     }
412 | 
413 | }
414 | 
415 | void PatchWorkpp::temporal_ground_revert(vector<PointXYZ> &cloud_ground, vector<PointXYZ> &cloud_nonground,
416 |                                          std::vector<double> ring_flatness, std::vector<patchwork::RevertCandidate> candidates,
417 |                                          int concentric_idx)
418 | {
419 |     if (params_.verbose) std::cout << "\033[1;34m" << "=========== Temporal Ground Revert (TGR) ===========" << "\033[0m" << endl;
420 | 
421 |     double mean_flatness = 0.0, stdev_flatness = 0.0;
422 |     calc_mean_stdev(ring_flatness, mean_flatness, stdev_flatness);
423 |     
424 |     if (params_.verbose)
425 |     {
426 |         cout << "[" << candidates[0].concentric_idx << ", " << candidates[0].sector_idx << "]"
427 |              << " mean_flatness: " << mean_flatness << ", stdev_flatness: " << stdev_flatness << std::endl;
428 |     }
429 | 
430 |     for( auto candidate : candidates )
431 |     {        
432 |         // Debug
433 |         if(params_.verbose)
434 |         {
435 |             cout << "\033[1;33m" << candidate.sector_idx << "th flat_sector_candidate"
436 |                  << " / flatness: " << candidate.ground_flatness
437 |                  << " / line_variable: " << candidate.line_variable
438 |                  << " / ground_num : " << candidate.regionwise_ground.size() 
439 |                  << "\033[0m" << endl;
440 |         }
441 | 
442 |         double mu_flatness = mean_flatness + 1.5*stdev_flatness;
443 |         double prob_flatness = 1/(1+exp( (candidate.ground_flatness-mu_flatness)/(mu_flatness/10) ));
444 | 
445 |         if (candidate.regionwise_ground.size() > 1500 && candidate.ground_flatness < params_.th_dist*params_.th_dist) prob_flatness = 1.0;
446 |        
447 |         double prob_line = 1.0;
448 |         if (candidate.line_variable > 8.0 )//&& candidate.line_dir > M_PI/4)//
449 |         {
450 |             // if (params_.verbose) cout << "line_dir: " << candidate.line_dir << endl;
451 |             prob_line = 0.0;
452 |         }
453 | 
454 |         bool revert = prob_line*prob_flatness > 0.5;
455 | 
456 |         if ( concentric_idx < params_.num_rings_of_interest )
457 |         {
458 |             if (revert)
459 |             {
460 |                 if (params_.verbose)
461 |                 {
462 |                     cout << "\033[1;32m" << "REVERT TRUE" << "\033[0m" << endl;
463 |                 }
464 |                 addCloud(cloud_ground, candidate.regionwise_ground);
465 |             }
466 |             else
467 |             {
468 |                 if (params_.verbose) 
469 |                 {
470 |                     cout << "\033[1;31m" << "FINAL REJECT" << "\033[0m" << endl;
471 |                 }
472 |                 addCloud(cloud_nonground, candidate.regionwise_ground);
473 |             }                
474 |         }
475 |     }
476 | 
477 |     if (params_.verbose) std::cout << "\033[1;34m" << "====================================================" << "\033[0m" << endl;
478 | }
479 | 
480 | // For adaptive
481 | void PatchWorkpp::extract_piecewiseground(
482 |         const int zone_idx, const vector<PointXYZ> &src,
483 |         vector<PointXYZ> &dst,
484 |         vector<PointXYZ> &non_ground_dst) {
485 |     
486 |     // 0. Initialization
487 |     if (!ground_pc_.empty()) ground_pc_.clear();
488 |     if (!dst.empty()) dst.clear();
489 |     if (!non_ground_dst.empty()) non_ground_dst.clear();
490 | 
491 |     // 1. Region-wise Vertical Plane Fitting (R-VPF) 
492 |     // : removes potential vertical plane under the ground plane
493 |     vector<bool> nonground_idx;
494 |     nonground_idx.resize(src.size(), false);
495 |     
496 |     if (params_.enable_RVPF)
497 |     {
498 |         for (int i = 0; i < params_.num_iter; i++)
499 |         {
500 |             vector<PointXYZ> src_wo_verticals;
501 |             for ( int r=0; r<nonground_idx.size(); r++ )
502 |             {
503 |                 if (!nonground_idx[r]) src_wo_verticals.push_back(src[r]);
504 |             }
505 | 
506 |             extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_, params_.th_seeds_v);
507 |             estimate_plane(ground_pc_, params_.th_dist_v);
508 | 
509 |             if (zone_idx == 0 && normal_(2) < params_.uprightness_thr)
510 |             {
511 |                 for ( int r=0; r<nonground_idx.size(); r++ ) {
512 |                     if (!nonground_idx[r]) {
513 | 
514 |                         double distance = calc_point_to_plane_d(src[r], normal_, d_);
515 |                         
516 |                         if ( abs(distance) < params_.th_dist_v ) {
517 |                             nonground_idx[r] == true;
518 |                             non_ground_dst.push_back(src[r]);
519 |                         }
520 |                     }   
521 |                 }
522 |             }
523 |             else break;
524 |         }
525 |     }
526 | 
527 |     // 2. Region-wise Ground Plane Fitting (R-GPF)
528 |     // : fits the ground plane
529 |     vector<PointXYZ> src_wo_nongrounds;
530 |     for ( int r=0; r<nonground_idx.size(); r++ )
531 |     {
532 |         if (!nonground_idx[r]) src_wo_nongrounds.push_back(src[r]);
533 |     }
534 | 
535 |     extract_initial_seeds(zone_idx, src_wo_nongrounds, ground_pc_);
536 |     estimate_plane(ground_pc_, params_.th_dist);
537 | 
538 |     for (int i = 0; i < params_.num_iter; i++) {
539 | 
540 |         ground_pc_.clear();        
541 | 
542 |         for ( auto point : src_wo_nongrounds ) {
543 |             
544 |             double distance = calc_point_to_plane_d(point, normal_, d_);
545 |             
546 |             if (i < params_.num_iter - 1) {
547 |                 if ( distance < params_.th_dist ) {
548 |                     ground_pc_.push_back(point);
549 |                 }
550 |             } else {
551 |                 if ( distance < params_.th_dist ) {
552 |                     dst.push_back(point);
553 |                 } else {
554 |                     non_ground_dst.push_back(point);
555 |                 }
556 |             }
557 |         }
558 | 
559 |         if (i < params_.num_iter -1) {
560 |             estimate_plane(ground_pc_, params_.th_dist);
561 |         }
562 |         else {
563 |             estimate_plane(dst, params_.th_dist);
564 |         }
565 |     }
566 |     //assert( dst.size() + non_ground_dst.size() == src.size()  );
567 | }
568 | 
569 | double PatchWorkpp::calc_point_to_plane_d(PointXYZ p, Eigen::VectorXf normal, double d)
570 | {
571 |     return normal(0)*p.x+normal(1)*p.y+normal(2)*p.z+d;
572 | }
573 | 
574 | 
575 | void PatchWorkpp::calc_mean_stdev(std::vector<double> vec, double &mean, double &stdev)
576 | {
577 |     if (vec.size() <= 1) return;
578 | 
579 |     mean = std::accumulate(vec.begin(), vec.end(), 0.0) / vec.size();
580 | 
581 |     for (int i=0; i<vec.size(); i++) { stdev += (vec.at(i)-mean)*(vec.at(i)-mean); }  
582 |     stdev /= vec.size()-1;
583 |     stdev = sqrt(stdev);
584 | }
585 | 
586 | double PatchWorkpp::xy2theta(const double &x, const double &y) { // 0 ~ 2 * PI
587 |     double angle = atan2(y, x);
588 |     return angle > 0 ? angle : 2*M_PI+angle;
589 | }
590 | 
591 | double PatchWorkpp::xy2radius(const double &x, const double &y) {
592 |     // return sqrt(pow(x, 2) + pow(y, 2));
593 |     return sqrt(x*x + y*y);
594 | }
595 | 
596 | void PatchWorkpp::pc2czm(const Eigen::MatrixX3f &src, std::vector<Zone> &czm) {
597 | 
598 |     double max_range = params_.max_range, min_range = params_.min_range;
599 |     double min_range_0 = min_ranges_[0], min_range_1 = min_ranges_[1], min_range_2 = min_ranges_[2], min_range_3 = min_ranges_[3];
600 |     int num_ring_0 = params_.num_rings_each_zone[0], num_sector_0 = params_.num_sectors_each_zone[0];
601 |     int num_ring_1 = params_.num_rings_each_zone[1], num_sector_1 = params_.num_sectors_each_zone[1];
602 |     int num_ring_2 = params_.num_rings_each_zone[2], num_sector_2 = params_.num_sectors_each_zone[2];
603 |     int num_ring_3 = params_.num_rings_each_zone[3], num_sector_3 = params_.num_sectors_each_zone[3];
604 | 
605 |     for (int i=0; i<src.rows(); i++) {
606 | 
607 |         float x = src.row(i)(0), y = src.row(i)(1), z = src.row(i)(2);
608 | 
609 |         int ring_idx, sector_idx;
610 |         // double r = xy2radius(pt.x, pt.y);
611 |         double r = xy2radius(x, y);
612 |         if ((r <= max_range) && (r > min_range)) {
613 |             // double theta = xy2theta(pt.x, pt.y);
614 |             double theta = xy2theta(x, y);
615 |             
616 |             if (r < min_range_1) { // In First rings
617 |                 ring_idx = min(static_cast<int>(((r - min_range_0) / ring_sizes_[0])), num_ring_0 - 1);
618 |                 sector_idx = min(static_cast<int>((theta / sector_sizes_[0])), num_sector_0 - 1);
619 |                 czm[0][ring_idx][sector_idx].emplace_back(PointXYZ(x, y, z));
620 |             } else if (r < min_range_2) {
621 |                 ring_idx = min(static_cast<int>(((r - min_range_1) / ring_sizes_[1])), num_ring_1 - 1);
622 |                 sector_idx = min(static_cast<int>((theta / sector_sizes_[1])), num_sector_1 - 1);
623 |                 czm[1][ring_idx][sector_idx].emplace_back(PointXYZ(x, y, z));
624 |             } else if (r < min_range_3) {
625 |                 ring_idx = min(static_cast<int>(((r - min_range_2) / ring_sizes_[2])), num_ring_2 - 1);
626 |                 sector_idx = min(static_cast<int>((theta / sector_sizes_[2])), num_sector_2 - 1);
627 |                 czm[2][ring_idx][sector_idx].emplace_back(PointXYZ(x, y, z));
628 |             } else { // Far!
629 |                 ring_idx = min(static_cast<int>(((r - min_range_3) / ring_sizes_[3])), num_ring_3 - 1);
630 |                 sector_idx = min(static_cast<int>((theta / sector_sizes_[3])), num_sector_3 - 1);
631 |                 czm[3][ring_idx][sector_idx].emplace_back(PointXYZ(x, y, z));
632 |             }
633 | 
634 |             // int zone_idx = 0;
635 |             // if ( r < min_range_1 ) zone_idx = 0;
636 |             // else if ( r < min_range_2 ) zone_idx = 1;
637 |             // else if ( r < min_range_3 ) zone_idx = 2;
638 |             // else zone_idx = 3;
639 |                         
640 |             // int ring_idx = min(static_cast<int>(((r - min_ranges_[zone_idx]) / ring_sizes_[zone_idx])), params_.num_rings_each_zone[zone_idx] - 1);
641 |             // int sector_idx = min(static_cast<int>((theta / sector_sizes_[zone_idx])), params_.num_sectors_each_zone[zone_idx] - 1);
642 |             
643 |             // czm[zone_idx][ring_idx][sector_idx].push_back(PointXYZ(x, y, z));
644 |         }
645 |     }
646 |     if (params_.verbose) cout << "\033[1;33m" << "PatchWorkpp::pc2czm() - Divides pointcloud into the concentric zone model successfully" << "\033[0m" << endl;
647 | }
648 | 


--------------------------------------------------------------------------------