【cartographer源码解析--外推器】

cartographer源码解析–外推器


文章目录

  • cartographer源码解析--外推器
  • 前言
  • 一、cartographer中的PoseExtrapolator类
  • 二、接受数据,并分析处理逻辑
    • 1、主逻辑AddPose
    • 2、odom的数据
    • 3、imu数据
  • 三、位姿推断
  • 四、总结


前言

在学习cartographer源码的时候发现外推器这部分总是不能更好的理解和应用,这里就详细分析cartographer的外推器,记录以便方便后续翻找查看。


一、cartographer中的PoseExtrapolator类

当分析到cartographer前端的核心类:LocalTrajectoryBuilder时,其正是进行局部地图构建和轨迹构建的关键,而在该类的轨道建设工作中,有一个重要成员(std:: unique_ptr < PoseExtrapolator > extrapolator_),其在LocalTrajectoryBuilder2D(3D)中功能简单的来说就是保持一段时间的pose并估计线性速度和角速度以推断运动(旋转与位移)和使用IMU和(或)odometry数据以改善extrapolation(外推)。
进一步的我们将讨论PoseExtrapolator类如何在处理IMU、Odom和ScanPose等数据时工作,以达到“获取相对准确的先验位姿估计”的目的。
那么我们就看看cartographer的pose_extrapolator.h文件

#ifndef CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_H_
#define CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_H_

#include 
#include 

#include "cartographer/common/time.h"
#include "cartographer/mapping/imu_tracker.h"
#include "cartographer/mapping/pose_extrapolator_interface.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/rigid_transform.h"

namespace cartographer {
namespace mapping {

// Keep poses for a certain duration to estimate linear and angular velocity.
// Uses the velocities to extrapolate motion. Uses IMU and/or odometry data if
// available to improve the extrapolation.
class PoseExtrapolator : public PoseExtrapolatorInterface {
 public:
  explicit PoseExtrapolator(common::Duration pose_queue_duration,
                            double imu_gravity_time_constant);

  PoseExtrapolator(const PoseExtrapolator&) = delete;
  PoseExtrapolator& operator=(const PoseExtrapolator&) = delete;

  //ium初始化
  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.
  // 返回上次外推器推算后位姿的时间戳,如果外推器不存在则返回Time::min()
  common::Time GetLastPoseTime() const override;
  common::Time GetLastExtrapolatedTime() const override;

  // 前端计算位姿后传入pose的接口,更新外推器相关变量
  void AddPose(common::Time time, const transform::Rigid3d& pose) override;
  void AddImuData(const sensor::ImuData& imu_data) override;
  void AddOdometryData(const sensor::OdometryData& odometry_data) override;
  // 位姿外推器推算位姿的调用接口
  transform::Rigid3d ExtrapolatePose(common::Time time) override;

  ExtrapolationResult ExtrapolatePosesWithGravity(
      const std::vector<common::Time>& times) override;

  // Returns the current gravity alignment estimate as a rotation from
  // the tracking frame into a gravity aligned frame.
  // 估计重力方向的接口,对应IMU,如果没有IMU,则返回单位四元数
  Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override;

 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();
};

}  // namespace mapping
}  // namespace cartographer

#endif  // CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_H_

可以看到PoseExtrapolator继承PoseExtrapolatorInterface类,那么我们又细细看一下PoseExtrapolatorInterface类:

#ifndef CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_
#define CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_

#include 
#include 

#include "cartographer/common/time.h"
#include "cartographer/mapping/proto/pose_extrapolator_options.pb.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/timestamped_transform.h"

namespace cartographer {
namespace mapping {

proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions(
    common::LuaParameterDictionary* const parameter_dictionary);

class PoseExtrapolatorInterface {
 public:
  struct ExtrapolationResult {
    // The poses for the requested times at index 0 to N-1.
    std::vector<transform::Rigid3f> previous_poses;
    // The pose for the requested time at index N.
    transform::Rigid3d current_pose;
    Eigen::Vector3d current_velocity;
    Eigen::Quaterniond gravity_from_tracking;
  };

  PoseExtrapolatorInterface(const PoseExtrapolatorInterface&) = delete;
  PoseExtrapolatorInterface& operator=(const PoseExtrapolatorInterface&) =
      delete;
  virtual ~PoseExtrapolatorInterface() {}

  // TODO: Remove dependency cycle.
  static std::unique_ptr<PoseExtrapolatorInterface> CreateWithImuData(
      const proto::PoseExtrapolatorOptions& options,
      const std::vector<sensor::ImuData>& imu_data,
      const std::vector<transform::TimestampedTransform>& initial_poses);

  // Returns the time of the last added pose or Time::min() if no pose was added
  // yet.
  virtual common::Time GetLastPoseTime() const = 0;
  virtual common::Time GetLastExtrapolatedTime() const = 0;

  virtual void AddPose(common::Time time, const transform::Rigid3d& pose) = 0;
  virtual void AddImuData(const sensor::ImuData& imu_data) = 0;
  virtual void AddOdometryData(const sensor::OdometryData& odometry_data) = 0;
  virtual transform::Rigid3d ExtrapolatePose(common::Time time) = 0;

  virtual ExtrapolationResult ExtrapolatePosesWithGravity(
      const std::vector<common::Time>& times) = 0;

  // Returns the current gravity alignment estimate as a rotation from
  // the tracking frame into a gravity aligned frame.
  virtual Eigen::Quaterniond EstimateGravityOrientation(common::Time time) = 0;

 protected:
  PoseExtrapolatorInterface() {}
};

}  // namespace mapping
}  // namespace cartographer

#endif  // CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_

其中的代码注释已经标明

二、接受数据,并分析处理逻辑

1、主逻辑AddPose

上代码

// 根据匀速运动假设,计算线速度和角速度,并删除旧数据,更新相关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_ =
        absl::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_ = absl::make_unique<ImuTracker>(*imu_tracker_);
  extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
}

其中UpdateVelocitiesFromPoses()函数调用计算linear_velocity_from_poses_和angular_velocity_from_poses_是在匀速运动假设下完成的。
函数实现如下:

void PoseExtrapolator::UpdateVelocitiesFromPoses() {
  //数据大小必须大于2,因为小于等于二没法作比较
  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 < 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;
  //线速度
  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、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);
  // 查看其定义,删除timed_pose_queue_之前的odometry
  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;
}

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 = absl::make_unique<PoseExtrapolator>(
      pose_queue_duration, imu_gravity_time_constant);
  // 加入IMU数据队列
  extrapolator->AddImuData(imu_data);
  // 构造imu_tracker_
  extrapolator->imu_tracker_ =
      absl::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();
}

imu积分

// IMU积分操作
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
                                         ImuTracker* const imu_tracker) const 
  // 时间检查,不往历史外推,传入的实参是extrapolation_imu_tracker_
  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.
    // 点击Advance()进入imu_tracker.cc,主要完成更新orientation,以及重力向量的表示
    imu_tracker->Advance(time);
    // 添加IMU的线加速度观测,没有IMU数据,只有z方向的重力加速度,添加观测值,是来纠正用的
    imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
    // 添加IMU角速度观测值
    imu_tracker->AddImuAngularVelocityObservation(
        odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_);
    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);
}

其中advance函数实现如下:

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
代码如下:

// 主要完成互补滤波
// 添加IMU加速度数据,更新重力方向和rotation,方便可以使用w*dt
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 = 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就剩下最后一步了,imu赋值

void ImuTracker::AddImuAngularVelocityObservation(
    const Eigen::Vector3d& imu_angular_velocity) {
  imu_angular_velocity_ = imu_angular_velocity;
}

到这里传感器数据就处理了,详细如下3步:
(1)轮速计数据进来后,直接进行线速度和角速度更新;
(2)位姿数据进来后,更新线速度和角速度,以该时刻为起点,对IMU数据进行积分,更新ImuTracker;
(3)IMU数据进来后,保留最新的一段数据等待进行积分操作。

三、位姿推断

到这里才是好戏开始
传入前端时间推算位姿

// 传入参数是时间,根据时间,推算位姿
transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
  // 得到前端pose的时间
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  // 检查,当前时间要比前段时间晚
  CHECK_GE(time, newest_timed_pose.time);
  // 如果,外推的pose的时间跟当前的时间不相等,则外推当前时间的pose
  // 外推就是预估当前点的平移和旋转
  if (cached_extrapolated_pose_.time != time) {
    // 上一时刻的平移加上当前的点预估的平移,点击ExtrapolateTranslation(time)函数查看怎么预估的?
    // dt+t
    const Eigen::Vector3d translation =
        ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
    // rotation使用四元数表示, 里程计最后时间的旋转 * 预估的当前点的旋转
    // 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 {
  // 检查当前点的时间晚于前一次记录的时间,传入的实参是extrapolation_imu_tracker_
  CHECK_GE(time, imu_tracker->time());
  // 点击查看此函数功能:局部的imu_tracker:extrapolation_imu_tracker_积分,旋转更新到当前时刻
  AdvanceImuTracker(time, imu_tracker);
  // 上一次的旋转last_orientation赋值为imu_tracker_的orientation
  // imu_tracker_一直记录前端pose的orientation,而extrapolation_imu_tracker_是(预估)外推pose的orientation
  // 纯IMU数据或Pose推算出来的旋转
  const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
  // 返回的是delta_orientation,他等于上一次pose的orientation的逆乘以传入的extrapolation_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) {
    // 如果里程计的数据小于2,则返回 delta_time * 历史pose的线速度 = 平移量
    return extrapolation_delta * linear_velocity_from_poses_;
  }
  // 如果里程计数据足够,则 delta_time * 从里程计得来的线速度 = 平移量
  // 如果有轮速计数据,使用轮速计推算的速度
  return extrapolation_delta * linear_velocity_from_odometry_;
}

四、总结

外推器如果不注意看有很多地方不好理解,平移在odom数据足够多的时候使用的是odom,而旋转直接使用的imu。

你可能感兴趣的:(c++,人工智能,学习,自动驾驶)