坐标系之间的关系和 InsertionResult

Frames(坐标系)

global map frame

这是表示全局 SLAM 结果的坐标系。它是包含所有闭环和优化结果的固定地图坐标系。当新的优化结果出现时，该帧与其他帧之间的转换可以跳转。它的 z 轴指向上方，也就是重力加速度矢量指向-z 方向，也就是说加速计测量的重力分量是 + z 方向

local map frame

局部SLAM结果的坐标系，不包括闭环以及位姿BA优化，对于给定某个时间点，该帧与全局地图之间的变换可能会发生变化，但是该帧和其他帧的变换不会发生变化。

submap frame

每个子图都有一个单独的坐标系

tracking frame

传感器数据表示的坐标系，自定义

gravity-aligned frame

重力对准坐标系，只在2D中使用，与跟踪坐标系有相同位置，但是坐标系方向不一致，类似于导航坐标系

Transforms (变换)

local_pose (局部姿态)

将数据从 Tracking frame 或者子图坐标系转换到 local map frame 的变换

global_pose (全局位姿)

将数据从Tracking frame或者子图坐标系转换到global map frame的变换

关系图

前端的MatchingResult包含：

time          此次matching的scan时间。
local_pose    此次matching的scan的位姿。
range_data_in_local    未过滤的距离数据。
insertion_result       range_data_in_local插入submap返回的结果。

local map frame 和 submap的坐标系不是一回事。

子图的坐标系原点来自 range_data_in_local.origin，也就是ActiveSubmaps2D::InsertRangeData中的AddSubmap(range_data.origin.head<2>());。

InsertionResult中的constant_data是此次range插入submap包含的主要内容，类型为TrajectoryNode::Data的共享指针。 constant_data其中有个成员const transform::Rigid3d local_pose，表示submap(node)在local map frame中的位姿，用于这两个坐标系之间的变换，对每个子图，local_pose在不断更新第1个local_pose是子图坐标系的原点，也就是上面的 range_data_in_local.origin，也是MotionFilter的判断依据，判断位姿和上一次插入scan的位姿有一定差别

MatchingResult里也有一个local_pose，和 constant_data 的那个local_pose是相同的，赋值用的都是下一帧位姿的观测值。不过在后端里用的是InsertionResult的local_pose，源码在PoseGraph2D::AddNode。这个是在AddSubmap函数中创建新的submap的时候加入的，是创建submap时的第一个ranges的原点。

constant_data还有一个成员global_pose，这是submap(node)在global map frame的位姿。