hdl_graph_slam源码解析（四）

4. floor detection

4.1 地面检测约束

在hdl_graph_slam算法中，地面检测是一种重要的约束，可用来增加位姿(pose graph)中的约束从而提高定位与建图的精度。该算法假设了全局一致地面的存在，然后根据每次检测到的地面参数对当前的位姿进行校正。这可以理解为，每当有一帧新的激光雷达数据时，检测一下地面特征，然后根据检测的地面特征，对位姿进行修正。

为了让上一段的描述看起来更科学严谨一点，我们还是用数学的方法解释一下吧。为了保证坐标变换的一致性，首先当然要假定一个全局坐标系XYZ，为不失一般性，假设全局一致地面在坐标系XYZ内的参数方程为：
$$Ax+By+Cz+D=0$$

当然还有激光雷达的本体坐标系X’Y’Z’，以及激光雷达在全局坐标系内的位姿也即坐标系X’Y’Z’到XYZ的变换矩阵T，则根据全局地面的参数方程，以及激光雷达的位姿，可以求解除该全局地面在激光雷达坐标系内的参数方程。为方便求解，假设该全局地面上一点的为$\vec{p}=[x_p,y_p,z_p{]}^T$，法向量为$\vec{n}=[x_n,y_n,z_n{]}^T$，根据面内一点$\vec{p}$和法向量$\vec{n}$可以求出其参数方程为：
$$(\vec{x}-\vec{p})\cdot \vec{n}=0$$
化简之后得到：
$$x_{n}x+y_{n}y+z_{n}z-(x_n{x}_p+y_n{y}_p+z_n{z}_p)=0$$
下一步就是根据激光雷达的位姿 T = { R , t } T=\{R,t\} T={R,t}，求该全局地面在激光雷达坐标系内的参数方程，同样通过向量的方法求解较为简单。首先点$\vec{p}$在激光雷达坐标系内的坐标为$\overrightarrow{p^{\prime }}=R\vec{p}+t$，法向量$\vec{n}$在激光雷达坐标系内的表示为:
$$\overrightarrow{n^{\prime }}=R[x_n,y_n,z_n{]}^T+t-(R[0,0,0{]}^T+t)=R[x_n,y_n,z_n{]}^T$$
则该全局地面在激光雷达坐标系内的参数方程可表示为：
$$(x-\overrightarrow{p^{\prime }})\cdot \overrightarrow{n^{\prime }}=0$$
化简后得到：
$$x_{n^{\prime }}x+y_{n^{\prime }}y+z_{n^{\prime }}z-(x_{n^{\prime }}{x}_{p^{\prime }}+y_{n^{\prime }}{y}_{p^{\prime }}+z_{n^{\prime }}{z}_{p^{\prime }})=0\text{\hspace{1em}(1)}$$
即根据上式，我们可以利用激光雷达的位姿估计$T$和全局一致地面方程，得到该地面在激光雷达坐标系内的参数方程表示。而另一方面，我们又进行了地面检测，注意该地面检测也是在激光雷达坐标系内进行的，并得到检测到的地面参数方程为：
$$x_{d}x+y_{d}y+z_{d}z+D_d=0\text{\hspace{1em}(2)}$$
此时，我们得到了两个地面，且这两个地面都是定义在激光雷达坐标系内的，若激光雷达的位姿$T$和基于激光雷达的地面检测都是没有误差的，那么公式(1)和(2)应该是完全一样的。然而理想总是美好的，现实有的时候总是很残酷，不仅位姿$T$估计存在误差，同样地面检测由于受激光测量误差以及环境的影响也必然是不准确的。因此，公式(1)和(2)对应的平面一定是不一致的。那么该怎样衡量这种不一致也就是误差项呢？

考虑到一个法向量和平面内的一点即可唯一的确定一个平面，因此首先应该横向两个平面法向量的差异。举个栗子，若一个平面的法向量为$[0,0,1{]}^T$，而另一个平面的法向量为$[1,0,0{]}^T$，则这两个平面是互相垂直的，也即是不一致的。为定量的描述这种差异，我们可以将构造一个旋转矩阵，将公式(1)对应的法向量旋转至$X$轴，即：
$$\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]=R\left[\begin{array}{c}{x}_{n^{\prime }}\\ y_{n^{\prime }}\\ z_{n^{\prime }}\end{array}\right]$$
然后将该旋转矩阵再作用于公式(2)对应的法向量即:
$$\left[\begin{array}{c}{x}_{d^{\prime }}\\ y_{d^{\prime }}\\ z_{d^{\prime }}\end{array}\right]=R\left[\begin{array}{c}{x}_d\\ y_d\\ z_d\end{array}\right]$$
然后就可以利用两个变换后的法向量的夹角来定义误差函数：

$$\alpha =arctan2(y_{d^{\prime }},x_{d^{\prime }}),\beta =arctan2(z_{d^{\prime }},\sqrt{y_{d^{\prime }}^2+x_{d^{\prime }}^2})$$
衡量了法向量的差异后，我们发现即使两个平面的法向量完全一致，两个平面还存在平行的情况对吧，因此呢还需要确定两个平面都经过同一个点。这就比较简单啦，我们只需要衡量两个平面在$Z$轴的截距就可以了。记$D^{\prime }=-(x_{n^{\prime }}{x}_{p^{\prime }}+y_{n^{\prime }}{y}_{p^{\prime }}+z_{n^{\prime }}{z}_{p^{\prime }})$，则整个误差函数就可以定义为：
$$e_{rr}=[\alpha ,\beta ,D^{\prime }-D_d{]}^T$$
对于基于pose graph的SLAM来讲，通常目标函数是$x^T\Omega x$这种二次型的形式，因此上式还可以结合信息矩阵进一步写为：
$$e=e_{rr}^T\Omega e_{rr}$$
好了，地面检测结果如何为整个pose graph添加约束已经描述清楚了，接下来就进入正题了，即hdl_graph_slam中是如何利用激光雷达的数据进行地面检测的呢？

4.2 基于激光雷达的地面检测

hdl_graph_slam中地面检测主要由上图中的三个模块构成，首先根据刚性链接假设，认为激光雷达与地面的距离在某一个阈值范围内，从而滤出掉过高或过低的点云。然后估计每一个点的法向量，并滤出那些与地面法向量夹角过大如垂直的点云。最后剩下的点云就是地面以及地面附件的扫描点，然后利用RANSAC算法就可以将地面提取出来。不废话了，直接来看源码吧。
hdl_graph_slam中的地面检测部分代码主要定义在detect函数里面，还是比较容易理解的：

boost::optional<Eigen::Vector4f> detect(const pcl::PointCloud<PointT>::Ptr& cloud){
	//倾斜补偿,tilt_deg是激光雷达的先验倾斜角度
	Eigen::Matrix4f tilt_matrix = Eigen::Matrix4f::Identity();
	tilt_matrix.topLeftCorner(3, 3) = Eigen::AngleAxisf(tilt_deg * M_PI / 180.0f, Eigen::Vector3f::UnitY()).toRotationMatrix();
	//高度滤波
	pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>);
    pcl::transformPointCloud(*cloud, *filtered, tilt_matrix);
    //滤出高度高于sensor_height+height_clip_range的点云
    filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height + height_clip_range), false);
    //滤出高度低于sensor_height-height_clip_range的点云
    filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height - height_clip_range), true);
    //法向量滤波
    if(use_normal_filtering) {
      filtered = normal_filtering(filtered);
    }
    //将倾斜补偿过的点云变换回到原坐标系内
    pcl::transformPointCloud(*filtered, *filtered, static_cast<Eigen::Matrix4f>(tilt_matrix.inverse()));
	//排除点云过少的情况
	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();
    //...
    ......
}

地面检测的主要流程就是上示的代码，下图是笔者利用hdl_graph_slam中的地面检测算法在自己校园中实验的结果图，其中图(a)是原始的velodyne点云，图(b)是检测到的地面。

好了，地面检测部分的原理、代码以及实验效果都已经描述清楚了。下一节将对hdl_graph_slam中的回环检测以及后端优化部分进行解析。