无人驾驶汽车系统入门（二十七）——基于地面平面拟合的激光雷达地面分割方法和ROS实现
无人驾驶汽车系统入门（二十七）——基于地面平面拟合的激光雷达地面分割方法和ROS实现
在博客的第二十四篇中，我们介绍了一种基于射线的地面过滤方法，此方法能够很好的完成地面分割，但是存在几点不足：第一，存在少量噪点，不能彻底过滤出地面;第二，非地面的点容易被错误分类，造成非地面点缺失;第三，对于目标接近激光雷达盲区的情况，会出现误分割，即将非地面点云分割为地面。通过本文我们一起学习一种新的地面分割方法，出自论文：Fast segmentation of 3D point clouds: A paradigm on LiDAR data for autonomous vehicle applications，在可靠性、分割精度方面均优于基于射线的地面分割方法。我们将其称之为地面平面拟合方法。
地面平面拟合（Ground Plane Fitting）
我们采用平面模型（Plane Model）来拟合当前的地面，通常来说，由于现实的地面并不是一个“完美的”平面，而且当距离较大时激光雷达会存在一定的测量噪声，单一的平面模型并不足以描述我们现实的地面。要很好的完成地面分割，就必须要处理存在一定坡度变化的地面的情况（不能将这种坡度的变化视为非地面，不能因为坡度的存在而引入噪声），一种简单的处理方法就是沿着x方向（车头的方向）将空间分割成若干个子平面，然后对每个子平面使用地面平面拟合算法（GPF）从而得到能够处理陡坡的地面分割方法。
那么如何进行平面拟合呢？其伪代码如下：
我们来详细的了解这一流程：对于给定的点云 P P P ,分割的最终结果为两个点云集合 P g P_g Pg :地面点云 和 P n g P_{ng} Png ：非地面点云。该算法有四个重要的参数，分别是：
- N i t e r N_{iter} Niter : 进行奇异值分解（singular value decomposition，SVD）的次数，也即进行优化拟合的次数
- N L P R N_{LPR} NLPR : 用于选取LPR的最低高度点的数量
- T h s e e d Th_{seed} Thseed : 用于选取种子点的阈值，当点云内的点的高度小于LPR的高度加上此阈值时，我们将该点加入种子点集
- T h d i s t Th_{dist} Thdist : 平面距离阈值，我们会计算点云中每一个点到我们拟合的平面的正交投影的距离，而这个平面距离阈值，就是用来判定点是否属于地面
种子点集的选取
我们首先选取一个种子点集(seed point set)，这些种子点来源于点云中高度（即z值）较小的点，种子点集被用于建立描述地面的初始平面模型，那么如何选取这个种子点集呢？我们引入最低点代表（Lowest Point Representative, LPR）的概念。LPR就是 N L P R N_{LPR} NLPR 个最低高度点的平均值，LPR保证了平面拟合阶段不受测量噪声的影响。这个LPR被当作是整个点云 P P P 的最低点，点云P中高度在阈值 T h s e e d Th_{seed} Thseed 内的点被当作是种子点，由这些种子点构成一个种子点集合。
种子点集的选取代码如下：
/*
@brief Extract initial seeds of the given pointcloud sorted segment.
This function filter ground seeds points accoring to heigt.
This function will set the `g_ground_pc` to `g_seed_pc`.
@param p_sorted: sorted pointcloud
@param ::num_lpr_: num of LPR points
@param ::th_seeds_: threshold distance of seeds
@param ::
*/
void PlaneGroundFilter::extract_initial_seeds_(const pcl::PointCloud &p_sorted)
{
// LPR is the mean of low point representative
double sum = 0;
int cnt = 0;
// Calculate the mean height value.
for (int i = 0; i < p_sorted.points.size() && cnt < num_lpr_; i++)
{
sum += p_sorted.points[i].z;
cnt++;
}
double lpr_height = cnt != 0 ? sum / cnt : 0; // in case divide by 0
g_seeds_pc->clear();
// iterate pointcloud, filter those height is less than lpr.height+th_seeds_
for (int i = 0; i < p_sorted.points.size(); i++)
{
if (p_sorted.points[i].z < lpr_height + th_seeds_)
{
g_seeds_pc->points.push_back(p_sorted.points[i]);
}
}
// return seeds points
}
输入是一个点云，这个点云内的点已经沿着z方向（即高度）做了排序，取 num_lpr_ 个最小点，求得高度平均值 lpr_height（即LPR），选取高度小于 lpr_height + th_seeds_的点作为种子点。
平面模型
接下来我们需要确定一个平面，点云中的点到这个平面的正交投影距离小于阈值 T h d i s t Th_{dist} Thdist 则认为该点属于地面，否则属于非地面，我们采用一个简单的线性模型用于平面模型估计，如下：
a x + b y + c z + d = 0 ax+by+cz+d = 0 ax+by+cz+d=0
也即：
n T x = − d \bm{n}^T \bm{x} = -d nTx=−d
其中 n = [ a , b , c ] T \bm{n} = [a, b, c]^T n=[a,b,c]T , x = [ x , y , z ] T \bm{x} = [x, y, z]^T x=[x,y,z]T ,通过初始点集的协方差矩阵 C ∈ R 3 x 3 C \in R^{3x3} C∈R3x3 来求解 n \bm{n} n （即a, b, c), 从而确定一个平面, 我们采用种子点集 S ∈ R 3 S \in R^{3} S∈R3 作为初始点集，其协方差矩阵为：
C = ∑ i = 1 : ∣ S ∣ ( s i − s ^ ) ( s i − s ^ ) T C = \sum_{i=1:|S|}(s_i - \widehat{s})(s_i - \widehat{s})^T C=i=1:∣S∣∑(si−s )(si−s )T
其中， s ^ \widehat{s} s 是所有点的均值。这个协方差矩阵 C C C 描述了种子点集的散布情况，其三个奇异向量可以通过奇异值分解（Singular Value Decomposition,SVD）求得，这三个奇异向量描述了点集在三个主要方向的散布情况。由于是平面模型，垂直于平面的法向量 n \bm{n} n 表示具有最小方差的方向，可以通过计算具有最小奇异值的奇异向量来求得。
那么在求得了 n \bm{n} n 以后， d d d 可以通过代入种子点集的平均值 s ^ \widehat{s} s (它代表属于地面的点) 直接求得。整个平面模型计算代码如下：
/*
@brief The function to estimate plane model. The
model parameter `normal_` and `d_`, and `th_dist_d_`
is set here.
The main step is performed SVD(UAV) on covariance matrix.
Taking the sigular vector in U matrix according to the smallest
sigular value in A, as the `normal_`. `d_` is then calculated
according to mean ground points.
@param g_ground_pc:global ground pointcloud ptr.
*/
void PlaneGroundFilter::estimate_plane_(void)
{
// Create covarian matrix in single pass.
// TODO: compare the efficiency.
Eigen::Matrix3f cov;
Eigen::Vector4f pc_mean;
pcl::computeMeanAndCovarianceMatrix(*g_ground_pc, cov, pc_mean);
// Singular Value Decomposition: SVD
JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU);
// use the least singular vector as normal
normal_ = (svd.matrixU().col(2));
// mean ground seeds value
Eigen::Vector3f seeds_mean = pc_mean.head<3>();
// according to normal.T*[x,y,z] = -d
d_ = -(normal_.transpose() * seeds_mean)(0, 0);
// set distance threhold to `th_dist - d`
th_dist_d_ = th_dist_ - d_;
// return the equation parameters
}
优化平面主循环
在确定如何选取种子点集以及如何估计平面模型以后，我们来看一下整个算法的主循环，以下是主循环的代码：
extract_initial_seeds_(laserCloudIn);
g_ground_pc = g_seeds_pc;
// Ground plane fitter mainloop
for (int i = 0; i < num_iter_; i++)
{
estimate_plane_();
g_ground_pc->clear();
g_not_ground_pc->clear();
//pointcloud to matrix
MatrixXf points(laserCloudIn_org.points.size(), 3);
int j = 0;
for (auto p : laserCloudIn_org.points)
{
points.row(j++) << p.x, p.y, p.z;
}
// ground plane model
VectorXf result = points * normal_;
// threshold filter
for (int r = 0; r < result.rows(); r++)
{
if (result[r] < th_dist_d_)
{
g_all_pc->points[r].label = 1u; // means ground
g_ground_pc->points.push_back(laserCloudIn_org[r]);
}
else
{
g_all_pc->points[r].label = 0u; // means not ground and non clusterred
g_not_ground_pc->points.push_back(laserCloudIn_org[r]);
}
}
}
得到这个初始的平面模型以后，我们会计算点云中每一个点到该平面的正交投影的距离，即 points * normal_，并且将这个距离与设定的阈值 T h d i s t Th_{dist} Thdist (即th_dist_d_) 比较，当高度差小于此阈值，我们认为该点属于地面，当高度差大于此阈值，则为非地面点。经过分类以后的所有地面点被当作下一次迭代的种子点集，迭代优化。
ROS实践
下面我们编写一个简单的ROS节点实现这一地面分割算法。我们仍然采用第24篇博客（激光雷达的地面-非地面分割和pcl_ros实践）中的bag进行实验，这个bag来自Velodyne的VLP32C，在此算法的基础上我们要进一步滤除雷达过近处和过高处的点，因为雷达安装位置的原因，近处的车身反射会对Detection造成影响，需要滤除; 过高的目标，如大树、高楼，对于无人车的雷达感知意义也不大，我们对过近过高的点进行切除，代码如下：
void PlaneGroundFilter::clip_above(const pcl::PointCloud::Ptr in,
const pcl::PointCloud::Ptr out)
{
pcl::ExtractIndices cliper;
cliper.setInputCloud(in);
pcl::PointIndices indices;
#pragma omp for
for (size_t i = 0; i < in->points.size(); i++)
{
if (in->points[i].z > clip_height_)
{
indices.indices.push_back(i);
}
}
cliper.setIndices(boost::make_shared(indices));
cliper.setNegative(true); //ture to remove the indices
cliper.filter(*out);
}
void PlaneGroundFilter::remove_close_far_pt(const pcl::PointCloud::Ptr in,
const pcl::PointCloud::Ptr out)
{
pcl::ExtractIndices cliper;
cliper.setInputCloud(in);
pcl::PointIndices indices;
#pragma omp for
for (size_t i = 0; i < in->points.size(); i++)
{
double distance = sqrt(in->points[i].x * in->points[i].x + in->points[i].y * in->points[i].y);
if ((distance < min_distance_) || (distance > max_distance_))
{
indices.indices.push_back(i);
}
}
cliper.setIndices(boost::make_shared(indices));
cliper.setNegative(true); //ture to remove the indices
cliper.filter(*out);
}
编译并运行该ROS节点，使用我们在第二十四篇博客中提供的bag进行测试，得到如下结果：
