cartographer之pose_extrapolator

Posted 2022-05-03 COCO_PEAK_NOODLE

pose_extrapolator实现了订阅imu、odom、scan的匹配结果进行位姿的推断，我们先分析一下它的类，几个重要的外部接口，就是public的方法

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

  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;

  //3D 
  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.
  Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override;

然后是内部私有的方法和变量

  void UpdateVelocitiesFromPoses();
  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;
  ;
  std::deque<TimedPose> timed_pose_queue_;
  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_;
  std::unique_ptr<ImuTracker> imu_tracker_;
  std::unique_ptr<ImuTracker> odometry_imu_tracker_;
  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-AddImuData
这个函数就是把imu_data存入双端队列imu_data_中，TrimImuData()的作用是移除imu_data_内过期不用的imu_data数据，后面再详细介绍。

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

2-AddOdometryData
这个函数就是把odometry_data存入odometry_data_。
TrimOdometryData()的作用是移除odometry_data_内过期不用的odometry_data数据，后面再详细介绍。
下面还做了一件事情，就是使用里程计来估计当前小车或agv的线速度和角速度。
这里出现了变量timed_pose_queue_，还有odometry_imu_tracker_，是不是很晕？我们先不管它，再看下一个函数

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;
  

  const Eigen::Vector3d
      linear_velocity_in_tracking_frame_at_newest_odometry_time =
          odometry_pose_delta.translation() / odometry_time_delta;

  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-AddPose
addPose存入的是前端一帧一帧激光匹配的位姿（这里加句写给能看懂的同行，就是两个node之间的位姿，看不懂也不影响），这里把pose存入双端对列timed_pose_queue_。
看到这里我们知道PoseExtrapolator里已经传入了3种数据，分别是IMU、odom、pose，由于pose是由激光匹配产生的，而激光的频率是比IMU和odom要低的，所以这里pose的传入频率比IMU、odom低，比如传入5帧IMU，才插入一个pose。画个图解释一下
这里->
IMU数据存入imu_data_
ODOM数据存入odometry_data_
pose数据存入timed_pose_queue_

timed_pose_queue_会根据保留时间间隔pose_queue_duration_来移除超时的pose，同时，
我们在上面看到的TrimImuData和TrimOdometryData的作用也是移除时间不在timed_pose_queue_范围的数据，准确的说是移除时间小于timed_pose_queue_,front()的数据
我们回到AddPose，它接下来根据pose来估算小车的线速度和角速度，刚才我们看到使用里程计数据也可以估算小车的线速度和角速度，他们之间有什么关系呢？后面再分析
接下来是AdvanceImuTracker，估计又该晕一大片了，这里要详细分析imu_tracker的代码，我先来告诉大家，它的作用就是用一帧一帧IMU来推断当前时刻的机器人位姿，所以叫imu_tracker（imu的跟踪器），就这么简单
接下来TrimImuData和TrimOdometryData的作用已经解释了
最后这2个变量是干啥？

  odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
  extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);

相信很多人都没有弄明白，因为弄明白不是只看一行代码可以理解的，这里我说一下我的理解（应该是对的），我们知道AdvanceImuTracker的作用是使用imu更新当前的状态，但是有些时候我需要保留上一位姿pose的imu_tracker的状态，所以产生了这两个变量，每次调用AddPose，imu_tracker都会同时更新odometry_imu_tracker_和extrapolation_imu_tracker_。
我们再来看看这2个分别起的作用：
odometry_imu_tracker_在AddOdometryData用于辅助计算线速度，extrapolation_imu_tracker_在ExtrapolatePose用于前端辅助实现激光运动畸变矫正。

void PoseExtrapolator::AddPose(const common::Time time,const transform::Rigid3d& pose) 
  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(TimedPosetime, pose);

  // double delta = common::ToSeconds(time - last_time);
  // LOG(INFO) << "AddPose delta " << delta;

  while (timed_pose_queue_.size() > 2 &&
         timed_pose_queue_[1].time <= time - pose_queue_duration_) 
    timed_pose_queue_.pop_front();
  

  UpdateVelocitiesFromPoses();
  AdvanceImuTracker(time, imu_tracker_.get());
  TrimImuData();
  //LOG(INFO) << "okagv AddPose start TrimOdometryData";
  TrimOdometryData();
  //okagv update the odometry_imu_tracker and extrapolation_imu_tracker_
  odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
  extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
  //LOG(INFO) << "timed_pose_queue_ size " << timed_pose_queue_.size();
  //last_time = time;

说到这基本上差不多结束了，但是细节还不止如此，再有领悟再来更新