Cartographer源码阅读（7）：轨迹推算和位姿推算的原理其实也就是包括两个⽅⾯的内容：类似于运动模型的位姿估计和扫描匹配，因为需要计算速度，所以时间就有必要了！
1. PoExtrapolator解决了IMU数据、⾥程计和位姿信息进⾏融合的问题。
该类定义了三个队列。
1 std::deque<TimedPo> timed_po_queue_;
2 std::deque<nsor::ImuData> imu_data_;
3 std::deque<nsor::OdometryData> odometry_data_;
定义了(a)通过位姿计算线速度和⾓速度对象
Eigen::Vector3d linear_velocity_from_pos_ = Eigen::Vector3d::Zero();
Eigen::Vector3d angular_velocity_from_pos_ = Eigen::Vector3d::Zero();
和(b)通过⾥程计计算线速度和⾓速度对象
1 Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();
2 Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero();
轮询处理三类消息 IMU消息、⾥程计消息，激光测距消息。有如下情况：
class的复数形式
1）不使⽤IMU和⾥程计
只执⾏AddPo，注意12-15⾏的代码，time−timed_po_queue_[1].time≥po_queue_duration_，队列中最前⾯的数据的时间，当距离当前时间超过⼀定间隔时执⾏，作⽤是将较早时间的数据剔除。接着，根据位姿计算运动速度。对齐IMU数据，⾥程计数据。
1void PoExtrapolator::AddPo(const common::Time time,
2const transform::Rigid3d& po) {
3if (imu_tracker_ == nullptr) {
4 common::Time tracker_start = time;
5if (!imu_data_.empty()) {
6 tracker_start = std::min(tracker_start, imu_data_.front().time);
7 }
8 imu_tracker_ =
9 common::make_unique<ImuTracker>(gravity_time_constant_, tracker_start);
10 }
11 timed_po_queue_.push_back(TimedPo{time, po});
12while (timed_po_queue_.size() > 2 &&
13 timed_po_queue_[1].time <= time - po_queue_duration_) {
14 timed_po_queue_.pop_front();
15 }
16 UpdateVelocitiesFromPos();
17 AdvanceImuTracker(time, imu_tracker_.get());
18 TrimImuData();
19 TrimOdometryData();
20 odometry_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);
21 extrapolation_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);
22 }
褚酒第16⾏，更新了根据Po计算的线速度和⾓速度。
1void PoExtrapolator::UpdateVelocitiesFromPos()
2 {
3if (timed_po_queue_.size() < 2)
4 {
5// We need two pos to estimate velocities.
6return;
7 }
8 CHECK(!timed_po_queue_.empty());
9const TimedPo& newest_timed_po = timed_po_queue_.back();
10const auto newest_time = newest_timed_po.time;
11const TimedPo& oldest_timed_po = timed_po_queue_.front();
12const auto oldest_time = oldest_timed_po.time;
13const double queue_delta = common::ToSeconds(newest_time - oldest_time);
14if (queue_delta < 0.001) { // 1 ms
15 LOG(WARNING) << "Queue too short for velocity estimation. Queue duration: "
16 << queue_delta << " ms";
17return;
18 }
19const transform::Rigid3d& newest_po = newest_timed_po.po;
20const transform::Rigid3d& oldest_po = oldest_timed_po.po;
21 linear_velocity_from_pos_ =
22 (anslation() - anslation()) / queue_delta;
23 angular_velocity_from_pos_ =
24 transform::RotationQuaternionToAngleAxisVector(
25 ation().inver() * ation()) /
26 queue_delta;
27 }
弱点
PoExtrapolator::UpdateVelocitiesFromPos()
17⾏执⾏了PoExtrapolator::AdvanceImuTracker⽅法，当不使⽤IMU数据时，将angular_velocity_from_pos_ 或者angular_velocity_from_odometry_数据传⼊了imu_tracker.
1void PoExtrapolator::AdvanceImuTracker(const common::Time time,
2 ImuTracker* const imu_tracker) const
3 {
4 CHECK_GE(time, imu_tracker->time());
5if (imu_data_.empty() || time < imu_data_.front().time)
6 {
7// There is no IMU data until 'time', so we advance the ImuTracker and u
8// the angular velocities from pos and fake gravity to help 2D stability.
9 imu_tracker->Advance(time);
10 imu_tracker->AddImuLinearAccelerationObrvation(Eigen::Vector3d::UnitZ());
11 imu_tracker->AddImuAngularVelocityObrvation(
12 odometry_data_.size() < 2 ? angular_velocity_from_pos_
13 : angular_velocity_from_odometry_);
六级模拟题
14return;
15 }
16if (imu_tracker->time() < imu_data_.front().time) {
17// Advance to the beginning of 'imu_data_'.通往天国的阶梯
18 imu_tracker->Advance(imu_data_.front().time);
19 }
20 auto it = std::lower_bound(
21 imu_data_.begin(), imu_data_.end(), imu_tracker->time(),
22 [](const nsor::ImuData& imu_data, const common::Time& time) {
23return imu_data.time < time;
24 });
25while (it != imu_data_.end() && it->time < time) {
26 imu_tracker->Advance(it->time);
27 imu_tracker->AddImuLinearAccelerationObrvation(it->linear_acceleration);
28 imu_tracker->AddImuAngularVelocityObrvation(it->angular_velocity);
29 ++it;
30 }
31 imu_tracker->Advance(time);
32 }
在执⾏ExtrapolatePo()，推测某⼀时刻的位姿的时候，调⽤了ExtrapolateTranslation 和 ExtrapolateRotation ⽅法。
1 transform::Rigid3d PoExtrapolator::ExtrapolatePo(const common::Time time) {
2const TimedPo& newest_timed_po = timed_po_queue_.back();
3 CHECK_GE(time, newest_timed_po.time);
4if (cached_extrapolated_po_.time != time) {
5const Eigen::Vector3d translation =
6 ExtrapolateTranslation(time) + newest_timed_anslation();
7const Eigen::Quaterniond rotation =
自我效能理论
8 newest_timed_ation() *
9 ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
10 cached_extrapolated_po_ =
11 TimedPo{time, transform::Rigid3d{translation, rotation}};
12 }
13return cached_extrapolated_po_.po;
14 }
可以看到使⽤的是：（1）旋转，imu_tracker的⽅位⾓⾓的变化量；（2）平移，⾥程计或者位姿线速度计算的移动量。
1 Eigen::Quaterniond PoExtrapolator::ExtrapolateRotation(
2const common::Time time, ImuTracker* const imu_tracker) const {
3 CHECK_GE(time, imu_tracker->time());
4 AdvanceImuTracker(time, imu_tracker);
5const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
6return last_orientation.inver() * imu_tracker->orientation();
7 }
8
9 Eigen::Vector3d PoExtrapolator::ExtrapolateTranslation(common::Time time) {
10const TimedPo& newest_timed_po = timed_po_queue_.back();
11const double extrapolation_delta =
12 common::ToSeconds(time - newest_timed_po.time);
13if (odometry_data_.size() < 2) {
14return extrapolation_delta * linear_velocity_from_pos_;
15 }
16return extrapolation_delta * linear_velocity_from_odometry_;
17 }
2）使⽤IMU和⾥程计
IMU频率最⾼，假设消息进⼊的先后顺序是IMU、⾥程计，最后是激光消息。
2. RealTimeCorrelativeScanMatcher解决了Scan和⼦图的扫描匹配的问题。通过 real_time_correlative_scan_matcher_ 和ceres_scan_matcher_ 实现的。
1 std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(
2const common::Time time, const transform::Rigid2d& po_prediction,
3const nsor::RangeData& gravity_aligned_range_data)
4 {
5 std::shared_ptr<const Submap> matching_submap =
6 active_submaps_.submaps().front();
7// The online correlative scan matcher will refine the initial estimate for
8// the Ceres scan matcher.
9 transform::Rigid2d initial_ceres_po = po_prediction;
10 nsor::AdaptiveVoxelFilter adaptive_voxel_filter(
11 options_.adaptive_voxel_filter_options());
12const nsor::PointCloud filtered_gravity_aligned_point_cloud =
13 adaptive_voxel_filter.Filter(gravity_aligned_urns);
14if (filtered_gravity_aligned_pty())
15 {
16return nullptr;
17 }
18if (options_.u_online_correlative_scan_matching())
19 {
20 real_time_correlative_scan_matcher_.Match(
21 po_prediction, filtered_gravity_aligned_point_cloud,
22 matching_submap->probability_grid(), &initial_ceres_po);
天下大事必做于细23 }
24
25 auto po_obrvation = common::make_unique<transform::Rigid2d>();
26 ceres::Solver::Summary summary;
27 ceres_scan_matcher_.Match(anslation(), initial_ceres_po,
28 filtered_gravity_aligned_point_cloud,
29 matching_submap->probability_grid(),
30 (), &summary);非攻鲁迅
31return po_obrvation;
32 }
Processing math: 100%
