[笔记]三维激光SLAM学习——LOAM总结&一些拓展

[笔记]三维激光SLAM学习——LOAM总结&一些拓展

概述

本篇文章主要总结和拓展一些代码中没体现到的细节。

1、一些基础

1.1 运动补偿

之前提到，论文中假设两帧间运动为匀速运动模型，然后使用线性插值计算。也使用这个运动补偿对激光点云进行去畸变。下面我们仔细分析一下。
运动中的载具：
在运动的汽车上，比如说速度为10m/s，直行，无旋转运动。激光扫描频率为10hz,，也就是一帧0.1秒，雷达在这0.1秒内实现了约360度的旋转。
那么0°和360°的激光点,，分别是在时刻0秒和时刻0.1秒扫描的。而第0秒和0.1秒，载具移动了10米/秒*0.1秒=1米。
运动中的激光点云测量：
激光返回的点云中的点，描述的是激光雷达坐标系下的坐标，假设0秒时,激光雷达扫描得到载具正前方一百米处的一个点A, 记下其在雷达坐标系下的坐标为(100, 0, 0), 扫描完了一圈, 激光雷达输出一帧点云, 时间戳为0.1秒.
综上所述:
也就是说, 激光雷达在0.1秒时, 输出点A的坐标为(100,0,0),而实际, 在0.1秒时, 汽车已经前进了1米, 点A在0.1秒这个时刻激光坐标系的真实坐标应该是(99, 0, 0)。
若我们知道每个点扫描的具体时间, 比如知道点B是在0.05秒时被扫描到的，结合这0.05秒载具的相对运动， 我们可以对点B的坐标位置进行修复。
同理，包含旋转的情况亦然。

laserOdometry节点在配准前基于IMU或者匀速运动模型实现了运动补偿。

1.2 航迹推算

在没有外部观测的情况下，一般使用航迹推算的方法对位姿进行预测。
已知$t$时刻的pose为$O_t$, 我们可以基于其他的odom信息(比如底盘编码器，激光里程计，视觉里程计，IMU等)进行航位推算。
以底盘为例：假如$[t,t+1]$时刻底盘odom的对应的pose为$O_{t+1}$, 我们可以得到载具在$t$到$t+1$时刻的相对pose变换$T_{t,t+1}$：
由$O_t\ast T_{t,t+1}=O_{t+1}$可得
$$T_{t,t+1}=O_t^{-1}\ast O_{t+1}$$
其中：$T_{t,t+1}$指的是Odom在$t$时刻位姿向$t+1$时刻位姿的相对变换。

由于噪声的引入，odom是包含累计误差的，因此我们只能用$T_{t,t+1}$大致的估算$t+1$时刻的pose。
$t+1$时刻的pose：
$$P_{t+1}=P_t\ast T_{t,t+1}$$
然后，我们可以基于观测来修正这个预测的pose。

航位推算在laserOdometry, laserMapping中用于计算配准优化的初始pose , 在transformMaintenance中为低频odom提供插值得到高频odom。上述代码部分， 有典型的航位推算位姿预测计算方式。

1.3 线束模型

建议去Velodyne官网下载 配合阅读，或使用其他品牌其他线束的手册配合阅读。

论文中称单个线束为一个Scan，对全部16线组成的一帧点云称为一个Sweep。

虽然是用的多线激光雷达，但是LOAM是针对单个Scan提取特征点的，这里主要考虑到 线束间角分辨率(竖直分辨率) 与 单个线内点间角分辨率(水平分辨率) 存在的差异。以VLP16为例：竖直分辨率约为2°(固定），而水平分辨率最大为0.4°(见下图)。

角分辨率越大，代表越远的物体，反射的两点距离越大，中间丢失的信息越多。因此, LOAM没有针对Scan和Scan之间的点的关联性提取和描述特征, 而是直接针对单个Scan提取特征。

LOAM的特征提取基于曲率$c$，只提取两种特征点：corner和surface, 分别对应场景的平缓区域和曲折区域。LOAM没有使用特征描述子(连曲率都没有参与后续的匹配)。从代码中的corner与surface的曲率判断阈值可以看出，LOAM提取的corner和surface特征点的曲率，并没有特别大的差别，这使得LOAM有较强的场景适应性，在场景中比较曲折的区域，corner点会占据主导，而在较为平缓的区域，surface点占据主导。在激光扫描到的一块区域，只要有物体存在，总会提取出几个特征点。

要读懂代码中特征提取中的一些处理，需要弄清楚VLP16扫描时的模型,简单的总结为:

一帧内所有的点，都是按顺序串行扫描的，同一个时间点,只会有一次发送，紧接着一次接收。先从水平第一个角度，一般在0°左右，扫描这个水平角度上竖直方向所有16个点(对应16个SCAN)的深度,当然这16个点也是串行按顺序的，然后转到下一个水平角度，比如0.3°开始，水平分辨率0.4°，那么下个角度就是0.7°，然后1.1°。一直顺时针扫完一圈。完成一个Sweep数据的采集。当然， Velodyne的User Manual里面讲的更清楚。

由于从驱动得到的一个Sweep是以点云的形式输出（也就是一堆点，每个点有XYZI的信息，点和点之间无其他关系信息）, 因此我们并不知道每个点属于哪个Scan, 对应哪个水平角度,因此, 我们需要根据上面的扫描模型去计算每个点的竖直角度和水平角度。

对于竖直角度，可以用来查找这个点属于哪个Scan，我们将所有的点根据竖直角度分配到16个Scan中,每个Scan单独提取特征点。

竖直角度与Scan的对应关系见下表：


对于水平角度, 每个角度对应一个扫描的相对时间, 这在后续的运动补偿中用来去掉点云的畸变。

需要说明，每个Sweep不一定是从水平0°开始的，大概是因为电机一直处于高速旋转中，没有一个复位的过程，雷达只在电机旋转到接近0°时记下当前对应激光点的精确坐标。同样,结束的位置，也不一定时0°。都是在0°周围。而第一个点和最后一个点的水平角度差也就不一定是2π， 而是接近2π的一个值，有可能大于2π，这就带来了一定的复杂性。比如说，起始角度是0rad，结束角度是（2π+0.5）rad。而我们通过点的坐标计算其水平角度，如果得到的角度是0.25rad，由于这是一个归一化的角度，可能是0.25rad，也可能是（0.25+2π）rad归一化得到的，那么这个激光点是开始的时候(0.25)扫描的，还是结束的时候(0.25+2π)时扫描的?

当然上面的描述不够严谨, 实际归一化的角度是在[-π,π]范围内的.请参考代码理解。

所以, 光从点的3d（XYZ）位置, 有时是无法得到其精确的扫描时间的, 我们还需要结合时序信息,因为一个Sweep中返回的点是按时间顺序排列的,所以,时间靠前的0.25,就是一开始扫描的, 而时间靠后的0.25,就是最后扫描的0.25+2π.

代码中用一个变量half_passed解决了此问题。

对于其他型号的激光雷达, 只要理解了其每帧激光扫描的点顺序,就可以很容易的套用。

1.4 VLP-16激光雷达的一些拓展知识

数据格式解析

激光雷达采用UDP协议传输数据，获取激光雷达数据的方法：

1. 自编写代码获取雷达的数据 (ROS的包其实是第三方在GitHub开源，并非官方提供)
2. 通过Wireshark软件实时的获取激光雷达的数据

激光雷达每一帧的数据长度固定为1248字节，其中分别为前42字节的前数据包标识、12组数据包、4字节时间戳和最后两字节雷达型号参数。12组数据包中前两字节为数据包的开始标识（0xFFEE）、接下去两字节为的旋转角度（当前角度）值和连续32*（2字节的距离值+1字节的激光反射强度值）字节的距离信息，其中323字节分别为雷达两次获取探测信息，每个数据包开头所携带的旋转角度是指当前数据包前163字节对应的角度，而后16*3字节对应的旋转角度激光雷达没有直接给出，需要通过计算前后两次旋转角度然后求取平均值获得。
其帧格式如下图所示：

雷达的数据包如下所示：


依据上图数据包的最左边为00000h表示行标识，图一第一行画红线部分FFEE为数据包的开头标识，E063十六进制两字节标识当前选择角度，B6072A三字节前两字节B607标识雷达激光的探测距离信息，2A表示激光反射强度。图二画红线部分最后面六个字节表示数据帧的时间戳和雷达信息参数，其中6D69940F表示时间戳而3722表示雷达的参数信息。

一、旋转角度值的计算

例如：上图第一个数据包的旋转角度为0xE0、0x63

1. 反转两个字节变成十六进制63E0
2. 把63E0变成无符号的十进制为25568
3. 再把25568处于100.0得到255.68，那么得到的值 255.68 就是当前的旋转角度值

二、16线激光分别测得的距离

例如：上一个图第一个数据包第一个激光线的距离，其值为B6072A

1. B6072A其中B607为距离2A为反射强度，首先反转两个距离的字节变成07B6 把07B6变成无符号的十进制为1974
2. 该型号雷达的分辨率为2.0mm，所以激光束测得的距离为1974 * 2mm = 3948mm
3. 3948mm转化为米等于 3.948m

三、获得帧的时间戳和雷达型号参数

例如：上一个图最后六个字节的数据6D69940F3722

1. 前四个字节的数据为时间戳为6D69940F，然后反转顺序0F94696D
2. 0F94696D无符号十进制的值为261384557，单位为us
3. 把261384557除以1000000可获得当前的时间（单位：秒）
4. 后两个字节3722表示雷达的型号和参数，具体的意义如下图所示：
   

四、球坐标系转化为三维直角坐标系

如下图所示：

其中$R$的值为激光雷达测得的与障碍物的直线距离，该值为上述第2点测得的值，其中垂直角度$\omega$可通过查表方法获得，每个激光束对应的角度$\omega$是固定的本文第一张图所示，而$\alpha$则有第1点测得的旋转角度值。已知旋转角度$\alpha$、垂直角度$\omega$和$R$通过图中所述的公式即可求得$X、Y$和$Z$的值。

2、算法理解的一些补充

2.1 scanRegistration特征点提取部分

一、曲率计算方式

以VLP16为例，每一次激光扫描，包含16条线，每条线由1800个点组成，我们对每条线，按照曲率提取特征点，代码上曲率$c$的计算方式为：

• 每条SCAN的边缘5个点不参与特征点选取，因为周边不满足左右各五个点计算曲率的条件。
• 对任意点A，选取左边五个点，右边五个点，共十个点。
• 邻近十个点每个点的$x$坐标与点A的$x$坐标求差,将所有的差求和，得到sx
• 邻近十个点每个点的$y$坐标与点A的$y$坐标求差,将所有的差求和，得到sy
• 邻近十个点每个点的$z$坐标与点A的$z$坐标求差,将所有的差求和，得到sz
• 曲率$c$即为：$$c={sx}^2+{sy}^2+{sz}^2$$

计算完曲率，就可以根据曲率挑选特征点，为了使得在一周360度上有均匀的约束，我们将一条激光线平均分为6块，将块内的点按曲率大小排列，设置一个曲率阈值$t$，比如$t=0.1$，来区分边缘点和平面点，我们设定一个每块的最大点数$N$。

二、边缘点选择条件

• 从曲率最大的点开始,最多选择$N$个, 只有曲率大于$t$的点才能被选取。
• 若一个点周围五个点中已有点被选为边缘点，跳过这个点，从曲率更小的点中选取。

三、平面点选择条件

• 从曲率最小的点开始，最多选择$N$个，只有曲率小于$t$的点才能被选取
• 若一个点周围五个点中已有点被选为平面点，跳过这个点，从曲率更大的点中选取

四、特征点云生成

针对corner点， 源码中取了两种$N$值，生成两份点云。分别为

• corner_sharp
• corner less_sharp

corner_sharp取得$N$值偏小，所以选取的是曲率更大的几个点， 放到一个点云中。
corner_less_sharp选取的$N$值偏大， 选取更多的点，放到另一个点云中。

同理可得： 针对surf点，源码中选取两份点云。分别为

• surf_flat
• surf_less_flat

不过代码具体实施与corner略有不同。具体见代码， 此部分操作很简单， 不再赘述。

corner_less_sharp和surf_less_flat中包含更多的点，用来充当配准时的target；
而corner_sharp和surf_flat则充当source的角色。

2.2 laserOdometry

此节点的功能在前面的文章中已有描述，其中运动补偿只是常规的运动补偿算法。特征点匹配以及优化部分的处理与laserMapping非常相似， 在下一部分laserMapping中重点推一下这部分，可以结合laserMapping中的逻辑来阅读和理解laserOdometry这部分。

如果要用LOAM实现一个定位算法，此部分除了运动补偿时必要的， ICP帧间匹配得到的odom并非必要， 完全可以用底盘编码器，视觉里程计或者IMU等其他传感器替代。帧间匹配odom的意义在于基于纯激光， 无任何外部输入， 也可以得到一个精度一般的odom。

需要说明下本部分的源码中充斥着大量的IMU相关的变量和逻辑处理。实际上IMU只被当成一个odom，用于预测位姿， 以为后续的位姿优化提供初始位姿。
所以：

• 用航位推算的思路去理解IMU相关代码
• 可以尝试着将IMU的代码封装成一个 predictWithIMU() 的接口，整个代码会简单很多

2.3 laserMapping

这里详述个人理解的laserMapping节点的处理流程：

一、位姿预测阶段

见航迹推算这一段， 使用laserOdometry节点输出的odom预测位姿。

二、地图修正阶段

通过LiDAR在map坐标系中的位姿$T_k^{\mathbf{W}}(t_{k+1})$,将LiDAR坐标系下的特征点转到map坐标系下。

• 针对map坐标系下的每个特征点，寻找与之接近的线或者面(corner对应线,，surface对应面)；
• 计算点与线/面的距离,这个距离就是该点对应的loss函数
• loss对位姿$T$求雅可比矩阵$J$,使用高斯牛顿的方法（代码中貌似使用这个方法，并没看到LM法痕迹）迭代优化$T$减少loss函数直到收敛

三、建图阶段

基于位姿将特征点云分别拼接corner地图和surface地图。只保留周围一定距离范围内的地图（一个圆柱，一个立方体，一个球体，都是OK的）。源码中是以立方体的形式。

Paper与代码的差别:
paper中使用LM方法进行优化,而代码中使用的是高斯牛顿法.
paper中使用的是angle-axis进行优化,而代码中使用的是旋转矩阵直接对欧拉角求导,最终优化的是欧拉角.
当然， 这不代表作者没有写基于LM优化angle-axis的代码。
引自：LOAM细节分析

2.4 论文中一些符号定义和公式推导

1. $X_{(k+1,i)}^L=[x,y,z{]}^T$即为$i$在LiDAR坐标系下，$k-1$时刻的三维直角坐标。这里的点指的是角特征和平面特征。
2. $T_{(k+1)}^W=\left(\begin{array}{cc}R& t\\ 0& 1\end{array}\right)$表示在世界坐标系下，k+1时刻，LiDAR的pose，其中$R$为对应欧拉角在三个轴的转角的旋转矩阵，这样表示是为了与代码对应，$t$指的是平移向量。这个位姿是我们后续迭代优化的初始位姿，由上一帧位姿predict得到。在原始版本的代码中, predict基于匀速运动模型或者IMU数据实现，当然我们也可以使用其他的里程计(比如车轮编码器,或者视觉里程计)来实现。
3. 平移向量$t$：$t=[t_x,t_y,t_z{]}^T$
4. 旋转矩阵$R$：下面推导
5. $E_{k+1}$表示k+1时刻这一帧激光的所有corner点
6. $H_{k+1}$表示k+1时刻这一帧激光的所有surface点
7. $d_{\epsilon }$表示一个corner点与匹配到的地图中的直线的距离，$d_{\eta }$表示一个surface点与匹配到的地图中的平面的距离，因为角点与平面点的过程基本一致，后面统一用$d$表示。($d=\sum d_{\epsilon }+\sum d_{\eta }$)
8. $m$为地图

LOAM对应的旋转矩阵推导

LiDAR&IMU坐标系：x轴向前，y轴向左，z轴向上的右手坐标系

LOAM坐标系：z轴向前，x轴向左，y轴向上的右手坐标系

如红框处：

右手坐标系下的基础旋转矩阵包括以下三个：

• 在右手系中绕$X$轴旋转$ex°$ 对应的矩阵$R_x$:
  $$R_x=\left[\begin{array}{ccc}1& 0& 0\\ 0& cosex& -sinex\\ 0& sinex& cosex\end{array}\right]$$
• 在右手系中绕$Y$轴旋转$ey°$ 对应的矩阵$R_y$:
  $$R_y=\left[\begin{array}{ccc}cosey& 0& siney\\ 0& 1& 0\\ -siney& 0& cosey\end{array}\right]$$
• 在右手系中绕$Z$轴旋转$ez°$ 对应的矩阵$R_z$:
  $$R_z=\left[\begin{array}{ccc}cosez& -sinez& 0\\ sinez& cosez& 0\\ 0& 0& 1\end{array}\right]$$
  故LOAM的欧拉角对应的旋转矩阵，推导如下:
  $$R=R_y\ast R_x\ast R_z=\left[\begin{array}{ccc}coseycosez+siney\ast sinex\ast sinez& cosez\ast siney\ast sinex-cosey\ast sinez& cosex\ast siney\\ cosex\ast sinez& cosex\ast cosez& -sinex\\ cosey\ast sinex\ast sinez-cosez\ast somey& cosey\ast cosez\ast sinex+siney\ast sinez& cosey\ast cosex\end{array}\right]$$公式(1)

构建特征点目标函数的推导

这部分推导有点问题，我重写一下。

这个过程中, 需要通过$T_{(k+1)}^W$将$X_{(k+1,i)}^L$转换到map坐标系（因为原先特征点在LiDAR坐标系）,我们定义这个函数为$G(.)$ :
$$X_{(k+1,i)}^W=G(X_{(k+1,i)}^L,T_{(k+1)}^W)=R\ast X_{(k+1,i)}^L+t$$公式（2）

已知世界坐标的点, 求其与地图中匹配直线的距离,我们定义这个函数为$D(.)$:
$$f=d=D(X_{(k+1,i)}^W,m)$$公式（3）
结合公式（2）和（3）得：
$$f=d=D(X_{(k+1,i)}^W,m)=D(G(X_{(k+1,i)}^L,T_{(k+1)}^W),m)=D(R\ast X_{(k+1,i)}^L+t,m)$$公式（4）
优化过程中,需要对每个分量的偏导，使用链式法则：
$$\frac{\partial f}{\partial ex}=\frac{\partial D(G(X_{(k+1,i)}^L,T_{(k+1)}^W),m)}{\partial ex}$$$$=\frac{\partial D(.)}{\partial G(.)}\ast \frac{\partial G(.)}{\partial ex}$$$$=\frac{\partial D(.)}{\partial G(.)}\ast \frac{\partial (R\ast X_{(k+1,i)}^L+t)}{\partial ex}$$$$=\frac{\partial D(.)}{\partial G(.)}\ast \frac{\partial (R\ast X_{(k+1,i)}^L)}{\partial ex}+\frac{\partial D(.)}{\partial G(.)}\ast \frac{\partial (t)}{\partial ex}$$$$=\frac{\partial D(.)}{\partial G(.)}\ast \frac{\partial (R\ast X_{(k+1,i)}^L)}{\partial ex}$$公式（5）
$$\frac{\partial f_{\epsilon }}{\partial x}=\frac{\partial D(G(X_{(k+1,i)}^L,T_{(k+1)}^W),m)}{\partial x}$$$$=\frac{\partial D(.)}{\partial G(.)}\ast \frac{\partial G(.)}{\partial x}$$$$=\frac{\partial D(.)}{\partial G(.)}\ast \frac{\partial (R\ast X_{(k+1,i)}^L+t)}{\partial x}$$$$=\frac{\partial D(.)}{\partial G(.)}\ast \frac{\partial (R\ast X_{(k+1,i)}^L)}{\partial x}+\frac{\partial D(.)}{\partial G(.)}\ast \frac{\partial (t)}{\partial x}$$$$=\frac{\partial D(.)}{\partial G(.)}\ast 1$$$$=\frac{\partial D(.)}{\partial G(.)}$$公式（6）
可见，对$ex$的偏导，比对$x$的偏导，多乘了一个部分：
$$\frac{\partial (R\ast X_{(k+1,i)}^L)}{\partial ex}$$
而公共部分是:
$$\frac{\partial D(.)}{\partial G(.)}$$
结合公式（2）和（3）得:
$$\frac{\partial D(.)}{\partial G(.)}=\frac{\partial d}{\partial X_{(k+1,i)}^W}$$公式（7）
公式（7）中距离对点求导，可以理解为求一个点的变化（移动）方向，使得$d$减小的更快，也就是求: 点往哪个方向移动, 点到平面/直线的距离减小的更快？
答案是沿着垂线方向。

针对corner点， 我们可以直接用点到与之匹配的直线之间的垂线方向对应的向量（直线的方向向量）表示；
针对surface点，我们可以直接用点到与之匹配的平面之间的垂线方向对应的向量(平面的法向量)表示。
我们假设垂线方向对应的单位向量为$(la,lb,lc)$，其中$la,lb,lc$都可以通过简单的几何运算写为关于 的表达式。那么，公式$f$中$(\frac{\partial f}{\partial x},\frac{\partial f}{\partial y},\frac{\partial f}{\partial z})$就等于$(la,lb,lc)$。
结合公式（1）和公式（5）得：$$\frac{\partial f}{\partial ex}=(la,lb,lc)\ast \frac{\partial (R\ast X_{(k+1,i)}^L)}{\partial ex}$$$$=(la,lb,lc)\ast \frac{\partial (R)}{\partial ex}\ast X_{(k+1,i)}^L$$$$=(la,lb,lc)\ast \frac{\partial (R)}{\partial ex}\ast (p_x,p_y,p_z{)}^T$$$$=(la,lb,lc)\ast \left[\begin{array}{ccc}siney\ast cosex\ast sinez& cosez\ast siney\ast cosex& -sinex\ast siney\\ -sinex\ast sinez& -sinex\ast cosez& -cosex\\ cosey\ast cosex\ast sinez& cosey\ast cosez\ast cosex& -cosey\ast sinex\end{array}\right]\ast (p_x,p_y,p_z{)}^T$$公式（8）
中间的3*3矩阵是公式（1）对$ex$求偏导所得，此公式与代码中arx的计算一致。
同理可得：$\frac{\partial f}{\partial ey},\frac{\partial f}{\partial ez}$与代码中ary，arz分别对应。

对目标函数的优化

对于每个观测项，我们可以将线性化后的cost写为：$$x^{\ast }=\underset{x}{\mathrm{argmin}}\sum _i{\Vert A_i{x}_i-b_i\Vert }_2^2=\underset{\Delta }{\mathrm{argmin}}{\Vert Ax-b\Vert }_2^2$$公式（9）
其中，$A_i={\sum }_i^{(-\frac{1}{2})}{H}_i,b_i={\sum }_i^{(-\frac{1}{2})}(z_i-h_i(p_i^0))$ 公式（10）
其中$A_i$和$b_i$分别为每个观测项的雅可比矩阵和预测误差通过各个观测项的协方差矩阵$\sum i$白化后的结果。
求解(公式9)，代码中使用高斯牛顿法，通过求解normal equation，我们可以得到高斯牛顿法的更新步长:$$A^{T}A{x}_{gn}=A^{T}b$$公式（11）
而且求解这个normal equation，代码中使用了QR分解的方式，求得更新步长 $x_{gn}$
我们将$x_{gn}$叠加到之前的pose中， 然后重复点与地图匹配，建立cost function，求步长$x_{gn}$，叠加$x_{gn}$的过程直到收敛。