这部分主要开始讲代码了，以下接口可根据需要自行修改，由于公司原因，只讲官方源码，

自己做的地方等哪天想不起来自己看把，写的这些基本都是怕之后很久不做忘了。

一、参数部分

类型


PoseGraph2D


是从类


PoseGraph


派生而来的，而PoseGraph又继承了接口类


PoseGraphInterface


。类PoseGraph和类PoseGraphInterface都是抽象类，定义了很多纯虚函数，并没有定义任何成员变量。而PoseGraph2D则是用于2D建图的实现，下表列举了它的成员变量。 我们所关心的位姿图应该是由三个容器构成的，submap_data_中记录了所有的子图对象，trajectory_nodes_中记录了所有的节点对象，它们通过容器constraints_中记录的约束建立起联系。

主要去说PoseGraphData


这个数据结构

InternalSubmapData





这个数据结构

这个结构体记录里子图的数据及其内部的节点信息，子图的状态主要是给后台的线程提供的。一开始子图的状态都是kActive的，当它切换到kFinished的状态下后就会与所有的节点进行一次扫描匹配操作。此外新增的节点也会与所有kFinished状态的子图进行扫描匹配。 这一操作我们可以理解为是进行闭环检测，通过遍历与所有的kFinished状态的子图，或者节点，应当可以找到发生闭环的地点并建立一个约束来描述。

TrajectoryNode





这个数据结构

Data数据结构

二、接口部分（这里都是官方的,自己的来得及后面重新弄个文档说）

1.






位姿图增加节点顶层接口AddNode

在global_trajectory_builder即顶层调用接口可以看到，前端和后端的主要联系则是经过pose_graph_->AddNode接口调用。每次经过一次前端有效匹配后，将前端匹配的结果传递于后端接口。

回忆下global_trajectory_builder中的调用

 // 将匹配后的结果加入图优化节点中
      auto node_id = pose_graph_->AddNode(
          matching_result->insertion_result->constant_data, trajectory_id_,
          matching_result->insertion_result->insertion_submaps);

可以看出后端增加一个位姿图节点需传递匹配后的结果包括submap和点云数据




（点云在


constant_data里


）




，最后返回一个位姿图节点ID。其中前端处理后的点云数据，包括水平投影的点云，重力方向和在local submap的位置。

函数接口为：

NodeId PoseGraph2D::AddNode(
    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(
      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
 添加一个节点,这里是具体添加节点的实现
  const NodeId node_id = AppendNode(constant_data, trajectory_id,
                                    insertion_submaps, optimized_pose);
  // We have to check this here, because it might have changed by the time we
  // execute the lambda.
// 查看维护的两个submap的front是否插入完成（两个submap，front用于匹配和更新的） // 如果一个submap finished，则需要进行闭环检测

  const bool newly_finished_submap =
      insertion_submaps.front()->insertion_finished();
// 最后通过lambda表达式和函数AddWorkItem注册一个为新增节点添加约束的任务ComputeConstraintsForNode。根据Cartographer的思想，
// 在该任务下应当会将新增的节点与所有已经处于kFinished状态的子图进行一次匹配建立可能存在的闭环约束。
// 此外，当有新的子图进入kFinished状态时，还会将之与所有的节点进行一次匹配。所以这里会通过insertion_submaps.front()来查询旧图的更新状态。

  AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    return ComputeConstraintsForNode(node_id, insertion_submaps,
                                     newly_finished_submap);
  });
  return node_id;
}

2.新增节点具体实现






AppendNode

//与顶层接口基本一致，增加了该节点在整个全局地图的全局绝对位置，也是后期需要优化的位姿。
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) {
  absl::MutexLock locker(&mutex_);
// 判断是否需要添加trajectory id
  AddTrajectoryIfNeeded(trajectory_id);
// 此 trajectory id的轨迹是否存在或更改，暂时只是判断无用
//由于多线程操作，为安全增加线程锁保护。其中AddTrajectoryIfNeeded是判断对轨迹进行的操作，包括增加，删除或者轨迹之间的关系操作，没有进一步研究，仍然假设仅有一个轨迹。
  if (!CanAddWorkItemModifying(trajectory_id)) {
    LOG(WARNING) << "AddNode was called for finished or deleted trajectory.";
  }
  const NodeId node_id = data_.trajectory_nodes.Append(
      trajectory_id, TrajectoryNode{constant_data, optimized_pose});
  ++data_.num_trajectory_nodes;
  // Test if the 'insertion_submap.back()' is one we never saw before.
//将全局数据data_中增加一个位姿图轨迹节点即点云数据帧，同时也保存对应的轨迹ID信息，同时记录其节点个数。
//在全局数据data_中添加submap信息，但是由于submap是不停地更新，但是不一定更换，因此添加时只考虑新增加的submap。在submap维护一节中分析过，前端输出的submap除初始时刻时有一个submap，而其他情况下都会存在两个submap。而insertion_submaps.back()则为前端结果中最新的submap，当更换时即与当前data_最后一个不一致时才会增加（简单来说一个submap仅增加一次）。
  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()'.
    const SubmapId submap_id =
        data_.submap_data.Append(trajectory_id, InternalSubmapData());
    data_.submap_data.at(submap_id).submap = insertion_submaps.back();
    LOG(INFO) << "Inserted submap " << submap_id << ".";
    kActiveSubmapsMetric->Increment();
  }
  return node_id;
}

3.PoseGraph中添加约束接口






ComputeConstraintsForNode

我们再来看一下函数ComputeConstraintsForNode，它除了有添加约束的作用，还会触发工作队列的构建和运行。下面是该函数的代码片段，它有三个参数。其中node_id是待更新的节点索引， insertion_submaps则是从Local SLAM一路传递过来的


新旧子图


，newly_finished_submap表示旧图是否结束更新了。

这段稍微长一点，拆成几段

void PoseGraph2D::ComputeConstraintsForNode(const NodeId& node_id,

std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,

const bool newly_finished_submap) {

在函数的一开始先获取节点数据，并通过函数



InitializeGlobalSubmapPoses



获取新旧子图的索引。 实际上这个函数还是蛮长的，它除了获取ID之外还检查了新子图是否第一次被后端看见，若是则为之计算全局位姿并喂给后端优化器optimization_problem_。 我们将在


后端优化过程


一文中在回头来看这个函数。

const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;

const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses(node_id.trajectory_id, constant_data->time, insertion_submaps);

接下来以旧图为参考，计算节点相对于子图的局部位姿




ε






ij




，以及它在世界坐标系下的位姿




ε






sj




。并将之提供给后端优化器optimization_problem_。

const SubmapId matching_id = submap_ids.front();

const transform::Rigid2d local_pose_2d = transform::Project2D(constant_data->local_pose * transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));

const transform::Rigid2d global_pose_2d = optimization_problem_->submap_data().at(matching_id).global_pose * constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *local_pose_2d;

optimization_problem_->AddTrajectoryNode(matching_id.trajectory_id,

optimization::NodeSpec2D{constant_data->time, local_pose_2d,

global_pose_2d, constant_data->gravity_alignment});

然后为新增的节点和新旧子图之间添加INTRA_SUBMAP类型的约束。

for (size_t i = 0; i < insertion_submaps.size(); ++i) {

const SubmapId submap_id = submap_ids[i];

CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);

submap_data_.at(submap_id).node_ids.emplace(node_id);

const transform::Rigid2d constraint_transform =  constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() * local_pose_2d;

constraints_.push_back(Constraint{submap_id,

node_id,

{transform::Embed3D(constraint_transform),

options_.matcher_translation_weight(),

options_.matcher_rotation_weight()},

Constraint::INTRA_SUBMAP});

}

紧接着遍历所有已经处于kFinished状态的子图，建立它们与新增节点之间可能的约束。

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);

}

}

如果旧图切换到kFinished状态，则遍历所有已经进行过优化的节点，建立它们与旧图之间可能的约束。

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) {

ComputeConstraint(node_id, newly_finished_submap_id);

}

}

}

对所有的已经完成的子图建立与当前node之间的约束

for (const auto& submap_id : finished_submap_ids) {

ComputeConstraint(node_id, submap_id);

}

如果是新的子图且子图还未和节点建立约束，则将优化器中的node与子图建立约束

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) {

ComputeConstraint(node_id, newly_finished_submap_id);

}

}

}

最后通知约束构建器新增节点的操作结束，增加计数器num_nodes_since_last_loop_closure_。这里的return


WorkItem


对于工作队列的运转有着重要的作用。

constraint_builder_.NotifyEndOfNode();

absl::MutexLock locker(&mutex_);

++num_nodes_since_last_loop_closure_;

if (options_.optimize_every_n_nodes() > 0 &&

num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes())

{

return WorkItem::Result::kRunOptimization;

}

return WorkItem::Result::kDoNotRunOptimization;