1. 针对性设计的平面提取方法
因为这里使用的传感器其实一个组装的VLP16型多线激光雷达+相机+imu的背包激光扫描仪，作者考虑到LiDAR有一定的安置误差(提前肯定标定了的，e.g.倾角)，而本文使用的又是室内场景平面，因此需要根据一个仪器安装的先验确定z轴方向指向哪里（默认传感器的坐标系为右前上，z轴指向天空）。得到这个倾角主要是为了修正LiDAR坐标系的z轴方向。这对后续的高度滤波和地面点判断都有一定的影响。
在提取平面时，为了提升效率，首先基于扫描仪的高度，通过高度滤波将一定高度之外的点去掉，同时对剩下的点云计算法向量（通过提前性的点云剔除，可以提升这里的计算效率）进行过滤，和z方向进行比较，保留地面点（和z轴夹角小就说明方向近似），用RANSAC的方式拟合平面参数方程。 这样就可以以较快的速度从单帧点云中提取出一个合适的平面。注意：原文中写的是直接用RANSAC提取平面的，这个非常的费时间，实际操作并不是这样子来的。
平面参数方程采用一般形式进行表达： \[ \begin{align} \pi &:= n_x*x+n_y*y+n_z*z+d=0 \\ \pi &:= [(n_x,n_y,n_z),d] \end{align} \]
2. 平面节点的构建以及优化
平面参数拿到之后如何抽象成为图中的一个节点，并参与优。在优化时，其实并不是所有帧都提取平面的，作者每隔10s从一个关键LiDAR帧中提取一个平面，然后引入到PGO中。不然也算不过来。
平面约束从何而来
约束嘛，肯定得有参考，作者的实验场景是室内，提取的还是地面，可想而知，约束是通过__该平面__（提取平面）的参数与__世界坐标系下的xoy平面平行的一个平面__（参考平面）确定的。更具体地，假设在t时刻从当前LiDAR激光帧中提取的平面方程为\(\pi(t):[n_p,d_p]\)(local坐标系下,这里省略了\(n_p(t),d_p(t)为n_p,d_p\))，而给定的世界坐标系(canonical or world)下的参考平面方程为\(\pi(0):[n_b,d_b]\)，也就是\(\pi(0):[(0,0,1),0]\)(和其他方向的平面需要设置不同的法向量朝向)。即： \[ \begin{align} nx_p*x+ny_p*y+nz_p*z+d_p=0 \\ \\ nx_b*x+ny_b*y+nz_b*z+d_b=0 \end{align} \] 这两个平面之间的差，两个平面之间的差需要比较法向量和截距（排除平行的情况）：
- 如果当前LiDAR帧的位姿估计的准确的话，提取得到的平面的法向量\(n_p\)应该和\(n_b\)是相近的，截距应该是相等的，排除沿着法向方向的平移现象：
- 作者这里参照的其实是一个发表在ICRA2016上的文章Cpa-SLAM1，中的方法完成了两个平面方程的参数化过程，
\[ \pi := [(n_x,n_y,n_z),d]\quad \Longrightarrow \quad\tau(\pi)=[arctan(\frac{n_y}{n_x}),arctan(\frac{n_z}{|n|}),d] \]
\(arctan(\frac{n_y}{n_x}),arctan(\frac{n_z}{|n|})\)分别表示的是球坐标系下的azimuth（方向角）和elevation（高度角）\(|n|\)应该是在水平面内的投影。具体这么参数化的好处是啥呢，暂时还没有了解清楚，TODO...
此外还需要注意的是注意：
- 平面约束是如何与节点位姿建立关系呢？（这个是平面可以作为约束参与优化的基础）
- 这里就涉及到一个坐标系转换的问题，一个是LiDAR点云帧的local系(因为点云就是从这个里面提取出来的)，一个是假定的canonical（world）系，两个需要在同一的坐标系下才能做运算，有两种思路，1）都变换到world下（koide采用的就是这种思路）；或者都同一变到canonical(world)坐标系下。这里就需要知道每一个LiDAR点云帧都保留了其在world坐标系下（相对于第一帧）的位姿\(T_{iw}\),通过\(T_{iw}\)可以将平面\(\pi(0)\)变换到\(\pi(t)\)的坐标系下，实现统一。
- 如何对一个平面进行变换？假设现在有了一个平面\(\pi\)，在经过T变换之后，对应的平面参数\(\pi^{'}\)应该是什么呢？如下所示。证明参见
\[ \pi^{'}=(T^{-1})^T·\pi=(T^T)^{-1}·\pi = \left[ \begin{array}{cc} R^{T} & 0^{T} \\ -R^{T}t & 1\end{array} \right]· \left[ \begin{array}{cc} n \\ d \end{array} \right] = \left[ \begin{array}{cc} R^Tn \\ d- R^Ttn \end{array}\right] \]
- hdl_graph_slam中使用的参考平面为世界坐标系下的参考平面为\(\pi(0):[(0,0,1),0]\)，而LiDAR激光帧的位姿\(T_{iw}\)表示的就是世界坐标系下的坐标，只是相对于第一帧而言的，因此从local系提取得到的平面参数中法向量本身可以看成是和\([0,0,1]\)的差，那么在计算误差时直接就可以将\(\tau(t)\)看成是\(e\)误差，只需要对截距t做变换\(T_{iw}\)即可了，而这里使用的是地面，d=0，就省略了这一部分在实际代码中有所体现。（PS：更加复杂的情况需要修改g2o中的代码才行，不太行的样子...）
误差方程的构建
误差=观测值-预测值（通常是这样的，如果反过来只会影响jacobian的方程，对结果没有影响）
这里将变换到当前LiDAR坐标系下的\(\pi(0)\)作为了观测值，提取的平面作为了预测值（按照我的理解，和他正好是反的，但是对结果没有啥影响） \[ \begin{aligned} 一直和z方向比:\quad e = \tau(\pi(0)^{'}) - \tau(\pi(t)) \\ 和前一个提取的平面参数比:\quad e = \tau(\pi(t-1)^{'}) - \tau(\pi(t)) \end{aligned} \] 最小二乘优化它的二次型即可。 \[ E = e^T \Omega e \]
 |
 |
|---|---|
| a | b |
平面参与优化过程
hdl_graph_slam中平面检测和LiDAR关键帧是放在独立的deque中存储的，又通过消息的发布和接收进行数据共享。运行launch时，会自动开启好几个nodelet，e.g.HdlGraphSlamNodelet, floor_detection_nodelet, ScanMatchingOdometryNodelet, PrefilteringNodelet。这几个nodelet处于一个进程中，但是可以独立干自己的事情，通过消息的publish和subscribe进行完成消息共享。
前端里程计在不断的完成关键帧的位姿估计，并存放在队列中，平面检测是每隔10s进行的（提取平面），也将估计结果存放在另一个队列中（1hz<10hz的LiDAR帧率，而且LiDAR关键帧更低）。将选择得到的关键帧的与时间戳绑定，平面检测结果也和时间戳绑定，这样就可以通过时间上的对比完成两个数据之间的同步（或者称为数据关联）。完成平面提取结果和关键LiDAR帧的绑定之后，就可以构建平面约束了。引入图中的平面节点是canonical（world）坐标系下的平面（参考平面），也就是\(\pi(0):[(0,0,1),0]\),利用与之时间匹配的LiDAR的pose，将参考平面和提取平面坐标系统一起来，构建平面约束边，误差计算方法是：首先将参考平面通过当前LiDAR帧pose（local系，表示世界坐标系下相对于第一帧的位姿？）的逆，变换到LiDAR点云的local系，然后和估计RANSAC估计出来的平面（本身就在local系下）做差。这一套都完成之后就构建好了一条平面边，加入到图中。如上图a
3. 源码解析
1. 平面提取
其实代码这部分和上述描述一致，注释好的代码见本仓库app/floor_detection_nodelet.cpp。
2. 引入平面节点的过程
实际引入过程和上述描述一致，注释好的代码见本仓库app/HdlGraphSlamNodelet->optimization_timer_callback()->flush_floor_queue()。
3. g2o已经实现好的平面节点以及边
- 注意：这个EdgeSE3Plane是koide自己实现的，可能和原始g2o中的不太一样。
1 | |
- g2o定义的Plane3D给出了平面边误差方程的计算实现代码，其中实现的挺好的，就一个成员是Eigen::Vector4d,包含了从一个平面参数中提取球坐标系下方向角azimuth，高度角elevation等过程，重点关注求差__ominus，运算符*__重载过程（和上述讲的平面变换的过程一致）。
1 | |
4. Reference
- 1.Ma L, Kerl C, Stückler J, et al. CPA-SLAM: Consistent plane-model alignment for direct RGB-D SLAM[C]//2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2016: 1285-1291. ↩︎
