(02)Cartographer源码无死角解析-(75) 2D后端优化→整体复盘，理解后端优化核心思想

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

再前面的博客中，本人有提出过自己对于后端优化的讲解如下：

$\color{red}个人理解:$ 这里，本人抛砖引为引玉，谈一下个人对后端优化的理解。首先后端优化是从一个整理来考虑的，其基于 global 系。利用到到子图与子图之间的变换关系，可以在前端(基于local系)过程中，子图单个相对于 local系的位姿，可以说他们是没有直接联系到一起的。除了子图，节点也一样，前端的节点都是根据活跃子图计算出相对于 local系的位姿，但是却没有和前面已经完成的子图联系到一起，也显得比较孤立。

但是在后端优化中，这些都被考虑了进去，其估算出来的节点位姿，不仅希望其在活跃的子图位姿比较准确，还希望其相对于其他子图的位姿也比较准确，并且子图与子图之间的位姿也比较准确。同时呢，还要这些准确的位姿传递给到前端，因为全局优化是以一定频率进行优化，而不是只优化一次。

优化的过程，就是对位姿进行调整，让约束(精确解)不变的情况下，对节点与子图的global位姿进行调整优化，优化过的位姿节点位姿与子图global位姿分别保存在 PoseGraph2D::data_::trajectory_nodes 与 PoseGraph2D::data_::global_submap_poses_2d 之中。另外，后端优化中还可以结合了 OdometryData、FixedFramePoseData、ImuData、LandmarkData 等数据对位姿进行优化，也可以理解为约束。

总的来说，就是保证约束不变的前提下，对其他的位姿进行调整，就好像又一个渔网，关系错综复杂，现在我们要把这个网展开。应该怎么做呢？可以找其中有特点的地方，比如四个角，或者之前打过标识的地方，先把这些位置固定好，固定之后再一点一点的调整其他的位置。直到把网铺平，展开为止。

$\color{red}总结：$ 后端优化是在图优化的框架下，通过调整节点和子图的位姿，以最大程度地满足各种约束条件，使得整个系统的位姿估计更加准确和一致。

在后端优化中，节点和子图的位姿是通过最小化误差函数来进行调整的。这个误差函数可以包含多种约束，如里程计测量、IMU数据、固定的参考帧位姿、特征点观测等等。优化的目标是找到一组位姿，使得所有的约束都得到满足，并且使得整个系统的一致性最好。

在优化的过程中，为了保证约束的准确性，需要对位姿进行调整，同时也要考虑到不同节点和子图之间的关系，以及它们与前端位姿估计的一致性。这样可以使得后端优化得到的位姿既在局部子图中准确，也在全局子图中准确，并且能够传递给前端进行实时的位姿更新。

上一篇博客中，将优化过程比喻为展开一个错综复杂的渔网是一种形象的描述。通过固定一些已知的位置（类似于固定特定的节点或子图），然后逐步调整其他位置（调整其他节点和子图的位姿），最终将整个系统的位姿展开为一个一致的整体。不过描述中可能存在一些技术上的细节或理解上的差异。

二、残差优化效果

1、基于节点与子图(子图内，子图间)约束的残差
2、基于Landmark的残差
3、基于odometry里程计的残差
4、基于节点local系下的残差
5、基于GPS数据的残差

$\color{blue}（1）：$ 对于第1种约束，其主要用于优化global系下的节点、子图的位姿。

$\color{blue}（2）：$ 对于第2种约束，其主要用于优化global系下两个节点之间的插值位姿，进而达到优化两节点的位姿。

$\color{blue}（3）：$ 对于第3种约束，主要对单个节点位姿进行优化。

$\color{blue}（4）：$ 对相邻的两个节点位姿进行优化

$\color{blue}（5）：$ 对单个节点进行优化

$\color{blue}注意：$ 通过循环以及插值的方式，上诉都可以实现对多个节点的位姿优化。

三、优化图示

在这里插入图片描述
为了象形表达优化的过程，本人把子图内约束、子图间约束、local约束画了出来，其优化的过程就是保持浅绿、深绿、红色线段不变，逐步调整黑色线段的过程。上图种，把子图的原点绘画在右上角本人目前是无法确定是否正确。后续有机会再为大家分析 Cartographer 的各种坐标系的朝向，不过子图的x，y轴理论上来说应该是与 local 系的x,y轴平行的，方向目前不是很确定。另外关于odometry里程计、GPS、Landmark 的数据都没有绘画，因为似乎不好共同体现出来，有兴趣的朋友可以单独绘画一下。

四、对外接口

后端优化的变量，主要在 OptimizationProblem2D::Solve() 函数的末尾体现出来，

  // 将优化后的所有数据进行更新 Store the result.
  for (const auto& C_submap_id_data : C_submaps) {
    submap_data_.at(C_submap_id_data.id).global_pose =
        ToPose(C_submap_id_data.data);
  }
  for (const auto& C_node_id_data : C_nodes) {
    node_data_.at(C_node_id_data.id).global_pose_2d =
        ToPose(C_node_id_data.data);
  }
  for (const auto& C_fixed_frame : C_fixed_frames) {
    trajectory_data_.at(C_fixed_frame.first).fixed_frame_origin_in_map =
        transform::Embed3D(ToPose(C_fixed_frame.second));
  }
  for (const auto& C_landmark : C_landmarks) {
    landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
  }

从这里可以看出，后端主要优化的变量就是子图、节点在globl系下的位姿，GPS初始帧在global系下的位姿，以及 landmark 数据，这些数据虽然更新，但是其是存储于 c 的私有成员之中，如何传递到外部呢？首先在 PoseGraph2D::RunOptimization() 函数中，在调用 optimization_problem_->Solve() 之后，执行了如下代码：

  // 获取优化后的结果
  // submap_data的类型是 MapById<SubmapId, optimization::SubmapSpec2D> 
  const auto& submap_data = optimization_problem_->submap_data();

  // node_data的类型是 MapById<NodeId, NodeSpec2D>
  // node_data是优化后的所有节点的新位姿
  const auto& node_data = optimization_problem_->node_data();

  // 更新轨迹内的节点位置
  for (const int trajectory_id : node_data.trajectory_ids()) {

    // Step: 根据优化后的结果对data_.trajectory_nodes的global_pose进行更新
    for (const auto& node : node_data.trajectory(trajectory_id)) {
	......

  } // end for trajectory_id

  // 更新data_.landmark_nodes
  for (const auto& landmark : optimization_problem_->landmark_data()) {
    data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
  }

  // 更新所有submap的位姿
  data_.global_submap_poses_2d = submap_data;

也就是说，其已经将部分数据传递到了 PoseGraph2D 之中，如 landmark 数据，子图global位姿以及节点的 global 位姿。但是这里依旧还存在一个问题，无论 PoseGraph2D 还是 OptimizationProblem2D 其是属于src/cartographer 下的代码，这些即使有了这些接口数据，又是如何与 src/cartographer_ros 的代码联系到一起的呢？在src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 文件中搜索，可以找到如下代码，后端优化与被 cartographer_ros 中代码查询或者调用的过程。

MapBuilderBridge::GetSubmapList()
	map_builder_->pose_graph()->GetAllSubmapPoses();
	
MapBuilderBridge::GetLocalTrajectoryData()
	map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id),
	
MapBuilderBridge::HandleTrajectoryQuery
	map_builder_->pose_graph()->GetTrajectoryNodePoses()

MapBuilderBridge::GetTrajectoryNodeList()
	map_builder_->pose_graph()->GetTrajectoryNodePoses()
	map_builder_->pose_graph()->constraints()
	
MapBuilderBridge::GetLandmarkPosesList()
	map_builder_->pose_graph()->GetLandmarkPoses()
	
MapBuilderBridge::GetConstraintList()
	pose_graph()->GetTrajectoryNodePoses()
	map_builder_->pose_graph()->GetAllSubmapPoses()
	map_builder_->pose_graph()->constraints()

五、优化过程不变量

在后端优化过程中，有些参数是一致保持不变的，首先前端相关的数据，如点云、节点local位姿，子图local位姿，其虽然有传递给后端，但是后端优化过程中并没有对这些前端的位姿以及数据进行改变，可以说他们是相互独立的。

除此之外即使有些被传递到了后端，也没有被修改，如点云数据、被冻结轨迹的子图，以及节点位姿等。对于第一个子图的位姿，无论其对应轨迹是否冻结，其在 global 坐标系下的位姿不会被优化所改变，且与 local 系的第一个子图位姿保持一致。

六、回环检测

PoseGraph2D::ComputeConstraintsForNode() 函数计算子图间约束的代码：

  // Step: 当前节点与所有已经完成的子图进行约束的计算---实际上就是回环检测
  for (const auto& submap_id : finished_submap_ids) {
    // 计算旧的submap和新的节点间的约束
    ComputeConstraint(node_id, submap_id);
  }

  // Step: 计算所有节点与刚完成子图间的约束---实际上就是回环检测
  if (newly_finished_submap) {
    const SubmapId newly_finished_submap_id = submap_ids.front();
    // We have a new completed submap, so we look into adding constraints for
    // old nodes.
    for (const auto& node_id_data : optimization_problem_->node_data()) {
      const NodeId& node_id = node_id_data.id;
      // 刚结束的子图内部的节点, 不再与这个子图进行约束的计算
      if (newly_finished_submap_node_ids.count(node_id) == 0) {
        // 计算新的submap和旧的节点间的约束
        ComputeConstraint(node_id, newly_finished_submap_id);
      }
    }
  }

本质上来说就是回环检测，为什么呢？首先来说局部约束，在建图的时候会调用到 ConstraintBuilder2D::MaybeAddConstraint() 函数，就是建立节点与一定范围之间的约束，如果之前机器人经过某个区域，那么该区域就是就那么机器再次靠近该区域时，其距离肯定比较近的，那么就会建立约束，用于后端优化。全局回环就是节点子图建立约束。

$\color{red}本质思想:$ 节点与子图建立约束的时候，其先构建多分辨率地图，然后通过分支定界算法进行粗匹配，最后使用 Ceres 进行精匹配。总的来说，局部约束本质就是局部匹配，全局约束就是全局匹配，前端中使用的匹配，是在节点所属子图内进行匹配。

七、子图修剪

该部分的代码，起始主要与重定位模式相关，这里进行一个初步的讲解，后续会单独出一篇博客进行详细的分析。首先在 PoseGraph2D::HandleWorkQueue() 函数中可以看到如下代码段：

    TrimmingHandle trimming_handle(this);
    // 进行子图的裁剪, 如果没有裁剪器就不裁剪
    for (auto& trimmer : trimmers_) {
      trimmer->Trim(&trimming_handle); // PureLocalizationTrimmer::Trim()
    }
    // 如果裁剪器处于完成状态, 就把裁剪器删除掉
    trimmers_.erase(
        // c++11: std::remove_if 如果回调函数函数返回真,则将当前所指向的参数移到尾部,返回值是被移动区域的首个元素
        std::remove_if(trimmers_.begin(), trimmers_.end(),
                       [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
                         return trimmer->IsFinished(); // 调用PureLocalizationTrimmer::IsFinished()
                       }),
        trimmers_.end());

该段代码主要的作用是对子图进行修剪，直白的说就是移除那些比较小的子图，或者与其他子图重叠部分很少的子图，认为这些子图没有太大的意义，所以从 PoseGraph2D::data_.submap_data 中删除这些子图，后续节点不与这些子图建立约束。

trimmer->Trim(&trimming_handle); 这句代码，起始最终会调用到 PoseGraph2D::TrimmingHandle::TrimSubmap() 这个函数，代码注释如下(简单看一下，后续重定位章节会进行重点分析)：

// 删除指定id的子图, 并删除相关的约束,匹配器,与节点
void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) {
  // TODO(hrapp): We have to make sure that the trajectory has been finished
  // if we want to delete the last submaps.
  // 只有kFinished状态的子图才能够裁剪
  CHECK(parent_->data_.submap_data.at(submap_id).state ==
        SubmapState::kFinished);

  // Compile all nodes that are still INTRA_SUBMAP constrained to other submaps
  // once the submap with 'submap_id' is gone.
  // We need to use node_ids instead of constraints here to be also compatible
  // with frozen trajectories that don't have intra-constraints.
  // 获取除submap_id外的所有子图的所有节点的id
  std::set<NodeId> nodes_to_retain;
  for (const auto& submap_data : parent_->data_.submap_data) {
    if (submap_data.id != submap_id) {
      nodes_to_retain.insert(submap_data.data.node_ids.begin(),
                             submap_data.data.node_ids.end());
    }
  }

  // Remove all nodes that are exlusively associated to 'submap_id'.
  // 找到在submap_id的子图内部同时不在别的子图内的节点, 这些节点需要删除
  std::set<NodeId> nodes_to_remove;
  // c++11: std::set_difference 求set的差集, 在first_set中出现, 在second_set中不出现的元素
  std::set_difference(parent_->data_.submap_data.at(submap_id).node_ids.begin(),
                      parent_->data_.submap_data.at(submap_id).node_ids.end(),
                      nodes_to_retain.begin(), nodes_to_retain.end(),
                      std::inserter(nodes_to_remove, nodes_to_remove.begin()));

  // Remove all 'data_.constraints' related to 'submap_id'.
  {
    // Step: 1 删除submap_id相关的约束
    std::vector<Constraint> constraints;
    for (const Constraint& constraint : parent_->data_.constraints) {
      if (constraint.submap_id != submap_id) {
        constraints.push_back(constraint);
      }
    }
    parent_->data_.constraints = std::move(constraints);
  }

  // Remove all 'data_.constraints' related to 'nodes_to_remove'.
  // If the removal lets other submaps lose all their inter-submap constraints,
  // delete their corresponding constraint submap matchers to save memory.
  {
    std::vector<Constraint> constraints;
    std::set<SubmapId> other_submap_ids_losing_constraints;
    // Step: 2 删除与nodes_to_remove中节点相关联的约束, 并对submap_id进行标记
    for (const Constraint& constraint : parent_->data_.constraints) {
      if (nodes_to_remove.count(constraint.node_id) == 0) {
        constraints.push_back(constraint);
      } else {
        // A constraint to another submap will be removed, mark it as affected.
        other_submap_ids_losing_constraints.insert(constraint.submap_id);
      }
    }
    parent_->data_.constraints = std::move(constraints);
    // Go through the remaining constraints to ensure we only delete scan
    // matchers of other submaps that have no inter-submap constraints left.
    // 检查剩余的约束以确保我们只删除没有子图间约束的其他子图的扫描匹配器
    for (const Constraint& constraint : parent_->data_.constraints) {
      if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) {
        continue;
      } 
      // 只要other_submap_ids_losing_constraints内submap_id还存在其他的子图间约束
      // 就把这个子图id从other_submap_ids_losing_constraints中删除, 可以留着
      else if (other_submap_ids_losing_constraints.count(
                     constraint.submap_id)) {
        // This submap still has inter-submap constraints - ignore it.
        other_submap_ids_losing_constraints.erase(constraint.submap_id);
      }
    }
    // Delete scan matchers of the submaps that lost all constraints.
    // TODO(wohe): An improvement to this implementation would be to add the
    // caching logic at the constraint builder which could keep around only
    // recently used scan matchers.
    // Step: 3 删除这些子图id的匹配器
    for (const SubmapId& submap_id : other_submap_ids_losing_constraints) {
      parent_->constraint_builder_.DeleteScanMatcher(submap_id);
    }
  }

  // Mark the submap with 'submap_id' as trimmed and remove its data.
  CHECK(parent_->data_.submap_data.at(submap_id).state ==
        SubmapState::kFinished);
  // Step: 4 删除这个子图的指针
  parent_->data_.submap_data.Trim(submap_id);
  // Step: 5 删除这个子图的匹配器, 与多分辨率地图
  parent_->constraint_builder_.DeleteScanMatcher(submap_id);
  // Step: 6 删除optimization_problem_中的这个子图
  parent_->optimization_problem_->TrimSubmap(submap_id);

  // We have one submap less, update the gauge metrics.
  kDeletedSubmapsMetric->Increment();
  if (parent_->IsTrajectoryFrozen(submap_id.trajectory_id)) {
    kFrozenSubmapsMetric->Decrement();
  } else {
    kActiveSubmapsMetric->Decrement();
  }

  // Remove the 'nodes_to_remove' from the pose graph and the optimization
  // problem.
  // Step: 7 删除节点
  for (const NodeId& node_id : nodes_to_remove) {
    parent_->data_.trajectory_nodes.Trim(node_id);
    parent_->optimization_problem_->TrimTrajectoryNode(node_id);
  }
}

八、结语

到目前为止，关于 Cartographer 数据处理，前端，后端都讲解完成了，也就是说，目前已经能够获得比较准确的地图以及位姿了。那么这些位姿外界 cartographer_ros 是如何获取到的，且进行可视化发布到 rviz 的呢？接下来，我们会从 src/cartographer 跳出来，回到最初的 cartographer_ros 部分继续分析源码。