cartographer源码阅读---位姿推测器_cartographer poseextrapolator作用

工程化的cartographer框架为了实现实时可靠的位姿推断,在算法中实现了位姿推测器(pose_extrapolator), 其作用是通过现有的有关机器人位姿的数据, 给前端匹配一个可靠的目标值进行ceres优化. 也就是以位姿推测器推出来的位姿为前端扫描匹配的目标位姿. 所以前端得到的位姿结果相当依赖于位姿推测器. 前端和位姿推测器的交集如下, 在local_trajectory_builder_2d.cc中:

   range_data_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast<float>());
   ......
   const transform::Rigid3d non_gravity_aligned_pose_prediction =
      extrapolator_->ExtrapolatePose(time);
   ......
  extrapolator_->AddPose(time, pose_estimate);
  ......
  extrapolator_->AddPose(time, transform::Rigid3d::Identity());
  ......
  extrapolator_->AddOdometryData(odometry_data);
  
1
2
3
4
5
6
7
8
9
10
11
12

cartographer为位姿推测器写了个接口, 文件为pose_extrapolator_Interface.cc/h,都是虚函数与结构体定义, 其接口的实现在pose_extrapolator.cc/h中.

1. PoseExtrapolator类

这个类构造函数如下

/**
 * @brief 构造函数
 * 
 * @param[in] pose_queue_duration 时间差 0.001s
 * @param[in] imu_gravity_time_constant 10
 */
PoseExtrapolator::PoseExtrapolator(const common::Duration pose_queue_duration,
                                   double imu_gravity_time_constant)
    : pose_queue_duration_(pose_queue_duration),
      gravity_time_constant_(imu_gravity_time_constant),
      cached_extrapolated_pose_{common::Time::min(),
                                transform::Rigid3d::Identity()} {}
1
2
3
4
5
6
7
8
9
10
11
12

pose_queue_duration_与gravity_time_constant_都是从lua配置文件中读取的参数, 意思是两次预测位姿需要最小间隔多长时间 与 重力常数.
定义在头文件中的私有变量:

  // 保存一定时间内的pose
  const common::Duration pose_queue_duration_;
  struct TimedPose {
    common::Time time;
    transform::Rigid3d pose;
  };
  std::deque<TimedPose> timed_pose_queue_; //带时间戳的pose
  // 线速度有2种计算途径
  Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d angular_velocity_from_poses_ = Eigen::Vector3d::Zero();

  const double gravity_time_constant_;
  std::deque<sensor::ImuData> imu_data_;

  // c++11: std::unique_ptr 是独享被管理对象指针所有权的智能指针
  // 它无法复制到其他 unique_ptr, 也无法通过值传递到函数,也无法用于需要副本的任何标准模板库 (STL) 算法
  // 只能通过 std::move() 来移动unique_ptr
  // std::make_unique 是 C++14 才有的特性

  std::unique_ptr<ImuTracker> imu_tracker_;               // 保存与预测当前姿态
  std::unique_ptr<ImuTracker> odometry_imu_tracker_;      // 用于计算里程计的姿态的ImuTracker
  std::unique_ptr<ImuTracker> extrapolation_imu_tracker_; // 用于预测姿态的ImuTracker
  TimedPose cached_extrapolated_pose_;

  std::deque<sensor::OdometryData> odometry_data_;
  Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero();
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

2. 通过odometer进行位姿推测

// 向odom数据队列中添加odom数据,并进行数据队列的修剪,并计算角速度与线速度
void PoseExtrapolator::AddOdometryData(
    const sensor::OdometryData& odometry_data) {
  CHECK(timed_pose_queue_.empty() ||
        odometry_data.time >= timed_pose_queue_.back().time);
  odometry_data_.push_back(odometry_data);

  // 修剪odom的数据队列
  TrimOdometryData();
  // 数据队列中至少有2个数据
  if (odometry_data_.size() < 2) {
    return;
  }

  // TODO(whess): Improve by using more than just the last two odometry poses.
  // Compute extrapolation in the tracking frame.
  // 取最新与最老的两个里程计数据
  const sensor::OdometryData& odometry_data_oldest = odometry_data_.front();
  const sensor::OdometryData& odometry_data_newest = odometry_data_.back();
  // 最新与最老odom数据间的时间差
  const double odometry_time_delta =
      common::ToSeconds(odometry_data_oldest.time - odometry_data_newest.time);
  // 计算两个位姿间的坐标变换
  const transform::Rigid3d odometry_pose_delta =
      odometry_data_newest.pose.inverse() * odometry_data_oldest.pose;

  // 两个位姿间的旋转量除以时间得到 tracking frame 的角速度
  angular_velocity_from_odometry_ =
      transform::RotationQuaternionToAngleAxisVector(
          odometry_pose_delta.rotation()) /
      odometry_time_delta;
  if (timed_pose_queue_.empty()) {
    return;
  }
  // 平移量除以时间得到 tracking frame 的线速度, 只在x方向有数值(机器人只能向前走)
  const Eigen::Vector3d
      linear_velocity_in_tracking_frame_at_newest_odometry_time =
          odometry_pose_delta.translation() / odometry_time_delta;

  // 根据位姿队列中最后一个位姿 乘以 上次添加位姿时的姿态预测到time时刻的姿态变化量
  // 得到预测的 最新里程计数据时刻 tracking frame 在 local 坐标系下的姿态
  const Eigen::Quaterniond orientation_at_newest_odometry_time =
      timed_pose_queue_.back().pose.rotation() *
      ExtrapolateRotation(odometry_data_newest.time,
                          odometry_imu_tracker_.get());
  // 将tracking frame的线速度进行旋转, 得到 local 坐标系下 tracking frame 的线速度
  linear_velocity_from_odometry_ =
      orientation_at_newest_odometry_time *
      linear_velocity_in_tracking_frame_at_newest_odometry_time;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

上面判断是不是有两个有效的odometer数据来判断是不是用了odometer. 在odom数据修剪后的队列中得到最新与最老的数据, 计算时间差, 然后通过odometer的位移和角度与时间差微分得到速度和角速度,然后在通过积分得到从上一时刻的位姿(这个位姿是优化后的位姿, 不是odom的位姿)到现在时刻的位姿的转换矩阵,从而推出现在的估计位姿.
修剪odomdata

// 修剪odom的数据队列,丢掉过时的odom数据
void PoseExtrapolator::TrimOdometryData() {
  // 保持odom队列中第二个数据的时间要大于最后一个位姿的时间, odometry_data_最少是2个
  while (odometry_data_.size() > 2 && !timed_pose_queue_.empty() &&
         odometry_data_[1].time <= timed_pose_queue_.back().time) { //同上
    odometry_data_.pop_front();
  }
}
1
2
3
4
5
6
7
8

3. 通过IMU进行位姿预测

实际用到的IMU位姿预测没有在pose_extrapolator中实现, 而是调用的imu_tracker. 其核心的预测代码在Advance中, 如下:

/**
 * @brief 预测出time时刻的姿态与重力方向
 * 
 * @param[in] time 要预测的时刻
 */
void ImuTracker::Advance(const common::Time time) {
  CHECK_LE(time_, time);
  const double delta_t = common::ToSeconds(time - time_);
  // 上一时刻的角速度乘以时间,得到当前时刻相对于上一时刻的预测的姿态变化量,再转换成四元数
  const Eigen::Quaterniond rotation =
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(imu_angular_velocity_ * delta_t));
  // 使用上一时刻的姿态 orientation_ 乘以姿态变化量, 得到当前时刻的预测出的姿态
  orientation_ = (orientation_ * rotation).normalized();

  // 根据预测出的姿态变化量,预测旋转后的线性加速度的值
  gravity_vector_ =
      rotation.conjugate() * // 一般不用inverse，在表示旋转时，共轭即可表示相反的的旋转
      gravity_vector_; 
  // 更新时间
  time_ = time;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

可以看到, Advance只用到了IMU的角速度信息(陀螺仪), 并没有用到加速度信息(加速度计). 只用陀螺仪积分得到了角度变化, 用角度的变化更新重力的方向.
在PoseExtrapolator类中, 与 ImuTracker类进行交互的方法是AdvanceImuTracker:

/**
 * @brief 更新imu_tracker的状态, 并将imu_tracker的状态预测到time时刻
 * 
 * @param[in] time 要预测到的时刻
 * @param[in] imu_tracker 给定的先验状态
 */
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
                                         ImuTracker* const imu_tracker) const {
  // 检查指定时间是否大于等于 ImuTracker 的时间
  CHECK_GE(time, imu_tracker->time());

  // 不使用imu 或者 预测时间之前没有imu数据的情况
  if (imu_data_.empty() || time < imu_data_.front().time) {
    // There is no IMU data until 'time', so we advance the ImuTracker and use
    // the angular velocities from poses and fake gravity to help 2D stability.
    // 在time之前没有IMU数据, 因此我们推进ImuTracker, 并使用姿势和假重力产生的角速度来帮助2D稳定
    
    // 预测当前时刻的姿态与重力方向
    imu_tracker->Advance(time);
    // 使用 假的重力数据对加速度的测量进行更新
    imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
    // 只能依靠其他方式得到的角速度进行测量值的更新
    imu_tracker->AddImuAngularVelocityObservation(
        odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_);
    return;
  }

  // imu_tracker的时间比imu数据队列中第一个数据的时间早, 就先预测到imu数据队列中第一个数据的时间
  if (imu_tracker->time() < imu_data_.front().time) {
    // Advance to the beginning of 'imu_data_'.
    imu_tracker->Advance(imu_data_.front().time); //Advance就是进行预测
  }

  // c++11: std::lower_bound() 是在区间内找到第一个大于等于 value 的值的位置并返回, 如果没找到就返回 end() 位置
  // 在第四个参数位置可以自定义比较规则,在区域内查找第一个 **不符合** comp 规则的元素

  // 在imu数据队列中找到第一个时间上 大于等于 imu_tracker->time() 的数据的索引
  auto it = std::
      lower_bound(  // 遍历的imu_data_传给lambda表达式的第一个参数imu_data,
                    // 条件imu_tracker->time()传给lambda表达式的第二个参数time
          imu_data_.begin(), imu_data_.end(), imu_tracker->time(),
          [](const sensor::ImuData& imu_data, const common::Time& time) {
            return imu_data.time < time;
          });

  // 然后依次对imu数据进行预测, 以及添加观测, 直到imu_data_的时间大于等于time截止
  while (it != imu_data_.end() && it->time < time) {
    // 预测出当前时刻的姿态与重力方向
    imu_tracker->Advance(it->time);
    // 根据线速度的观测,更新重力的方向,并根据重力的方向对上一时刻预测的姿态进行校准
    imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration);
    // 更新角速度观测
    imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);
    ++it;
  }
  // 最后将imu_tracker的状态预测到time时刻
  imu_tracker->Advance(time);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59

上面代码说明了有或者没有IMU的情况下, 怎么预测旋转. 大家可能会疑惑, 为啥没有IMU还能调用imu_tracker进行Advance? 这要看一下ImuTracker的构造函数

/**
 * @brief Construct a new Imu Tracker:: Imu Tracker object
 * 
 * @param[in] imu_gravity_time_constant 这个值在2d与3d情况下都为10
 * @param[in] time 
 */
ImuTracker::ImuTracker(const double imu_gravity_time_constant,
                       const common::Time time)
    : imu_gravity_time_constant_(imu_gravity_time_constant),
      time_(time),
      last_linear_acceleration_time_(common::Time::min()),
      orientation_(Eigen::Quaterniond::Identity()), // 初始方向角
      gravity_vector_(Eigen::Vector3d::UnitZ()),    // 重力方向初始化为[0,0,1]
      imu_angular_velocity_(Eigen::Vector3d::Zero()) {}
1
2
3
4
5
6
7
8
9
10
11
12
13
14

可以看到构造函数里面都有初值, 那么初值是在哪更新的呢?
如果有IMU的话, 直接通过IMU得到的角速度更新:

imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);
1

如果没有IMU的话, 就只能通过其他方式获得估计的角速度, 即有odometer数据就用odom推测的角速度, 没有的话就用pose推测的角速度.

    imu_tracker->AddImuAngularVelocityObservation(
        odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_);
1
2
3

4. 通过Pose进行位姿预测

由于cartographer中只有激光雷达是必须的, 如果没有odom和IMU, 只用激光雷达推测出的Pose也是可以用位姿推测器的.

// 根据pose队列计算tracking frame 在 local坐标系下的线速度与角速度
void PoseExtrapolator::UpdateVelocitiesFromPoses() {
  if (timed_pose_queue_.size() < 2) {
    // We need two poses to estimate velocities.
    return;
  }
  CHECK(!timed_pose_queue_.empty());
  // 取出队列最末尾的一个 Pose,也就是最新时间点的 Pose,并记录相应的时间
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  const auto newest_time = newest_timed_pose.time;
  // 取出队列最开头的一个 Pose, 也就是最旧时间点的 Pose,并记录相应的时间
  const TimedPose& oldest_timed_pose = timed_pose_queue_.front();
  const auto oldest_time = oldest_timed_pose.time;
  // 计算两者的时间差
  const double queue_delta = common::ToSeconds(newest_time - oldest_time);
  // 如果时间差小于pose_queue_duration_(1ms), 不进行计算
  if (queue_delta < common::ToSeconds(pose_queue_duration_)) {
    LOG(WARNING) << "Queue too short for velocity estimation. Queue duration: "
                 << queue_delta << " s";
    return;
  }
  const transform::Rigid3d& newest_pose = newest_timed_pose.pose;
  const transform::Rigid3d& oldest_pose = oldest_timed_pose.pose;
  // 平移量除以时间得到 tracking frame 在 local坐标系下的线速度
  linear_velocity_from_poses_ =
      (newest_pose.translation() - oldest_pose.translation()) / queue_delta;
  // 角度变化量除以时间得到角速度得到 tracking frame 在 local坐标系下的角速度
  angular_velocity_from_poses_ =
      transform::RotationQuaternionToAngleAxisVector(
          oldest_pose.rotation().inverse() * newest_pose.rotation()) /
      queue_delta;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

方法和odom的预测方式大同小异, 就不介绍了.

5. 插入历史位姿AddPose

AddPose是位姿推测器中最重要的函数, 作用是通过插入上一帧前端匹配后的位姿来进行当前时间点的位姿预测. 这个函数调用了上面提到的函数, 预测出角速度与线速度.

// 将扫描匹配后的pose加入到pose队列中,计算线速度与角速度,并将imu_tracker_的状态更新到time时刻
void PoseExtrapolator::AddPose(const common::Time time,
                               const transform::Rigid3d& pose) {
  // 如果imu_tracker_没有初始化就先进行初始化
  if (imu_tracker_ == nullptr) {
    common::Time tracker_start = time;
    if (!imu_data_.empty()) {
      tracker_start = std::min(tracker_start, imu_data_.front().time);
    }
    // imu_tracker_的初始化
    imu_tracker_ =
        absl::make_unique<ImuTracker>(gravity_time_constant_, tracker_start);
  }

  // 在timed_pose_queue_中保存pose
  timed_pose_queue_.push_back(TimedPose{time, pose});

  // 保持pose队列中第二个pose的时间要大于 time - pose_queue_duration_
  while (timed_pose_queue_.size() > 2 && // timed_pose_queue_最少是2个数据
         timed_pose_queue_[1].time <= time - pose_queue_duration_) {
    timed_pose_queue_.pop_front(); //丢弃旧的
  }

  // 根据加入的pose计算线速度与角速度
  UpdateVelocitiesFromPoses();

  // 将imu_tracker_更新到time时刻
  AdvanceImuTracker(time, imu_tracker_.get());

  // pose队列更新了,之前imu及里程计数据已经过时了
  // 因为pose是匹配的结果,之前的imu及里程计数据是用于预测的,现在结果都有了,之前的用于预测的数据肯定不需要了
  TrimImuData();
  TrimOdometryData();

  // 用于根据里程计数据计算线速度时姿态的预测
  odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
  // 用于位姿预测时的姿态预测
  extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39

AddPose只用了pose和imu进行预测, 为啥没见到odom呢?这需要结合local_trajectory_builder_2d.cc这个文件看, 不同传感器运用的方案有不同位姿推测器的方案.

6. 位姿推测器的总结

有3中位姿推测的方法, 但是不同情况下用哪一个呢? 当然是哪个准用哪个, 具体哪个准是工程经验得到的. 预测位姿分为4中情况, 共同点是都是用匀速模型进行预测:

1. 使用imu, 使用里程计
   平移的预测: 通过里程计数据队列开始和末尾的2个数据计算出的线速度乘以时间
   姿态的预测: 通过imu的角速度乘以时间
2. 使用imu, 不使用里程计
   平移的预测: 通过pose数据队列开始和末尾的2个数据计算出的线速度乘以时间
   姿态的预测: 通过imu的角速度乘以时间
3. 不使用imu, 使用里程计
   平移的预测: 通过里程计数据队列开始和末尾的2个数据计算出的线速度乘以时间
   姿态的预测: 通过里程计数据队列开始和末尾的2个数据计算出的角速度乘以时间
4. 不使用imu, 不用里程计
   平移的预测: 通过pose数据队列开始和末尾的2个数据计算出的线速度乘以时间
   姿态的预测: 通过pose数据队列开始和末尾的2个数据计算出的角速度乘以时间

总结:

1. 预测平移时: 有里程计就用里程计的线速度, 没有里程计就用pose计算的线速度进行预测
2. 预测姿态时: 有IMU就用IMU的角速度, 没有IMU时, 如果有里程计就用里程计计算出的角速度, 没有里程计就用pose计算的角速度进行预测
3. 预测的都是相对值, 要加上最后一个pose的位姿