讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证
一、前言
在分析源码之前，这里先把前面没有解答的疑问复制一下，免得忘记了
疑问 1 : \color{red}疑问1: 疑问1: global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。
疑问 2 : \color{red}疑问2: 疑问2: 为什么要等待约束计算完成之后再调用 PoseGraph2D::HandleWorkQueue()，同时源码又是如何实现的。
疑问 4 : \color{red}疑问4: 疑问4: ComputeConstraintsForNode() 如果返回需要优化，源码中是在哪里执行优化的呢？
不过看起来，貌似我们的 疑问 2 : \color{red}疑问2: 疑问2: 与 疑问 4 : \color{red}疑问4: 疑问4: 在上一篇博客中已经得到了答案，因为在 PoseGraph2D::HandleWorkQueue() 函数中，通过调用 PoseGraph2D::RunOptimization() 进行优化。因为优化是在 PoseGraph2D::HandleWorkQueue() 中完成的，当然需要 等待约束计算完成之后再调用 PoseGraph2D::HandleWorkQueue()。
那么到目前为止，我们只有 疑问 1 : \color{red}疑问1: 疑问1: global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。 还没有解答了。相信大家和我一样，对于后端优化的整体架构有一定认知了。后续我再为大家总结一下，肯定会更加清晰明了。
二、DispatchScanMatcherConstruction()
依照 ConstraintBuilder2D::MaybeAddConstraint() 与 ConstraintBuilder2D::MaybeAddGlobalConstraint() 中调用 ispatchScanMatcherConstruction() 与 ComputeConstraint() 的顺序来分析代码，所以先分析前者。
从函数命名来看，该函数的主要目的把扫描匹配 ScanMatcher 的构建过程分发至线程池。初步看起来貌似比较蒙蔽，为什么 ScanMatcher 的构建过程都要进行分发，这是不是太奇怪了。并不是，而是构建的 ScanMatcher 对象过程太消耗时间了，所以放置到线程池中完成。先看一下函数的整体注释，再一句一句的进行分析：
// 为每个子图新建一个匹配器
const ConstraintBuilder2D::SubmapScanMatcher*
ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id,
const Grid2D* const grid) {
CHECK(grid);
// 如果匹配器里已经存在, 则直接返回对应id的匹配器
if (submap_scan_matchers_.count(submap_id) != 0) {
return &submap_scan_matchers_.at(submap_id);
}
// submap_scan_matchers_新增加一个 key
auto& submap_scan_matcher = submap_scan_matchers_[submap_id];
kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size());
// 保存栅格地图的指针
submap_scan_matcher.grid = grid;
auto& scan_matcher_options = options_.fast_correlative_scan_matcher_options();
auto scan_matcher_task = absl::make_unique<common::Task>();
// 生成一个将初始化匹配器的任务, 初始化时会计算多分辨率地图, 比较耗时
scan_matcher_task->SetWorkItem(
[&submap_scan_matcher, &scan_matcher_options]() {
// 进行匹配器的初始化, 与多分辨率地图的创建
submap_scan_matcher.fast_correlative_scan_matcher =
absl::make_unique<scan_matching::FastCorrelativeScanMatcher2D>(
*submap_scan_matcher.grid, scan_matcher_options);
});
// 将初始化匹配器的任务放入线程池中, 并且将任务的智能指针保存起来
submap_scan_matcher.creation_task_handle =
thread_pool_->Schedule(std::move(scan_matcher_task));
return &submap_scan_matchers_.at(submap_id);
}
（ 1 ）： \color{blue} （1）： （1）：该函数接收一个 submap_id，以及该子图对应的 Grid2D 数据指针，其存储的都是都是直接与地图相关的数据。首先其判断一下是否已经为该子图创建 SubmapScanMatcher 实例了，如果已经创建则直接放回。需要注意，传递到该函数的子图都是处于完成状态的子图(前面的博客有分析过)。
（ 2 ）： \color{blue} （2）： （2）：如果目前 submap_scan_matchers_ 还没有为 submap_id 对应 SubmapScanMatcher 实例，则新增一个key，然后记录一下目前 submap_scan_matchers_ 中元素的个数。随后把参数 grid 传递给这个新的 SubmapScanMatcher 对象 submap_scan_matcher。
（ 3 ）： \color{blue} （3）： （3）：获得分支定界也就是 fast_correlative_scan_matcher 中的相关参数，在 src/cartographer/configuration_files/pose_graph.lua 文件中可以看到参数配置大致如下：
-- 基于分支定界算法的2d粗匹配器
fast_correlative_scan_matcher = {
linear_search_window = 7.,
angular_search_window = math.rad(30.),
branch_and_bound_depth = 7,
},
接着创建一个任务，且添加至线程池中，该任务的目的就是构建为子图构建对应的 scan_matching::FastCorrelativeScanMatcher2D 实例对象赋值给 submap_scan_matcher.fast_correlative_scan_matcher。需要注意到构建该实例对象时，传递参数 *submap_scan_matcher.grid 是使用解引用的方式，其会涉及到地图数据的拷贝。关于 scan_matching::FastCorrelativeScanMatcher2D 的细节后续再进行分析。
（ 4 ）： \color{blue} （4）： （4）： 剩下的就十分简单了，把创建实例对象的任务 scan_matcher_task 添加至线程池，并且返回 ConstraintBuilder2D::SubmapScanMatcher 实例对象的指针。
三、ComputeConstraint()细节分析
先进行细节分析，后面再给出整体代码注释
（ 1 ）： \color{blue}（1）： （1）：首先来分析一个该函数䣌输入，大部分参数前面已经很熟悉，这里就不再重复啰嗦，有个参数是比较重要的，那就是 match_full_submap，如下：
/**
* @brief 计算节点和子图之间的一个约束(回环检测)
* 用基于分支定界算法的匹配器进行粗匹配,然后用ceres进行精匹配
*
* @param[in] submap_id submap的id
* @param[in] submap 地图数据
* @param[in] node_id 节点id
* @param[in] match_full_submap 是局部匹配还是全子图匹配
* @param[in] constant_data 节点数据
* @param[in] initial_relative_pose 约束的初值
* @param[in] submap_scan_matcher 匹配器
* @param[out] constraint 计算出的约束
*/
void ConstraintBuilder2D::ComputeConstraint(
const SubmapId& submap_id, const Submap2D* const submap,
const NodeId& node_id, bool match_full_submap,
const TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose,
const SubmapScanMatcher& submap_scan_matcher,
std::unique_ptr<ConstraintBuilder2D::Constraint>* constraint)
match_full_submap 在计算 local 约束时会设置为 false，当然，在计算 global 约束时会被设置为 true，因为其需要与所有的子图进行匹配。关于 initial_relative_pose，如果计算 local 约束其时相对于子图的位姿，如果计算 global 约束其被设置为 transform::Rigid2d::Identity()。这些在前面介绍的 MaybeAddConstraint() 与 MaybeAddGlobalConstraint() 调用 ComputeConstraint 时可以体现出来，如下所示：
// 生成个计算 local 约束的任务
auto constraint_task = absl::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */
constant_data, initial_relative_pose, *scan_matcher,
constraint);
// 生成个计算 global 约束的任务
constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */
constant_data, transform::Rigid2d::Identity(),
*scan_matcher, constraint);
（ 2 ）： \color{blue}（2）： （2）： 首先其会执行代码 CHECK(submap_scan_matcher.fast_correlative_scan_matcher)，判断该实例是否已经成功创建赋值，前面在添加认识时，有为 constraint_task 添加如下依赖：
// 等匹配器之后初始化才能进行约束的计算
constraint_task->AddDependency(scan_matcher->creation_task_handle);
所以会先进行匹配器初始化，然后再进行约束的计算，该处的判断主要是为了保证前面的逻辑正确。虽然通过如下代码，得到初始位姿：
const transform::Rigid2d initial_pose =
ComputeSubmapPose(*submap) * initial_relative_pose;
ComputeSubmapPose(*submap) 是求得子图的局部位姿，这里记录为 S u b m a p s u b m a p l o c a l \mathbf {Submap}^{local}_{submap} Submapsubmaplocal，initial_relative_pose 前面提到如果是计算 local 约束，表示节点相对于 Submap 的位姿，记为 R o b o t t r a c k i n g s u b m a p \mathbf {Robot}^{submap}_{tracking} Robottrackingsubmap，那么对应的数学公式如下： R o b o t t r a c k i n g l o c a l = S u b m a p s u b m a p l o c a l ∗ R o b o t t r a c k i n g s u b m a p (01) \color{Green} \tag{01} \mathbf {Robot}^{local}_{tracking} = \mathbf {Submap}^{local}_{submap}*\mathbf {Robot}^{submap}_{tracking} Robottrackinglocal=Submapsubmaplocal∗Robottrackingsubmap(01)也就是说，此时 initial_pose 计算表示机器人再 local 系下的位姿。如果是计算 global 约束，则 initial_relative_pose 是一个单位矩阵，直接忽略即可，此时 initial_pose 仅仅表子图的局部位姿。
（ 3 ）： \color{blue}（3）： （3）： 来看一段源码中的英文注释：
// The 'constraint_transform' (submap i <- node j) is computed from:
// - a 'filtered_gravity_aligned_point_cloud' in node j,
// - the initial guess 'initial_pose' for (map <- node j),
// - the result 'pose_estimate' of Match() (map <- node j).
// - the ComputeSubmapPose() (map <- submap i)
大致的翻译如下(需要注意，坐标变换与坐标系变换是互逆的)
constraint_transform 表示 node j 到 submap i 的变换关系。主要由如下几个部分求解而来：
1.node j 中的 'filtered_gravity_aligned_point_cloud'
2.node j 到地图 map 的初始坐标变换认为是 initial_pose
3.pose_estimate 用于存储扫面匹配的结果，其表示node j 到地图 map的坐标变换。
4.ComputeSubmapPose 表示 submap i 到 map 的坐标变换。
这段注释的后面，执行了如下代码，主要是创建一个用于存储扫描匹配结果的变量，且分值默认设置为0。
float score = 0.;
transform::Rigid2d pose_estimate = transform::Rigid2d::Identity();
（ 4 ）： \color{blue}（4）： （4）：计算 pose_estimate 主要分为三个阶段，分别为：
①先使用快速的相关性扫描匹配 (fast correlative scan matcher-分支定界)算法。
②如果得分过低，则进行修剪。
③提纯，提高精度。
// Compute 'pose_estimate' in three stages:
// 1. Fast estimate using the fast correlative scan matcher.
// 2. Prune if the score is too low.
// 3. Refine.
// param: global_localization_min_score 对整体子图进行回环检测时的最低分数阈值
// param: min_score 对局部子图进行回环检测时的最低分数阈值
对于是否匹配全局地图，设置的分值阈值是不一样。
（ 5 ）： \color{blue}（5）： （5）：先来看其是如何计算进行全局global匹配的：
// Step:2 使用基于分支定界算法的匹配器进行粗匹配
if (match_full_submap) {
// 节点与全地图进行匹配
kGlobalConstraintsSearchedMetric->Increment();
if (submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap(
constant_data->filtered_gravity_aligned_point_cloud,
options_.global_localization_min_score(), &score, &pose_estimate)) {
CHECK_GT(score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
kGlobalConstraintsFoundMetric->Increment();
kGlobalConstraintScoresMetric->Observe(score);
} else {
// 计算失败了就退出
return;
}
}
核心就就是调用子图对应的 submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap()。这里是节点与全局地图进行匹配，匹配的分值需要大于 global_localization_min_score()。另外，前面提到，只有再建图的时候，才会进行全局匹配，所以这里为了保证逻辑的正确性，验证了 node_id.trajectory_id 与 nsubmap_id.trajectory_id 是否都为0。关于 submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap() 的细节部分，后面再进行分析。
（ 5 ）： \color{blue}（5）： （5）：如果 match_full_submap 为 false，那么其节点只与局部地图进行匹配，执行如下代码段：
else {
// 节点与局部地图进行匹配
kConstraintsSearchedMetric->Increment();
if (submap_scan_matcher.fast_correlative_scan_matcher->Match(
initial_pose, constant_data->filtered_gravity_aligned_point_cloud,
options_.min_score(), &score, &pose_estimate)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
kConstraintsFoundMetric->Increment();
kConstraintScoresMetric->Observe(score);
} else {
return;
}
}
主要是调用了 submap_scan_matcher.fast_correlative_scan_matcher->Match() 函数，同样，该函数暂时也不进行讲解。后续再详细分析。
（ 6 ）： \color{blue}（6）： （6）： 通过前面分支定界的算法，获得了一个初始值，且赋值给了 pose_estimate，利用该初始值，再进行一个精匹配，也就是如下代码：
// Use the CSM estimate as both the initial and previous pose. This has the
// effect that, in the absence of better information, we prefer the original
// CSM estimate.
// Step:3 使用ceres进行精匹配, 就是前端扫描匹配使用的函数
ceres::Solver::Summary unused_summary;
ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
constant_data->filtered_gravity_aligned_point_cloud,
*submap_scan_matcher.grid, &pose_estimate,
&unused_summary);
关于 ceres_scan_matcher_.Match() 函数在前端部分已经进行过很细致的分析，总的来说，其就是希望所有经过畸变的点云数据都打在障碍物上，其累计的点约占该帧点云数据的比例越大，则得分越高。
（ 7 ）： \color{blue}（7）： （7）： 对位姿进行提提纯之后，其会把 pose_estimate 又原来相对于 local 系变换成相对于子图 submap 的位姿。接着把该位姿 constraint_transform 作为一个约束添加赋值给 constraint。且标记为内间约束。
// Step:4 获取节点到submap坐标系原点间的坐标变换
// pose_estimate 是 节点在 loacl frame 下的坐标
const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate;
// Step:5 返回计算后的约束
constraint->reset(new Constraint{
submap_id,
node_id,
{
transform::Embed3D(constraint_transform),
options_.loop_closure_translation_weight(),
options_.loop_closure_rotation_weight()},
Constraint::INTER_SUBMAP});
一般来说，loop_closure_translation_weight 与 loop_closure_rotation_weight 这两个参数是不需要调整的。
（ 8 ）： \color{blue}（8）： （8）： 剩下的就是打印一些日志信息，可以通过 src/cartographer/configuration_files/pose_graph.lua 文件中的 log_matches 参数选择是否打印。可以用于调试使用。
四、ComputeConstraint()整体注释
为了方便大家自行分析代码，这里给出整体注释：
/**
* @brief 计算节点和子图之间的一个约束(回环检测)
* 用基于分支定界算法的匹配器进行粗匹配,然后用ceres进行精匹配
*
* @param[in] submap_id submap的id
* @param[in] submap 地图数据
* @param[in] node_id 节点id
* @param[in] match_full_submap 是局部匹配还是全子图匹配
* @param[in] constant_data 节点数据
* @param[in] initial_relative_pose 约束的初值
* @param[in] submap_scan_matcher 匹配器
* @param[out] constraint 计算出的约束
*/
void ConstraintBuilder2D::ComputeConstraint(
const SubmapId& submap_id, const Submap2D* const submap,
const NodeId& node_id, bool match_full_submap,
const TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose,
const SubmapScanMatcher& submap_scan_matcher,
std::unique_ptr<ConstraintBuilder2D::Constraint>* constraint) {
CHECK(submap_scan_matcher.fast_correlative_scan_matcher);
// Step:1 得到节点在local frame下的位姿
const transform::Rigid2d initial_pose =
ComputeSubmapPose(*submap) * initial_relative_pose;
// The 'constraint_transform' (submap i <- node j) is computed from:
// - a 'filtered_gravity_aligned_point_cloud' in node j,
// - the initial guess 'initial_pose' for (map <- node j),
// - the result 'pose_estimate' of Match() (map <- node j).
// - the ComputeSubmapPose() (map <- submap i)
float score = 0.;
transform::Rigid2d pose_estimate = transform::Rigid2d::Identity();
// Compute 'pose_estimate' in three stages:
// 1. Fast estimate using the fast correlative scan matcher.
// 2. Prune if the score is too low.
// 3. Refine.
// param: global_localization_min_score 对整体子图进行回环检测时的最低分数阈值
// param: min_score 对局部子图进行回环检测时的最低分数阈值
// Step:2 使用基于分支定界算法的匹配器进行粗匹配
if (match_full_submap) {
// 节点与全地图进行匹配
kGlobalConstraintsSearchedMetric->Increment();
if (submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap(
constant_data->filtered_gravity_aligned_point_cloud,
options_.global_localization_min_score(), &score, &pose_estimate)) {
CHECK_GT(score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
kGlobalConstraintsFoundMetric->Increment();
kGlobalConstraintScoresMetric->Observe(score);
} else {
// 计算失败了就退出
return;
}
}
else {
// 节点与局部地图进行匹配
kConstraintsSearchedMetric->Increment();
if (submap_scan_matcher.fast_correlative_scan_matcher->Match(
initial_pose, constant_data->filtered_gravity_aligned_point_cloud,
options_.min_score(), &score, &pose_estimate)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
kConstraintsFoundMetric->Increment();
kConstraintScoresMetric->Observe(score);
} else {
return;
}
}
{
absl::MutexLock locker(&mutex_);
score_histogram_.Add(score);
}
// Use the CSM estimate as both the initial and previous pose. This has the
// effect that, in the absence of better information, we prefer the original
// CSM estimate.
// Step:3 使用ceres进行精匹配, 就是前端扫描匹配使用的函数
ceres::Solver::Summary unused_summary;
ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
constant_data->filtered_gravity_aligned_point_cloud,
*submap_scan_matcher.grid, &pose_estimate,
&unused_summary);
// Step:4 获取节点到submap坐标系原点间的坐标变换
// pose_estimate 是 节点在 loacl frame 下的坐标
const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate;
// Step:5 返回计算后的约束
constraint->reset(new Constraint{
submap_id,
node_id,
{
transform::Embed3D(constraint_transform),
options_.loop_closure_translation_weight(),
options_.loop_closure_rotation_weight()},
Constraint::INTER_SUBMAP});
// log相关
if (options_.log_matches()) {
std::ostringstream info;
info << "Node " << node_id << " with "
<< constant_data->filtered_gravity_aligned_point_cloud.size()
<< " points on submap " << submap_id << std::fixed;
if (match_full_submap) {
info << " matches";
} else {
const transform::Rigid2d difference =
initial_pose.inverse() * pose_estimate;
info << " differs by translation " << std::setprecision(2) // c++11: std::setprecision(2) 保留2个小数点
<< difference.translation().norm() << " rotation "
<< std::setprecision(3) << std::abs(difference.normalized_angle());
}
info << " with score " << std::setprecision(1) << 100. * score << "%.";
LOG(INFO) << info.str();
}
}
五、结语
关于配置文件中的 max_constraint_distance 这个参数提及一下，其可以理解为局部子图进行回环检测时能匹配上，或者说形成约束的最大距离，超过这个距离检测就检测不到了。该值设置越大，则需要与节点计算约束的子图就越多，耗费的性能也越多。
还有一个参数就是 optimize_every_n_nodes，这里可以设置为0，或者小于0的数，如根据 src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc 文件中 PoseGraph2D::ComputeConstraintsForNode() 函数的如下代码：
// Step: 插入的节点数大于optimize_every_n_nodes时执行一次优化
// optimize_every_n_nodes = 0 时不进行优化, 这样就可以单独分析前端的效果
if (options_.optimize_every_n_nodes() > 0 && // param: optimize_every_n_nodes
num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
// 正在建图时只有这一块会返回 执行优化
return WorkItem::Result::kRunOptimization;
}
return WorkItem::Result::kDoNotRunOptimization;
其返回的结果永远都是 WorkItem::Result::kDoNotRunOptimization，那么系统则不会进行后端优化了，相当于关闭的后端优化。如果大家只想看前端的效果，可以对该值进行配置。