PoseExtrapolator & ImuTracker

1. ImuTracker

• 使用IMU的角速度和线性加速度跟踪方向。 因为平均线性加速度（假设慢速运动）是重力的直接测量，所以滚动/俯仰(roll/pitch)不会漂移，尽管偏航(yaw)会漂移。
• 输入：
  • 时间 (common::Time time)
  • 线加速度 (Eigen::Vector3d linear_acceleration)
  • 角速度 (Eigen::Vector3d angular_velocity)
  • 输入实例：

header: 
  seq: 34496
  stamp: 
    secs: 1474964355
    nsecs: 312731655
  frame_id: "gyro_link"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.0015271630955
  y: 0.024740042147
  z: -0.0226020138133
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.0
  y: 0.0
  z: 9.8
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

• 输出：当前的方向估计（Eigen::Quaterniond orientation_）

2. PoseExtrapolator

• 解决IMU、Odometry(里程计)和位姿信息进行融合的问题
• 输入
  • IMU
  • 里程计
  • 位姿信息(Pose)
• 输出
  • 根据位姿计算的线速度和角速度
  • 根据里程计计算的线速度和角速度
  • 推测某一时刻的位姿(ExtrapolatePose(const common::Time time))
    • 旋转：ImuTracker的方向角的变化量
    • 平移：里程计或者位姿线速度计算的移动量