Cartographer后端之PoseGraph2D二

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

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

一、参数部分

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

对象类型	对象名称	说明
proto::PoseGraphOptions	options_	配置信息
GlobalSlamOptimizationCallback	global_slam_optimization_callback_	回调，传出优化结果
mutable absl::Mutex	mutex_	锁
std::unique_ptr<WorkQueue>	work_queue_	// 相当于一个缓存buffer，将要处理的数据缓存
absl::flat_hash_map<int, std::unique_ptr<common::FixedRatioSampler>>	global_localization_samplers_	全局采样器
int	num_nodes_since_last_loop_closure_	自从上次闭环处理后增加的node的个数
std::unique_ptr<optimization::OptimizationProblem2D>	optimization_problem_	优化器
constraints::ConstraintBuilder2D	constraint_builder_	约束构造器
common::ThreadPool* const	thread_pool_	线程池，可处理work queue
std::vector<std::unique_ptr<PoseGraphTrimmer>>	trimmers_	子图删除部分
PoseGraphData	data_	PoseGraphData全局数据 这个重要后面再拆开说
ValueConversionTables	conversion_tables_	value变换查询表

主要去说PoseGraphData 这个数据结构

对象类型	对象名称	说明
MapById<SubmapId, InternalSubmapData>	submap_data	submap 对应的submapID集合
MapById<SubmapId, optimization::SubmapSpec2D>	global_submap_poses_2d	submap的全局位置
MapById<SubmapId, optimization::SubmapSpec3D>	global_submap_poses_3d	ubmap的全局位置
MapById<NodeId, TrajectoryNode>	trajectory_nodes	NodeID 与trajectory对应集合
std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>	landmark_nodes	记录路标点的容器。
TrajectoryConnectivityState	trajectory_connectivity_state	轨迹之间的状态
int	num_trajectory_nodes	轨迹中的node的个数
std::map<int, InternalTrajectoryState>	trajectories_state	轨迹状态集合
std::map<int, PoseGraph::InitialTrajectoryPose>	initial_trajectory_poses	每个trajectory的初始位置
std::vector<PoseGraphInterface::Constraint>	constraints	约束队列

InternalSubmapData这个数据结构

对象类型	对象名称	说明
std::shared_ptr<const Submap>	submap	子图数据
SubmapState	state	子图状态
std::set<NodeId>	node_ids	node集合

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

TrajectoryNode这个数据结构

对象类型	对象名称	说明
std::shared_ptr<const Data>	constant_data
transform::Rigid3d	global_pose	节点在世界坐标系下的位姿

Data数据结构

对象类型	对象名称	说明
common::Time	time	字段time记录了扫描数据被插入子图的时刻，在TrajectoryNode中可以通过函数time获取
Eigen::Quaterniond	gravity_alignment	字段gravity_alignment是当时的重力方向
sensor::PointCloud	filtered_gravity_aligned_point_cloud	根据重力方向修正之后的点云数据
transform::Rigid3d	local_pose	节点在子图中的相对位姿

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

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;