其实也就是包括两个方面的内容：类似于运动模型的位姿估计和扫描匹配，因为需要计算速度，所以时间就有必要了！
1. PoseExtrapolator解决了IMU数据、里程计和位姿信息进行融合的问题。
该类定义了三个队列。
std::deque<TimedPose> timed_pose_queue_;
std::deque<sensor::ImuData> imu_data_;
std::deque<sensor::OdometryData> odometry_data_;
定义了(a)通过位姿计算线速度和角速度对象
Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero();
Eigen::Vector3d angular_velocity_from_poses_ = Eigen::Vector3d::Zero();
和(b)通过里程计计算线速度和角速度对象
Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();
Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero();
轮询处理三类消息 IMU消息、里程计消息，激光测距消息。有如下情况：
1）不使用IMU和里程计
只执行AddPose，注意12-15行的代码，$ time{\rm{ }} - timed\_pose\_queue\_\left[ 1 \right].time \ge pose\_queue\_duration\_ $，队列中最前面的数据的时间，当距离当前时间超过一定间隔时执行，作用是将较早时间的数据剔除。接着，根据位姿计算运动速度。对齐IMU数据，里程计数据。
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_ =
common::make_unique<ImuTracker>(gravity_time_constant_, tracker_start);
}
timed_pose_queue_.push_back(TimedPose{time, pose});
while (timed_pose_queue_.size() > &&
timed_pose_queue_[].time <= time - pose_queue_duration_) {
timed_pose_queue_.pop_front();
}
UpdateVelocitiesFromPoses();
AdvanceImuTracker(time, imu_tracker_.get());
TrimImuData();
TrimOdometryData();
odometry_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);
extrapolation_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);
}
第16行，更新了根据Pose计算的线速度和角速度。
void PoseExtrapolator::UpdateVelocitiesFromPoses()
{
if (timed_pose_queue_.size() < )
{
// 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 < 0.001) { // 1 ms
LOG(WARNING) << "Queue too short for velocity estimation. Queue duration: "
<< queue_delta << " ms";
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;
}
PoseExtrapolator::UpdateVelocitiesFromPoses()
17行执行了PoseExtrapolator::AdvanceImuTracker方法，当不使用IMU数据时，将 angular_velocity_from_poses_ 或者 angular_velocity_from_odometry_ 数据传入了imu_tracker.
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
ImuTracker* const imu_tracker) const
{
CHECK_GE(time, imu_tracker->time());
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.
imu_tracker->Advance(time);
imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
imu_tracker->AddImuAngularVelocityObservation(
odometry_data_.size() < ? angular_velocity_from_poses_
: angular_velocity_from_odometry_);
return;
}
if (imu_tracker->time() < imu_data_.front().time) {
// Advance to the beginning of 'imu_data_'.
imu_tracker->Advance(imu_data_.front().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;
});
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->Advance(time);
}
在执行ExtrapolatePose()，推测某一时刻的位姿的时候，调用了 ExtrapolateTranslation 和 ExtrapolateRotation 方法。
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) {
const Eigen::Vector3d translation =
ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
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;
}
可以看到使用的是：（1）旋转，imu_tracker的方位角角的变化量；（2）平移，里程计或者位姿线速度计算的移动量。
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
const common::Time time, ImuTracker* const imu_tracker) const {
CHECK_GE(time, imu_tracker->time());
AdvanceImuTracker(time, imu_tracker);
const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
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() < ) {
return extrapolation_delta * linear_velocity_from_poses_;
}
return extrapolation_delta * linear_velocity_from_odometry_;
}
2）使用IMU和里程计
IMU频率最高，假设消息进入的先后顺序是IMU、里程计，最后是激光消息。
2. RealTimeCorrelativeScanMatcher解决了Scan和子图的扫描匹配的问题。
通过 real_time_correlative_scan_matcher_ 和 ceres_scan_matcher_ 实现的。
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(
const common::Time time, const transform::Rigid2d& pose_prediction,
const sensor::RangeData& gravity_aligned_range_data)
{
std::shared_ptr<const Submap> matching_submap =
active_submaps_.submaps().front();
// The online correlative scan matcher will refine the initial estimate for
// the Ceres scan matcher.
transform::Rigid2d initial_ceres_pose = pose_prediction;
sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.adaptive_voxel_filter_options());
const sensor::PointCloud filtered_gravity_aligned_point_cloud =
adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
if (filtered_gravity_aligned_point_cloud.empty())
{
return nullptr;
}
if (options_.use_online_correlative_scan_matching())
{
real_time_correlative_scan_matcher_.Match(
pose_prediction, filtered_gravity_aligned_point_cloud,
matching_submap->probability_grid(), &initial_ceres_pose);
} auto pose_observation = common::make_unique<transform::Rigid2d>();
ceres::Solver::Summary summary;
ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
filtered_gravity_aligned_point_cloud,
matching_submap->probability_grid(),
pose_observation.get(), &summary);
return pose_observation;
}