一、cartographer位姿推断器分析

最新推荐文章于 2025-09-07 16:30:11 发布

slam_coder

最新推荐文章于 2025-09-07 16:30:11 发布

阅读量1.3k

点赞数 18

CC 4.0 BY-SA版权

分类专栏： cartographer 文章标签：自动驾驶

本文链接：https://blog.csdn.net/weixin_42006374/article/details/125112223

cartographer位姿推断器分析

一、使用imu、odom的情况

１、添加imu数据（carto默认是以imu坐标系为`tracking frame`，所以先从添加imu数据开始分析）

void AddSensorData(const std::string& sensor_id, 
				   const sensor::ImuData& imu_data) override {
   
   
   if (local_trajectory_builder_) {
   
   
     local_trajectory_builder_->AddImuData(imu_data);　// 重点函数
   }
    
   // 后端优化计算残差时没用到imu数据，只是预留了添加imu数据到后端残差计算的接口
   pose_graph_->AddImuData(trajectory_id_, imu_data); 
 }

// 着重分析local_trajectory_builder_->AddImuData(imu_data)具体实现如下：

// 将IMU数据加入到Extrapolator中
void LocalTrajectoryBuilder2D::AddImuData(const sensor::ImuData& imu_data) {
   
   
  // 检查是否使用imu
  CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
   
  // InitializeExtrapolator()分析见下文
  InitializeExtrapolator(imu_data.time);
    
  extrapolator_->AddImuData(imu_data);
}

// InitializeExtrapolator(imu_data.time)具体实现如下：

void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) {
   
   
  //　extrapolator_是LocalTrajectoryBuilder2D类里面的一个unique_ptr智能指针，
  //　未初始化时默认是nullptr
  if (extrapolator_ != nullptr) {
   
   
    return;
  }
　
   // 2D建图时，use_imu_based该参数设置为false
  CHECK(!options_.pose_extrapolator_options().use_imu_based());
  // TODO(gaschler): Consider using InitializeWithImu as 3D does.

  // 初始化位姿推测器extrapolator_
  // pose_queue_duratione用于修剪保存的前端匹配位姿队列timed_pose_queue_。
  // 后续分析代码时，再分析该参数的作用。
  extrapolator_ = absl::make_unique<PoseExtrapolator>(
      ::cartographer::common::FromSeconds(options_.pose_extrapolator_options()
                                              .constant_velocity()
                                              .pose_queue_duration()), // 1ms
      options_.pose_extrapolator_options().
      		   constant_velocity().
               imu_gravity_time_constant()); // 10
    
  // 添加初始位姿。AddPose()函数分析见下方
  extrapolator_->AddPose(time, transform::Rigid3d::Identity()); // 重点函数AddPose()
}

// AddPose()具体实现如下：
整个位姿推断器的内容都属于LocalTrajectoryBuilder2D这个大类里面，因此只在初始位姿推断器 \ 以及在前端匹配完后，添加前端匹配的local pose时才调AddPose()这个函数。

// extrapolator_->AddPose(time, transform::Rigid3d::Identity());　　初始化位姿推断器
// extrapolator_->AddPose(time, pose_estimate);　　添加前端匹配位姿

// 这里以添加前端匹配位姿pose_estimate，来分析这个函数AddPose()
void PoseExtrapolator::AddPose(const common::Time time, const transform::Rigid3d& pose) {
   
   
  // imu_tracker_没有初始化就先初始化
  if (imu_tracker_ == nullptr) {
   
   
    common::Time tracker_start = time;　　//　将当前imu数据的时间做为一个起始时间标记
    
    // 从添加imu数据函数AddImuData()可以看到，第一次添加imu数据时，
    // 先初始化位姿位姿推测器extrapolator_，此时还没有添加imu数据，
    // imu_data_.empty()　==　true，因此以第一个imu数据的时间来初始化imu_tracker_
    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_.push_back(TimedPose{
   
   time, pose});

  // 假如前面已经添加了两次前端匹配的位姿到timed_pose_queue_，这是第三姿添加，
  // 因此timed_pose_queue_.size() ＝＝ 3。当这次匹配位姿时间　－　前一次匹配位姿时间　＞ 
  // pose_queue_duration_（1ms）时，将第一次添加的匹配位姿删掉。因此，
  // timed_pose_queue_保存的是　当前这次的匹配位姿　和　上一次的匹配位姿。
  while (timed_pose_queue_.size() > 2 && 
  		 timed_pose_queue_[1].time <= time - pose_queue_duration_) {
   
   
    timed_pose_queue_.pop_front();
  }

  // UpdateVelocitiesFromPoses()函数分析见下文
  // 根据前端匹配位姿计算出平移线速度linear_velocity_from_poses_　和　
  // 旋转角速度angular_velocity_from_poses_。
  UpdateVelocitiesFromPoses();　

  // 函数分析见下文
　//　imu_tracker_记录的时间time_为上一个匹配位姿的时间，
　//  其记录的姿态orientation_和重力向量gravity_vector_都是上一个匹配位姿时刻下的。
　//  AdvanceImuTracker()主要是把上一个匹配位姿时间的姿态orientation_和
　//　重力向量gravity_vector　更新到当前匹配位姿time时刻下。
　//　同时将imu_tracker_记录的time_更新为当前匹配位姿时刻time。
  AdvanceImuTracker(time, imu_tracker_.get()); 
　
　// 函数分析见下文
　// 有了这次的匹配位姿后，这次匹配位姿前的imu数据都可以删掉了
  TrimImuData();
  
　// 函数分析见下文
　// 有了这次的匹配位姿后，这次匹配位姿前的odometry数据都可以删掉了
  TrimOdometryData();

  // 重点：
  // imu_tracker_里面保存了当前匹配位姿时刻下的姿态orientation_和重力向量gravity_vector_,
  // 并利用imu_tracker_的数据重新初始化odometry_imu_tracker_和extrapolation_imu_tracker_。
  // 因此,odometry_imu_tracker_和extrapolation_imu_tracker_也保存了
  // 当前匹配位姿时刻下的姿态orientation_和重力向量gravity_vector_
  odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
  extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
}

// UpdateVelocitiesFromPoses()具体实现如下：

void PoseExtrapolator::UpdateVelocitiesFromPoses() {
   
   
  if (timed_pose_queue_.size() < 2) {
   
   
    // We need two poses to estimate velocities.
    return;
  }
  CHECK(!timed_pose_queue_.empty());

　//　取出timed_pose_queue_队列中的最后一个匹配位姿以及其时间。也就是当前匹配出来的位姿和时间
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  const auto newest_time = newest_timed_pose.time;
  
　//　取出timed_pose_queue_队列中的头部匹配位姿以及其时间,也就是前一次匹配出来的位姿和时间
  const TimedPose& oldest_timed_pose = timed_pose_queue_.front();
  const auto oldest_time = oldest_timed_pose.time;
  
  // 计算当前前端匹配位姿的时间　与　前一次匹配出来的位姿的时间　的差值queue_delta
  const double queue_delta = common::ToSeconds(newest_time - oldest_time);
  
  // 如果queue_delta ＜ pose_queue_duration_（１ms），则不进行后续计算
  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;
}