Cartographer源码阅读2D&3D-前端-位姿外推器-PoseExtrapolator

Cartographer源码阅读-2D前端-位姿外推器-PoseExtrapolator

PoseExtrapolator类：

class PoseExtrapolator {
 public:
  explicit PoseExtrapolator(common::Duration pose_queue_duration,
                            double imu_gravity_time_constant);

  PoseExtrapolator(const PoseExtrapolator&) = delete;
  PoseExtrapolator& operator=(const PoseExtrapolator&) = delete;
  
  // IMU初始化相关，暂时不关心
  static std::unique_ptr<PoseExtrapolator> InitializeWithImu(
      common::Duration pose_queue_duration, double imu_gravity_time_constant,
      const sensor::ImuData& imu_data);

  // Returns the time of the last added pose or Time::min() if no pose was added
  // yet.
  common::Time GetLastPoseTime() const;
  // 返回上次外推器推算后位姿的时间戳，如果外推器不存在则返回Time::min()
  common::Time GetLastExtrapolatedTime() const;
  // 前端计算位姿后传入pose的接口，更新外推器相关变量
  void AddPose(common::Time time, const transform::Rigid3d& pose);
  // IMU数据接口，暂不关心
  void AddImuData(const sensor::ImuData& imu_data);
  // 轮速计数据接口
  void AddOdometryData(const sensor::OdometryData& odometry_data);
  // 位姿外推器推算位姿的调用接口
  transform::Rigid3d ExtrapolatePose(common::Time time);
  // 估计重力方向的接口，对应IMU，如果没有IMU，则返回单位四元数
  Eigen::Quaterniond EstimateGravityOrientation(common::Time time);

 private:
  // 根据前端解算的位姿,更新速度
  void UpdateVelocitiesFromPoses();
  // 删除IMU数据
  void TrimImuData();
  // 删除轮速计数据
  void TrimOdometryData();
  // 更新外推器
  void AdvanceImuTracker(common::Time time, ImuTracker* imu_tracker) const;
  // 推算旋转矩阵
  Eigen::Quaterniond ExtrapolateRotation(common::Time time,
                                         ImuTracker* imu_tracker) const;
  // 推算平移矩阵
  Eigen::Vector3d ExtrapolateTranslation(common::Time time);
  // 存放时间差
  const common::Duration pose_queue_duration_;
  struct TimedPose {
    common::Time time;
    transform::Rigid3d pose;
  };
  // 只保留最新的2帧Pose
  std::deque<TimedPose> timed_pose_queue_;
  // 由位姿推算的线速度和角速度
  Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d angular_velocity_from_poses_ = Eigen::Vector3d::Zero();
  // IMU的重力加速度常量
  const double gravity_time_constant_;
  // 存放IMU数据的队列
  std::deque<sensor::ImuData> imu_data_;
  // 全局的IMU的外推器
  std::unique_ptr<ImuTracker> imu_tracker_;
  // 轮速计的外推器
  std::unique_ptr<ImuTracker> odometry_imu_tracker_;
  // 局部IMU的外推器
  std::unique_ptr<ImuTracker> extrapolation_imu_tracker_;
  // 缓存最新一次位姿推算的结果
  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）前端CSM解算的位姿

// 根据匀速运动假设，计算线速度和角速度，并删除旧数据，更新相关imu_tracker
void PoseExtrapolator::AddPose(const common::Time time,
                                   const transform::Rigid3d &pose)
    {
      // 如果之前imu_tracker_没有被初始化，则以该时间戳或imu数据front的时间戳构建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_ =
            common::make_unique<ImuTracker>(gravity_time_constant_, tracker_start);
      }
      // 将该位姿放入队列
      timed_pose_queue_.push_back(TimedPose{time, pose});
	  // 保证存放最新的两个位姿
      while (timed_pose_queue_.size() > 2 &&
             timed_pose_queue_[1].time <= time - pose_queue_duration_)
      {
        timed_pose_queue_.pop_front();
      }
      // cartographer的匀速运动假设
      // 更新linear_velocity_from_poses_和angular_velocity_from_poses_
      UpdateVelocitiesFromPoses();
      // 更新imu_tracker_，使用IMU进行旋转推算，如果没有IMU数据，则使用angular_velocity_from_poses_进行旋转推算。推算的时间点为当前位姿的时间点
      // 该imu_tracker_会一直积分更新，每次都是更新到前端最新解算的时间time
      AdvanceImuTracker(time, imu_tracker_.get());
      // 删除多余的IMU数据
      TrimImuData();
      // 删除多余的轮速计数据
      TrimOdometryData();
      // 将imu_tracker_传给odometry_imu_tracker_和extrapolation_imu_tracker_
      odometry_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);
      extrapolation_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);
    }
// 更新linear_velocity_from_poses_和angular_velocity_from_poses_的方法：
// 线速度 = (最新的平移-上一次的平移)/时间差
// 角速度 = (最新的角度-上一次的角度)/时间差
void PoseExtrapolator::UpdateVelocitiesFromPoses()
    {
      if (timed_pose_queue_.size() < 2)
      {
        // We need two poses to estimate velocities.
        return;
      }
      CHECK(!timed_pose_queue_.empty());
      const TimedPose &newest_timed_pose = timed_pose_queue_.back();
      const auto newest_time = newest_timed_pose.time;
      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);
      if (queue_delta < 0.001)
      { // 1 ms
        LOG(WARNING) << "Queue too short for velocity estimation. Queue duration: "
                     << queue_delta << " ms";
        return;
      }
      const transform::Rigid3d &newest_pose = newest_timed_pose.pose;
      const transform::Rigid3d &oldest_pose = oldest_timed_pose.pose;
      linear_velocity_from_poses_ =
          (newest_pose.translation() - oldest_pose.translation()) / queue_delta;
      angular_velocity_from_poses_ =
          transform::RotationQuaternionToAngleAxisVector(
              oldest_pose.rotation().inverse() * newest_pose.rotation()) /
          queue_delta;
    }

（2）轮速计数据：

// 使用轮速计的位姿数据（不用线速度和角速度），计算线速度和角速度
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);
      // 删除轮速计数据
      TrimOdometryData();
      // 至少两帧
      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();
      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;
      angular_velocity_from_odometry_ =
          transform::RotationQuaternionToAngleAxisVector(
              odometry_pose_delta.rotation()) /
          odometry_time_delta;
      if (timed_pose_queue_.empty())
      {
        return;
      }
      // 计算在轮速计坐标系下的tracking坐标系的速度
      const Eigen::Vector3d
          linear_velocity_in_tracking_frame_at_newest_odometry_time =
              odometry_pose_delta.translation() / odometry_time_delta;
      // 位姿推算odometry_imu_tracker_计算两个时刻的相对旋转：前端计算的最新位姿的时间和轮速计最新数据的时间
      const Eigen::Quaterniond orientation_at_newest_odometry_time =
          timed_pose_queue_.back().pose.rotation() *
          ExtrapolateRotation(odometry_data_newest.time,
                              odometry_imu_tracker_.get());
      // 将线速度转换到前端计算的最新位姿的坐标系下
      linear_velocity_from_odometry_ =
          orientation_at_newest_odometry_time *
          linear_velocity_in_tracking_frame_at_newest_odometry_time;
    }

// 上次前端计算的最新位姿的时间为t,则保留t之前的一帧轮速计数据和t之后所有的轮速计数据
void PoseExtrapolator::TrimOdometryData()
    {
      while (odometry_data_.size() > 2 && !timed_pose_queue_.empty() &&
             odometry_data_[1].time <= timed_pose_queue_.back().time)
      {
        odometry_data_.pop_front();
      }
    }

（3）IMU数据

IMU数据初始化位姿外推器：

std::unique_ptr<PoseExtrapolator> PoseExtrapolator::InitializeWithImu(
    const common::Duration pose_queue_duration,
    const double imu_gravity_time_constant, const sensor::ImuData& imu_data) {
  // 构造位姿外推器
  auto extrapolator = common::make_unique<PoseExtrapolator>(
      pose_queue_duration, imu_gravity_time_constant);、
  // 加入IMU数据队列
  extrapolator->AddImuData(imu_data);
  // 构造imu_tracker_
  extrapolator->imu_tracker_ =
      common::make_unique<ImuTracker>(imu_gravity_time_constant, imu_data.time);
  // imu_tracker加入线加速度，更新orientation
  extrapolator->imu_tracker_->AddImuLinearAccelerationObservation(
      imu_data.linear_acceleration);
  // imu_tracker_加入角速度，更新角速度
  extrapolator->imu_tracker_->AddImuAngularVelocityObservation(
      imu_data.angular_velocity);
  // 进行积分操作
  extrapolator->imu_tracker_->Advance(imu_data.time);
  // 加入初始位姿，只有旋转
  extrapolator->AddPose(
      imu_data.time,
      transform::Rigid3d::Rotation(extrapolator->imu_tracker_->orientation()));
  return extrapolator;
}

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();
    }
// 上次前端计算的最新位姿的时间为t,则保留t之前的一帧IMU数据和t之后所有的IMU数据
void PoseExtrapolator::TrimImuData()
    {
      while (imu_data_.size() > 1 && !timed_pose_queue_.empty() &&
             imu_data_[1].time <= timed_pose_queue_.back().time)
      {
        imu_data_.pop_front();      }
    }

看一下IMU积分是怎么操作的：

void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
                                             ImuTracker *const imu_tracker) const
    {
      CHECK_GE(time, imu_tracker->time());
      // 如果没有IMU数据或者要推算的时间戳之前没有IMU数据，只能用angular_velocity_from_poses_来推算
      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.
        imu_tracker->Advance(time);
        imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());

        imu_tracker->AddImuAngularVelocityObservation(angular_velocity_from_poses_);
        return;
      }
      // 积分到imu_data_.front().time
      if (imu_tracker->time() < imu_data_.front().time)
      {
        // Advance to the beginning of 'imu_data_'.
        imu_tracker->Advance(imu_data_.front().time);
      }
      // 找到所有小于time的IMU数据
      auto it = std::lower_bound(
          imu_data_.begin(), imu_data_.end(), imu_tracker->time(),
          [](const sensor::ImuData &imu_data, const common::Time &time) {
            return imu_data.time < 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推算到当前时间
      imu_tracker->Advance(time);
    }

关于ImuTracker类：

class ImuTracker {
 public:
  ImuTracker(double imu_gravity_time_constant, common::Time time);

  // Advances to the given 'time' and updates the orientation to reflect this.
  // 更新旋转至当前时间time
  void Advance(common::Time time);

  // Updates from an IMU reading (in the IMU frame).
  void AddImuLinearAccelerationObservation(
      const Eigen::Vector3d& imu_linear_acceleration);
  void AddImuAngularVelocityObservation(
      const Eigen::Vector3d& imu_angular_velocity);

  // Query the current time.
  common::Time time() const { return time_; }

  // Query the current orientation estimate.
  Eigen::Quaterniond orientation() const { return orientation_; }

 private:
  const double imu_gravity_time_constant_;
  common::Time time_;
  common::Time last_linear_acceleration_time_;
  Eigen::Quaterniond orientation_;
  Eigen::Vector3d gravity_vector_;
  Eigen::Vector3d imu_angular_velocity_;
};

积分操作：

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_ * rotation).normalized();
  // 更新重力方向
  gravity_vector_ = rotation.conjugate() * gravity_vector_;
  time_ = time;
}

// 添加IMU加速度数据，更新重力方向和rotation，方便可以使用w*dt
// dt = time_ - last_linear_acceleration_time_
// alpha = 1 - e^(-dt/g)
// gravity_vector_ = (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration
void ImuTracker::AddImuLinearAccelerationObservation(
    const Eigen::Vector3d& imu_linear_acceleration) {
  // Update the 'gravity_vector_' with an exponential moving average using the
  // 'imu_gravity_time_constant'.
  const double delta_t =
      last_linear_acceleration_time_ > common::Time::min()
          ? common::ToSeconds(time_ - last_linear_acceleration_time_)
          : std::numeric_limits<double>::infinity();
  last_linear_acceleration_time_ = time_;
  const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
  gravity_vector_ =
      (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;
  // Change the 'orientation_' so that it agrees with the current
  // 'gravity_vector_'.
  const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors(
      gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());
  orientation_ = (orientation_ * rotation).normalized();
  CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
  CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);
}

// IMU角速度赋值
void ImuTracker::AddImuAngularVelocityObservation(
    const Eigen::Vector3d& imu_angular_velocity) {
  imu_angular_velocity_ = imu_angular_velocity;
}

总结：

（1）轮速计数据进来后，直接进行线速度和角速度更新；

（2）位姿数据进来后，更新线速度和角速度，以该时刻为起点，对IMU数据进行积分，更新ImuTracker；

（3）IMU数据进来后，保留最新的一段数据等待进行积分操作。

位姿推算

// 根据时间，推算位姿
transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time)
    {
      // 上次位姿的时间
      const TimedPose &newest_timed_pose = timed_pose_queue_.back();
      CHECK_GE(time, newest_timed_pose.time);
      if (cached_extrapolated_pose_.time != time)
      {
        // dt+t
        const Eigen::Vector3d translation =
            ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
        // q * dq
        const Eigen::Quaterniond rotation =
            newest_timed_pose.pose.rotation() *
            ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
        cached_extrapolated_pose_ =
            TimedPose{time, transform::Rigid3d{translation, rotation}};
      }
      return cached_extrapolated_pose_.pose;
    }
    // 推算旋转
	Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
        const common::Time time, ImuTracker *const imu_tracker) const
    {
      CHECK_GE(time, imu_tracker->time());
      // 局部的imu_tracker:extrapolation_imu_tracker_积分，旋转更新到当前时刻
      AdvanceImuTracker(time, imu_tracker);
      // 纯IMU数据或Pose推算出来的旋转
      const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
      // 计算相对增量dq
      return last_orientation.inverse() * imu_tracker->orientation();
    }
    // 推算平移
	Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time)
    {
      // 取出上一次的位姿
      const TimedPose &newest_timed_pose = timed_pose_queue_.back();
      // 计算时间差
      const double extrapolation_delta =
          common::ToSeconds(time - newest_timed_pose.time);
      // 如果没有轮速计数据，使用位姿推算的速度
      if (odometry_data_.size() < 2)
      {
        return extrapolation_delta * linear_velocity_from_poses_;
      }
      // 如果有轮速计数据，使用轮速计推算的速度
      return extrapolation_delta * linear_velocity_from_odometry_;
    }