本次阅读的源码为 release-1.0 版本的代码

https://github.com/googlecartographer/cartographer_ros/tree/release-1.0

https://github.com/googlecartographer/cartographer/tree/release-1.0

也可以下载我上传的全套工作空间的代码，包括 protobuf, cartographer, cartographer_ros, ceres,

https://download.csdn.net/download/tiancailx/11224156

第二篇文章分析传感器数据的处理流程，这篇文章将进行如何使用　传感器数据以及扫描匹配的处理过程。

GlobalTrajectoryBuilder类是连接 local SLAM 与后端优化的桥梁，它将　传感器信息　和　local_slam匹配的结果传入到pose_graph中。

cartographer/mapping/internal/global_trajectory_builder.cc

template <typename LocalTrajectoryBuilder, typename PoseGraph>
class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {public:// Passing a 'nullptr' for 'local_trajectory_builder' is acceptable, but no// 'TimedPointCloudData' may be added in that case.GlobalTrajectoryBuilder(std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder,const int trajectory_id, PoseGraph* const pose_graph,const LocalSlamResultCallback& local_slam_result_callback): trajectory_id_(trajectory_id),pose_graph_(pose_graph),local_trajectory_builder_(std::move(local_trajectory_builder)),local_slam_result_callback_(local_slam_result_callback) {}~GlobalTrajectoryBuilder() override {}GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;void AddSensorData(const std::string& sensor_id,const sensor::TimedPointCloudData& timed_point_cloud_data) override {CHECK(local_trajectory_builder_)<< "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>matching_result = local_trajectory_builder_->AddRangeData(sensor_id, timed_point_cloud_data);if (matching_result == nullptr) {// The range data has not been fully accumulated yet.return;}kLocalSlamMatchingResults->Increment();std::unique_ptr<InsertionResult> insertion_result;if (matching_result->insertion_result != nullptr) {kLocalSlamInsertionResults->Increment();auto node_id = pose_graph_->AddNode(matching_result->insertion_result->constant_data, trajectory_id_,matching_result->insertion_result->insertion_submaps);CHECK_EQ(node_id.trajectory_id, trajectory_id_);insertion_result = common::make_unique<InsertionResult>(InsertionResult{node_id, matching_result->insertion_result->constant_data,std::vector<std::shared_ptr<const Submap>>(matching_result->insertion_result->insertion_submaps.begin(),matching_result->insertion_result->insertion_submaps.end())});}if (local_slam_result_callback_) {local_slam_result_callback_(trajectory_id_, matching_result->time, matching_result->local_pose,std::move(matching_result->range_data_in_local),std::move(insertion_result));}}void AddSensorData(const std::string& sensor_id,const sensor::ImuData& imu_data) override {if (local_trajectory_builder_) {local_trajectory_builder_->AddImuData(imu_data);}pose_graph_->AddImuData(trajectory_id_, imu_data);}void AddSensorData(const std::string& sensor_id,const sensor::OdometryData& odometry_data) override {CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;if (local_trajectory_builder_) {local_trajectory_builder_->AddOdometryData(odometry_data);}pose_graph_->AddOdometryData(trajectory_id_, odometry_data);}void AddSensorData(const std::string& sensor_id,const sensor::FixedFramePoseData& fixed_frame_pose) override {if (fixed_frame_pose.pose.has_value()) {CHECK(fixed_frame_pose.pose.value().IsValid())<< fixed_frame_pose.pose.value();}pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);}void AddSensorData(const std::string& sensor_id,const sensor::LandmarkData& landmark_data) override {pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);}void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>local_slam_result_data) override {CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with ""local_trajectory_builder_ present.";local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_);}private:const int trajectory_id_;PoseGraph* const pose_graph_;std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;LocalSlamResultCallback local_slam_result_callback_;
};

可以看到，通过重载函数AddSensorData() 将 lidar imu odom数据传入到 local_trajectory_builder_2d.h 下的对应方法中AddImuData() .

void AddSensorData(const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) 方法将雷达数据传入local slam ，得到匹配的位姿之后，在进行后端优化。

cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

通过global_trajectory_builder.cc 对 AddImuData() 和 AddOdometryData() 进行调用，将imu 和 odom 数据传入 pose extrapolator 进行位资预测。

LocalTrajectoryBuilder2D::AddRangeData() 方法中存在一个变量 num_accumulated_ ，这个变量对应着配置文件的 TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 2 参数，就是将几帧数据合成一组点云数据。

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(...)
{// ...if (num_accumulated_ >= options_.num_accumulated_range_data()) {num_accumulated_ = 0;const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(extrapolator_->EstimateGravityOrientation(time));// TODO(gaschler): This assumes that 'range_data_poses.back()' is at time// 'time'.accumulated_range_data_.origin = range_data_poses.back().translation();return AddAccumulatedRangeData(time,TransformToGravityAlignedFrameAndFilter(gravity_alignment.cast<float>() * range_data_poses.back().inverse(),accumulated_range_data_),gravity_alignment);}
}

之后进入LocalTrajectoryBuilder2D::AddAccumulatedRangeData() ，首先使用 Extrapolator 先根据之前时刻的线速度和角速度乘以时间来对下一时刻的位姿进行预测，然后将这个预测的位姿输入到scanMatch()中，对其进行最优化求解。

位姿预测

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(const common::Time time,const sensor::RangeData& gravity_aligned_range_data,const transform::Rigid3d& gravity_alignment) {if (gravity_aligned_range_data.returns.empty()) {LOG(WARNING) << "Dropped empty horizontal range data.";return nullptr;}// Computes a gravity aligned pose prediction.// 计算重力对齐的姿势预测。const transform::Rigid3d non_gravity_aligned_pose_prediction =extrapolator_->ExtrapolatePose(time);const transform::Rigid2d pose_prediction = transform::Project2D(non_gravity_aligned_pose_prediction * gravity_alignment.inverse());// local map frame <- gravity-aligned framestd::unique_ptr<transform::Rigid2d> pose_estimate_2d =ScanMatch(time, pose_prediction, gravity_aligned_range_data);

可以看到，在进行 PoseExtrapolator 初始化的时候将估计pose的时间间隔固定在了0.001s，也就是最小1ms进行一次姿态预测（对不对？？？）

// cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) {if (extrapolator_ != nullptr) {return;}// We derive velocities from poses which are at least 1 ms apart for numerical// stability. Usually poses known to the extrapolator will be further apart// in time and thus the last two are used.// 我们从姿势中获得速度，这些姿势相对于数值稳定性至少间隔1 ms。 // 通常，外推器已知的姿势将在时间上进一步分开，因此使用最后两个。constexpr double kExtrapolationEstimationTimeSec = 0.001;// TODO(gaschler): Consider using InitializeWithImu as 3D does.extrapolator_ = common::make_unique<PoseExtrapolator>(::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),options_.imu_gravity_time_constant());extrapolator_->AddPose(time, transform::Rigid3d::Identity());
}// cartographer/mapping/pose_extrapolator.h// Keep poses for a certain duration to estimate linear and angular velocity.
// Uses the velocities to extrapolate motion. Uses IMU and/or odometry data if
// available to improve the extrapolation.
//保持姿势一定时间以估计线性和角速度。
//使用速度来推断运动。 如果可用，使用IMU和/或测距数据来改进外推。
class PoseExtrapolator {public:explicit PoseExtrapolator(common::Duration pose_queue_duration,double imu_gravity_time_constant);
}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;
}

使用 ExtrapolatePose(const common::Time time) 类获得预测后的位姿。这个方法调用了 ExtrapolateRotation() ExtrapolateTranslation() 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() < 2) {return extrapolation_delta * linear_velocity_from_poses_;}return extrapolation_delta * linear_velocity_from_odometry_;
}

ExtrapolateTranslation() 计算平移的预测是平移的线速度乘以时间求得的。如果有odom 就使用odom的线速度，如果没有odom就是用匹配得到的2个pose的平移差除以时间得到线速度。

ExtrapolateRotation() 是使用imu_tracker 进行角度的预测，也是用角速度乘以时间。当没有imu时，将用odom计算的角速度来代替，如果还没有odom，那就用匹配得到的2个pose的平移差除以时间得到角速度。

ScanMatch

这样就得到了初始位姿的预测，再将这个预测输入到最小二乘的目标函数中，进行最优化求解。

其中，online correlative scan matcher 将会对这个初始位姿预测进行二次确认，这个优化是可选的，优化方式比较简单，就是在之前预测位姿附近开一个三维窗口，然后在这个三维窗口内，均匀播撒候选粒子，对每个粒子计算匹配得分，选取最高得分的粒子作为优化的位姿，再传入ceres中。

扫描匹配的过程：

std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(const common::Time time, const transform::Rigid2d& pose_prediction,const sensor::RangeData& gravity_aligned_range_data) {std::shared_ptr<const Submap2D> 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()) {CHECK_EQ(options_.submaps_options().grid_options_2d().grid_type(),proto::GridOptions2D_GridType_PROBABILITY_GRID);double score = real_time_correlative_scan_matcher_.Match(pose_prediction, filtered_gravity_aligned_point_cloud,*static_cast<const ProbabilityGrid*>(matching_submap->grid()),&initial_ceres_pose);kFastCorrelativeScanMatcherScoreMetric->Observe(score);}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->grid(), pose_observation.get(),&summary);if (pose_observation) {kCeresScanMatcherCostMetric->Observe(summary.final_cost);double residual_distance =(pose_observation->translation() - pose_prediction.translation()).norm();kScanMatcherResidualDistanceMetric->Observe(residual_distance);double residual_angle = std::abs(pose_observation->rotation().angle() -pose_prediction.rotation().angle());kScanMatcherResidualAngleMetric->Observe(residual_angle);}return pose_observation;
}

之后进入到Ceres, ceres的具体分析可以根据参考4 进行阅读。

// cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc
void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,const transform::Rigid2d& initial_pose_estimate,const sensor::PointCloud& point_cloud,const Grid2D& grid,transform::Rigid2d* const pose_estimate,ceres::Solver::Summary* const summary) const {double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),initial_pose_estimate.translation().y(),initial_pose_estimate.rotation().angle()};ceres::Problem problem;CHECK_GT(options_.occupied_space_weight(), 0.);problem.AddResidualBlock(OccupiedSpaceCostFunction2D::CreateAutoDiffCostFunction(options_.occupied_space_weight() /std::sqrt(static_cast<double>(point_cloud.size())),point_cloud, grid),nullptr /* loss function */, ceres_pose_estimate);CHECK_GT(options_.translation_weight(), 0.);problem.AddResidualBlock(TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(options_.translation_weight(), target_translation),nullptr /* loss function */, ceres_pose_estimate);CHECK_GT(options_.rotation_weight(), 0.);problem.AddResidualBlock(RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(options_.rotation_weight(), ceres_pose_estimate[2]),nullptr /* loss function */, ceres_pose_estimate);ceres::Solve(ceres_solver_options_, &problem, summary);*pose_estimate = transform::Rigid2d({ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
}

references

1. https://blog.csdn.net/liuxiaofei823/article/details/70207814

2. https://blog.csdn.net/roadseek_zw/article/details/66968762

3. https://blog.csdn.net/u012209790/article/details/82735923

4. https://blog.csdn.net/u012209790/article/details/82735923

5. https://blog.csdn.net/xiaoma_bk/article/details/81128482 Pose Extrapolate 理解。

cartographer探秘第四章之代码解析（三） --- scan match相关推荐

学习笔记-第十四章恶意代码分析实战
第十四章恶意代码的网络特征 1.网络应对措施. 网络行为的基本属性包括IP地址,TCP端口,以及流量内容等,网络和安全设备可以利用它们,来提供网络应对措施.根据IP地址和端口,防火墙和路由器可以限 ...
第四章 PX4-Pixhawk-MPU6000传感器驱动解析
第四章MPU6000传感器驱动解析 Mpu6000是一个3轴加速度和3轴陀螺仪传感器,这一章节我们将对MPU6000这个传感器进行解析,依照这个解析步骤同样可以对L3GD20(3轴陀螺仪).HMC58 ...
第四章MPU6000传感器驱动解析
版权声明:本文为博主原创文章,未经博主允许不得转载. 第四章MPU6000传感器驱动解析 Mpu6000是一个3轴加速度和3轴陀螺仪传感器,这一章节我们将对MPU6000这个传感器进行解析,依照这个解 ...
Kali Linux 网络扫描秘籍第四章指纹识别（三）
第四章指纹识别(三) 作者:Justin Hutchens 译者:飞龙协议:CC BY-NC-SA 4.0 4.13 SNMPwalk SNMP 分析 SNMPwalk 是个更加复杂的 SNMP ...
第四章 ContextCapture 19 空三控制点平差
第四章 ContextCapture 19 空三控制点平差一.检查空三成果空三结束后,会生成连接点云,再三维中,可以观察生成的结果,根据点云的状态,判断空三是否正常. 在这个位置,可以导出空三报告 ...
算法基础课——第四章数学知识（三）
第四章数学知识(三) 如无特殊说明,所有数均为正整数. 高斯消元高斯消元是用来解方程的,可以在 O(n3)O(n^3)O(n3) 时间复杂度内求解一个 nnn 个方程 nnn 个未知数的多元线 ...
CodeCombat代码全记录（Python学习利器）--安息之云山峰（第四章）代码1
我们已经有了前三章的基础了,到了第四章,你会发现提示少之又少了.这时,我们大都要靠自己了!!!加油!!如果你没有思路,请仔细看下装备的说明,毕竟到了这关,你需要购买好多其他的新装备,而新装备中存在了新 ...
Paxos算法之旅（四）zookeeper代码解析--转载
ZooKeeper是近期比较热门的一个类Paxos实现.也是一个逐渐得到广泛应用的开源的分布式锁服务实现.被认为是Chubby的开源版,虽然具体实现有很多差异.ZooKeeper概要的介绍可以看官方文 ...
一个基于JRTPLIB的轻量级RTSP客户端(myRTSPClient)——收流篇：（四）example代码解析...
--------------------更新2018.08.20------------------- 添加http_tunnel_example.cpp作为RtspOverHttp示例程序. --- ...
视觉SLAM十四讲CH10代码解析及课后习题详解
g2o_viewer问题解决在进行位姿图优化时候,如果出现g2o_viewer: command not found,说明你的g2o_viewer并没有安装上,打开你之前安装的g2o文件夹,打开bi ...

cartographer探秘第四章之代码解析（三） --- scan match