cartographer_learn19子图内的约束添加及回环检测前的准备

续接上篇

上一篇我们简单的介绍了后端用于处理优化问题的类OptimizationProblem2D，介绍了它是怎么存储数据的，存储的都是哪些数据。同时也学习了后端中轨迹的联系，及如何找出轨迹间的间接联系等（这里有点绕）。我们继续回到上一篇中的ComputeConstraintsForNode，继续看看cartographer是如何计算约束的。

子图内约束的添加

上代码：

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;
  { 
   ......//获取节点数据, 如果submap没有加入优化问题则加入，并返回insertion_submaps中submap的submap的id
   // 获取这两个submap中前一个的id
    const SubmapId matching_id = submap_ids.front();
    // lTn * wRn
    const transform::Rigid2d local_pose_2d =
        transform::Project2D(constant_data->local_pose *
                             transform::Rigid3d::Rotation(
                                 constant_data->gravity_alignment.inverse()));
    // 计算该节点在global坐标系下位姿
    const transform::Rigid2d global_pose_2d =
        optimization_problem_->submap_data().at(matching_id).global_pose *
        constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
        local_pose_2d;
    
    // 把该节点的信息加入到OptimizationProblem中
    optimization_problem_->AddTrajectoryNode(
        matching_id.trajectory_id,
        optimization::NodeSpec2D{constant_data->time, local_pose_2d,
                                 global_pose_2d,
                                 constant_data->gravity_alignment});

    // 遍历处理每一个insertion_submaps
    for (size_t i = 0; i < insertion_submaps.size(); ++i) {
      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);
      // 将node_id放到子图保存的node_ids的set中
      data_.submap_data.at(submap_id).node_ids.emplace(node_id);
      //节点到submap坐标系 间的坐标变换
      const transform::Rigid2d constraint_transform =
          constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
          local_pose_2d;
      // 新生成的约束放入容器中
      data_.constraints.push_back(
          Constraint{submap_id,
                     node_id,
                     {transform::Embed3D(constraint_transform),
                      options_.matcher_translation_weight(),
                      options_.matcher_rotation_weight()},
                     Constraint::INTRA_SUBMAP});
    }
    .......//一堆其他的操作
  }

先看代码中计算所谓的local_pose_2d，其实它实质上并不是lTn，而是节点在local坐标系中的位姿lTn乘以一个旋转，这个旋转是来自imu来自位姿推断断器的结果。至于为什么要这样做，作者的猜想是由于是二维建图，要考虑是在哪个平面上的建图。而cartographer选定的平面是垂直于重力的平面，位姿推断器其实就是在依据重力加速度的方向推断tracking坐标系的姿态gravity_alignment（见第11篇）。所以作者认为乘以一个gravity_alignment逆就是在把tracking坐标系（就是节点）做一个旋转，使其xoy平面与重力方向垂直。计算该节点在global坐标系下位姿的过程是wTs * sTl * lTn（T均表示旋转平移矩阵）。接下来是把节点加入优化器中——OptimizationProblem2D（见上一篇第二节）。接下来是把该节点加入到子图的存储中（见第17篇的子图的存储）。最后计算节点和子图的约束，即sTn，并把约束记录在后端中的data_中。

回环检测前的准备

继续看看ComputeConstraintsForNode这个函数剩余的操作：

WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
    const NodeId& node_id,
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
    const bool newly_finished_submap) {
       ......//获取节点数据, 如果submap没有加入优化问题则加入，并返回insertion_submaps中submap的submap的id
       ......//子图内节点的添加
       // 找到所有完成插入的submap的id
    for (const auto& submap_id_data : data_.submap_data) {
      if (submap_id_data.data.state == SubmapState::kFinished) {
        CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
        finished_submap_ids.emplace_back(submap_id_data.id);
      }
    }

    // 如果是刚刚finished的submap
    if (newly_finished_submap) {
      const SubmapId newly_finished_submap_id = submap_ids.front();
      InternalSubmapData& finished_submap_data =
          data_.submap_data.at(newly_finished_submap_id);
      CHECK(finished_submap_data.state == SubmapState::kNoConstraintSearch);
      // 把它设置成kFinished
      finished_submap_data.state = SubmapState::kFinished;
      newly_finished_submap_node_ids = finished_submap_data.node_ids;
    }
  }
	//将该节点和所有已经完成插入的子图做回环检测
  for (const auto& submap_id : finished_submap_ids) {
    ComputeConstraint(node_id, submap_id);
  }
	//将当前子图和过去所有节点做回环检测
  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);
      }
    }
  }
  ......//一堆其他操作
 }

我们看到做回环检测时ComputeConstraint这个函数实现的，我们去看看这个函数：

void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
                                    const SubmapId& submap_id) {
  bool maybe_add_local_constraint = false;
  bool maybe_add_global_constraint = false;
  const TrajectoryNode::Data* constant_data;
  const Submap2D* submap;

  {
    absl::MutexLock locker(&mutex_);
    CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
    //检查子图是不是完成插入了
    if (!data_.submap_data.at(submap_id).submap->insertion_finished()) {
      // Uplink server only receives grids when they are finished, so skip
      // constraint search before that.
      return;
    }
    // 该节点时间t1，与该子图有子图内约束的节点中最晚的时间t2，返回t1和t2中的较晚的时间
    const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
    // 子图和节点所属轨迹可能不一样，这里取最近一次两条轨迹产生联系的时间
    const common::Time last_connection_time =
        data_.trajectory_connectivity_state.LastConnectionTime(
            node_id.trajectory_id, submap_id.trajectory_id);

    // 如果节点和子图属于同一轨迹或上述两个时间相差不大，则进行局部窗口的搜索
    if (node_id.trajectory_id == submap_id.trajectory_id ||
        node_time <
            last_connection_time +
                common::FromSeconds(
                    options_.global_constraint_search_after_n_seconds())) {
      // If the node and the submap belong to the same trajectory or if there
      // has been a recent global constraint that ties that node's trajectory to
      // the submap's trajectory, it suffices to do a match constrained to a
      // local search window.
      maybe_add_local_constraint = true;
    }
    // 
    else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
      maybe_add_global_constraint = true;
    }
......//其他操作
}

可以看到当子图和节点同属同一轨迹时，或者是两者时间相差比较近时，cartographer采用的时所谓的局部窗口搜索，其他情况均采用的是全局窗口搜索。至于这些搜索时什么意思，相信随着学习的深入，我们会具体的讨论它们。本篇就到这了，今天是周日，作者稍微偷一下懒。