cartographer位姿估计整理
cartographer位姿估计整理
代码所在目录：
cartographer/mapping/pose_extraplator.cc /.h中
代码整体框架：
cartographer在扫描匹配阶段（realtimescanmatcher Ceres 非线性优化）之前，使用 优化后的位姿、IMU、码盘 三个数据源进行融合，估计出最新时刻的位姿，用来作为下次非线性优化的初值。下面，针对这部分数据融合模块进行分析整理，如下图红圈部分。
代码入口：
PoseExtrapolator的构造函数 VS 通过IMU初始化InitializeWithImu方法。 AddImuData
cartographer/mapping/internal/2d/local_trajectory_build.cc 局部位姿估计中
AddImuData和AddOdometryData 增加Imu和Odom数据
代码解析：
1.数据源：
AddPose中传入的pose，保存在timed_pose_queue_中
|
① |
获取最新优化的位姿并删除历史数据 |
|
② |
由最新优化的位姿解算出角速度和线速度（angular_velocity_from_poses_、linear_velocity_from_poses_） |
|
③ |
使用IMU数据进行姿态估计（imu_tracker_），并将此时估计出的姿态用于计算 码盘线速度 和最新估计姿态 的初值（odometry_imu_tracker_、extrapolation_imu_tracker_） |
|
④ |
删除最新优化的位姿时刻之前的码盘和IMU数据 |
AddImuData中传入的imu_data，保存在imu_data_中
|
① |
获取IMU数据 |
|
② |
删除 最新优化的位姿时刻 之前的IMU数据 |
AddOdometryData传入的odometry_data，保存在odometry_data_中
|
① |
获取码盘数据 |
|
② |
删除最新优化的位姿时刻之前的码盘数据 |
|
③ |
由码盘数据解算出角速度和线速度（angular_velocity_from_odometry_、linear_velocity_from_odometry_） |
2.数据融合
|
ExtrapolatePose |
使用码盘计算出的线速度进行位置估计，使用IMU计算出的角度增量进行姿态估计 |
||||||
|
AdvanceImuTracker |
使用IMU的陀螺仪和加速度计进行姿态估计，估计方法是比较简单的互补滤波算法。用此函数进行的姿态估计共有三个，分别是 imu_tracker_、odometry_imu_tracker_和extrapolation_imu_tracker_
|
||||||
|
ExtrapolatePose |
获取融合完成的位姿数据 |