patchwork++地面分割学习笔记

参考资料：古月居 - 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++;//区域序号