LeGO-LOAM代码解析---基础知识

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

目录

前言

一、基础知识

1. 位姿

2. 坐标系

3. 位姿变换

4. 转换矩阵

4.1 旋转矩阵

4.2 欧拉角

4.3 四元数

前言

在学习LeGO-LOAM之前，先介绍一下基础知识：

一、基础知识

1. 位姿

        位姿是旋转和平移的统称。在三维世界中，物体的位姿变换既要考虑物体的位置的变化，也要考虑姿态的变化。

        举个例子，你在宿舍里，以宿舍某个点取个固定坐标系，例如东北天坐标系。现在你要去食堂吃饭，无容置疑在这个坐标系中你的位置肯定发生变化，此外还有一个“东东”也在不断发生变化，也就是你脸的朝向（PS:假设是刚体变换）。假设在起点，你脸的朝向正东面，左手在北面，右手在南面，在行进过程中，显然你自身的姿态肯定不断发生变化。

        我室友是个路痴，不认识去食堂的路。为了引导他去食堂，我只好每走5步，用手机拍个周围环境照片。一段时间后，我到了食堂，手机累计拍了大量的照片。根据拍照顺序发给他，然后他根据我拍的相邻的两张图片，逐个推断我是如何走的。这样一来，他就能根据我拍的照片，抵达我的位置(PS：来了之后，估计食堂也关门了)。

        现在，把人替换为机器人后，手机改为搭载在机器人上的传感器时，而且当传感器获取周围环境信息足够短时，理论上是应该能复原整个路线的，拼接这些环境信息，当然也可以还原地图。讲了这么多，其实这就是SLAM呀！

        SLAM就是搭载特定传感器的主体(机器人，车辆，无人机等)，通过感知周围未知的环境信息，估计自身位姿变化并构建环境地图。自身位姿变化，其实就是根据相邻时刻传感器提供的信息，估算出自身的旋转和平移变化。

        好了，从位姿到SLAM，其实SLAM最核心的就是估计位姿变化。现在，你应该对位姿有足够的了解了。

2. 坐标系

        在SLAM中，描述一个位姿离不开坐标系。当然任何一个SLAM算法，都需要一个固定坐标系。大家统一为世界坐标系，此外还有相机坐标系，雷达坐标系，IMU坐标系等等。

        世界坐标系：在整个SLAM系统中，坐标系是固定不变的。一般我们认为起点为世界坐标系的原点，方向可以自行定义。在LOAM和LeGO-LOAM中，世界坐标系为左上前坐标系。请牢记！！！(左上前：以机器人前进方向为基准，左表示行进方向的左边，对应X轴正方向朝向；上(天)表示车辆上方，也就是天空，对应Y轴正方向朝向；前表示行进方向，也是就Z轴正方向朝向)

        雷达坐标系：一般是前左上坐标系。在LAOM和LeGO-LOAM，雷达坐标系是前左上坐标系。在SLAM算法中，

        相机坐标系：普遍是右下前坐标系。

        IMU坐标系：一般是前左上坐标系。

        GPS坐标系：一般是前左上坐标系。

有时候，大家也听到东北天坐标系，里程计坐标系等，

        东北天坐标系：右前上坐标系，根据“上北下南，左西右东”，东便是右，前便是北，天便是上。

        里程计坐标系：每个时刻的激光雷达/相机坐标系，它是不断变化的。

        在LeGO里存在大量的坐标系转换，主要原因就是世界坐标系和雷达坐标系，IMU坐标系的不统一。

3. 位姿变换

        位姿变换包括旋转和平移的变换，统称为变换矩阵。

        位姿变换最常见的包括固定坐标系变换和变化坐标系变换。牢记：固定坐标系变换用左乘，变化坐标系变换用右乘。下面进行解释：

        任何一个矩阵X可以写成E*X，其中E表示单位矩阵。左边E表示固定坐标系，右边表示向量X。若绕固定坐标系变换，固定坐标系不变，第一次变换T1，写成T1X；第二次变换T2在T1X的基础上，可以写成T2T1X；第三次变换T3，同理可以写成T3T2T1X。若绕变化坐标系变换，X不变，第一次变换可以写成E*T1*X；第二次变换在ET1基础上，可以简写成ET1T2X；同理第三次变换T3可以写成ET1T2T3X。

        可以观察到，两者的变换矩阵互为转置关系。所以，位姿变换时，首先判断是固定还是变化坐标系变换。若是变化坐标系变化，可以先当成固定左边系变换，再取转置，就变成固定坐标系变换的转换矩阵。

        举个实际例子，如下图：

        世界坐标系O为左上前坐标系，雷达坐标系A为前左上坐标系，雷达坐标系B为前左上坐标系。通过帧间配准，也就是第0帧到第1帧的点云配准，可以得到世界坐标系O到雷达坐标系A的位姿变换(旋转+平移)，记为；同理通过可以得到雷达坐标系A到雷达坐标系B的位姿变换为（以A为基准求B）。A点相对世界坐标系O的位姿为：。B点相对世界坐标系O的位姿为：。

        那么B点相对A的位姿变换是多少呢？很多人可能认为：这不是相对固定坐标系变换吗？直接根据上述所讲，，也就是。

        再看另一种解法，雷达坐标系A到雷达坐标系B的位姿变换，显然这是坐标系变换，即根据上述所讲，，也就是B相对A的位姿变换为。

        这两种结果完全不一致，什么原因导致呢？

        原因出在坐标系的不同，前者是确实是相对固定坐标系变换的，求得的结果是B点相对A点在世界坐标系中的位姿变换。后者是相对变化坐标系变换的，也就是相对A点所在的坐标系变换的。在帧间匹配时，同一点P在坐标A和坐标系B中的坐标变换，实际是变化坐标系变换，也就是右乘。        

        结论：k+1时刻相对k时刻的位姿变换矩阵为。

        注：这里是相对k时刻的雷达/相机坐标系而言，此外这里的位姿和分别是相对当前时刻的雷达坐标系而言的，并不是世界坐标系下的位姿。

4. 转换矩阵

    描述旋转的有：旋转矩阵、欧拉角、旋转向量以及四元数。通过齐次方程将位移和旋转结合，便构成转换矩阵。

4.1 旋转矩阵

        在三维空间中，旋转矩阵是一个3*3矩阵，转换矩阵且行列式为1。当描述一个物体的旋转时，可以用一个完整的矩阵表示，也可以拆分为基于XYZ轴的旋转。后者在LeGO-LOAM中很常见，方便与欧拉角进行转换。

        假设坐标系为前左上坐标，物体绕Z轴旋转得偏航角Yaw，绕旋转后的Y轴旋转得到Pitch角，绕旋转后的X轴旋转得到Roll。

        绕固定坐标系将物体按上述旋转可得变换后的位姿为：Y=R(Roll)R(Pitch)R(Yaw)X

        绕变化坐标系将物体按上述旋转可得变换后的位姿（单独是旋转或者位移均可）为：Y=R(Yaw)R(Pitch)R(Roll)X。

        显然，两者互为转置关系。

        现在知道了上述欧拉角Yaw,Pitch和Roll，如何得到旋转矩阵呢？

        在二维空间中，物体旋转如下：        

        在三维空间中，物体先绕Z轴旋转，可知z轴坐标不变，仅x,y坐标发生变化，则：

        

        绕旋转后的Y轴旋转Pitch角：

       

         绕旋转后的X轴旋转Roll角：

        类似常见的欧拉角RPY，实际是绕ZYX旋转。

4.2 欧拉角

        欧拉角提供了一种将旋转分解为3次绕不同轴的旋转。

4.3 四元数

        四元数具有一个实部和三个虚部。单位四元数可以表示任何旋转。三维空间中的点可以用一个虚四元数进行描述。