讲解关于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()，同时源码又是如何实现的。
疑问 3 : \color{red}疑问3: 疑问3: data_.global_submap_poses_2d 中存储子图的全局位姿，那么子图的全局位姿又是从哪里来的呢？
疑问 4 : \color{red}疑问4: 疑问4: ComputeConstraintsForNode() 如果返回需要优化，源码中是在哪里执行优化的呢？
该篇博客主要是对 PoseGraph2D::ComputeConstraintsForNode() 函数中调用的 InitializeGlobalSubmapPoses() 进行讲解，其同样实现于 src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc 文件中。首先回顾一下调用该函数的过程：
// 获取节点信息数据
const auto& constant_data =
data_.trajectory_nodes.at(node_id).constant_data;
// 获取 trajectory_id 下的正处于活跃状态下的子图的SubmapId
submap_ids = InitializeGlobalSubmapPoses(
node_id.trajectory_id, constant_data->time, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
其先获得节点的静态数据 constant_data (主要由前端计算而来)，然后利用其t成员变量 constant_data->time 与 其对应的 node_id.trajectory_id 及 活跃的子图 insertion_submaps 作为形参传入。
二、整体注释
在进行细节分析之前，各位朋友可以简单的过一下整体注释(后面有十分详细的讲解)：
// 返回指定轨迹id下的正处于活跃状态下的子图的SubmapId
std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
const int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
CHECK(!insertion_submaps.empty());
// submap_data中存的: key 为 SubmapId, values 为对应id的Submap在global坐标系下的全局位姿
const auto& submap_data = optimization_problem_->submap_data();
// 只有slam刚启动时子图的个数才为1
if (insertion_submaps.size() == 1) {
// If we don't already have an entry for the first submap, add one.
// 如果判断指定id的submap_data的size为0, 这条轨迹上还没有添加submap的pose
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
// 如果没设置初始位姿就是0, 设置了就是1
if (data_.initial_trajectory_poses.count(trajectory_id) > 0) {
// 把该trajectory_id与其初始位姿的基准轨迹的id关联起来
data_.trajectory_connectivity_state.Connect(
trajectory_id,
data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id,
time);
}
// 将该submap的global pose加入到optimization_problem_中
optimization_problem_->AddSubmap(
trajectory_id, transform::Project2D(
ComputeLocalToGlobalTransform(
data_.global_submap_poses_2d, trajectory_id) *
insertion_submaps[0]->local_pose()));
}
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
// 因为是第一个submap, 所以该submap的ID是(trajectory_id,0), 其中0是submap的index, 从0开始
const SubmapId submap_id{
trajectory_id, 0};
// 检查这个SubmapId下的submap是否等于insertion_submaps的第一个元素.因为我们初始化第一个submap肯定是要被插入的那个submap
CHECK(data_.submap_data.at(submap_id).submap == insertion_submaps.front());
// 因为是第一个submap, 那就把刚刚建立的submap的id返回
return {
submap_id};
}
CHECK_EQ(2, insertion_submaps.size());
// 获取 submap_data 的末尾 trajectory_id
const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
// end_it是最后一个元素的下一个位置, 所以它之前的一个submap的id就是submap_data中的最后一个元素
// 注意, 这里的last_submap_id 是 optimization_problem_->submap_data() 中的
const SubmapId last_submap_id = std::prev(end_it)->id;
// 如果是等于第一个子图, 说明insertion_submaps的第二个子图还没有加入到optimization_problem_中
// 拿着optimization_problem_中子图的索引, 根据这个索引在data_.submap_data中获取地图的指针
if (data_.submap_data.at(last_submap_id).submap ==
insertion_submaps.front()) {
// In this case, 'last_submap_id' is the ID of
// 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
// 这种情况下, 要给新的submap分配id, 并把它加到OptimizationProblem的submap_data_这个容器中
const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
// 解算新的submap的global pose, 插入到OptimizationProblem2D::submap_data_中
optimization_problem_->AddSubmap(
trajectory_id,
// first_submap_pose * constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() = globla指向local的坐标变换
// globla指向local的坐标变换 * 第二个子图原点在local下的坐标 = 第二个子图原点在global下的坐标
first_submap_pose *
constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
constraints::ComputeSubmapPose(*insertion_submaps[1]));
return {
last_submap_id,
SubmapId{
trajectory_id, last_submap_id.submap_index + 1}};
}
// 如果是等于第二个子图, 说明第二个子图已经分配了id, 已经在OptimizationProblem的submap_data_中了
CHECK(data_.submap_data.at(last_submap_id).submap ==
insertion_submaps.back());
// 那么第一个子图的index就是last_submap_id.submap_index的前一个, 所以要-1
const SubmapId front_submap_id{
trajectory_id,
last_submap_id.submap_index - 1};
CHECK(data_.submap_data.at(front_submap_id).submap ==
insertion_submaps.front());
return {
front_submap_id, last_submap_id};
}
三、函数输入
前面已经提到了，该函数接收一个 trajectory_id，节点数据 constant_data 的时间 time，以及目前两个活跃的子图 insertion_submaps。接收到参数之后，首先执行了如下代码：
// submap_data中存的: key 为 SubmapId, values 为对应id的Submap在global坐标系下的全局位姿
const auto& submap_data = optimization_problem_->submap_data();
其首先获取 optimization_problem_ 中的所有子图数据，赋值给 submap_data。
四、初始处理
如果此时系统刚启动，那么此时 insertion_submaps 中只存储了一个子图，也就符合条件 insertion_submaps.size() == 1。此时会做那些处理呢？源码如下所示：
// 只有slam刚启动时子图的个数才为1
if (insertion_submaps.size() == 1) {
// If we don't already have an entry for the first submap, add one.
// 如果判断指定id的submap_data的size为0, 这条轨迹上还没有添加submap的pose
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
// 如果没设置初始位姿就是0, 设置了就是1
if (data_.initial_trajectory_poses.count(trajectory_id) > 0) {
// 把该trajectory_id与其初始位姿的基准轨迹的id关联起来
data_.trajectory_connectivity_state.Connect(
trajectory_id,
data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id,
time);
}
// 将该submap的global pose加入到optimization_problem_中
optimization_problem_->AddSubmap(
trajectory_id, transform::Project2D(
ComputeLocalToGlobalTransform(
data_.global_submap_poses_2d, trajectory_id) *
insertion_submaps[0]->local_pose()));
}
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
// 因为是第一个submap, 所以该submap的ID是(trajectory_id,0), 其中0是submap的index, 从0开始
const SubmapId submap_id{
trajectory_id, 0};
// 检查这个SubmapId下的submap是否等于insertion_submaps的第一个元素.因为我们初始化第一个submap肯定是要被插入的那个submap
CHECK(data_.submap_data.at(submap_id).submap == insertion_submaps.front());
// 因为是第一个submap, 那就把刚刚建立的submap的id返回
return {
submap_id};
}
其首先判断一下当前这个活跃的子图是否被添加到 submap_data 之中，如果没有添加，则调用 optimization_problem_->AddSubmap() 函数添加至 optimization_problem_ 之中。当然，在这之前首先会判断一下是否有为该轨迹设置初始位姿，如果设置了会调用 data_.trajectory_connectivity_state.Connect 把 trajectory_id 与其初始位姿的基准轨迹的id关联起来。简单的说，就是如果之前已经存在一条轨迹 t 了，可以同通过配置文件中的 initial_trajectory_pose 参数，为 trajectory_id 这条轨迹设置一个相对于轨迹 t 的基准位姿作为初始位置。
需要注意的是，在为 optimization_problem_ 添加子图时，其调用了 ComputeLocalToGlobalTransform 函数计算子图的全局位姿。该函数前面在前面的博客 (02)Cartographer源码无死角解析-(55) 2D后端优化→ComputeLocalToGlobalTransform()，TrajectoryNode 中有进行讲解，先放一下，稍后我们回过来再分析一下。
因为这是第一个子图，所以 trajectory_id 轨迹对应的子图只能存在一个，所以执行
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
这段代码判断一下，确保逻辑上的正确性。接着为该子图创建一个 SubmapId 对象，且子图的序列好为 0，表示 trajectory_id 轨迹上的第一个子图。然后以列表的形式返回这个 SubmapId 对象 {submap_id}。
四、添加第二个活跃的子图
完成初始处理之后，后续的都是常规处理了。除了建图开始阶段只有一个活跃的子图，后续都存在两个活跃的子图，地图更新或者插入点云时，都是同时往这两个活跃的子图中插入的。所以源码中执行了 CHECK_EQ(2, insertion_submaps.size())，同理是为了确保逻辑的正确性。接着可以看到如下代码：
// 获取 submap_data 的末尾 trajectory_id
const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
// end_it是最后一个元素的下一个位置, 所以它之前的一个submap的id就是submap_data中的最后一个元素
// 注意, 这里的last_submap_id 是 optimization_problem_->submap_data() 中的
const SubmapId last_submap_id = std::prev(end_it)->id;
第一步是确保 submap_data 中轨迹 trajectory_id 对应的子图数不为0，另外获得最后一个子图的 SubmapId。随后，运行了如下这段代码：
// 如果是等于第一个子图, 说明insertion_submaps的第二个子图还没有加入到optimization_problem_中
// 拿着optimization_problem_中子图的索引, 根据这个索引在data_.submap_data中获取地图的指针
if (data_.submap_data.at(last_submap_id).submap ==
insertion_submaps.front()) {
// In this case, 'last_submap_id' is the ID of
// 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
// 这种情况下, 要给新的submap分配id, 并把它加到OptimizationProblem的submap_data_这个容器中
const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
// 解算新的submap的global pose, 插入到OptimizationProblem2D::submap_data_中
optimization_problem_->AddSubmap(
trajectory_id,
// first_submap_pose * constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() = globla指向local的坐标变换
// globla指向local的坐标变换 * 第二个子图原点在local下的坐标 = 第二个子图原点在global下的坐标
first_submap_pose *
constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
constraints::ComputeSubmapPose(*insertion_submaps[1]));
return {
last_submap_id,
SubmapId{
trajectory_id, last_submap_id.submap_index + 1}};
}
其先判断一下 trajectory_id 对应的最后子图 last_submap_id，其是否为活跃子图的第一个子图，如果是，则说明第二个活跃的子图还没有添加到 optimization_problem_ 之中，接下来的操作不用多说，也知道就是调用 optimization_problem_->AddSubmap() 函数进行添加。first_submap_pose 活跃的第一个子图(第二个此时还没有添加)的 global 位姿，这里我们记为 S u b m a p 1 p o s e g l o b a l \mathbf {Submap1}^{global}_{pose} Submap1poseglobal，constraints::ComputeSubmapPose(*insertion_submaps[0]) 是第一个活跃子图的第 local 位姿，这里我们记为 S u b m a p 1 p o s e l o c a l \mathbf {Submap1}^{local}_{pose} Submap1poselocal，constraints::ComputeSubmapPose(*insertion_submaps[1]) 当然表示第二个子图的 local 位姿，同理记为 S u b m a p 2 p o s e l o c a l \mathbf {Submap2}^{local}_{pose} Submap2poselocal，那么最终等价的数学公式如下：
S u b m a p 2 p o s e g l o b a l = S u b m a p 1 p o s e g l o b a l ∗ [ S u b m a p 1 p o s e l o c a l ] − 1 ∗ S u b m a p 2 p o s e l o a c l (01) \color{Green} \tag{01} \mathbf {Submap2}^{global}_{pose} = \mathbf {Submap1}^{global}_{pose}*[\mathbf {Submap1}^{local}_{pose}]^{-1}*\mathbf {Submap2}^{loacl}_{pose} Submap2poseglobal=Submap1poseglobal∗[Submap1poselocal]−1∗Submap2poseloacl(01)
可以很明显的知道最终求得 Submap2，也就是第二个活跃的子图 global 系下的位姿。添加了一个新的子图到 optimization_problem_ 之中，其构建的 SubmapId 对应的 submap_id 比之前进行 +1 操作。然后返回两个活跃子图的 SubmapId。
五、添加第二个活跃的子图
通过前面四、五的两种情况，就会为把所有活跃的子图都添加至 optimization_problem_ 之中了，且每个子图都只被添加了一次。添加之后，optimization_problem_ 就存储了子图对应的 SubmapId，后续自己获取返回即可，代码如下：
// 如果是等于第二个子图, 说明第二个子图已经分配了id, 已经在OptimizationProblem的submap_data_中了
CHECK(data_.submap_data.at(last_submap_id).submap ==
insertion_submaps.back());
// 那么第一个子图的index就是last_submap_id.submap_index的前一个, 所以要-1
const SubmapId front_submap_id{
trajectory_id,
last_submap_id.submap_index - 1};
CHECK(data_.submap_data.at(front_submap_id).submap ==
insertion_submaps.front());
return {
front_submap_id, last_submap_id};
六、ComputeLocalToGlobalTransform
疑问 3 : \color{red}疑问3: 疑问3: data_.global_submap_poses_2d 中存储子图的全局位姿，那么子图的全局位姿又是从哪里来的呢？
现在就是要解答这个疑问了，回到前面提到的 ComputeLocalToGlobalTransform() 函数，首先要注意的是，其只在 optimization_problem_ 添加第一个子图的时候需要调用该函数，后续子图的 global 位姿都是依靠前一个子图的 global 位姿计算出来的。该函数又是依旧 data_.global_submap_poses_2d 推算子图 global 位姿的。但是， 注意 : \color{red}注意: 注意: , 此时第一个子图都还没有添加，那么也就是说 data_.global_submap_poses_2d 肯定是空的，也就是说 ComputeLocalToGlobalTransform() 函数执行的是下面这段代码：
// 没找到这个轨迹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;
}
// note: 没设置初始位姿就将返回(0,0,0)的平移和旋转
else {
return transform::Rigid3d::Identity();
}
总的来说，如果 trajectory_id 轨迹有设置先对于其他轨迹的初始位姿，则会使用线性插值计算出子图的 global 位姿，如果没有设置，则认为第一个子图的位姿就是 transform::Rigid3d::Identity()。
七、总结
这样我们就解答了
疑问 3 : \color{red}疑问3: 疑问3: data_.global_submap_poses_2d 中存储子图的全局位姿，那么子图的全局位姿又是从哪里来的呢？
总的来说，第一个子图的 global 位姿认为是 transform::Rigid3d::Identity()，后面的子图位姿都是参考结合子图的局部位姿，近而推算出 global_ 位姿。