Skip to content

Commit 95181b8

Browse files
seungjae24seungjae24
committed
Update R-VPF
1 parent 56bbef8 commit 95181b8

File tree

6 files changed

+93
-121
lines changed

6 files changed

+93
-121
lines changed

‎.gitignore

+3-1
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,6 @@
11
.vscode/
22

33
./launch/video.launch
4-
./src/video.cpp
4+
./src/video.cpp
5+
6+
scripts/

‎config/params.yaml

+3-1
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,9 @@ patchworkpp:
3434
flatness_margin: 0.0
3535
noise_filter_channel_num: 20
3636

37-
enable_TGR : true
3837
enable_RNR : true
38+
enable_RVPF : true
39+
enable_TGR : true
40+
3941
pc_num_channel: 2048
4042
intensity_thr: 0.2

‎include/patchworkpp/patchworkpp.hpp

+54-57
Original file line numberDiff line numberDiff line change
@@ -89,6 +89,7 @@ class PatchWorkpp {
8989
node_handle_.param("/patchworkpp/max_flatness_storage", max_flatness_storage_, 1000);
9090
node_handle_.param("/patchworkpp/max_elevation_storage", max_elevation_storage_, 1000);
9191
node_handle_.param("/patchworkpp/enable_RNR", enable_RNR_, true);
92+
node_handle_.param("/patchworkpp/enable_RVPF", enable_RVPF_, true);
9293
node_handle_.param("/patchworkpp/enable_TGR", enable_TGR_, true);
9394

9495
ROS_INFO("Sensor Height: %f", sensor_height_);
@@ -141,7 +142,6 @@ class PatchWorkpp {
141142

142143
revert_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH);
143144
ground_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH);
144-
non_ground_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH);
145145
regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH);
146146
regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH);
147147

@@ -208,6 +208,7 @@ class PatchWorkpp {
208208

209209
bool verbose_;
210210
bool enable_RNR_;
211+
bool enable_RVPF_;
211212
bool enable_TGR_;
212213

213214
int max_flatness_storage_, max_elevation_storage_;
@@ -219,9 +220,8 @@ class PatchWorkpp {
219220
double intensity_thr_;
220221

221222
float d_;
222-
float th_dist_d_;
223223

224-
MatrixXf normal_;
224+
VectorXf normal_;
225225
MatrixXf pnormal_;
226226
VectorXf singular_values_;
227227
Eigen::Matrix3f cov_;
@@ -245,7 +245,7 @@ class PatchWorkpp {
245245

246246
ros::Publisher PlaneViz, pub_revert_pc, pub_reject_pc, pub_normal, pub_noise, pub_vertical;
247247
pcl::PointCloud<PointT> revert_pc_, reject_pc_, noise_pc_, vertical_pc_;
248-
pcl::PointCloud<PointT> ground_pc_, non_ground_pc_;
248+
pcl::PointCloud<PointT> ground_pc_;
249249

250250
pcl::PointCloud<pcl::PointXYZINormal> normals_;
251251

@@ -273,7 +273,7 @@ class PatchWorkpp {
273273

274274
double xy2radius(const double &x, const double &y);
275275

276-
void estimate_plane(const pcl::PointCloud<PointT> &ground, double th_dist);
276+
void estimate_plane(const pcl::PointCloud<PointT> &ground);
277277

278278
void extract_piecewiseground(
279279
const int zone_idx, const pcl::PointCloud<PointT> &src,
@@ -339,7 +339,7 @@ void PatchWorkpp<PointT>::flush_patches(vector<Zone> &czm) {
339339
}
340340

341341
template<typename PointT> inline
342-
void PatchWorkpp<PointT>::estimate_plane(const pcl::PointCloud<PointT> &ground, double th_dist) {
342+
void PatchWorkpp<PointT>::estimate_plane(const pcl::PointCloud<PointT> &ground) {
343343
pcl::computeMeanAndCovarianceMatrix(ground, cov_, pc_mean_);
344344
// Singular Value Decomposition: SVD
345345
Eigen::JacobiSVD<Eigen::MatrixXf> svd(cov_, Eigen::DecompositionOptions::ComputeFullU);
@@ -355,8 +355,6 @@ void PatchWorkpp<PointT>::estimate_plane(const pcl::PointCloud<PointT> &ground,
355355

356356
// according to normal.T*[x,y,z] = -d
357357
d_ = -(normal_.transpose() * seeds_mean)(0, 0);
358-
// set distance threshold to `th_dist - d`
359-
th_dist_d_ = th_dist - d_;
360358
}
361359

362360
template<typename PointT> inline
@@ -480,16 +478,12 @@ void PatchWorkpp<PointT>::estimate_ground(
480478
start = ros::Time::now().toSec();
481479

482480
// 1. Reflected Noise Removal (RNR)
483-
if (!enable_RNR_) reflected_noise_removal(cloud_in, cloud_nonground);
481+
if (enable_RNR_) reflected_noise_removal(cloud_in, cloud_nonground);
484482

485483
t1 = ros::Time::now().toSec();
486484

487485
// 2. Concentric Zone Model (CZM)
488-
// for (int k = 0; k < num_zones_; ++k) {
489-
// flush_patches_in_zone(ConcentricZoneModel_[k], num_sectors_each_zone_[k], num_rings_each_zone_[k]);
490-
// }
491486
flush_patches(ConcentricZoneModel_);
492-
493487
pc2czm(cloud_in, ConcentricZoneModel_);
494488

495489
t2 = ros::Time::now().toSec();
@@ -542,13 +536,13 @@ void PatchWorkpp<PointT>::estimate_ground(
542536

543537
// Status of each patch
544538
// used in checking uprightness, elevation, and flatness, respectively
545-
const double ground_uprightness = normal_(2, 0);
539+
const double ground_uprightness = normal_(2);
546540
const double ground_elevation = pc_mean_(2, 0);
547541
const double ground_flatness = singular_values_.minCoeff();
548542
const double line_variable = singular_values_(1) != 0 ? singular_values_(0)/singular_values_(1) : std::numeric_limits<double>::max();
549543

550544
double heading = 0.0;
551-
for(int i=0; i<3; i++) heading += pc_mean_(i,0)*normal_(i,0);
545+
for(int i=0; i<3; i++) heading += pc_mean_(i,0)*normal_(i);
552546

553547
if (visualize_) {
554548
auto polygons = set_polygons(zone_idx, ring_idx, sector_idx, 3);
@@ -560,9 +554,9 @@ void PatchWorkpp<PointT>::estimate_ground(
560554
tmp_p.x = pc_mean_(0,0);
561555
tmp_p.y = pc_mean_(1,0);
562556
tmp_p.z = pc_mean_(2,0);
563-
tmp_p.normal_x = normal_(0,0);
564-
tmp_p.normal_y = normal_(1,0);
565-
tmp_p.normal_z = normal_(2,0);
557+
tmp_p.normal_x = normal_(0);
558+
tmp_p.normal_y = normal_(1);
559+
tmp_p.normal_z = normal_(2);
566560
normals_.points.emplace_back(tmp_p);
567561
}
568562

@@ -856,73 +850,76 @@ void PatchWorkpp<PointT>::extract_piecewiseground(
856850
pcl::PointCloud<PointT> src_wo_verticals;
857851
src_wo_verticals = src;
858852

859-
for (int i = 0; i < num_iter_; i++)
853+
if (enable_RVPF_)
860854
{
861-
extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_, th_seeds_v_);
862-
estimate_plane(ground_pc_, th_dist_v_);
863-
864-
if (zone_idx == 0 && normal_(2) < uprightness_thr_-0.1)
855+
for (int i = 0; i < num_iter_; i++)
865856
{
866-
pcl::PointCloud<PointT> src_tmp;
867-
src_tmp = src_wo_verticals;
868-
src_wo_verticals.clear();
869-
870-
Eigen::MatrixXf points(src_tmp.points.size(), 3);
871-
int j = 0;
872-
for (auto &p:src_tmp.points) {
873-
points.row(j++) << p.x, p.y, p.z;
874-
}
875-
// ground plane model
876-
Eigen::VectorXf result = points * normal_;
857+
extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_, th_seeds_v_);
858+
estimate_plane(ground_pc_);
877859

878-
for (int r = 0; r < result.rows(); r++) {
879-
if (result[r] < th_dist_ - d_ && result[r] > -th_dist_ - d_) {
880-
non_ground_dst.points.push_back(src_tmp[r]);
881-
vertical_pc_.points.push_back(src_tmp[r]);
882-
} else {
883-
src_wo_verticals.points.push_back(src_tmp[r]);
860+
if (zone_idx == 0 && normal_(2) < uprightness_thr_-0.1)
861+
{
862+
pcl::PointCloud<PointT> src_tmp;
863+
src_tmp = src_wo_verticals;
864+
src_wo_verticals.clear();
865+
866+
Eigen::MatrixXf points(src_tmp.points.size(), 3);
867+
int j = 0;
868+
for (auto &p:src_tmp.points) {
869+
points.row(j++) << p.x, p.y, p.z;
870+
}
871+
// ground plane model
872+
Eigen::VectorXf result = points * normal_;
873+
874+
for (int r = 0; r < result.rows(); r++) {
875+
if (result[r] < th_dist_v_ - d_ && result[r] > -th_dist_v_ - d_) {
876+
non_ground_dst.points.push_back(src_tmp[r]);
877+
vertical_pc_.points.push_back(src_tmp[r]);
878+
} else {
879+
src_wo_verticals.points.push_back(src_tmp[r]);
880+
}
884881
}
885882
}
883+
else break;
886884
}
887-
else break;
888885
}
889-
886+
890887
extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_);
891-
estimate_plane(ground_pc_, th_dist_);
888+
estimate_plane(ground_pc_);
892889

893890
// 2. Region-wise Ground Plane Fitting (R-GPF)
894891
// : fits the ground plane
892+
893+
//pointcloud to matrix
894+
Eigen::MatrixXf points(src_wo_verticals.points.size(), 3);
895+
int j = 0;
896+
for (auto &p:src_wo_verticals.points) {
897+
points.row(j++) << p.x, p.y, p.z;
898+
}
899+
895900
for (int i = 0; i < num_iter_; i++) {
896901

897902
ground_pc_.clear();
898-
899-
//pointcloud to matrix
900-
Eigen::MatrixXf points(src_wo_verticals.points.size(), 3);
901-
int j = 0;
902-
for (auto &p:src_wo_verticals.points) {
903-
points.row(j++) << p.x, p.y, p.z;
904-
}
903+
905904
// ground plane model
906905
Eigen::VectorXf result = points * normal_;
907906
// threshold filter
908907
for (int r = 0; r < result.rows(); r++) {
909908
if (i < num_iter_ - 1) {
910-
if (result[r] < th_dist_d_ ) {// && result[r] > - th_dist_ - d_) {
909+
if (result[r] < th_dist_ - d_ && result[r] > - (th_dist_+0.8) - d_) {
911910
ground_pc_.points.push_back(src_wo_verticals[r]);
912911
}
913912
} else { // Final stage
914-
if (result[r] < th_dist_d_ ) {
913+
if (result[r] < th_dist_ - d_ && result[r] > - (th_dist_+0.8) - d_) {
915914
dst.points.push_back(src_wo_verticals[r]);
916915
} else {
917-
if (i == num_iter_ - 1) {
918-
non_ground_dst.points.push_back(src_wo_verticals[r]);
919-
}
916+
non_ground_dst.points.push_back(src_wo_verticals[r]);
920917
}
921918
}
922919
}
923920

924-
if (i < num_iter_ -1) estimate_plane(ground_pc_, th_dist_);
925-
else estimate_plane(dst, th_dist_);
921+
if (i < num_iter_ -1) estimate_plane(ground_pc_);
922+
else estimate_plane(dst);
926923
}
927924
}
928925

‎launch/offline_kitti.launch

+2-2
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,8 @@
11
<launch>
22

3-
3+
<node name="tf" pkg="tf" type="static_transform_publisher" args= "0.0 0.0 0.0 0.0 0.0 0.0 map base_link 100"/>
44
<rosparam command="load" file="$(find patchworkpp)/config/params.yaml" />
5-
<!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find patchworkpp)/rviz/patchworkpp_viz.rviz"/> -->
5+
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find patchworkpp)/rviz/patchworkpp_viz.rviz"/>
66
<node name="$(anon offline_kitti)" pkg="patchworkpp" type="offline_kitti" output="screen">
77

88
<rosparam param="/algorithm">"patchworkpp"</rosparam>

0 commit comments

Comments
 (0)