闭合优化问题也是一个非线性最小二乘问题

$$arg\ \mathop{min}\limits_{\Xi^m \ \Xi^s} \frac{1}{2}\sum \limits_{ij} \rho(E^2(\xi_i^2, \xi_j^2; \Sigma_{ij}, \xi_{ij})\ ) \tag1$$

全局地图是由很多个子图拼接而成的，那么每一个子图在世界坐标系下都有一个位姿，它们的位姿可以用下面的集合表示

$$\Xi^m = \left\{\xi_i^m\right\}_{i=1,2...m}$$

前端每完成一次子图更新，会把一帧激光扫描数据插入其维护的子图当中。这个插入结果将被Cartographer看做是构成一条轨迹的节点，并以此时的机器人位姿作为节点的位姿，将其看做是一次激光扫描的参考位姿，所有位姿的集合如下表示

$$\Xi^s = \left\{\xi_j^s\right\}_{j=1,2...n}$$

这些被优化的submap位姿和Scan位姿给出了一些constraint（约束）。constraint的表现形式就是位姿 $\xi_{ij}$ 和协方差矩阵 $\Sigma_{ij}$。 位姿 $\xi_{ij}$ 代表 j 帧Scan在子图 i 下的位姿，描述scan和哪个submap匹配

(1)式中的残差E计算公式是

损失函数ρ（例如Huber损失），可以用于减少异常值的影响，而 异常值可能会出现在局部对称的环境（包含隔间的办公室）中

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_);
  // 判断对轨迹进行的操作，包括增加，删除或者轨迹之间的关系操作
  // 仍然假设仅有一个轨迹
  AddTrajectoryIfNeeded(trajectory_id);
  // 此 trajectory id 的轨迹是否存在或更改，只是判断
  if (!CanAddWorkItemModifying(trajectory_id))
  {
      LOG(WARNING) << "AddNode was called for finished or deleted trajectory";
  }

这些对于一条轨迹的情况都不重要，先不深入分析

// 添加scan的node_id，返回 trajectory id 和对应的 scan idex
const NodeId node_id = data_.trajectory_nodes.Append(
    trajectory_id, TrajectoryNode{constant_data, optimized_pose} );
// 记录轨迹节点个数  +1
++data_.num_trajectory_nodes;

data_ 是 PoseGraphData data_ GUARDED_BY(mutex_);

trajectory_nodes的类型是MapById<NodeId, TrajectoryNode>，对于Append函数，不必关心细节。里面的TrajectoryNode类型是

struct TrajectoryNode
{
  // 记录了前端传来的 点云、重力方向、局部位姿等数据
  struct Data {
    // 扫描数据被插入子图的时刻
    common::Time time;
    // Transform to approximately gravity align the tracking frame as
    // determined by local SLAM.
    Eigen::Quaterniond gravity_alignment;
    // Used for loop closure in 2D: voxel filtered returns in the
    // 'gravity_alignment' frame.
    sensor::PointCloud filtered_gravity_aligned_point_cloud;
    // 省略用于3D建图时闭环检测的字段

    // The node pose in the local SLAM frame.
    transform::Rigid3d  local_pose;
  };
  common::Time time() const { return constant_data->time; }
  // 实际只有这两个成员
  std::shared_ptr<const Data>  constant_data;
  // The node pose in the global SLAM frame.
  transform::Rigid3d  global_pose;

最重要的是global_pose，节点在世界坐标系下的位姿，论文里的 $\xi_j^s$
返回去查，发现是GetLocalToGlobalTransform返回的

// Test if the 'insertion_submap.back()' is one we never saw before.
// 前端最新的子图与当前 data_ 最后一个子图不一致时，才会增加
if ( data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
      std::prev(data_.submap_data.EndOfTrajectory(trajectory_id)  )
              ->data.submap != insertion_submaps.back()  )
{
    // 在全局数据data_中添加submap信息，添加时只考虑新增加的submap
    // InternalSubmapData() 在这里的意思是无参的构造函数，什么都未处理
    const SubmapId   submap_id = 
          data_.submap_data.Append(trajectory_id, InternalSubmapData()  );
    // 闭环中submap节点，采用最新的子图
    // submap_data 是 MapById<SubmapId, InternalSubmapData>
    // 成员submap是个智能指针
    data_.submap_data.at(submap_id).submap = insertion_submaps.back();
    LOG(INFO) << "Inserted submap " << submap_id << ".";
    kActiveSubmapsMetric->Increment();
}
    return node_id;
}

增加了该节点在 global map坐标系的全局位姿，也是后期需要优化的位姿。把node加入到trajectory_nodes列表。 最后返回的位姿图ID为data_存储的轨迹节点ID。

前端最新的子图与当前 data_ 最后一个子图不一致时，给该子图分配id并将其加入其中(其实就是把前端最新子图加入到后端)。注意，这时候的子图 还没有计算global pose，也就是 $\xi_i^m$。 所以，后面要初始化submap的global pose，也就是InitializeGlobalSubmapPoses