描述

主要分析node_main.cc中的Run()函数中的RunFinalOptimization()

node.RunFinalOptimization();

RunFinalOptimization()的基本逻辑

第一层:node.cc

RunFinalOptimization()函数定义在node.cc

void Node::RunFinalOptimization() {
  {
    for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
      const int trajectory_id = entry.first;
      if (entry.second == TrajectoryState::ACTIVE) {
        LOG(WARNING)
            << "Can't run final optimization if there are one or more active "
               "trajectories. Trying to finish trajectory with ID "
            << std::to_string(trajectory_id) << " now.";
        CHECK(FinishTrajectory(trajectory_id))
            << "Failed to finish trajectory with ID "
            << std::to_string(trajectory_id) << ".";
      }
    }
  }
  // Assuming we are not adding new data anymore, the final optimization
  // can be performed without holding the mutex.
  map_builder_bridge_.RunFinalOptimization();
}

显然,使用for循环遍历了全部的轨迹状态

for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) 

并且使用if语句判断,如果某条轨迹的value是ACTIVE,就要打log并报错

if (entry.second == TrajectoryState::ACTIVE) 

如果每条轨迹的value都不是ACTIVE,可能已经完成了建图,我们现在要做的就是优化一下轨迹,执行的也就是核心语句

map_builder_bridge_.RunFinalOptimization();

备注的翻译:假设我们不再添加新的数据,最终的优化可以在不持有互斥锁的情况下执行。

第二层:map_builder_bridge.cc
map_builder_bridge_.RunFinalOptimization();中的RunFinalOptimization()函数定义在map_builder_bridge.cc中

void MapBuilderBridge::RunFinalOptimization() {
  LOG(INFO) << "Running final trajectory optimization...";
  map_builder_->pose_graph()->RunFinalOptimization();
}

map_builder_是声明在map_builder_bridge.h中的一个MapBuilderInterface类型指针

std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;

pose_graph()MapBuilderInterface类的一个成员函数,是一个返回值为PoseGraphInterface类型指针的虚函数,是一个纯虚函数

virtual mapping::PoseGraphInterface* pose_graph() = 0;

RunFinalOptimization()声明在pose_graph_interface.h,是PoseGraphInterface类的一个成员函数,仍是一个纯虚函数

// Waits for all computations to finish and computes optimized poses.
virtual void RunFinalOptimization() = 0;

注释的翻译解释了RunFinalOptimization的功能:等待所有的计算完成并计算优化的姿态。

RunFinalOptimization的重写函数在哪里呢,我们查看源码发现。pose_graph.h中声明的PoseGraph类,继承了pose_graph_interface.h中的PoseGraphInterface类。

而在cartographer/cartographer/mapping/internal/2d/pose_graph_2d.h中,声明的PoseGraph2D类继承了PoseGraph类,并对RunFinalOptimization函数进行了重写;并且在cartographer/cartographer/mapping/internal/3d/pose_graph_3d.h中,声明的PoseGraph3D类同样继承了PoseGraph类,也一样对RunFinalOptimization函数进行了重写。2D和3D的RunFinalOptimization函数都定义在了它们对应的cc文件中pose_graph_2d.cc和pose_graph_3d.cc

因此我们不难看出

PoseGraph2D/PoseGraph3D 继承自-> PoseGraph 继承自 -> PoseGraphInterface