cartographer的重力对齐原理

# 对应cartographer里的PoseExtrapolator类（代码实现为pose_extrapolator.cc）

# 参考文章：重力对齐推导 - Thomas's Blog

# pitch()：俯仰，将物体绕y轴旋转
   yaw()：航向，将物体绕z轴旋转
   roll()：横滚，将物体绕x轴旋转

一、为什么要使用重力对齐：

        在进行点云匹配时，我们需要匹配平移和姿态共6个自由度，但是如果使用重力对齐，两个点云之间的配准只需要平移和姿态的航向角共4个自由度。假设源点云为A，目标点云为B，B = R*A + t，未对齐时6个自由度。重力对齐是将姿态的z轴转到与重力的方向一致，当A、B所在的本体坐标系的姿态的z轴都转到跟重力方向对齐时，只需要优化平移和航向角共4个自由度。这提高了匹配精度和减少了计算量。

二、如何找出重力方向：

        当IMU静止时，这时候IMU线加速度测量的就是因为重力而产生的反向支持力，如果重力是[0, 0, -9.8]，则IMU测量是[0, 0, 9.8]。但是IMU绝大多数时候都是在运动，测量出来的线加速度不止是重力还有IMU的加速度影响，对此cartographer采用指数平均滤波来求重力方向。

        举个例子，假设IMU线加速度测量4次，分别为[1, 2, 9.8]、[-1, 2, 9.8]、[5, 3, 9.8]、[3, 2, 10]，对这个取平均值得到[2, 2.25, 9.85]，明显看到第三个数9.85大于前两个数，事实上，当测量数据越多时候，第三个数会远远大于前两个数，因为重力是恒存在的，而加速、减速时有时无，这时候对[2, 2.25, 9.85]归一化得到[0, 0.031, 1]，这个值与z轴的值已经很接近了，可以认为是重力的方向了。

三、重力对齐推导：

 参考文章：重力对齐推导 - Thomas's Blog，已经写的很详细了，这里就不重复描述了。