- 旧图、新图。insertion_submaps存储着某个点云会画到的子图，最多两张，旧图指front，新图指back。
- INTRA_SUBMAP、INTER_SUBMAP。节点和子图之间约束的两种类型。节点和它画到的子图之间是INTRA_SUBMAP约束，和其它子图则是INTER_SUBMAP，即和那些没画到过的。具体到一个节点，INTRA_SUBMAP最多两条。
- main线程、后台线程。为提高效率，cartographer用多线程处理Global SLAM。cartographer_ros节点main函数所在的称为main线程，其它的称后台线程。PoseGrapher::AddNode在main线程，计算两种约束、优化全局位姿都在后台线程。
- 示例的lua配置。num_range_data=35；optimize_every_n_nodes=35；
一、为什么要Global SLAM
SLAM有两大任务，一是计算机器人实时位姿，二是建图。对建图，是由若干子图拼成一张大地图。AddRangeData生成了这些子图内容(概率图)，并把第一次画到该子图的点云位姿作为它的初始位姿。2D时，这个初始位姿只有平移，没有旋转，位姿就是该子图左上角在世界坐标系的直角坐标。很显然，最终地图质量严重依赖子图位姿。但由于噪声，子图位姿没法不存在误差，而且是个累积误差。Global SLAM主要目的是优化这些子图位姿，提高地图质量。
ros查询子图时，会调用Global SLAM模块中的PoseGraph2D::GetSubmapDataUnderLock。
PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapDataUnderLock(
const SubmapId& submap_id) const {
const auto it = data_.submap_data.find(submap_id);
if (it == data_.submap_data.end()) {
return {};
}
auto submap = it->data.submap;
if (data_.global_submap_poses_2d.Contains(submap_id)) {
// We already have an optimized pose.
return {submap,
transform::Embed3D(
data_.global_submap_poses_2d.at(submap_id).global_pose)};
}
// We have to extrapolate.
return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,
submap_id.trajectory_id) *
submap->local_pose()};
}GetSubmapDataUnderLock会根据参数submap_id，返回该子图的内容和位姿。位姿值存放在data_.global_submap_poses_2d，类型transform::Rigid2d。也就是说，data_.global_submap_poses_2d存储着各子图在世界坐标系的位姿，称全局位姿。于是在代码层，Global SLAM目的是要优化各子图位姿，然后把算出的全局位姿存放在data_.global_submap_poses_2d。
在PoseGraph2D，还有个地方存放着子图全局位姿：optimization_problem_->submap_data_。一次优化后，它直接整个赋值给data_.global_submap_poses_2d。但后台线程会经常修改它，对main线程，要做到同步、安全访问它就变得困难了，为安全得使用data_.global_submap_poses_2d。
二、位姿图与约束
参考“Global SLAM的核心——PoseGraph2D”中的“2. 位姿图与约束”。
三、总逻辑
PoseGraph2D封装了Global SLAM，目的是优化各子图位姿，然后把算出的全局位姿存放在data_.global_submap_poses_2d。为执行优化，需至少准备三个数据。
- data_.constraints。节点和子图的约束，约束有两种类型：INTRA_SUBMAP、INTER_SUBMAP。
- optimization_problem_->node_data_。存储着optimization::NodeSpec2D节点，注意不是PoseGraph2D中的data_.trajectory_nodes。
- optimization_problem_->submap_data_。存储着各子图优化后的全局位姿，值等于data_.global_submap_poses_2d。虽然是优化后生成，但需要为它提供各子图的一个初始全局位姿。
整个Global SLAM过程，可说就是不断接收AddRangeData处理出点云、子图，然后更新上述三个数据，同时隔一段时间执行一次优化。优化结果放在data_.global_submap_poses_2d，而优化间隔大约等于收到“optimize_every_n_nodes + 1”帧点云的时间。
- [main线程]AddRangeData处理完一个点云
后，进入PoseGraph2D::AddNode。
1：以。把它添加到data_.trajectory_nodes。
2：如果是第一次看到新图，把新图添加到data_.submap_data。
3：触发后台线程运行ComputeConstraintsForNode任务。 - [后台线程]执行PoseGraph2D::ComputeConstraintsForNode。
1：InitializeGlobalSubmapPoses。如果optimization_problem_->submap_data_是第一次看到新图，算出该子图的初始全局位姿，并添加进来。
2：创建个optimization::NodeSpec2D结点，并添加到optimization_problem_->node_data_。
3：生成
4：判断出此刻需要生成的INTER_SUBMAP约束，判断出一条就向后台触发执行一次PoseGraph2D::ComputeConstraint。
5：距离上一次优化，已处理的node数>optimize_every_n_nodes，触发后台执行一次优化。 - [后台线程]执行PoseGraph2D::ComputeConstraint。功能很纯粹：生成一条INTER_SUBMAP约束，两个参数指示了这条约束的节点和子图。距离远等原因，最后判断出可能是不生成。该函数会调用ConstraintBuilder2D::ComputeConstraint，后者会依次执行两个操作：分支界定法进行相关性扫描匹配(CSM)、ceres优化。最后生成的约束添加到ConstraintBuilder2D.constraints_。
- [后台线程]优化各子图全局位姿。
1：ConstraintBuilder2D.constraints_存储着两次优化间隔中新生成的INTER_SUBMAP约束，把它们添到data_.constraints。添加完就清空ConstraintBuilder2D.constraints_。
2：执行PoseGraph2D::RunOptimization，进行ceres优化。优化结果更新到data_.global_submap_poses_2d。
3.1 一次运行实例
1：ComputeConstraintsForNode开始处理#35节点
由于num_range_data=35，意味着一直到#35之前data_.submap_data只有一个子图。收到#35后，AppendNode()把data_.submap_data变成两个。而且ComputeConstraintsForNode在处理#35时，由于满足条件“num_nodes_since_last_loop_closure_(36) > options_.optimize_every_n_nodes()(35)”返回kRunOptimization，触发第一次优化。
ConstraintBuilder2D.constraints_存储着这段时间生成的INTER_SUBMAP类型约束。但这段时间没有这种类型约束，ConstraintBuilder2D.constraints_是空。
进入RunOptimization()时，data_.constraints包含着37个约束，都是INTRA_SUBMAP类型。optimization_problem_->node_data_则有36个节点。
optimization_problem_->submap_data_中有两个子图，优化时，会更新所有子图的全局位姿。以下是变化前后子图位姿变化情况。
| 优化前 | 优化后 | |
| #0 | { t: [0, 0], r: [0] } | { t: [0, 0], r: [0] } |
| #1 | { t: [0.705256, -0.169862], r: [0] } | { t: [0.705256, -0.169862], r: [7.83034e-16] } |
当执行完此次优化后，optimization_problem_->submap_data_要赋值给data_.global_submap_poses_2d，后者将由0个变成包含2个子图：#0、#1。
2：ComputeConstraintsForNode开始处理#71节点
由于满足条件“num_nodes_since_last_loop_closure_(36) > options_.optimize_every_n_nodes()(35)”返回kRunOptimization，触发第二次优化。
上一次优化后现在，没有INTER_SUBMAP类型约束，ConstraintBuilder2D.constraints_是空。
进入RunOptimization()时，data_.constraints包含着110个约束，都是INTRA_SUBMAP类型。optimization_problem_->node_data_则有72个节点。
optimization_problem_->submap_data_中有三个子图，优化时，会更新所有子图的全局位姿。以下是变化前后子图位姿变化情况。
| 优化前 | 优化后 | |
| #0 | { t: [0, 0], r: [0] } | { t: [0, 0], r: [0] } |
| #1 | { t: [0.705256, -0.169862], r: [7.83034e-16] } | { t: [0.685646, -0.118816], r: [1.1228] } |
| #2 | { t: [0.772205, -0.423076], r: [7.83034e-16] } | { t: [0.757561, -0.369401], r: [1.34722] } |
当执行完此次优化后，optimization_problem_->submap_data_要赋值给data_.global_submap_poses_2d，后者将由2个变成包含3个子图：#0、#1、#2。
3：ComputeConstraintsForNode开始处理#107节点
由于满足条件“num_nodes_since_last_loop_closure_(36) > options_.optimize_every_n_nodes()(35)”返回kRunOptimization，触发第三次优化。
上一次优化后现在，曾尝试生成23(ConstraintBuilder2D.constraints_.size() = 23)个INTER_SUBMAP类型约束，但真正能生成20个。
[0/22]constraint, sid#0->nid#73 : { t: [0.737129, -0.279751, 0], q: [4.30851] }
[1/22]constraint, sid#0->nid#76 : { t: [0.679119, -0.181883, 0], q: [3.94285] }
[2/22]constraint, sid#0->nid#80 : { t: [0.660037, -0.224274, 0], q: [10.7475] }
[3/22]constraint, sid#0->nid#83 : { t: [0.6418, -0.272251, 0], q: [2.42982] }
[4/22]constraint, sid#0->nid#86 : { t: [0.651128, -0.340315, 0], q: [6.10027] }
[5/22]constraint, sid#0->nid#90 : { t: [0.654459, -0.477992, 0], q: [4.78181] }
[6/22]constraint, sid#0->nid#93 : { t: [0.628862, -0.565445, 0], q: [11.4649] }
[7/22]constraint, sid#0->nid#96 : { t: [0.622108, -0.537745, 0], q: [5.20316] }
[8/22]constraint, sid#0->nid#100 : { t: [0.579583, -0.63541, 0], q: [6.04865] }
[9/22]constraint, sid#0->nid#103 : { t: [0.499305, -0.685049, 0], q: [10.4177] }
[10/22]constraint is nullptr
[11/22]constraint is nullptr
[12/22]constraint is nullptr
[13/22]constraint, sid#1->nid#10 : { t: [-0.277576, 0.174087, 0], q: [-11.2381] }
[14/22]constraint, sid#1->nid#13 : { t: [-0.231667, 0.155811, 0], q: [-2.35679] }
[15/22]constraint, sid#1->nid#16 : { t: [-0.269424, 0.166747, 0], q: [-1.53323] }
[16/22]constraint, sid#1->nid#20 : { t: [-0.0797406, 0.0981122, 0], q: [-4.55389] }
[17/22]constraint, sid#1->nid#23 : { t: [-0.0784615, 0.101867, 0], q: [-6.38667] }
[18/22]constraint, sid#1->nid#26 : { t: [-0.108556, 0.111449, 0], q: [-2.79428] }
[19/22]constraint, sid#1->nid#30 : { t: [0.0267142, 0.0217272, 0], q: [-9.70906] }
[20/22]constraint, sid#1->nid#33 : { t: [0.055966, 0.0100615, 0], q: [-3.4572] }
[21/22]constraint, sid#0->nid#106 : { t: [0.526513, -0.673719, 0], q: [8.20909] }
[22/22]constraint, sid#1->nid#106 : { t: [-0.0349002, -0.539345, 0], q: [2.6599] }
{ConstraintBuilder2D::RunWhenDoneCallback}23 computations resulted in 20 additional constraints.
INFO: Score histogram: Count: 21 Min: 0.673956 Max: 0.777079 Mean: 0.726766
[0.673956, 0.684268) # Count: 1 (4.7619%) Total: 1 (4.7619%)
[0.684268, 0.694580) Count: 0 (0%) Total: 1 (4.7619%)
[0.694580, 0.704893) ### Count: 3 (14.2857%) Total: 4 (19.0476%)
[0.704893, 0.715205) #### Count: 4 (19.0476%) Total: 8 (38.0952%)
[0.715205, 0.725518) ### Count: 3 (14.2857%) Total: 11 (52.381%)
[0.725518, 0.735830) ## Count: 2 (9.52381%) Total: 13 (61.9048%)
[0.735830, 0.746142) #### Count: 4 (19.0476%) Total: 17 (80.9524%)
[0.746142, 0.756455) # Count: 1 (4.7619%) Total: 18 (85.7143%)
[0.756455, 0.766767) Count: 0 (0%) Total: 18 (85.7143%)
[0.766767, 0.777079] ### Count: 3 (14.2857%) Total: 21 (100%)进入RunOptimization()时，data_.constraints包含着202个约束，其中INTRA_SUBMAP类型182个，INTER_SUBMAP类型20个。optimization_problem_->node_data_则有108个节点。
optimization_problem_->submap_data_中有四个子图，优化时，会更新所有子图的全局位姿。以下是变化前后子图位姿变化情况。
| 优化前 | 优化后 | |
| #0 | { t: [0, 0], r: [0] } | { t: [0, 0], r: [0] } |
| #1 | { t: [0.685646, -0.118816], r: [1.1228] } | { t: [0.479014, -0.140803], r: [5.51913] } |
| #2 | { t: [0.757561, -0.369401], r: [1.34722] } | { t: [0.629333, -0.387556], r: [5.03015] } |
| #3 | { t: [0.619026, -0.692314], r: [1.34722] } | { t: [0.506977, -0.717864], r: [5.43524] } |
当执行完此次优化后，optimization_problem_->submap_data_要赋值给data_.global_submap_poses_2d，后者将由3个变成包含4个子图：#0、#1、#2、#3。
四、计算相关位姿
4.1、由点云生成TrajectoryNode的global_pose
NodeId PoseGraph2D::AddNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data,
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
const NodeId node_id = AppendNode(constant_data, trajectory_id,
insertion_submaps, optimized_pose);
...
}
transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses,
const int trajectory_id) const {
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
if (begin_it == end_it) {
// 该轨迹没有计算过子图全局位姿。
const auto it = data_.initial_trajectory_poses.find(trajectory_id);
if (it != data_.initial_trajectory_poses.end()) {
return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id,
it->second.time) *
it->second.relative_pose;
} else {
// 没有初始位姿（initial_trajectory_poses，是不是对应rviz中的初始位姿？）时，取0。
return transform::Rigid3d::Identity();
}
}
const SubmapId last_optimized_submap_id = std::prev(end_it)->id;
// last_optimized_submap_id：最近一次的SubmapID。
// global_submap_poses：PoseGraph模块生成的全局位姿。
// data_.submap_data：LocalTrajectoryBuilder2D::AddRangeData模块生成的Submap2D，PoseGraph不会修改这部分内容。
// Accessing 'local_pose' in Submap is okay, since the member is const.
// 计算可认为是global_submap_poses[last_optimized_submap_id]“减去”submap_data[last_optimized_submap_id]。
return transform::Embed3D(
global_submap_poses.at(last_optimized_submap_id).global_pose) *
data_.submap_data.at(last_optimized_submap_id)
.submap->local_pose()
.inverse();
}constant_data是LocalTrajectoryBuilder2D::AddRangeData处理完的一帧点云，constant_data->local_pose是该点云位姿。要由该点云生成一个节点(TrajectoryNode)，以收到#72帧点云为例看TrajectoryNode.global_pose的生成过程。
- 算出LocalToGlobalTransform，它表示最新子图的位姿误差。误差等于“PoseGraph2D认为该子图的位姿”减去“AddRangeData系统生成的该子图位姿”。此时最新子图是(0, 2)，算出的子图位姿误差是[-0.02437, 0.0354, 0], [1.3472]。在图1，从#72一直到#107，子图误差都是这个值。
- 算出“PoseGraph2D认为该节点/点云的位姿”。位姿等于“最新子图的位姿误差”加上constant_data->local_pose。
4.2、INTRA_SUBMAP类型约束中的位姿
针对#72点云，它会画向#1、#2两张子图，它和这两张子图会生成两个INTRA_SUBMAP类型的约束。
WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
const NodeId& node_id,
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
const bool newly_finished_submap) {
std::vector<SubmapId> submap_ids;
std::vector<SubmapId> finished_submap_ids;
std::set<NodeId> newly_finished_submap_node_ids;
{
absl::MutexLock locker(&mutex_);
const auto& constant_data =
data_.trajectory_nodes.at(node_id).constant_data;
// submap_ids存储着该点云画过的子图。
submap_ids = InitializeGlobalSubmapPoses(
node_id.trajectory_id, constant_data->time, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
// 当该点云画到两个子图时，取旧图。
const SubmapId matching_id = submap_ids.front();
// local_pose_2d是经过重力修正后的点云位姿。
const transform::Rigid2d local_pose_2d =
transform::Project2D(constant_data->local_pose *
transform::Rigid3d::Rotation(
constant_data->gravity_alignment.inverse()));
// global_pose：matching_id子图的全局位姿。
// 这是在后台线程，这个位姿取自optimization_problem_->submap_data，而不是data_.global_submap_poses_2d。
const transform::Rigid2d global_pose =
optimization_problem_->submap_data().at(matching_id).global_pose;
const transform::Rigid2d global_pose_2d =
global_pose *
constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
local_pose_2d;
optimization_problem_->AddTrajectoryNode(
matching_id.trajectory_id,
optimization::NodeSpec2D{constant_data->time, local_pose_2d,
global_pose_2d,
constant_data->gravity_alignment});
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
// insertion_submaps存储着画有该点云的子图，这些子图和点云的约束类型是INTRA_SUBMAP。
const SubmapId submap_id = submap_ids[i];
// Even if this was the last node added to 'submap_id', the submap will
// only be marked as finished in 'data_.submap_data' further below.
CHECK(data_.submap_data.at(submap_id).state ==
SubmapState::kNoConstraintSearch);
data_.submap_data.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid2d constraint_transform =
constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
local_pose_2d;
// constraint_transform是该约束的位姿。对平移，可认为是以点云为原点到子图的向量。
data_.constraints.push_back(
Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
options_.matcher_translation_weight(),
options_.matcher_rotation_weight()},
Constraint::INTRA_SUBMAP});
}
}
4.3、INTER_SUBMAP类型约束中的位姿
ConstraintBuilder2D::ComputeConstraint负责生成INTER_SUBMAP类型的位姿，分两个步骤。
- 分支界定法执行相关性扫描匹配(CSM)，得到一个位姿pose_estimate1。
- 以pose_estimate1为初值送到ceres_scan_matcher_进行非线性优化，得到pose_estimate。
一个节点最多画向两张子图，它和这两张之外的其它子图（state==kFinished）可能会生成类型是INTER_SUBMAP的约束。加可能，是会没有。举个例子，收到#72点云时，data_.global_submap_poses_2d存在#0、#1、#2这三张子图的全局位姿，#72不画向#0子图，但它和#0子图过不了相关性扫描匹配，即fast_correlative_scan_matcher->Match返回false，就没法生成约束。但#106节点和#1子图可以生成。
4.4 optimization_problem_中节点位姿
五、PoseGraph2D::WaitForAllComputations()出现阻塞
1. tros_instance::stop_navigation_node()一直无法停止，原因是在阻塞在了cartographer_node_.reset()。
void tros_instance::stop_navigation_node()
{
...
SDL_Log("[stop_node]pre laser_node_.reset()");
// laser_node_.reset();
laser_driver_.stop_laser();
SDL_Log("[stop_node]pre cartographer_node_.reset()");
cartographer_node_.reset(); <---在这里阻塞
SDL_Log("[stop_node]pre map_2_laser_.reset()");
map_2_laser_.reset();
SDL_Log("[stop_node]pre cartographer_.reset()");
...
}2. cartographer_ros::Run(exit)阻塞在了node.RunFinalOptimization();
3. node.RunFinalOptimization()阻塞在了map_builder_->pose_graph()->RunFinalOptimization();
4. map_builder_->pose_graph()->RunFinalOptimization()阻塞在了PoseGraph2D::RunFinalOptimization()
5. PoseGraph2D::RunFinalOptimization()阻塞在了PoseGraph2D::WaitForAllComputations()
关于PoseGraph2D::WaitForAllComputations()注释，参考：(02)Cartographer源码无死角解析-(82) 轨迹结束处理Node::FinishTrajectoryUnderLock()、最后一次优化RunFinalOptimization()。
深入WaitForAllComputations()，分析阻塞情况。针对上图这实例，最近一次优化后，work_queue_队列中还有251个后台任务。若此时停止，即进入WaitForAllComputations()，要一直阻塞，阻塞发生在第一个while循环，即取空work_queue_队列、生成所有子图内约束。这个阻塞累计花了65秒。那这65秒是花在哪里？
- (后台线程)处理251个任务。任务基本是ComputeConstraintsForNode()函数。
- (后台线程)优化（通过 HandleWorkQueue()调用RunOptimization()）。假设optimize_every_n_nodes=35，那要进行8次优化。以每次优化500毫秒（上图显示最近一次优化花了705ms），光优化要花费4秒。
相比下来，阻塞时间更多花在ComputeConstraintsForNode。要缓减阻塞，解决办法是在WaitForAllComputations期间，修改ComputeConstraintsForNode行为。1）啥也不做。2）返回WorkItem::Result::kDoNotRunOptimization，让251个任务后都不执行优化。
WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
const NodeId& node_id,
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
const bool newly_finished_submap) {
std::vector<SubmapId> submap_ids;
std::vector<SubmapId> finished_submap_ids;
std::set<NodeId> newly_finished_submap_node_ids;
{
absl::MutexLock locker(&mutex_);
if (is_wait_for_all_computations_) {
constraint_builder_.NotifyEndOfNode();
return WorkItem::Result::kDoNotRunOptimization;is_wait_for_all_computations_是新增变量，它在WaitForAllComputations期间置为true。
说是什么都不做，但还是要执行constraint_builder_.NotifyEndOfNode()。用于更新num_finished_nodes_，以便WaitForAllComputations()能得到个和data_.num_trajectory_nodes数值一样的constraint_builder_.GetNumFinishedNodes()。
} ... } ... }
解释下图中几个变量。
- constraints: 11105。累计已生成11105个约束，对应变量data_.constraints.size()。随着不断运行，data_.constraints尺寸一直在增大。当中同时有子图内约束和子图间约束。优化时，data_.constraints将送到RunOptimization()，见optimization_problem_->Solve(data_.constraints,...)。
- work_queue: 251。要执行最近一次优化时，work_queue_队列还积压着251个任务。这些任务基本都是ComputeConstraintsForNode()函数。
- optimization: use 705ms。最近一次优化用了705毫秒。是这统计RunOptimization()函数的执行时间。
总之，在WaitForAllComputations出现阻塞，根源还是处理能力不够，导致结束时work_queue_还积压着任务，取空这些任务需要些时间。
“测试数据”有个7-WaitForAllComputations.log，它存储了没修改相关代码时的一次运行实例。进入WaitForAllComputations时，队列中还有264个任务。
10:28:15.697 ... 566236 {dbg_stop_slow}PoseGraph2D::WaitForAllComputations pre predicate, work_queue_size: 264
...
10:29:21.169 ... 631710 {dbg_stop_slow}PoseGraph2D::WaitForAllComputations pre constraint_builder_.WhenDone, ...从10:28:15.697到10:29:21.169，第一个while循环就花了65秒。