(02)Cartographer源码无死角解析-(58) 2D后端优化→ PoseGraph2D::AddNode()、PoseGraph2D::AppendNode()

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

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

一、前言

通过前面博客分析，对一些基础的类与结构讲解完成之后，现在再回过头来看一下 PoseGraph2D::AppendNode()：

/**
 * @brief 向节点列表中添加一个新的节点, 并保存新生成的submap
 * 
 * @param[in] constant_data 节点数据的指针
 * @param[in] trajectory_id 轨迹id
 * @param[in] insertion_submaps 子地图指针的vector
 * @param[in] optimized_pose 当前节点在global坐标系下的坐标
 * @return NodeId 返回新生成的节点id
 */
NodeId PoseGraph2D::AppendNode(
    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) {
    
    
	......
	......
}

把上一篇博客的内容在这里重复一下：需要注意的是 insertion_submaps 中存储的子图是目前活跃的子图，通常情况只包含两个子图，如果不是记得很清楚的朋友可能需要回过头分析一下子图创建与插入点云相关代码了。optimized_pose 是 Robot 在 global 系下的位姿。

二、AddTrajectoryIfNeeded(trajectory_id)

$\color{blue}(1)$ PoseGraph2D::AppendNode() 首先调用 AddTrajectoryIfNeeded(trajectory_id) 函数。首先如果 trajectory_id 对应轨迹的状态未被添加至成员变量 PoseGraph2D::data_::trajectories_state 中，则会为该轨迹创建一个默认的 InternalTrajectoryState 对象，后续会一直使用其存储该轨迹的状态。

$\color{blue}(2)$ 接会对轨迹的状态进行检查，轨迹不能处于完成FINISHED或者删除DELETED状态，否则报错。因为处于这两种状态的轨迹是不能调用 PoseGraph2D::AppendNode() 函数添加节点的，自然也不会执行PoseGraph2D::AddTrajectoryIfNeeded() 函数。除此之外，轨迹的删除状态 data_.trajectories_state.at(trajectory_id).deletion_state 需要标识为正常，也就是说等待删除或者计划要删除的轨迹，也不能调用 PoseGraph2D::AppendNode() 函数添加节点。

$\color{blue}(3)$ 调用 data_.trajectory_connectivity_state.Add(trajectory_id) 函数进行轨迹添加，本质上就是自己与自己连接，往 TrajectoryConnectivityState::connected_components_::forest_ 中添加一个(trajectory_id,trajectory_id) 元素。

$\color{blue}(4)$ 为每个轨迹都会构建一个采样器，即 FixedRatioSampler 类型的实例对象，然后添加到成员变量 PoseGraph2D::global_localization_samplers_ 中，注意，该处不会重复添加。

// 如果轨迹不存在, 则将轨迹添加到连接状态里并添加采样器
void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) {
    
    
  // 如果不存在就添加map中,调用默认构造函数创建value对象
  data_.trajectories_state[trajectory_id];

  //trajectory_id轨迹不能是完成或者删除状态
  CHECK(data_.trajectories_state.at(trajectory_id).state !=
        TrajectoryState::FINISHED);
  CHECK(data_.trajectories_state.at(trajectory_id).state !=
        TrajectoryState::DELETED);
  //trajectory_id轨迹.deletion_state需要标识为NORMAL
  CHECK(data_.trajectories_state.at(trajectory_id).deletion_state ==
        InternalTrajectoryState::DeletionState::NORMAL);

  // 将轨迹添加到连接状态里,并与自己做连接
  data_.trajectory_connectivity_state.Add(trajectory_id);

  // Make sure we have a sampler for this trajectory.
  // 为轨迹添加采样器
  if (!global_localization_samplers_[trajectory_id]) {
    
    
    global_localization_samplers_[trajectory_id] =
        absl::make_unique<common::FixedRatioSampler>(
            options_.global_sampling_ratio());
  }
}

三、CanAddWorkItemModifying()

PoseGraph2D::AppendNode 在调用 AddTrajectoryIfNeeded() 函数之后，接着执行如下代码：

  // 根据轨迹状态判断是否可以添加修改任务
  if (!CanAddWorkItemModifying(trajectory_id)) {
    
    
    LOG(WARNING) << "AddNode was called for finished or deleted trajectory.";
  }

其主要功能就是根据轨迹的状态，判断是否可以添加修改任务，如果不能添加修改任务则会进行打印。CanAddWorkItemModifying() 函数其实比较简单。

$\color{blue}(1)$ 首先判断一下 trajectory_id 轨迹状态是否被保存过，如果已经 PoseGraph2D::data_.trajectories_state 中没有记录，说明其时一个新轨迹，返回true，表示该轨迹可以添加修改任务。

$\color{blue}(2)$ 对于完成或者删除状态(含等待删除与计划删除)的轨迹，都返回 false，表示无法添加修改任务。

$\color{blue}(3)$ 剩余情况都返回 true，表示该轨迹可以添加修改任务。函数的代码注释如下：

// 根据轨迹状态判断是否可以添加任务
bool PoseGraph2D::CanAddWorkItemModifying(int trajectory_id) {
    
    
  auto it = data_.trajectories_state.find(trajectory_id);
  if (it == data_.trajectories_state.end()) {
    
    
    return true;
  }
  if (it->second.state == TrajectoryState::FINISHED) {
    
    
    // TODO(gaschler): Replace all FATAL to WARNING after some testing.
    LOG(FATAL) << "trajectory_id " << trajectory_id
               << " has finished "
                  "but modification is requested, skipping.";
    return false;
  }
  if (it->second.deletion_state !=
      InternalTrajectoryState::DeletionState::NORMAL) {
    
    
    LOG(FATAL) << "trajectory_id " << trajectory_id
               << " has been scheduled for deletion "
                  "but modification is requested, skipping.";
    return false;
  }
  if (it->second.state == TrajectoryState::DELETED) {
    
    
    LOG(FATAL) << "trajectory_id " << trajectory_id
               << " has been deleted "
                  "but modification is requested, skipping.";
    return false;
  }
  return true;
}

从源码上来看，PoseGraph2D::AppendNode() 调用 CanAddWorkItemModifying() 似乎没有太大作用，

四、data_.trajectory_nodes.Append()

在调用 CanAddWorkItemModifying() 之后，接着执行如下核心代码：

  // 向节点列表中添加一个新的节点
  const NodeId node_id = data_.trajectory_nodes.Append(
      trajectory_id, TrajectoryNode{
    
    constant_data, optimized_pose});
  // 节点总个数加1
  ++data_.num_trajectory_nodes;

该处就比较简单了，就是以 constant_data 与 optimized_pose(优化后的全局位姿) 构建一个 TrajectoryNode 实例，然后存储到 data_.trajectory_nodes 之中。然后执行 +1 操作计数。

五、剩余代码

先来看一下代码注释，然后再纤细讲解：

  // Test if the 'insertion_submap.back()' is one we never saw before.
  // 如果是刚开始的轨迹, 或者insertion_submaps.back()是第一次看到, 就添加新的子图
  if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
      std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))
              ->data.submap != insertion_submaps.back()) {
    
    
    // We grow 'data_.submap_data' as needed. This code assumes that the first
    // time we see a new submap is as 'insertion_submaps.back()'.

    // 如果insertion_submaps.back()是第一次看到, 也就是新生成的
    // 在data_.submap_data中加入一个空的InternalSubmapData
    const SubmapId submap_id =
        data_.submap_data.Append(trajectory_id, InternalSubmapData());
    
    // 保存后边的地图, 将后边的地图的指针赋值过去
    // 地图是刚生成的, 但是地图会在前端部分通过插入点云数据进行更新, 这里只保存指针
    // tag: 画图说明一下
    data_.submap_data.at(submap_id).submap = insertion_submaps.back();
    LOG(INFO) << "Inserted submap " << submap_id << ".";
    kActiveSubmapsMetric->Increment();
  }

首先要明确的知道一点，该段代码的目的是把子图添加至 PoseGraph2D::data_::submap_data 之中。那么第一个问题就是，什么时候才需要进行子图插入呢？

第一点就是不能重复插入，所以源码中是这样做的，对于如下两种情况会进行插入：①如果子图对应的轨迹中，还没有任何节点数据，那么当然就还没有插入过子图，此时需要插入；② data_.submap_data 中默认一条轨迹的最后一个子图为 insertion_submaps 的第二个子图，在前面的子图章节分析过，insertion_submaps 的最大容量为两个子图，如果 data_.submap_data 对最后一个子图不是 insertion_submaps.back()(最后或者第二个子图)，则说明之前的 insertion_submaps.back() 子图已经变成第一个子图，也就是增加了一个新的子图到 insertion_submaps 中，此时需要把这个新的子图加入到 data_.submap_data 之中。

符合上述的两种情况，说明都需要把 insertion_submaps.back() 加入到 data_.submap_data 之中。从上面的代码可以看出，先调用 data_.submap_data.Append() 函数，构建一组 (SubmapId, InternalSubmapData) 元素，添加到 data_.submap_data 之中，SubmapId 是根据 trajectory_id 构建的。

不过需要注意一点的是， (SubmapId, InternalSubmapData) 元素中的 InternalSubmapData 是调用默认构造函创建的，也就是说其内部还没有真正的存储或者指向子图，所以最后还执行了如下代码：

    data_.submap_data.at(submap_id).submap = insertion_submaps.back();

该代码执行之后，才算把 insertion_submaps 的最后(第二个)子图添加到 data_.submap_data 之中。

$\color{red} 注意$ : insertion_submaps 是一个共享指针，说明加入到 data_.submap_data 中的子图，仅仅是添加了一个指针而已。所以子图在前端或者其他的地方有更新的时候，通过 data_.submap_data 对应的子图也会跟着更新，因为本质上是同一份子图。

六、PoseGraph2D::AddNode()

通过前面对 PoseGraph2D::AppendNode() 的分析，可知该函数主要目的是根据扫描匹配的相关数据构建一个轨迹节点然后添加到对应的轨迹之中，同时还会把每个子图的共享指针记录下来。现在回到主调函数 PoseGraph2D::AddNode()，接着剩下的代码其实就不多了，如下：

  // We have to check this here, because it might have changed by the time we
  // execute the lambda.
  // 获取第一个submap是否是完成状态
  const bool newly_finished_submap =  
      insertion_submaps.front()->insertion_finished();

  // 把计算约束的工作放入workitem中等待执行
  AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    
    
    return ComputeConstraintsForNode(node_id, insertion_submaps,
                                     newly_finished_submap);
  });

  return node_id;

获得前端中第一个子图的状态(完成或者未完成)，随后添加一个任务到到线程池之中。是的，接下来就涉及到线程池了。这里先简单了解一下。其上使用了 lambda 表达式，其主体函数十分简单，就是调用了 ComputeConstraintsForNode() 函数。该函数也放在后面进行分析。

需要注意的一点的是，lambda 表达式的捕获列表是以 = 进行捕获的，也就是说都是赋值操作，即添加任务的时候，node_id、insertion_submaps、newly_finished_submap 三个参数会被固定下来，后续线程执行该任务时是也是同样的值。特殊的是 insertion_submaps ，其存储的是指针地址，不会发生改变，但是其指向的子图可能是动态改变的。

七、结语

通过一系列的分析，终于完成了对 PoseGraph2D::AddNode() 的讲解，总的来说：其就是根据扫描匹配的相关数据，以及全局位姿构建轨迹节点且添加到对应的轨迹之中，同时，对于每个子图都会记录下来。这些数据都保存在成员变量 PoseGraph2D::data_ 之中，其类型为 PoseGraphData。

由于之前的疑问依旧没有解决，所以这里还要再复制一下，免得后面忘记了

$\color{red}疑问1:$ global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。