参考资料：古月居 - 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())) {//过滤掉噪点区域不转换好CZM
noise_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^T
Eigen::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 value
Eigen::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=result
Eigen::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 stage
if (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++;//区域序号
