参考资料：古月居 - ROS机器人知识分享社区

https://zhuanlan.zhihu.com/p/644297447

patchwork++算法一共包含四部分内容：提出了以下四个部分：RNR、RVPF、A-GLE 和 TGR。

1）基于 3D LiDAR 反射模型的反射噪声消除 (RNR)，以有效消除虚拟噪声点。

2）引入了区域垂直平面拟合 (RVPF) 来正确分割地面，即使地面被不同层抬高。

3）利用适应地面似然估计 (A-GLE) 根据先前的地面分割结果自适应地计算适当的参数。

4）时间地面恢复 (TGR) 通过使用临时地面属性来缓解部分欠分割问题。

0.CMZ同心圆模型构造

以极坐标形式，将每个点按照半径和极坐标角度，按照[环区域][环带号][扇区号]进行分类，即先定位环区域，再定位在当前区域的哪个环带，再定位在当前环带的哪个扇区。通过将整个区域划分为同心圆模型，对每一个扇区bin区域（【区域】【环带】【扇区】）进行遍历和地面非地面分析，来拟合整个路面。

代码：

//将点云根据半径角度放入同心圆模型（构造同心圆模型存储对应索引点云）
template<typename PointT> inline
void PatchWorkpp<PointT>::pc2czm(const pcl::PointCloud<PointT> &src, std::vector<Zone> &czm, pcl::PointCloud<PointT> &cloud_nonground) {for (int i=0; i<src.size(); i++) {//如果被标记为噪声则跳过if ((!noise_idxs_.empty()) &&(i == noise_idxs_.front())) {//过滤掉噪点区域不转换好CZMnoise_idxs_.pop();continue;}PointT pt = src.points[i];//当前点double r = xy2radius(pt.x, pt.y);//当前点半径// 只识别XY一定范围内的地面，类似于栅格化 min_range_ 0.2米  max_range_ 40米if ((r <= max_range_) && (r > min_range_)) {double theta = xy2theta(pt.x, pt.y);//计算点在极坐标下的角度，确定对应的区域（X/Y角度）//根据半径确定所述的同心圆区域int zone_idx = 0;if ( r < min_ranges_[1] ) zone_idx = 0;else if ( r < min_ranges_[2] ) zone_idx = 1;else if ( r < min_ranges_[3] ) zone_idx = 2;else zone_idx = 3;//ring_idx 环数确定（半径）//每个区域环的编号=（半径-当前区域(环)的起始边界）/每个环带的宽度；num_rings_each_zone_保证使其不超过环的总数[2,4,4,4]int ring_idx = min(static_cast<int>(((r - min_ranges_[zone_idx]) / ring_sizes_[zone_idx])), num_rings_each_zone_[zone_idx] - 1);//sector_idx扇形区域确定（角度）//计算所在环区域的，扇形区域的编号int sector_idx = min(static_cast<int>((theta / sector_sizes_[zone_idx])), num_sectors_each_zone_[zone_idx] - 1);//将点放入同心圆模型（环区域号，环带号，扇形区域号）czm[zone_idx][ring_idx][sector_idx].points.emplace_back(pt);}else {// 当点云距离大于阈值时，直接默认为是非地面点cloud_nonground.push_back(pt);}}if (verbose_) cout << "[ CZM ] Divides pointcloud into the concentric zone model" << endl;
}

1.噪声点去除

由于雷达的穿透效应，会在地面之下产生一个高度很低的噪声点，R-GPF算法会选取最低点作为初始点，选到这些噪声点时就会出现欠分割了(真实地面没找到，也就是FN错误拒绝)

由于入射角较小(与竖直面夹角)会导致雷达光线被穿透，而被穿透后的点的坐标较小，其反射率也会相应较小，以入射角角度，高度，反射率来判断该点是否为噪声点，如果为噪声点则认为其为非地面点，防止后续在RVPF进行竖直面分割和平面拟合时选到该点。

代码：

//移除反射噪声点，并将点合并到cloud_nonground非地面点云
template<typename PointT> inline
void PatchWorkpp<PointT>::reflected_noise_removal(pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_nonground)
{for (int i=0; i<cloud_in.size(); i++){double r = sqrt( cloud_in[i].x*cloud_in[i].x + cloud_in[i].y*cloud_in[i].y );//水平距离double z = cloud_in[i].z;//高度（机身系）double ver_angle_in_deg = atan2(z, r)*180/M_PI;//计算夹角角度//如果入射角度小于阈值，高度小于阈值，反射率小于阈值（作为噪声和非地面点）if ( ver_angle_in_deg < RNR_ver_angle_thr_ && z < -sensor_height_-0.8 && cloud_in[i].intensity < RNR_intensity_thr_){cloud_nonground.push_back(cloud_in[i]);noise_pc_.push_back(cloud_in[i]);noise_idxs_.push(i);}}if (verbose_)ROS_INFO("[ RNR ] Num of noises :%d", noise_pc_.points.size());
}

2.区域垂直平面拟合

对于某一个BIN（扇区），如果未能剔除垂直区域，以Z坐标较小的种子点进行平面拟合时，会出现平面异常清空。如楼梯面。使用R-VPF来剔除垂直方向上的点云，并通过剔除后的点云拟合平面，以点到该平面的距离来分类地面和非地面点。

2.1种子点选取

对按高度升序后的点云进行处理，选取高度低于某一阈值的点云作为种子点云，这些点云用于后续的平面拟合。

代码：

//从输入原始点云p_sorted中提取初始地面种子点（小于某高度平均值），并存储到init_seeds
template<typename PointT> inline
void PatchWorkpp<PointT>::extract_initial_seeds(const int zone_idx, const pcl::PointCloud<PointT> &p_sorted,pcl::PointCloud<PointT> &init_seeds, double th_seed) {init_seeds.points.clear();// LPR是低点代表的平均值double sum = 0;int cnt = 0;int init_idx = 0;//如果在0区域有点，会对起始索引init_idx进行处理，否则以最低点init_idx取if (zone_idx == 0) {//0环区域（最内圈）for (int i = 0; i < p_sorted.points.size(); i++) {//遍历输入点云（经过高度由小到大排序后）if (p_sorted.points[i].z < adaptive_seed_selection_margin_ * sensor_height_) {//高度小于阈值-1.2*1.5++init_idx;//计算起始点索引（最低点）} else {break;}}}// Calculate the mean height value.//以init_idx为起始索引，选取num_lpr_个，高度高于adaptive_seed_selection_margin_ * sensor_height_的点计算平均高度for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) {sum += p_sorted.points[i].z;cnt++;}double lpr_height = cnt != 0 ? sum / cnt : 0;// 计算平均高度lpr_height// 迭代点云，将高度小于lpr_height + th_seed点作为种子点for (int i = 0; i < p_sorted.points.size(); i++) {if (p_sorted.points[i].z < lpr_height + th_seed) {init_seeds.points.push_back(p_sorted.points[i]);}}
}

2.2以种子点的协方差和均值，拟合平面

对输入的种子点云，计算协方差和平均值，对协方差进行奇异值分解，得到对应的特征值。特征值越小，其对应方向向量的方差越小，离散度越小。以最小奇异值方向作为平面法线方向（平面法线方向离散度最小）。以均值坐标（质心）作为平面点。拟合平面

代码：

//以输入点云的协方差和均值，拟合平面
template<typename PointT> inline
void PatchWorkpp<PointT>::estimate_plane(const pcl::PointCloud<PointT> &ground) {pcl::computeMeanAndCovarianceMatrix(ground, cov_, pc_mean_);//计算协方差矩阵cov_和点云均值pc_mean_（所有坐标）// 对协方差矩阵奇异值分解cov=USU^TEigen::JacobiSVD<Eigen::MatrixXf> svd(cov_, Eigen::DecompositionOptions::ComputeFullU);//点云协方差的奇异值，表示点云在不同方向的离散程度singular_values_ = svd.singularValues();// 使用最小奇异向量作为法线（最小奇异值方向的特征向量）//最小特征值表明该方向离散程度最小，该方向近似平面normal_ = (svd.matrixU().col(2));//法向量方向修正（始终向上）if (normal_(2) < 0) { for(int i=0; i<3; i++) normal_(i) *= -1; }//以点云均值为平面上的点，// mean ground seeds valueEigen::Vector3f seeds_mean = pc_mean_.head<3>();// according to normal.T*[x,y,z] = -d//ax+by+cz=-d;[a,b,c]为法向量，[x,y,z]为平面上的点d_ = -(normal_.transpose() * seeds_mean)(0, 0);
}

2.3垂直平面剔除

如果拟合的平面过于垂直（单位法线Z方向的分量小于阈值，即法向量过于平坦，平面过于垂直）。则认为该拟合平面不符合要求，在拟合时可能存在其他竖直平面的干扰，需要将竖直平面去除。认为与当前拟合平面距离小于某一阈值的点，认为其为竖直平面点，需要去除。

代码：

if (enable_RVPF_)//如果使能enable_RVPF_{for (int i = 0; i < num_iter_; i++){/*******1.1在高度小于一定区域选取种子点******* *///从输入点云p_sorted中提取初始地面种子点（小于某高度平均值），并存储到init_seeds//zone_idx：区号，src_wo_verticals：输入排序后的点云，ground_pc_：种子点，th_seeds_v_：平均值以上的筛选高度extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_, th_seeds_v_);/********1.2.以输入的种子点云的协方差和均值拟合平面***/estimate_plane(ground_pc_);/********1.3.如果拟合的平面过于垂直，剔除点云中垂直平面点，如墙面等** *///区域0，法向量Z分离小于阈值（过于垂直），uprightness_thr_=0.707，则法向量小于45度，平面垂直大于45度if (zone_idx == 0 && normal_(2) < uprightness_thr_){pcl::PointCloud<PointT> src_tmp;src_tmp = src_wo_verticals;src_wo_verticals.clear();Eigen::MatrixXf points(src_tmp.points.size(), 3);int j = 0;//转换为矩阵模式，每一行代表一个点for (auto &p:src_tmp.points) {points.row(j++) << p.x, p.y, p.z;}// 计算点到平面距离p*n+d=dis==>p*n=dis-d=resultEigen::VectorXf result = points * normal_;//遍历点云进行分类，如果点与平面距离小于阈值，则认为其为垂直平面点，否则为其他点//d_是estimate_plane拟合平面的平面偏移量参数for (int r = 0; r < result.rows(); r++) {//result+d_=dis=p*n+d<th_dist_v_,点到平面距离在阈值[-th_dist_v_,th_dist_v_]if (result[r] < th_dist_v_ - d_ && result[r] > -th_dist_v_ - d_) {non_ground_dst.points.push_back(src_tmp[r]);vertical_pc_.points.push_back(src_tmp[r]);} else {src_wo_verticals.points.push_back(src_tmp[r]);}}}else break;}}

2.4对经过R-VPF竖直面剔除后的点云，进行平面拟合

对剔除垂直点后的点云，寻找z值小于阈值的种子点，以种子点方差最小方向为法向量方向，以平均点坐标为平面坐标，构建拟合平面。

2.5以点云与拟合平面的距离来区分地面和非地面点

代码：

    for (int i = 0; i < num_iter_; i++) {ground_pc_.clear();// ground plane model//计算点到平面的距离，平面方程：ax+by+cz=-d//点到平面距离为dis=|ax1+by1+cz1+d|/sqrt(a*a+b*b+c*c)=points * normal_+d_//result=dis-d_Eigen::VectorXf result = points * normal_;// threshold filter//按照点到平面距离，进行点云分类，与拟合平面小于th_dist_距离的认为是地面点for (int r = 0; r < result.rows(); r++) {if (i < num_iter_ - 1) {if (result[r] < th_dist_ - d_ ) {ground_pc_.points.push_back(src_wo_verticals[r]);}} else { // Final stageif (result[r] < th_dist_ - d_ ) {dst.points.push_back(src_wo_verticals[r]);} else {non_ground_dst.points.push_back(src_wo_verticals[r]);}}}if (i < num_iter_ -1) estimate_plane(ground_pc_);//使用当前地面点，重新拟合平面else estimate_plane(dst);//使用dst最后一次的地面点，拟合平面}

3.A-GLE自适应地面估计

以法向量夹角，高程差，平坦度等指标，对RVPF平面拟合后的地面点进行进一步的地面估计，RVPF的估计的非地面点，则直接作为非地面点处理。对上述每个BIN中的点重新评估后，合并成为地面点和非地面点。

1）以2中拟合的平面法向量，来判断拟合平面与水平面（雷达系）的夹角，来判断是否为地面

2）以高程差，判断其是否为地面

3）以平坦度判断是否为地面

4）以质心向量与平面法向量（朝上）是否同方向判断是否为地面，若同方向，则雷达应该在拟合平面下面，则该平面不会为地面。

代码：

//.计算每个扇区Bin的地面特征
const double ground_uprightness = normal_(2);//垂直度：地面法向量与垂直方向夹角（地面法向量的z分量，法向量与z轴夹角余弦，地面倾角越小，越接近1）
const double ground_elevation   = pc_mean_(2, 0);//高程差：地面高度（当前区域种子点云平均高度Z）
const double ground_flatness    = singular_values_.minCoeff();//平坦度（λ3）,最小的特征值（平面法向量）//线性特性（点云沿某一方向的延申）
//最大奇异值与次大奇异值的比值，比值越大，说明越接近一条直线
const double line_variable      = singular_values_(1) != 0 ? singular_values_(0)/singular_values_(1) : std::numeric_limits<double>::max();// pc_mean_(i,0)，点云平均值在第i个轴的值，normal_(i)法向量在第i个轴的分量（i=0是x轴）double heading = 0.0;//法向量与传感器方向（<0法向量与传感器方向一致->地面，>0法向量与传感器方向不一致->非地面）
//点云质心向量与平面法向量的点积运算(x,y,z)*(a,b,c)，表示是否同向，其中normal_(2)>0,朝上
for(int i=0; i<3; i++) heading += pc_mean_(i,0)*normal_(i);//法线与雷达射线方向是否同向（点坐标为原点到当前点的射线）
/***************4.使用A-GLE进行自适应地面估计******* *///地面法向量与垂直方向夹角大于阈值，表明拟合平面与水平面夹角较小，符合平面特性bool is_upright         = ground_uprightness > uprightness_thr_;//法线方向是否符合地面特性//种子平均高度小于区域的海拔高度（concentric_idx：区域序号）bool is_not_elevated    = ground_elevation < elevation_thr_[concentric_idx];//高度是否符合地面特性
//平面平坦度小于阈值bool is_flat            = ground_flatness < flatness_thr_[concentric_idx];//平坦度是否符合地面特性//当前区域序号是否没有超限bool is_near_zone       = concentric_idx < num_rings_of_interest_;//如果平面的法向量与种子质心向量的朝向同向，则false，为非地面点
bool is_heading_outside = heading < 0.0;//地面法线方向是否朝向传感器
//4.1该BIN地面点、非地面点、候选点判断 
//如果拟合平面与水平面夹角较小&&种子平均高度小于区域的海拔高度&&当前区域序号是否没有超限
//将当前bin种子的Z高度，存入update_elevation_[序号]数组中
//将当前bin种子的平坦度，存入update_flatness_[序号]数组中
//将当前bin种子的平坦度，存入ringwise_flatness数组中
if (is_upright && is_not_elevated && is_near_zone){update_elevation_[concentric_idx].push_back(ground_elevation);update_flatness_[concentric_idx].push_back(ground_flatness);ringwise_flatness.push_back(ground_flatness);}// 如果拟合平面与水平面夹角较大，说明该bin非水平，非地面点
if (!is_upright)
{cloud_nonground += regionwise_ground_;}
//如果zone区域超限，该bin非地面点
else if (!is_near_zone){cloud_ground += regionwise_ground_;}//如果平面法向量与传感器同向，非地面点（即雷达在平面下面，非地面点）
else if (!is_heading_outside)
{cloud_nonground += regionwise_ground_;
}
//如果种子平均高度小于区域的海拔高度，或者拟合平面平坦度小于阈值，则该BIN为地面点
else if (is_not_elevated || is_flat){cloud_ground += regionwise_ground_;}
else//不确定的候选点
{//构造候选点，共后续的TGE时间回退机制修正//候选点属性：区域编号，环编号，平坦度，线性特征，质心坐标，拟合的平面点RevertCandidate<PointT> candidate(concentric_idx, sector_idx, ground_flatness, line_variable, pc_mean_, regionwise_ground_);candidates.push_back(candidate);}
// Every regionwise_nonground is considered nonground.//每个bin中，进行垂直平面拟合时，认为垂直的点regionwise_nonground_都是非地面点
cloud_nonground += regionwise_nonground_;

4.TGR时间回退处理

对不满足3中分类的不确定候选点云进行修正

4.1将不满足A-GLE条件划分的点作为候选点，加入数组中

4.2通过平坦度和线性特性，来判断候选点中的平面拟合点是否为地面

代码：

/***********5.时间回退处理TGR**********/
double t_bef_revert = ros::Time::now().toSec();//每个环带的遍历
if (!candidates.empty())//候选点不为空
{if (enable_TGR_)//使能TGR时间回退功能{//输入当前环带ring的地面点，非地面点，平坦度，候选点，当前区域编号temporal_ground_revert(cloud_ground, cloud_nonground, ringwise_flatness, candidates, concentric_idx);}else{for (size_t i=0; i<candidates.size(); i++){cloud_nonground += candidates[i].regionwise_ground;}}candidates.clear();//将当前环带的候选点数组清空ringwise_flatness.clear();//将当前环带的平坦度数组清空
}double t_aft_revert = ros::Time::now().toSec();t_revert += t_aft_revert - t_bef_revert;concentric_idx++;//区域序号