LOAM源代码分析附公式推导之MultiScanRegistration

更新日志

同步更新地址知乎：LOAM SLAM原理之源代码分析附公式推导
有细节性出入！！主要是细节部分的解疑，问题讨论部分，建议前往知乎阅读，此博客只是原始版本（还不是因为大家讨论得太少了，修改都在知乎上进行了 ） 。

• 2020-12-07
  整理完成LaserOdometry节点内容，地址请转到LOAM源代码分析附公式推导之LaserOdometry
  最近有点忙，因此这个时间跨度有点大，两个月了，我自己都没有想到！害，很多公式自己有重新推导了一次，希望能够帮助大家！
• 2020-10-11
  整理完成MultiScanRegistration节点内容
• 2020-10-09
  第一次编辑，初版

1. 概述

整理一下LOAM中对激光点云数据、IMU数据的处理以及相关的公式推导，希望能帮助大家，同时供自己以后复习参考。由于相关的东西较多，可能会有疏漏，并且由于时间有限，精力有限，此博客长期更新，欢迎各位小伙伴批评指正。

同步更新地址知乎：LOAM SLAM原理之源代码分析附公式推导，有细节性出入！！主要是细节部分的解疑，问题讨论部分，建议前往知乎阅读，此博客只是原始版本（还不是因为大家讨论得太少了，修改都在知乎上进行了 ）。

2. 前言

LOAM是 bulabula。
LOAM参考源代码仓库github地址为LOAM 源代码链接具体配置可以参照博客LOAM SLAM安装与配置
关于LOAM原始论文的解读可以参考LOAM SLAM之论文原理解读，
相关链接LOAM SLAM原理之防止非线性优化解退化。
其他参考链接，感谢各位在此之前分享对LOAM原理以及代码的分析理解
LOAM细节分析
LOAM:3D激光里程计及环境建图的方法和实现（一）

每个人对代码的理解是不一样，均有可能出现偏差，欢迎探讨！

3. 准备工作

在LOAM中，使用的坐标系是左上前坐标系，也就是X指向左，Y指向上，Z指向前。一定要注意坐标系问题，后面的一系列工作全都是基于这一基准坐标系开展的。

3.1 激光雷达

LOAM原始论文使用的是一个二维激光雷达，通过机械机构产生3D点云数据，但是现在的代码中大多使用的是Velodyne美国威力登公司的16线激光雷达：vlp16也称为PUCK。因此本文以代码中使用的vlp16雷达开展（代码使用的这个，如果想用其他的就自己改改吧，哈哈）。那么简单介绍一下vlp16：

• Velodyne PUCK（VLP-16）16通道
• TOF测量距离，最远100m，精度3cm
• 垂直分辨率2°
• 水平分辨率600、1200RPM对应0.2°、0.4°
• 旋转速率5-20Hz
• 最大点云数据量 360/0.4161200/60=288000（30万）

我们主要关注点云数据的结构，根据官方文档，具体结构示意如下图，在实际扫描中，激光雷达的激光束按照id号依次进行，例如发射id为0激光，发射id为1激光，最后发射id为15激光。
需要注意相邻激光束的ID差别较大，也就是激光每次发射激光时不是角度上依次发射的，而是隔了半个垂直视场的范围，id相邻的激光束空间上不相邻。

评论区有小伙伴对激光束发射提出疑问，此问题针对vlp16激光雷达，这里我解释一下，激光束是间断发射的16束垂直方向的激光，不是同时发射的，但是间断时间很短，你可以认为是同时发射的，只是认为。可以想一个问题，激光如果是同时发射的，如何根据反射的激光区分他们呢，所以一次只能发射一束激光。根据官方资料，在垂直方向发射激光时，相邻激光束时间差为$2.304\mu s$微秒，16束激光发射完成时会有$18.43\mu s$微秒的空闲时间，一次16束激光完成发射需要花费$16\ast 2.034\mu s+18.43\mu s=55.296\mu s$微秒。当水平旋转频率为600RPM时，对应的水平方向分辨率为$Azimut{h}_{resolution}=10rev/s\ast 360°/rev\ast 55.296\times 10^{-6}s/firingcycle=0.199°/firingcycle$，与之前给出的数据对应，这下应该都明白了吧。

vlp16的坐标系为右前上，并且扫描时顺时针旋转，通过安装可以将坐标系变换为前左上，因此我们在数据处理时只需要将前左上坐标系的点云数据转换为左上前坐标系即可。

3.2 IMU

根据代码可以看到IMU安装坐标系为前左上，因此实际使用时需要将测量的比力信号转换到左上前坐标系，并且注意对重力加速度的处理之后才是真正的激光运动加速度。

3.3 坐标变换

前面谈到了坐标系问题，这里讲一下坐标变换的问题，网上相关的博客很多，为了不引起误解，对于LOAM，我们统一用下面的方式描述旋转变换：
左乘，旋转坐标轴，先Z再X后Y得到旋转矩阵$R_{ZXY}$。
举例说明：激光坐标系$L$和世界坐标系$W$原点开始时一样，经过绕Z轴旋转$rz$角度，绕X轴$rx$角度，绕Y轴$ry$角度（以坐标轴正方向为旋转正方向）之后得到激光坐标系$L^{\prime }$，那么根据上面的定义有
从世界坐标系$W$到激光坐标系$L^{\prime }$的变换$R_{ZXY}$为
根据左乘
$$R_{ZXY}=R_{ry}{R}_{rx}{R}_{rz}$$
根据旋转坐标轴矩阵
$$R_{ZXY}=\left[\begin{array}{ccc}cry& 0& -sry\\ 0& 1& 0\\ sry& 0& cry\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& crx& srx\\ 0& -srx& crx\end{array}\right]\left[\begin{array}{ccc}crz& srz& 0\\ -srz& crz& 0\\ 0& 0& 1\end{array}\right]$$
进一步得到
$$R_{ZXY}=[\begin{array}{c}cycz-sxsysz\\ -cxsz\\ sycz+sx\end{array}$$