pose extrapolator简要分析

Y3_flybird

已于 2024-03-28 17:19:19 修改

阅读量634

点赞数 7

分类专栏： SLAM 文章标签：算法

于 2024-03-28 17:17:04 首次发布

本文链接：https://blog.csdn.net/Y3_flybird/article/details/137038548

版权

SLAM 专栏收录该内容

4 篇文章 0 订阅

订阅专栏

前言

cartographer中的位姿外推器作为google cartographer代码中的核心类：集成了估计线速度、角速度，推算运动（旋转与位移）和初级的集中式多传感器数据级融合，等等，更重要的是该类：PoseExtrapolator在SLAM和定位功能节点中可用作点云数据去畸变、点云配准初值提供、重力方向校准等一系列重要基础功能的集成，是定位算法中非常重要的一个模块。

一、相关基础类

1.1 ImuTracker

1.1.1 功能简介

使用IMU data的角速度和线加速度跟踪和维护orientation，由于IMU的频率高，在瞬时则可以假定平均线性加速度(相对缓慢的移动)是重力的直接测量，roll/pitch没有漂移，而yaw会有。

1.1.2 成员变量

名称	类型	注释
imu_gravity_time_constant_	const double	更新gravity_vector_时“加权平均”系数的变化参数,用于调整更新速度
time_	common::Time	当前时间戳
last_linear_acceleration_time_	common::Time	上一次计算线加速度时间戳，结合time_记录实例化ImuTracker的更新时间戳
orientation_	Eigen::Quaterniond	当前方向估计
gravity_vector_	Eigen::Vector3d	重力向量
imu_angular_velocity_	Eigen::Vector3d	IMU角速度

1.1.3 主要函数

  // 预测出time时刻的姿态与重力方向
  void Advance(common::Time time);
  // 更新线性加速度的值,并根据重力的方向对上一时刻的姿态进行校准
  void AddImuLinearAccelerationObservation(
      const Eigen::Vector3d& imu_linear_acceleration);
  // 更新IMU角速度
  void AddImuAngularVelocityObservation(
      const Eigen::Vector3d& imu_angular_velocity);

二、PoseExtrapolator维护的相关变量

名称	类型	注释
imu_data_	std::dequesensor::ImuData	存储imu data的队列
timed_pose_queue_	std::deque	存储timedpose的队列
odometry_data_	std::dequesensor::OdometryData	存储odom的队列
imu_data_	std::dequesensor::ImuData	存储imu data的队列

linearvelocity_from_poses_	Eigen::Vector3d>	通过点云匹配的位姿获取的线速度
angular_velocity_from_poses_	Eigen::Vector3d>	通过点云匹配的位姿获取的角速度

linear velocity_from_odometry	Eigen::Vector3d>	通过轮式里程计获取的线速度
angular_velocity_from_odometry_	Eigen::Vector3d>	通过轮式里程计获取的角速度

imu_tracker_	std::unique_ptr	1.保持最新的CSM匹配结果的姿态;2.跟踪维护CSM当前激光帧frame姿态的旋转信息
odometry_imu_tracker_	std::unique_ptr	1.保持最新ODOM位姿的递归姿态2.跟踪维护当前里程计frame下的姿态的旋转量信息
extrapolation_imu_tracker_	std::unique_ptr	1.保持最新推算位姿的递归姿态2.跟踪维护当前推算位姿frame下的姿态的旋转量信息

三、PoseExtrapolator成员函数

3.1 私有函数

3.1.1 UpdateVelocitiesFromPoses

作用：根据pose队列计算tracking frame 在 local坐标系下的线速度与角速度
实现步骤：
- 从timed_pose_queue_队尾和对首取两个节点速度
- 平移量除以时间得到tracking frame 在 local坐标系下的线速度
- 角度变化量除以时间得到角速度得到 tracking frame 在 local坐标系下的角速度

3.1.2 TrimImuData

作用：丢弃imu_data_旧数据
实现步骤：将索引为1的成员的时间戳与timed_pose_queue_中最新成员的时间戳进行比较。若imu_data_[1]数据更早则进行imu_data_.pop_front()

3.1.3 TrimOdometryData

作用实现步骤同上

3.1.4 AdvancelmuTracker

作用：更新imu_tracker的状态, 并将imu_tracker的状态预测到time时刻
代码及注释：

/**
 * @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 {
  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);
  }
  // 在imu数据队列中找到第一个时间上 大于等于 imu_tracker->time() 的数据的索引
  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;
      });
  // 然后依次对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);
}

3.1.5 ExtrapolateRotation

作用：计算从imu_tracker到time时刻的姿态变化量
代码及注释：

Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
    const common::Time time, ImuTracker* const imu_tracker) const {
  CHECK_GE(time, imu_tracker->time());
  // 更新imu_tracker的状态到time时刻
  AdvanceImuTracker(time, imu_tracker);
  // 通过imu_tracker_获取上一次位姿校准时的姿态
  const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
  // 求取上一帧到当前时刻预测出的姿态变化量：上一帧姿态四元数的逆 乘以 当前时刻预测出来的姿态四元数
  return last_orientation.inverse() * imu_tracker->orientation();
}

3.1.6 ExtrapolateTranslation

作用：返回从最后一个位姿的时间到time时刻的tracking frame在local坐标系下的平移量
代码及注释：

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);
  // 使用tracking frame 在 local坐标系下的线速度 乘以时间 得到平移量的预测
  if (odometry_data_.size() < 2) {
    // 如果不使用里程计就使用通过pose计算出的线速度（其中的pose指的是由CSM获得的姿态）
    return extrapolation_delta * linear_velocity_from_poses_;
  }
  return extrapolation_delta * linear_velocity_from_odometry_;
}

3.2 公有函数

PoseExtrapolator类中的public成员函数除了定义了构造函数、初始化函数和一些数据信息获取的函数外，关键的是定义了关于数据输入处理的接口函数（AddPose，AddImuData和AddOdometryData）和外推位姿输出函数ExtrapolatePose。
基本的调用和使用的场景是：

当传感器数据预处理完成后（时间同步、垃圾和无效数据过滤等），或者激光匹配计算出位姿后会通过输入处理的接口函数对imuTracker、linear velocity和angular velocity进行更新维护；
当需要为激光匹配提供初值和为点云进行去畸变时，则通过外推位姿输出函数ExtrapolatePose（time）插值估算出对应时间戳的位姿。

3.2.1 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_ =
        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_[1].time <= time - pose_queue_duration_) {
    timed_pose_queue_.pop_front();
  }
  // 根据加入的pose计算线速度与角速度
  UpdateVelocitiesFromPoses();
  // 将imu_tracker_更新到time时刻
  AdvanceImuTracker(time, imu_tracker_.get());
  // 过期数据删除
  TrimImuData();
  TrimOdometryData();
  // 用于根据里程计数据计算线速度时姿态的预测
  odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
  // 用于位姿预测时的姿态预测
  extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
}

3.2.2 AddlmuData

作用：由于系统调用过程中IMU频率最高，故而在进行数据的处理时假设信息的顺序是IMU，odometry data，最后是激光匹配计算出来的位姿信息。获取到新的IMU数据只是对imu_data队列进行更新（当且仅当imu_data的时间戳比timed_pose_queue_.back()的时间戳更“fresh”时，才需要将imu数据回推到imu_data_的deque中）

3.2.3 AddOdometryData

作用：向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();
  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;
}

3.2.4 ExtrapolatePose

作用：向odom数据队列中添加odom数据,并进行数据队列的修剪,并计算角速度与线速度
代码及注释：

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) {
    // 预测tracking frame在local坐标系下time时刻的位置
    const Eigen::Vector3d translation =
        ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
    // 预测tracking frame在local坐标系下time时刻的姿态    
    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;
}

3.2.5 EstimateGravityOrientation

作用：计算重力向量,用imu-tracker来进行计算，它主要关注重力对齐估计，并将当前的重力对齐估计作为从跟踪帧到重力对齐帧的旋转返回。
代码及注释：

Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation(
    const common::Time time) {
  ImuTracker imu_tracker = *imu_tracker_;
  AdvanceImuTracker(time, &imu_tracker);
  return imu_tracker.orientation();
}

四、总结

cartographer中的位姿外推器Pose_Extrapolator_是假定使用在一个低速环境，具体来讲就是简单的室内场景中慢速的建图定位场景，如果在室外或者高速，甚至无人驾驶中使用相同的PoseExtrapolator类处理思路的话，就不再使用。另外PoseExtrapolator类利用的惯性传感器进行translation推算主要通过linear_velocity_from_odometry_及基于轮式里程计的估算速度进行推算。在无轮式里程计或者里程计失效（打滑机械磨损等）情况下又会严重影响激光匹配精度。这也是为什么我们在初期使用cartographer建图开启里程计效果极差的原因之一。

较好的处理方式就是对另一个惯性传感器元件—IMU的数据进行“深入”处理，这也是近年来的发展趋势：从LiDAR-SLAM system -> LiDAR-Inertial SLAM system。而与当前PoseExtrapolator类的设计思路较为贴合的IMU处理方式则是IMU预积分及其因子图优化，该方法可以“完全”融合来自两种类型的传感器，而且由于IMU bias的在线估计，该方式模型可以获得更好的结果。
具体的来说就是需要在当前的IMU数据处理的基础上（orientation更新、重力方向校准、推算关于timestamp的Rotation），再通过IMU预积分获取一个新的imu_odometry，然后再基于imu_odometry来更新类似velocity_from_odometry_、velocity_from_poses_的velocity_from_imu_odometry_。

当然，若是确立了IMU预积分的处理方式，另一个容易被忽略但相当重要的模块就是IMU的外参标定和初始化了，外参标定话方法有很多，这次不做细节上的展开，初始化的过程可以参考的思路是将IMU预积分值与CSM计算所得的相对位姿值对齐来估计初始时刻的重力向量，从而将系统与世界系对齐。可参考使用LIO-SAM中利用gtsam实现的imu预积分模块。

Y3_flybird

关注

7
点赞
踩
13

收藏

觉得还不错? 一键收藏
0
评论
pose extrapolator简要分析

cartographer中的位姿外推器作为google cartographer代码中的核心类：集成了估计线速度、角速度，推算运动（旋转与位移）和初级的集中式多传感器数据级融合，等等，更重要的是该类：PoseExtrapolator在SLAM和定位功能节点中可用作点云数据去畸变、点云配准初值提供、重力方向校准等一系列重要基础功能的集成，是定位算法中非常重要的一个模块。
复制链接

扫一扫