Cartographer后端之PoseGraph2D一

这部分比较多，后面会分两块单独来说明，我们先简单的说明算法原理和各个类中变量的意义，再在之后的文档中解释各个方法的意义。这块很多都抄了这里无处不在的小土（因为他写的太好了）。

对象map_builder在它的构造函数中就用类型PoseGraph2D例化了对象pose_graph， 它是后端完成闭环检测以及全局优化的关键对象。在研究该对象之前， 我们还是先回到Cartographer的总体框架上来，总体了解一下Global SLAM的任务。 下图是从总体框图中抠下来的关于Global SLAM的框图。原图中的输入传感器数据还有激光雷达的扫描数据，但是因为Global SLAM没有直接使用所以被拿掉了，激光数据从local_slam_result_callback_回调进后端。

 

上图中间的虚线框就是Global SLAM的主要工作内容，大体上可以分为三个部分。其一接收来自Local SLAM的子图更新结果，在位姿图中相应的增加节点和约束， 这点我们已经在连接前端与后端的桥梁一文中有了初步的了解。 其二结合里程计、IMU、全局位姿测量数据(Fixed Frame Pose, 类似GPS这样具有全局测量能力的传感器)的测量信息， 使用SPA(Sparse Pose Adjustment)技术求解后端的优化问题。 其三，在每次完成后端优化之后，都需要根据优化的结果对后来添加到位姿图中的节点和子图的位姿做修正。

本文中，我们先研究一下论文（这里建议把cartographer的几篇论文都看一下，有助于后面理解各个优化为什么这么写），着重看一下后端所面临的优化问题是什么， 以及这个位姿图到底是个啥。再来看看PoseGraph2D的成员变量，大体了解一下它都要处理什么样的数据。

1.后端优化问题

我们知道Cartographer中全局地图是由很多个子图拼接而成的，那么每一个子图在世界坐标系下都有一个位姿用εm来表示，上角标m表示map。 假设我们一共有m个子图，它们的位姿可以用集合Ξm={εmi}i=1,⋯,m来记录，下角标i表示第i个子图。

前端每完成一次子图更新，一定会把一次激光扫描数据插入其维护的子图当中。这个插入结果将被Cartographer看做是构成一条轨迹的节点，并以此时的机器人位姿作为节点的位姿， 将其看做是一次激光扫描的参考位姿，在论文的上下文中称为扫描位姿(Scan Pose)，用符号εs表示，上角标s表示scan。假设我们一共有n个节点， 它们的位姿可以用集合Ξs={εsj}j=1,⋯,n来记录，下角标j表示第j个节点。

后端的优化问题就可以描述成一个非线性的最小二乘问题，在后台线程中每隔几秒钟使用Ceres库求解一次。 如果我们用εij表示第i个子图与第j个节点之间的相对位姿，并用协方差矩阵Σij来描述εij的可信度，那么后端优化问题可以形式化的用下式表述:

argminΞm,Ξs12∑ijρ(E2(εmi,εsj;Σij,εij))(1)

E2(εmi,εsj;Σij,εij)=e(εmi,εsj;εij)TΣ−1ije(εmi,εsj;εij)(2)

e(εmi,εsj;εij)=εij−⎡⎣R−1εmi(tεmi−tεsj)εmi;θ−εsj;θ⎤⎦(3)

上式(3)描述了全局位姿估计的偏差。其中temi和tesj分别是子图位姿和节点位姿的位置矢量，两者求差得到世界坐标系下节点相对于子图的位置矢量。 矩阵Rεmi是由子图位姿写出的旋转矩阵，对其求逆即可得到世界坐标系下的矢量在子图的局部坐标系下的转换关系。 所以R−1εmi(tεmi−tεsj)实际就是将世界坐标下子图和节点位姿投影到子图的局部坐标系下，并计算两者的相对偏差量。 而emi;θ和esj;θ分别是子图位姿和节点位姿中的方向角，两者求差即是子图局部坐标系下的相对方向角。 将根据世界坐标系下的位姿估计计算出的相对变换关系与前端输出的相对变换关系求差，就可以计算全局估计的偏差量。

从式(3)中偏差量的估算我们可以看到，Cartographer认为前端的定位结果在一段很短的时间内还是比较可信的，但是它的误差会随着时间累积以至于全局的估计不准确。 所以可以通过上面的方法计算全局估计的偏差量。 上式(2)中计算的E2(εmi,εsj;Σij,εij)实际上是一个关于全局位姿估计偏差的二次型。我们可以简单的理解为求平方， 但实际上它还考虑了前端定位结果的不确定性Σij。

式(2)中的二次型将被用作评价每个子图-节点对的位姿估计代价函数，将其代入式(1)中就得到了后端优化的目标函数。式中的核函数ρ(⋅)用于过滤因为错误的匹配而产生的一些异常约束， 降低它们对于最终优化结果的影响，原文中提到的一个Huber loss的核函数。

2.位姿图与约束

通过对上述后端优化问题的定义，我们可以看到Cartographer中的位姿图实际上是一种二部图。如下图所示有两类顶点，三角表示子图，圆圈表示节点。 由于传感器的一次扫描数据可能插入多个不同的子图，所以同一个节点可能与多个子图之间存在一定的匹配关系，这个匹配关系就是所谓的约束， 可以理解为前文中的Σij和εij。

   struct Constraint {
            struct Pose {
                transform::Rigid3d zbar_ij;    
                double translation_weight;
                double rotation_weight;
            };
            SubmapId submap_id;
            NodeId node_id;
            Pose pose;
            enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
        }

其实节点之间也不是孤立的，它们都是在上一个节点的基础上累加一些运动得到的。这些运动的测量就体现在了里程计以及IMU的积分定位、前端的扫描匹配等方面。 各个节点前后串连起来就得到了一条轨迹，我想这也是为什么Cartographer中定义了各类TrajectoryBuilder的一个出发点吧。

Cartographer在接口类PoseGraphInterface内部定义了一个结构体Constraint来描述节点j相对于子图i的约束。 其定义如下面的代码片段所示，在该结构体的内部还定义了一个Pose的结构体，描述了节点j相对于子图i的相对位置关系。其中，字段zbar_ij描述的是在子图的εij， 而字段translation_weight和rotation_weight分别是平移和旋转的权重，应该对应着刚刚的Σij，用于描述不确定度。

结构体Constraint中用字段submap_id来记录约束对应的子图索引，node_id记录节点索引。它们的数据类型SubmapId和NodeId都是定义在文件id.h中的结构体。它们有一个相同的字段trajectory_id，用于标记当前跟踪的轨迹。各自有一个从0开始计数的submap_index和node_index，分别为每个子图和节点提供一个唯一的编号。

在Cartographer中有两类约束，被称为子图内约束(INTRA_SUBMAP)和子图间(INTER_SUBMAP)约束。INTRA_SUBMAP的约束是指在子图的更新过程中节点j被直接插入到子图i中。 而INTER_SUBMAP类型的约束中节点j并不是直接插入到子图i中，我猜应该是因为闭环检测而添加的约束。