Cartographer源码阅读3D-后端PoseGraph3D

和PoseGraph2D的基本流程差不多，主要的差别是：
（1）Constraint的形式不一样。

（2）分支定界采用了直方图匹配（基于点云切片的思想），且分支定界的复杂度高于2D；

（3）后端SPA优化的残差不一样。
3D的Constraint：

constraint = submap.global_pose * node.local_pose(包含重力对齐)

而2D的Constraint：

constraint = submap.global_pose_2d * node.local_pose_2d(去掉重力对齐)

具体体现在：

void PoseGraph3D::ComputeConstraintsForNode(
    const NodeId& node_id,
    std::vector<std::shared_ptr<const Submap3D>> insertion_submaps,
    const bool newly_finished_submap) {
  // 获取Node数据
  const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
  // 将submap数据加入后端优化，获取Node所在的submap(1或2个)
  const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses(
      node_id.trajectory_id, constant_data->time, insertion_submaps);
  CHECK_EQ(submap_ids.size(), insertion_submaps.size());
  const SubmapId matching_id = submap_ids.front();
  // node的local pose和2D不一样，没有去掉重力
  const transform::Rigid3d& local_pose = constant_data->local_pose;
  // node的global pose和2D不一样，没有去掉重力
  const transform::Rigid3d global_pose =
      optimization_problem_->submap_data().at(matching_id).global_pose *
      insertion_submaps.front()->local_pose().inverse() * local_pose;
  // 后端优化加入node
  optimization_problem_->AddTrajectoryNode(
      matching_id.trajectory_id,
      optimization::NodeSpec3D{constant_data->time, local_pose, global_pose});
  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 'submap_data_' further below.
    CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
    // 将node的id放入submap中
    submap_data_.at(submap_id).node_ids.emplace(node_id);
    // sequence边的约束
    const transform::Rigid3d constraint_transform =
        insertion_submaps[i]->local_pose().inverse() * local_pose;
    // 添加sequence约束
    constraints_.push_back(
        Constraint{submap_id,
                   node_id,
                   {constraint_transform, options_.matcher_translation_weight(),
                    options_.matcher_rotation_weight()},
                   Constraint::INTRA_SUBMAP});
  }
  // node和所有的submap计算回环
  for (const auto& submap_id_data : submap_data_) {
    if (submap_id_data.data.state == SubmapState::kFinished) {
      CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
      ComputeConstraint(node_id, submap_id_data.id);
    }
  }
  // 新结束的submap和所有的old node计算回环
  if (newly_finished_submap) {
    const SubmapId finished_submap_id = submap_ids.front();
    InternalSubmapData& finished_submap_data =
        submap_data_.at(finished_submap_id);
    CHECK(finished_submap_data.state == SubmapState::kActive);
    finished_submap_data.state = SubmapState::kFinished;
    // We have a new completed submap, so we look into adding constraints for
    // old nodes.
    ComputeConstraintsForOldNodes(finished_submap_id);
  }
  constraint_builder_.NotifyEndOfNode();
  ++num_nodes_since_last_loop_closure_;
  CHECK(!run_loop_closure_);
  if (options_.optimize_every_n_nodes() > 0 &&
      num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
    // 加入线程池，执行计算
    DispatchOptimization();
  }
}

其余的和2D的基本相同，不再详细展开。