cartographer源码阅读---位姿推测器

工程化的cartographer框架为了实现实时可靠的位姿推断,在算法中实现了位姿推测器(pose_extrapolator), 其作用是通过现有的有关机器人位姿的数据, 给前端匹配一个可靠的目标值进行ceres优化. 也就是以位姿推测器推出来的位姿为前端扫描匹配的目标位姿. 所以前端得到的位姿结果相当依赖于位姿推测器. 前端和位姿推测器的交集如下, 在local_trajectory_builder_2d.cc中:

   range_data_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast<float>());
   ......
   const transform::Rigid3d non_gravity_aligned_pose_prediction =
      extrapolator_->ExtrapolatePose(time);
   ......
  extrapolator_->AddPose(time, pose_estimate);
  ......
  extrapolator_->AddPose(time, transform::Rigid3d::Identity());
  ......
  extrapolator_->AddOdometryData(odometry_data);
  

cartographer为位姿推测器写了个接口, 文件为pose_extrapolator_Interface.cc/h,都是虚函数与结构体定义, 其接口的实现在pose_extrapolator.cc/h中.

1. PoseExtrapolator类

这个类构造函数如下

/**
 * @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()} {}

pose_queue_duration_与gravity_time_constant_都是从lua配置文件中读取的参数, 意思是两次预测位姿需要最小间隔多长时间 与 重力常数.
定义在头文件中的私有变量:

  // 保存一定时间内的pose
  const common::Duration pose_queue_duration_;
  struct TimedPose {
    common::Time time;
    transform::Rigid3d pose;
  };
  std::deque<TimedPose> timed_pose_queue_; //带时间戳的pose
  // 线速度有2种计算途径
  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_;

  // c++11: std::unique_ptr 是独享被管理对象指针所有权的智能指针
  // 它无法复制到其他 unique_ptr, 也无法通过值传递到函数,也无法用于需要副本的任何标准模板库 (STL) 算法
  // 只能通过 std::move() 来移动unique_ptr
  // std::make_unique 是 C++14 才有的特性

  std::unique_ptr<ImuTracker> imu_tracker_;               // 保存与预测当前姿态
  std::unique_ptr<ImuTracker> odometry_imu_tracker_;      // 用于计算里程计的姿态的ImuTracker
  std::unique_ptr<ImuTracker> extrapolation_imu_tracker_; // 用于预测姿态的ImuTracker
  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();

2. 通过odometer进行位姿推测

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

上面判断是不是有两个有效的odometer数据来判断是不是用了odometer. 在odom数据修剪后的队列中得到最新与最老的数据, 计算时间差, 然后通过odometer的位移和角度与时间差微分得到速度和角速度,然后在通过积分得到从上一时刻的位姿(这个位姿是优化后的位姿, 不是odom的位姿)到现在时刻的位姿的转换矩阵,从而推出现在的估计位姿.
修剪odomdata

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

3. 通过IMU进行位姿预测

实际用到的IMU位姿预测没有在pose_extrapolator中实现, 而是调用的imu_tracker. 其核心的预测代码在Advance中, 如下:

/**
 * @brief 预测出time时刻的姿态与重力方向
 * 
 * @param[in] time 要预测的时刻
 */
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_ = (orientation_ * rotation).normalized();

  // 根据预测出的姿态变化量,预测旋转后的线性加速度的值
  gravity_vector_ =
      rotation.conjugate() * // 一般不用inverse,在表示旋转时,共轭即可表示相反的的旋转
      gravity_vector_; 
  // 更新时间
  time_ = time;
}

可以看到, Advance只用到了IMU的角速度信息(陀螺仪), 并没有用到加速度信息(加速度计). 只用陀螺仪积分得到了角度变化, 用角度的变化更新重力的方向.
在PoseExtrapolator类中, 与 ImuTracker类进行交互的方法是AdvanceImuTracker:

/**
 * @brief 更新imu_tracker的状态, 并将imu_tracker的状态预测到time时刻
 * 
 * @param[in] time 要预测到的时刻
 * @param[in] imu_tracker 给定的先验状态
 */
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
                                         ImuTracker* const imu_tracker) const {
  // 检查指定时间是否大于等于 ImuTracker 的时间
  CHECK_GE(time, imu_tracker->time());

  // 不使用imu 或者 预测时间之前没有imu数据的情况
  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.
    // 在time之前没有IMU数据, 因此我们推进ImuTracker, 并使用姿势和假重力产生的角速度来帮助2D稳定
    
    // 预测当前时刻的姿态与重力方向
    imu_tracker->Advance(time);
    // 使用 假的重力数据对加速度的测量进行更新
    imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
    // 只能依靠其他方式得到的角速度进行测量值的更新
    imu_tracker->AddImuAngularVelocityObservation(
        odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_);
    return;
  }

  // imu_tracker的时间比imu数据队列中第一个数据的时间早, 就先预测到imu数据队列中第一个数据的时间
  if (imu_tracker->time() < imu_data_.front().time) {
    // Advance to the beginning of 'imu_data_'.
    imu_tracker->Advance(imu_data_.front().time); //Advance就是进行预测
  }

  // c++11: std::lower_bound() 是在区间内找到第一个大于等于 value 的值的位置并返回, 如果没找到就返回 end() 位置
  // 在第四个参数位置可以自定义比较规则,在区域内查找第一个 **不符合** comp 规则的元素

  // 在imu数据队列中找到第一个时间上 大于等于 imu_tracker->time() 的数据的索引
  auto it = std::
      lower_bound(  // 遍历的imu_data_传给lambda表达式的第一个参数imu_data,
                    // 条件imu_tracker->time()传给lambda表达式的第二个参数time
          imu_data_.begin(), imu_data_.end(), imu_tracker->time(),
          [](const sensor::ImuData& imu_data, const common::Time& time) {
            return imu_data.time < time;
          });

  // 然后依次对imu数据进行预测, 以及添加观测, 直到imu_data_的时间大于等于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的状态预测到time时刻
  imu_tracker->Advance(time);
}

上面代码说明了有或者没有IMU的情况下, 怎么预测旋转. 大家可能会疑惑, 为啥没有IMU还能调用imu_tracker进行Advance? 这要看一下ImuTracker的构造函数

/**
 * @brief Construct a new Imu Tracker:: Imu Tracker object
 * 
 * @param[in] imu_gravity_time_constant 这个值在2d与3d情况下都为10
 * @param[in] time 
 */
ImuTracker::ImuTracker(const double imu_gravity_time_constant,
                       const common::Time time)
    : imu_gravity_time_constant_(imu_gravity_time_constant),
      time_(time),
      last_linear_acceleration_time_(common::Time::min()),
      orientation_(Eigen::Quaterniond::Identity()), // 初始方向角
      gravity_vector_(Eigen::Vector3d::UnitZ()),    // 重力方向初始化为[0,0,1]
      imu_angular_velocity_(Eigen::Vector3d::Zero()) {}

可以看到构造函数里面都有初值, 那么初值是在哪更新的呢?
如果有IMU的话, 直接通过IMU得到的角速度更新:

imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);

如果没有IMU的话, 就只能通过其他方式获得估计的角速度, 即有odometer数据就用odom推测的角速度, 没有的话就用pose推测的角速度.

    imu_tracker->AddImuAngularVelocityObservation(
        odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_);

4. 通过Pose进行位姿预测

由于cartographer中只有激光雷达是必须的, 如果没有odom和IMU, 只用激光雷达推测出的Pose也是可以用位姿推测器的.

// 根据pose队列计算tracking frame 在 local坐标系下的线速度与角速度
void PoseExtrapolator::UpdateVelocitiesFromPoses() {
  if (timed_pose_queue_.size() < 2) {
    // We need two poses to estimate velocities.
    return;
  }
  CHECK(!timed_pose_queue_.empty());
  // 取出队列最末尾的一个 Pose,也就是最新时间点的 Pose,并记录相应的时间
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  const auto newest_time = newest_timed_pose.time;
  // 取出队列最开头的一个 Pose, 也就是最旧时间点的 Pose,并记录相应的时间
  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);
  // 如果时间差小于pose_queue_duration_(1ms), 不进行计算
  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;
  // 平移量除以时间得到 tracking frame 在 local坐标系下的线速度
  linear_velocity_from_poses_ =
      (newest_pose.translation() - oldest_pose.translation()) / queue_delta;
  // 角度变化量除以时间得到角速度得到 tracking frame 在 local坐标系下的角速度
  angular_velocity_from_poses_ =
      transform::RotationQuaternionToAngleAxisVector(
          oldest_pose.rotation().inverse() * newest_pose.rotation()) /
      queue_delta;
}

方法和odom的预测方式大同小异, 就不介绍了.

5. 插入历史位姿AddPose

AddPose是位姿推测器中最重要的函数, 作用是通过插入上一帧前端匹配后的位姿来进行当前时间点的位姿预测. 这个函数调用了上面提到的函数, 预测出角速度与线速度.

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

AddPose只用了pose和imu进行预测, 为啥没见到odom呢?这需要结合local_trajectory_builder_2d.cc这个文件看, 不同传感器运用的方案有不同位姿推测器的方案.

6. 位姿推测器的总结

有3中位姿推测的方法, 但是不同情况下用哪一个呢? 当然是哪个准用哪个, 具体哪个准是工程经验得到的. 预测位姿分为4中情况, 共同点是都是用匀速模型进行预测:

  1. 使用imu, 使用里程计
    平移的预测: 通过里程计数据队列开始和末尾的2个数据计算出的线速度乘以时间
    姿态的预测: 通过imu的角速度乘以时间
  2. 使用imu, 不使用里程计
    平移的预测: 通过pose数据队列开始和末尾的2个数据计算出的线速度乘以时间
    姿态的预测: 通过imu的角速度乘以时间
  3. 不使用imu, 使用里程计
    平移的预测: 通过里程计数据队列开始和末尾的2个数据计算出的线速度乘以时间
    姿态的预测: 通过里程计数据队列开始和末尾的2个数据计算出的角速度乘以时间
  4. 不使用imu, 不用里程计
    平移的预测: 通过pose数据队列开始和末尾的2个数据计算出的线速度乘以时间
    姿态的预测: 通过pose数据队列开始和末尾的2个数据计算出的角速度乘以时间

总结:

  1. 预测平移时: 有里程计就用里程计的线速度, 没有里程计就用pose计算的线速度进行预测
  2. 预测姿态时: 有IMU就用IMU的角速度, 没有IMU时, 如果有里程计就用里程计计算出的角速度, 没有里程计就用pose计算的角速度进行预测
  3. 预测的都是相对值, 要加上最后一个pose的位姿
  • 1
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
### 回答1: 在cartographer中,使用2D点云进行扫描匹配时,可以使用ceresscanmatch功能。这个功能是基于Ceres Solver库实现的。Ceres Solver是一个非线性优化库,用于解决各种最小化问题。在cartographer中,ceresscanmatch被用于解决2D点云匹配的问题。 具体来说,ceresscanmatch用于匹配两个相邻帧的2D点云。在进行扫描匹配时,需要先对数据进行滤波处理,然后使用ceres进行优化,找到两个点云之间的最佳匹配。在这个过程中,需要使用一种优化算法来最小化匹配误差,这个误差是通过计算点云之间的距离来得到的。 相比于其他扫描匹配方法,ceresscanmatch的优势在于它能够进行非常精准的匹配。这是因为它使用了一个非线性优化算法,能够处理复杂的误差函数和约束条件。此外,ceresscanmatch还支持使用多种不同的误差函数,以适应不同的应用场景。 总之,ceresscanmatch是cartographer中用于2D点云扫描匹配的一个非常重要的功能,它能够让我们更加准确、稳定地进行扫描匹配,并且支持广泛的应用场景。 ### 回答2: 本文将继续介绍cartographer中的ceres扫描匹配部分,ceres扫描匹配是利用Ceres Solver进行的位姿优化,可以准确估计机人运动的姿态。 ceres扫描匹配部分主要包括ceres_scan_matcher.cc和ceres_scan_matcher.h两个文件。其中ceres_scan_matcher.cc包含了ceres扫描匹配算法的具体实现,而ceres_scan_matcher.h则是相关的头文件。 ceres_scan_matcher.cc中的函数主要有两个,分别是CeresScanMatcher::Match()和CeresScanMatcher::MatchFullSubmap()。其中,CeresScanMatcher::Match()函数用于实现一次扫描匹配,输入参数为当前激光数据和候选的位姿,输出参数为匹配的位姿和评估值。 在CeresScanMatcher::Match()函数中,先通过叶芽上下文来获取轨迹和submap,然后将当前激光数据转换为点云,并对点云进行滤波和预处理,接着定义优化问题和相关的参数,其中优化问题使用ceres::Problem类来定义,相关参数则定义在CeresScanMatcherOptions结构体中,最后通过ceres::Solve()函数进行位姿优化。 CeresScanMatcher::MatchFullSubmap()函数则用于在整个submap上进行匹配,并返回匹配的位姿和评估值。它的实现与CeresScanMatcher::Match()函数类似,只是输入参数为整个submap的信息。 综上所述,ceres扫描匹配部分利用Ceres Solver进行位姿优化,可以准确估计机人运动的姿态,是cartographer中重要的功能之一。 ### 回答3: cartographer是一款开的SLAM系统,其代码完整透明,方便研究和理解。其中,2D点云扫描匹配是cartographer中的一个重要功能,而这一功能又是由ceres扫描匹配实现的。 ceresscanmatch是cartographer中的一个重要模块,用于实现2D点云的扫描匹配。在这个模块中,ceres solver被用来进行优化过程。具体来说,ceresscanmatch会将已知位姿下的实测点云与预测的点云进行匹配,得到匹配误差。随后,ceres solver会对这些匹配误差进行非线性优化,最终得到最优位姿。这样,就能够实现快速准确的2D点云扫描匹配,从而提高了SLAM系统的性能和精度。 在详细研究ceresscanmatch之前,首先需要了解一下ceres solver。ceres solver是一个基于C++的非线性优化库,用于解决复杂的数值优化问题。在cartographer中,ceres solver被用来进行扫描匹配的优化过程,应用目标函数和求解来寻求最优解。其中,目标函数是由误差项和状态变量构成的,求解则用来解决这个目标函数并确定状态变量的最优化值。 具体而言,在cartographer中,扫描匹配的目标函数是根据传感数据得出的,其包括一系列误差项和参考帧的相对位姿。在每个迭代步骤中,ceres solver会计算目标函数的梯度和海森矩阵,并利用这些值来更新参考帧的位姿。当误差项最小化时,相对位姿就能够得到最优解。 总之,ceresscanmatch是cartographer中的一个重要模块,用于实现2D点云的扫描匹配。借助ceres solver进行优化,可以实现高效准确的扫描匹配,为SLAM系统的实现提供了重要的基础。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值