点云地面检测算法

1 为什么对点云进行地面检测

• 原因1： 从三维激光雷达获取的点云信息往往是稠密的，对三维点云进行分割segmentation或者聚类clustering是对点云的基本操作之一。在进行分割之前，首先将地面点提取出来，会加速后面的分割。
• 原因2：在做slam的时候，比如hdl_graph_slam把提取出的地面作为一个约束加入到slam后端优化中，提升slam建图精度

本文根据使用经历，总结三种方法

• 直接法
• 地面拟合法
• 条件筛选法

2 直接法

这个方法是lego-loam中用到的地面点检测的算法，比较简单粗暴。 利用激光雷达扫描的特点，对于16线激光雷达，地面点只可能在下面7线中出现。然后上下两条线xyz做差值，如果遇到的是地面点，则角度$\theta$会比较小，否则会比较大。
$$\theta =ta{n}^{-1}(diffz,sqrt(diffx\ast diffx+diffy\ast diffy))$$

伪代码如下

//上一线减去下一线
diffX = cloud->points[upperInd].x - cloud->points[lowerInd].x;
diffY = cloud->points[upperInd].y - cloud->points[lowerInd].y;
diffZ = cloud->points[upperInd].z - cloud->points[lowerInd].z;

angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
// sensorMountAngle表示如果激光雷达安装相对地面有一定倾斜角
if (abs(angle - sensorMountAngle) <= 10){
	groundLabel.at(i,j) = true;
}

3 地面拟合法

• ref： @Su Pengpeng’s Notes

3.1 平面方程和平面法向量的表示

对靠近地面的的n个点，计算其协方差矩阵。对协方差矩阵进行SVD分解，可以得到对应的特征值和特征向量。其中，最小特征值对应的特征向量就是地面平面的法向量。

求证：平面Ax+By+Cz+D=0的法向量为（A,B,C）.

证明：假设$(x_1,y_1,z_1),(x_2,y_2,z_2)$是当前平面上的两个点。

则有：$A{x}_1+B{y}_1+C{z}_1+D=0$,$A{x}_2+B{y}_2+C{z}_2+D=0$,所以两式相减，可得：

$A(x_1-x_2)+B(y_1-y_2)+C(z_1-z_2)=0$,即
$$\left[\begin{array}{ccc}A& B& C\end{array}\right]\left[\begin{array}{c}(x_1-x_2)\\ (y_1-y_2)\\ (z_1-z_2)\end{array}\right]=0$$
右边的矩阵表示平面上的任一点，且该式对平面上的任意两点都成立。

所以$n=(A,B,C)$即是所在平面的法向量。

3.2 拟合地面方程Ax+By+Cz+d=0

1. 取n个z值最小的点，认为其是地面点

取n个地面点，计算这n个点的协方差矩阵$Cov$，然后对其做SVD分解，得到其在各个分量。最小奇异值所对应的向量便是地面的法向量$n$.

由前面的证明可知：$n=(A，B，C)$

2. 对n个靠近地面的点遍历加和，计算一个均值$\bar{X}=(\bar{x},\bar{y},\bar{z})$。认为此均值带入地面所在方程
   $$\begin{gathered} A\bar{x}+B\bar{y}+C\bar{z}+D\approx 0 \\ 即：A\bar{x}+B\bar{y}+C\bar{z}\approx -D \end{gathered}$$

此时$-D$的值已知。

此时，均值$\bar{X}$因为是n个点的均值，默认是最靠近地面所在平面的点。其他所有的n个点，都可以认为更偏离所拟合的平面。即：
$$\begin{gathered} A\bar{x}+B\bar{y}+C\bar{z}+D\pm \delta \approx 0 \\ 即：A\bar{x}+B\bar{y}+C\bar{z}\approx -D\pm \delta \end{gathered}$$
因此，在对\velodyne_points中所有的topic进行筛选地面点的过程中，所有的点$X_i=(x_i,y_i,z_i)$带入式(3)所得到的值符合以下约束：
$$A{x}_i+B{y}_i+C{z}_i\in [-D-\delta ,-D+\delta ]$$
此时，$\delta$的值需要自己设定，代表了对地面点的筛选条件。

算法流程，此图引自原论文

此图引自@Liu Yingtian Master Thesis

• 代码实现： https://github.com/VincentCheungM/Run_based_segmentation
• 论文：Fast Segmentation of 3D Point Clouds: A Paradigm on LiDAR Data for Autonomous Vehicle Applications

4 条件筛选法

顾名思义，就是根据地面点的特征，加以限制条件筛选得到。这个方法是在hdl_graph_slam中见到的方法。

算法流程

• 根据传感器距离地面的安装高度，进行高度范围的取点，保证地面点包含其中
• 根据地面点的法线应该和z轴方向夹角比较小取出地面点
• 使用RANSAC算法拟合地平面参数
• 验证算出的法线是否是真正的地面法线：是否和z轴夹角小于10°

直接上代码，注释很详细

boost::optional<Eigen::Vector4f> detect(const pcl::PointCloud<PointT>::Ptr &cloud) const {
        // compensate the tilt rotation
        // 激光雷达安装如果有倾斜的话，做一个矫正
        Eigen::Matrix4f tilt_matrix = Eigen::Matrix4f::Identity();
        tilt_matrix.topLeftCorner(3, 3) = Eigen::AngleAxisf(tilt_deg*M_PI/180.0f, Eigen::Vector3f::UnitY()).toRotationMatrix();

        // filtering before RANSAC (height and normal filtering)
        // 根据传感器距离地面的安装高度，进行高度范围的取点，保证地面点包含其中
        pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>);
        pcl::transformPointCloud(*cloud, *filtered, tilt_matrix);
        filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height + height_clip_range), false);
        filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height - height_clip_range), true);

        // 根据地面点的法线应该和z轴方向夹角比较小取出地面点
        if (use_normal_filtering) {
            filtered = normal_filtering(filtered);
        }

        pcl::transformPointCloud(*filtered, *filtered, static_cast<Eigen::Matrix4f>(tilt_matrix.inverse()));

        if (floor_filtered_pub.getNumSubscribers()) {
            filtered->header = cloud->header;
                floor_filtered_pub.publish(filtered);
        }

        // too few points for RANSAC
        if (filtered->size() < floor_pts_thresh) {
            return boost::none;
        }


        // 使用RANSAC算法拟合地平面参数
        pcl::SampleConsensusModelPlane<PointT>::Ptr model_p(new pcl::SampleConsensusModelPlane<PointT>(filtered));
        pcl::RandomSampleConsensus<PointT> ransac(model_p);
        ransac.setDistanceThreshold(0.1);
        ransac.computeModel();

        pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
        ransac.getInliers(inliers->indices);

        // too few inliers
        if (inliers->indices.size() < floor_pts_thresh) {
            return boost::none;
        }

        // 验证算出的法线是否是真正的地面法线：是否和z轴夹角小于10°
        Eigen::Vector4f reference = tilt_matrix.inverse()*Eigen::Vector4f::UnitZ();

        Eigen::VectorXf coeffs;
        ransac.getModelCoefficients(coeffs);
				
        double dot = coeffs.head<3>().dot(reference.head<3>());
        if (std::abs(dot) < std::cos(floor_normal_thresh*M_PI/180.0)) {
            // the normal is not vertical
            return boost::none;
        }

        // make the normal upward
        if (coeffs.head<3>().dot(Eigen::Vector3f::UnitZ()) < 0.0f) {
            coeffs *= -1.0f;
        }

        if (floor_points_pub.getNumSubscribers()) {
            pcl::PointCloud<PointT>::Ptr inlier_cloud(new pcl::PointCloud<PointT>);
            pcl::ExtractIndices<PointT> extract;
            extract.setInputCloud(filtered);
            extract.setIndices(inliers);
            extract.filter(*inlier_cloud);
            inlier_cloud->header = cloud->header;

            floor_points_pub.publish(inlier_cloud);
        }

        return Eigen::Vector4f(coeffs);
    }

5 总结

没有哪一种地面检测算法是最完美的，也不可能覆盖到方方面面。总的来说，根据传感器的特点、环境特点进行地面点的去除是一个基本思路。