在SLAM中，重力可以算是一个非常有用的量，他能为小车提供一个绝对的姿态参考。虽然加速对该向量影响很大，但通过时间平均能够得到一个稳定的重力向量。最近我在思考，将该向量耦合进scan-match的优化函数中，能否解决scan-match在某些环境下性能退化的问题，在耦合进优化函数前，我先参考cartographer推导一下如何使用重力对齐比较合适。
代码方面开源的仓库能直接看，我这里就用数学的公式进行简单的推导。（图片不知道放什么，就放一张阿梓喵的照片吧o(￣▽￣)ブ）
姿态积分
cartographer并没有直接使用imu出来的姿态，而是在内部自己解算。估计是为了兼容只提供3轴加速度和3轴角速度的IMU。
现在大部分IMU都会内部积分结算出姿态，所以cartographer这一步好像有点多余，即降低精度又浪费算力，实际使用时还是推荐直接使用IMU解算出来的姿态（会不会是某些IMU解算姿态没有重力矫正？）。
姿态解算的原理很容易想到，就是直接对角速度积分更新姿态： \[ R=\int_0^t{V_{angle}dt} \] 这样缺点很明显，没又用到加速度信息（浪费信息就是可耻），且随时间的进行，角度误差累积会越来越大。设想一下，IMU静止，加速度为\(\overrightarrow{a}=[0,0,9.8]^T\)，这时候姿态积分出来告诉你当前姿态是竖着的，这不是搞笑吗🤡。
为了解决上述问题，姿态积分还需要通过重力进行矫正，我们把上述连续积分过程换成离散的迭代相加，那么k-1时刻到k时刻两帧之间坐标变换为： \[ \Delta R=V_{angle}\times \Delta t \] 然后更新k时刻的姿态： \[ R_k=R_{k-1}\times\Delta R \]
因为是内旋（\(\Delta R\)为相较于上一次的增量，也就是以上一次为参考的增量，而不是绝对坐标系），所以这里是左乘。其实用角标格式的坐标变换也很好理解：\(^w_kR=^w_{k-1}R\times ^{k-1}_kR\)，其中\(^{k-1}_kR\)就是增量\(\Delta R\)
姿态更行后，显然，加速度也需要更新（加速度是相对于当前IMU姿态的）。这里更新是为了后面指数平均，只有把重力的坐标系变成当前的坐标系，才能和观测数据进行加权。 \[ \tilde{\mathbf{a}}_k=\Delta R^{-1}\times\tilde{\mathbf{a}}_{k-1} \]
这里用角标格式的坐标变换也很好理解，我们把各个参考坐标系标出来可以得到：\(^k\tilde{\mathbf{a}}=^k_{k-1}R\times^{k-1}\tilde{\mathbf{a}}\)，也就是将相对于k姿态的重力变换到相对于k-1参考系的重力向量。
然后就是使用观测的重力更新了。至于为什么不直接使用加速度作为重力向量，实际上使用加速度时，加速度向量上不止重力，还有其他加速度。为了减轻比如加速减速带来的对重力向量的影响，我们通常会对重力的观测量进行指数平均滤波：
\[
{\mathbf{a}}_k={\mathbf{a}}_{k-1}+(1-e^{-\frac{\Delta
t}{\lambda}})\overrightarrow{a}_{k}
\]
接下来就是巧妙的地方了，我们根据当前的姿态，可以估计出加速度向量\(\tilde{\mathbf{a}}\)（假设只有重力加速度），其实就是将z轴旋转到对应的姿态：
\[
\tilde{\mathbf{a}}=R_k\times \begin{bmatrix}0&0&1\end{bmatrix}^T
\] 而上面还有一个观测的重力加速度\(\mathbf{a}_k\)，我们可以得到这两个向量的坐标变换（可以直接使用Eigen库的FromTwoVectors函数）：
\[
\Delta R_{g}=\left<{\mathbf{a}}_k,\tilde{\mathbf{a}}\right>
\] 然后将姿态旋转对应的角度，消除累计误差： \[
\overline{R}_k=R_k\times\Delta R_g
\]
重力对齐的应用
上面IMU的数据得到了，用法有很多，在SLAM中，一般只用他的相对位姿，用于提供初始预测。当然，如果是9轴IMU，理论上是可以直接紧耦合进系统进行优化的（磁力计也可以提供绝对参考，不会有误差累计）。
那么什么是重力对齐呢。我们知道，重力虽然有用，但他却只能提供2个轴的参考。想象一下，任意姿态拿着一个物体，让他绕垂直地球表面的方向转任意角度，得到的重力向量是一样的（本来想画图的，但是3D的图太难画了😭）。
也就是说我们的IMU所提供的观测，只有重力方向算是真正的直观的观测，所谓重力对齐，就是将要匹配的点云（我这里用在SLAM）,转化到重力方向。再移动一个轴去匹配，也就是说，刚开始我匹配需要调整rpy，现在只有调整一个角去匹配，不仅可以提高匹配的精度，还可以降低计算的复杂度。看到这里，相信你会直呼：”我去，还等什么，快给我端上来罢！！！！“。别急，下面就开始介绍。
实际上，重力的方向由于抖动、加速等原因并不一定是准确的（静止的时候应该很准确）。也就是说它是有误差的，在实际使用中，我这里打算的是加一个损失函数再耦合进scan-match的优化函数，得到损失值最小的结果。本来是想写cartographer，但过程较复杂，这里写简化版本（关键步骤）。
首先，需要将激光等数据都转换到tracking坐标系（一般tracking和IMU一致），对于每一个点，我们转化方式如下： \[ ^{t}P_i=^t_lT\times ^lP_i \] 然后就是重力对齐了，先将点云左乘IMU的姿态，得到旋转后的点云： \[ ^IP_i=^I_tR\times ^tP_i \] 该点云就是和IMU重力对齐了的点云，此时旋转点云的yaw角，都能保证得到的重力方向和实际一致。该点云的初始位姿我们也要先来个重力对齐： \[ ^m_IT=^m_tT\times ^t_IR \] 这时候，让\(^m_IT\)作为优化变量，得到优化的结果。最后，将结果再转化回去： \[ ^m_tT=^m_IT\times ^I_tR \] 得到匹配的结果。
式中：
- \(t\)：tracking frame
- \(l\)：laser frame
- \(m\)：map frame
- \(I\)：IMU frame
