【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 可以看到PoseExtrapolator继承PoseExtrapolatorInterface类，那么我们又细细看一下PoseExtrapolatorInterface类：
#ifndef CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_
#define CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_
#include 其中的代码注释已经标明
二、接受数据，并分析处理逻辑
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。