位姿推测器-cartographer
Kong Liangqian Lv6
 2021-11-14 18:38:29  

位姿推测器的实现位于cartographer/mapping/pose_extrapolator.h中

他继承于类PoseExtrapolatorInterface，在这个类的上方，有一个函数，创建位姿推测器的参数配置

CreatePoseExtrapolatorOptions

1
2
3
4
5
6
7
8
9
10
11
12
proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions(
    common::LuaParameterDictionary* const parameter_dictionary) {
  proto::PoseExtrapolatorOptions options;
  options.set_use_imu_based(parameter_dictionary->GetBool("use_imu_based"));
  *options.mutable_constant_velocity() =
      CreateConstantVelocityPoseExtrapolatorOptions(
          parameter_dictionary->GetDictionary("constant_velocity").get());
  *options.mutable_imu_based() = CreateImuBasedPoseExtrapolatorOptions(
      parameter_dictionary->GetDictionary("imu_based").get());

  return options;
}

从这里加载三个位姿推测器的参数，返回configuration_files/trajectory_builder_2d.lua文件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
-- 位姿预测器
pose_extrapolator = {
  use_imu_based = false,
  constant_velocity = {
    imu_gravity_time_constant = 10.,
    pose_queue_duration = 0.001,
  },
  imu_based = {
    pose_queue_duration = 5.,
    gravity_constant = 9.806,
    pose_translation_weight = 1.,
    pose_rotation_weight = 1.,
    imu_acceleration_weight = 1.,
    imu_rotation_weight = 1.,
    odometry_translation_weight = 1.,
    odometry_rotation_weight = 1.,
    solver_options = {
      use_nonmonotonic_steps = false;
      max_num_iterations = 10;
      num_threads = 1;
    },
  },
},

可以看见，pose_extrapolator 里面就是这 三项

最后他会把option返回给CreateLocalTrajectoryBuilderOption2D，返回CreateTrajectoryOptions，最后返回的就是LoadOption

位姿推测器

思维导图

构造函数

1
2
3
4
5
6
7
8
9
10
11
12
/**
 * @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()} {}

GetLastPoseTime

通过一个队列把数据进行返回

1
2
3
4
5
6
7
8
// 返回上次校准位姿的时间
common::Time PoseExtrapolator::GetLastPoseTime() const {
  // 如果尚未添加任何位姿, 则返回Time::min()
  if (timed_pose_queue_.empty()) {
    return common::Time::min();
  }
  return timed_pose_queue_.back().time;
}

GetLastExtrapolatedTime

获取上一次预测位姿的时间

1
2
3
4
5
6
7
// 获取上一次预测位姿的时间
common::Time PoseExtrapolator::GetLastExtrapolatedTime() const {
  if (!extrapolation_imu_tracker_) {
    return common::Time::min();
  }
  return extrapolation_imu_tracker_->time();
}

AddPose

将扫描匹配后的pose加入到pose队列中,计算线速度与角速度,并将imutracker的状态更新到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
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_);
}

AddImuData

被调用处

于LocalTrajectoryBuilder2D::AddImuData中调用，在这里初始化了位姿估计器，并且添加了IMU数据、

上面的函数被调用于 GlobalTrajectoryBuilder文件的AddSensorData函数中，此函数分别把imu数据传入前端和后端

定义

1
2
3
4
5
6
7
// 向imu数据队列中添加imu数据,并进行数据队列的修剪
void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) {
  CHECK(timed_pose_queue_.empty() ||
        imu_data.time >= timed_pose_queue_.back().time);
  imu_data_.push_back(imu_data);
  TrimImuData();
}

直接把数据添加到imu数据队列，并且对他进行一个裁剪TrimImuData

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

AddOdometryData

向odom数据队列中添加odom数据,并进行数据队列的修剪,并计算角速度与线速度

注意，这里的sensor::OdometryData中的Pose已经是转化为tracking_frame的pose了。

计算角速度的方式为，记录odom数据队列的队首和队尾的两个Pose的时间差$\Delta t$，两个Pose的旋转角度差$\Delta\theta$

计算线速度的方式和上面一样，只是从计算角度差变成计算为平移量的差

最后把线速度变换到local 坐标系下

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
// 向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;
}

TrimOdometryData

最后pop的情况为，假设有两个数据了，那么pose为在中间，两个odom为在一个左边一个右边

1
2
3
4
5
6
7
8
// 修剪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();
  }
}
 Comments