位姿图优化小记2021.10.18

1.场景描述

现在有一个小车在运动，并搭载相机或激光雷达进行建图工作，由于SLAM的作用，在建图的同时小车也可以进行自身的定位，因此建立的地图的参考都是相对于自身坐标系的，也就是每一个时刻看到的地图点都是相对于小车当前的位姿的。注意建图的真实性反应在它要是全局一致（不知道这里用这个词对不对）的地图，或者简单来说建立的这个地图要对比GPS上的地图是一样的。但实际上由于建立的地图是相对于自身坐标系的，所以如果某个时刻的小车位姿不准确，此时虽然建立的相对于小车坐标系的位姿是准确的，但是看整个地图来说，其相比GPS定位的真实的地图就是不准确的了，原因就在于参考的小车自身坐标系就是不准的。

为了小车自身位姿的不准确性这个问题，有两种办法可以解决。

• 方式1：一个是SLAM中的回环检测，假设小车到达了之前到过的同一个地方，但是此时通过视觉里程计进行SLAM得到的小车的位姿和起始点的位姿并不相同，这就是之前每次SLAM过程都有误差，最后误差累积导致的。假设此时检测到回环的位姿顶点序号为N，初始的位姿为0。那么再进行后端优化的时候，就向把当前位姿L和初始位姿0对齐。
• 方式2：一个是外加其他可以提供绝对参考的传感器，比如GPS。那么此时的场景就变成从初始位姿开始，小车得到一个与在GPS下定位的位姿（应该是位置，姿态是得不到的）。那么此时小车开始进行SLAM过程，假设此时进入了建筑群中GPS信号丢失，那么后面的整个过程就只能靠相机或激光雷达进行纯SLAM过程。假设过了一段时间之后GPS信号恢复，同样的道理，由于累计误差此时通过SLAM定位的小车位置和GPS反映的真实位置是不一样的。此时就想要把当前的位置N对齐到GPS定位的位置上。

2.位姿图优化过程（以GPS辅助定位为例）

2.1.思路概述

如上图所示，就描述了GPS辅助定位的场景。此时需要对SLAM过程中得到的小车的各个位姿进行优化，从而得到全局一致的地图。注意建立的地图点一般不优化也可以，因为每一个位姿中建立的地图点相对来说都是比较准确的，即使优化它的变化也不会太大。因此建立的地图和实际的全局地图不一样，主要原因还是在于小车的位姿不准确，也就是参考坐标系不准确，最后建立的全局地图自然也是不准确的。

那么此时要进行优化，优化的目标就是调整SLAM过程中小车的各个位姿，从而让使用SLAM得到的小车起始点和最终点的位姿和GPS的数据是一样的，也就是位姿图优化。注意位姿图优化的各个顶点是小车的位姿，边就是相邻两个位姿之间的观测关系，位姿图优化就是要调整各个顶点的位姿，让他们尽量能够满足各个边的观测关系。

这个优化的本质思想就是固定住头和尾，让头尾的位姿等于GPS的真实位姿，然后把造成SLAM的位姿和这两个位姿不一样的误差加权分批到各个位姿定点上，调整这些位姿，从而让头尾的位姿和GPS的位姿相等。

2.2.实际操作

2.2.1.固定起始点

在rviz中，把起始点固定住，因为起始点的位姿是有GPS信息的，认为它是绝对准确的。

2.2.2.在最后一个位姿顶点新此时的GPS位姿顶点

在最后一个顶点的地方，给他新添加一个顶点，这个顶点的位姿就是GPS得到的位姿。也就是优化的目的就是想让最后这个顶点的位姿变成GPS定位给的位姿。

另外非常重要的是，需要固定这个顶点。因为最后优化需要有一个目标，就是让某个位姿顶点变成什么样子。所以起始和结束的位姿顶点都需要进行固定。

2.2.3.修改最后一个顶点和GPS顶点之间的边

注意这个非常重要。边表达的是相邻两个顶位姿之间的关系，这里是想要最后一个定点和GPS的顶点位姿相等，所以就相当于假想了一个观测，比如是里程计，观测到这两个顶点之间的位姿变换关系应该是0，因为最后想要的就是这两个位姿顶点是同一个位姿顶点，也就是他们之间并没有坐标变换关系。

对于这个边，另外一个很重要的是修改它的权重，也就是信息矩阵。信息矩阵相当于表明了这个边的观测有多可信，因为它是协方差的倒数，协方差越大说明数据越不可信，那么此时信息矩阵也就越小。反之，信息矩阵越大，说明协方差越小，也就是这个观测越可信。所以假设SLAM建立的其他边的协方差都是1，那么这里设置新加的这个GPS顶点和最后一个顶点之间的边的协方差是1000，也就是非常相信这个边的观测，相对于其他边的观测来说，这个边的观测非常准确。

从另一个角度来理解协方差矩阵，就是优化中定义的残差。残差的定义是

其中e是使用当前的两个位姿顶点算出来的边和观测到的边有多大的不一样。中间的就是信息矩阵，也就是信息矩阵越大，那么这一项的误差所占的权重就越大，也就是让这个误差变小，会对让整个误差变小做出很大的贡献。而这个误差是什么呢？就是使用当前两个位姿顶点算出来二者的位姿关系，和边所观测的位姿关系之间的差距。而新加的GPS位姿顶点又是固定的，所以最后让误差减小的结果就是把最后一个位姿顶点调整到新加的GPS位姿顶点一样。

注意：边是没有固定不固定这一个说法的，因为优化的变量是位姿顶点，固定顶点就是在优化的过程中不改变这个顶点的位姿。

2.3.进行优化

在进行了上面的优化准备操作之后，进行优化即可。最后优化的结果，是调整各个顶点的位姿，从而让最后一个顶点的位姿尽量对齐到GPS位姿上。

3.优化完成后重新建图

由于是对位姿进行优化，所以优化完成之后，得到位姿图一定是变化的。因为之前的位姿图相对真实的GPS位姿图是有很大偏差的，优化后得到的是相对真实的GPS位姿图误差很小的。

那么由于前面局部见图都是相对于小车的每一个位姿图，尽管这个局部建图是准确的，但是由于小车的位姿不准确，所以最后的建图和GPS的全局地图相比有误差。现在小车的位姿图已经进行了优化矫正，现在需要把原先建立的地图在新的小车位姿图上进行重建，从而得到和GPS全局一致的地图。

所以最后的结果就是看起来位姿图并没有变化，并且相对于地图来说位姿图也没有变化。其实位姿图的绝对位姿（相对GPS的真实位姿）是变化了的，而相对地图不变是因为地图是相对于小车的每一个位姿建立的，这些都是准确的，最后优化的是小车的位姿图，并没有优化局部的建图，自然重新建图之后地图相对于小车的位姿图也是不变的。但是实际上此时全局的地图已经变化了，已经变成了和GPS全局地图相近的地图。

4.视频记录

注意这个优化的视频，注意最后优化完毕之后，地图动了一下，其实就是此时在优化后的位姿图上，重新进行了建图。