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_

|

imu_tracker_

|

直接使用IMU数据进行，只进行一次初始化

—|---

odometry_imu_tracker_

|

表示AddPose到AddOdometryData之间（即 最新优化的位姿时刻 到 最新码盘数据时刻 之间的时间段）的姿态变化

extrapolation_imu_tracker_

|

表示AddPose到ExtrapolatePose之间（即 最新优化的位姿时刻 到 最新估计位姿时刻 之间的时间段）的姿态变化

ExtrapolatePose

|

获取融合完成的位姿数据