ekf pose使用方法 ros_上海交通大学邹丹平：基于环境结构性特征的视觉SLAM方法...
2018年7月，深蓝学院发起并承办的“第一届全国SLAM技术论坛”上，邀请了上海交通大学邹丹平老师作《基于环境结构性特征的视觉SLAM方法》的报告。现将邹老师的报告内容整理公布，希望更多SLAMer受益（文末有彩蛋）。
一、基于环境结构性特征的视觉SLAM方法
本文报告的内容是基于环境结构性特征的视觉SLAM方法，主要内容分成两个部分，第一个部分介绍基于卡尔曼滤波的视觉SLAM方法，包括经典的卡尔曼滤波SLAM方法，多状态约束卡尔曼滤波方法，并讨论这两种方法与关键帧优化方法之间的联系和差异。第二部分介绍相关的工作，第一个是StructSLAM，这是在2015年开发的视觉SLAM系统，第二个是StructVIO，是近期的工作，将StructSLAM和IMU融合起来形成一个紧耦合系统。
二．视觉SLAM方法分类
关于视觉SLAM，在近十年的发展发展非常迅速，而且涌现了很多非常优秀的方法和代表系统，包括MonoSLAM (2003)、PTAM (2007)、ORB-SLAM (2015)、DTAM (2012)、LSD-SLAM (2014)、SVO (2014)和DSO (2016)等等。
从数学的框架下，视觉SLAM可以分为两大类，一个是基于关键帧优化的方法；第二个是基于卡尔曼滤波的方法。这两种方法各有千秋，目前大家使用比较多是基于关键帧优化的方法，包括PTAM, OKVIS, VINS等等。
2.1 基于卡尔曼滤波的视觉SLAM方法
虽然目前使用较多的是关键帧优化的方法，但是基于卡尔曼滤波的视觉SLAM方法仍有许多的优点。由于这是非常成熟的一种框架，首先它实现起来比较简单；其次，在很多情况下，相对来说要稳定很多，具有很好的鲁棒性；第三点比较重要的是它的可扩展性强，自主的无人系统，像无人机、无人车上面有很多传感器，用卡尔曼滤波很方便融合其它的传感器，快速形成一个多源融合的系统。
基于卡尔曼滤波的视觉SLAM方法其实也有不少，包括第一个实时能跑的视觉SLAM系统MonoSLAM，还有在2015年提出的基于视觉惯导组合的视觉SLAM系统ROVIO，这两种是基于经典的卡尔曼滤波的方法做的。
其它还有像MSCKF是基于多状态约束的卡尔曼滤波，这是卡尔曼滤波变种，非常成功的一种方法。作者已开发的两个系统(StructSLAM和StructVIO)主要是基于经典的卡尔曼滤波以及多状态约束滤波这两个方法做的。
2.2 经典扩展卡尔曼滤波的视觉SLAM方法
首先我们回顾一下经典的扩展卡尔曼滤波的视觉SLAM方法。如下图所示，这是一个非常固定的流程。
首先我们是初始化，包括状态值的初始化、协方差矩阵的初始化。之后进入EKF滤波循环，这个循环是EKF (扩展卡尔曼滤波)都具备的过程，包括状态预测、数据关联、卡尔曼更新。然而，对视觉SLAM有一些特殊的步骤。由于特征不断地变化，相机从一个地方移动到另一个地方，所看到的东西也是发生变化的。这时需要不断在状态中初始化新的变量，然后将旧的特征和变量剔除掉，以保持状态的维度在可控的范围，这就是所谓的特征管理。这即是EKF的基本框架。
2.3 EKF视觉SLAM
如果要去设计这么一个系统，需要做什么事情呢？首先需要设计一个状态。比如经典的EKF视觉SLAM，其状态包括两个部分，第一个部分是相机自身的运行信息，比如它的位置、姿态，还有它的运动速度；第二个部分是场景的三维点。所以EKF的滤波方法就是把相机自身的拍摄位置与三维点放在一起做状态估算。
我们可以看到状态变量 x 可以分成两个部分，一个R，一个M， 其中R表示我们相机自身的信息，L表示我们场景三维点。相应的它的协方差矩阵可以分为两个部分，分别是R和L，意思就是相机和特征之间有相关的特殊矩阵，描述它们相关耦合的关系。
设计这么一个经典的EKF视觉SLAM系统，核心就是把观测模型定下来。对于视觉SLAM，这个观测模型其实就是一个透视投影的过程。
一个三维点透视到二维，这个过程可以用如下公式表示：
这个h可以认为是投影过程，这个过程可以进行线性展开。在L0和R0附近进行局部展开，这样可以得到一个线性的观测模型：
这个线性观测模型可以分成两个部分。第一个部分，是对于相机自身姿态的观测矩阵；第二个是特征变量的观测矩阵。对于每个点都可以得到这样一个观测矩阵，然后把所有的点综合起来得到一个很大的矩阵，这个矩阵用来做卡尔曼滤波的更新：
观测模型设计好之后，接下来，需要把状态量与观测点对应起来。在这个过程我们称之为数据关联的过程，就是找出观测量与特征点变量的一一对应关系。它类似于特征匹配，但是又有一些不一样。
因为这是通过状态与点之间去匹配，数据关联不可避免存在外点(outlier)，如何处理？一般我们通过卡方测试，还有其它的RANSAC方法也可以，这部会在后文的StructSLAM系统里详细介绍。
同时还有预测问题。当观测模型定义好之后，数据关联的方法也找好了，接下来需要再把预测模型设计好。一般来说，点的状态预测函数是这样的，它的上一帧跟下一帧点的坐标是保持静止的，不会发生变化，从而状态转移和噪声传递的定义就非常简单。
首先，转移矩阵对应是I，关键是对相机运动信息的预测，我们可以用匀速模型，IMU积分模型来处理。
定义好之后，接下来是特征管理。特征管理包括两个部分，首先是插入，插入是往状态中加一个变量；其次还需要对协方差进行相应的更新。相应的协方差更新方法需要从严格的数学推导推出来，如果这一步被忽略掉的话，那么EKF的系统就成功不了。
另外，设计EKF还有很多的细节，包括关键的步骤，点的表示。
当特征越来越多，状态变量越来越多的时候，怎么去剔除这些变量，如何去管理特征呢？这实质上就是一个边缘化操作。
经典的EKF-SLAM方法比较简单有效，它对于相机历史的位姿信息进行迅速边缘化处理。所谓的边缘化，就是把状态量直接剔除，把相对应的协方差矩阵的行和列去掉。迅速边缘化会造成相关的信息的丢失，它会导致对非线性影响的依赖比较严重，最终它的精度没有优化方法这么好。另外，当特征点比较多的时候计算量很大。这些都将对系统带来许多性能和精度上的问题。
接下来要讲的多状态约束滤波的方法就是为了去解决上述这些问题，其核心思想首先就是解耦。
2.4 多状态约束滤波方法
前面简单回顾了EKF，接下来回顾一下多状态约束滤波方法。
上面提到的问题该怎么解决呢？首先，保留历史的相机的位姿信息，进行延时边缘化，降低非线性因素导致的误差。其次，所谓的解耦就是特征点与相机位姿分开处理，卡尔曼滤波过程中只留下相机姿态估算，特征点估算放在滤波器外面单独处理。这就是多状况约束滤波的方法。
上面提到它的思想之一是保存历史的位姿信息。这涉及到状态克隆。
状态克隆具体的做法与EKF特征点的插入是类似的，不仅需要把当前的位姿信息插入，同时对历史的位姿进行保存，需要进行相应的更新。接下来，核心思想就是解耦，解耦是怎么做的呢？
我们在经典的EKF里面，是把特征都放在一起，我们的观测模型里Y值依赖于相机的位姿和特征点的坐标，这样就导致L肯定要放在状态里。多状态约束的思想想办设法把L如何消除掉。
刚才提到了一个视角的观测方程，那么，当有多个视角对于同样的一个特征点处理时，我们可以把多个时刻的观测方程写在一起，同时分成两个部分，最终可以想办法把L部分给消掉，这是多状态约束的核心思想。
比如说，我们得到二维的特征点轨迹：
可以通过整条轨迹的观测模型投影到HL的左零空间上，在左零空间上可以发现，整个HL会被消除掉：
我们假设A的每一列都是由左零空间的基组成，
可以得到与特征点坐标没有任何关系的观测模型：
从而，我们的问题从如下左边的拓扑结构变成这样右边的拓扑结构：
上图是什么意思呢？这是相机的姿态关系。假设有4个时态，相机的四个时态同时看到一个特征点。之前的观测模型是3D点的重投影，现在模型的相机位姿跟3D点没有更多的关系（虚线）。所以，这个模型可以设计一个只依赖于相机位姿的一个滤波器。
其它的流程暂不仔细讲。接下来，我总结一下这几类方法的关联和区别。
上图是滤波方法与关键帧优化方法的联系，这三个图描述我们当前视觉SLAM三种最有代表性的框架。第一个框架就是经典EKF的SLAM框架的拓扑结构，这代表什么意思呢？圆圈表示每个姿态，黑色小框表示特征点，黑线表示它们有相关性。所以，EKF的状态里面是所有的特征点加当前的相机的位姿，特征点与特征点之间，特征点与相机的姿态之间都有关联。这是经典的滤波方法。
对于多状态滤波方法，可以看到，实线变成了虚线。虚线表示特征点跟位姿是没有相关联的，滤波器里面只有相机姿态的变量。所以，它只是描述姿态之间的相关性。
对于关键帧优化方法，图3是最原始的图示，黑线表示它们之间的一个约束关系。这个图可以比较直观地去表示它们之间的区别。关键帧优化方法是所有点跟所有的相机姿态都拿进去一起去求解，然后，点与姿态之间有很强的约束关系。
我们稍微对多状态滤波方法作一些探讨。
根据实践验证，多状态滤波相对于经典滤波方法来说，它通过特征点坐标与位姿解耦，把特征点的计算放在滤波器之外，这样对滤波器的运行效率大大提升，可以处理非常多的特征点。一般基于EKF的SLAM，其特征点数量大约在上百左右就差不多算力不足了，到200左右已经无法实时运行。如果是使用多状态约束滤波方法，基本可以处理300-400，甚至400-500左右数量的特征点。所以，它与关键帧优化方法有很相近的性能。
相对于关键帧优化方法来说，利用多状态约束滤波方法能逐帧更新，逐帧边缘化，充分地利用了持续时间比较短的特征点轨迹，而且，由于这种轨迹的随机性，充分利用上可以增加系统的鲁棒性和精度；同时，它也可以单条轨迹更新以及控制更新的时间，从而增强了系统的灵活性。通过PID设计，让更新跟你的系统的运行的峰值相关，这样把计算峰值控制在一定的范围内，从而保持它的实时性。一般达到相近的精度的情况下，多状态约束方法计算成本比关键帧优化要低，一般不需要两个线程，一个线程就可以。
三．结构化环境下的视觉SLAM方法
3.1 结构化特征
这是本文报告的第二个部分，主要讲解结构化环境的视觉SLAM方法。实际上，结构化思想是非常的直接，我们每天都生活在这种人造的环境，这个环境有规则性，结构性是无处不在，
自然场景里面看上去杂乱无章，特别是对于图像处理，这里找不到什么直线或者平面，平面也有，比如水面，但是不具备稳定的结构信息。街道的场景，或者日常室内或者车库可以找到大量的结构化的信息，怎么样去把这些信息给利用起来？
结合我们的视觉SLAM，让系统运行得更鲁棒和精确，我们首先可以把这个线条特征利用好，因为规则性导致平整，平整就会有线条。
当然结构特性包含的含义很广，线条只是最初级的一种，我们目前的深度学习做得比较好的，道路上面可以抽取路标，还有分割物体，这些信息是非常有用的结构化的信息，未来都可以往这些方向去提升。
3.2 StructSLAM
接下来主要是讲解利用这种初级特征来提升我们SLAM的系统。这项工作是2015年开发的一套SLAM系统，StructSLAM的第一个版本，下面介绍我们是怎么样去结合结构化的信息。
这是我们结构化环境下视觉SLAM的一种实现方法，StructSLAM，其核心思想是利用一种新的特征，这个特征称之为结构线条特征。什么叫做结构线条呢？这个线条与建筑或者是与环境的主方向一致的线条，不是杂乱无章的线条，主方向在规整的建筑里面，肯定能够找到垂直的方向，这是主方向之一，有的时候水平方向也是会有主方向，比如有一条很长的走廊，这个方向可以也当成一个主方向之一。
关键是怎么把这个特征进行参数化。这里面我们选择一个与主方向垂直的平面作为一个参数平面，然后用参数平面去对结构线条特征进行参数化。首先，我们把结构线条特征初始化，使相机的中心投影到参数里成一个点，一根结构线条就可以由它在平面上的一个点去表示。
这个点我们采取反深度表示法。这里不是用X和Y来直接表示，而是用方向加上反深度来表示。结构线条由两部分组成，第一个部分是相机中心在参数平面上的投影，θ和h分别是参数平面焦点与原点，也即相机投影点的角度，其距离用反深度来表示。
接下来，我们采用经典卡尔曼滤波的方法，也即是StructSLAM第一个版本用的方法，其状态跟协方差矩阵定义如上图。第一个部分是相机姿态，第二个部分是点特征。这里说的结构化，并不是说线条完全可以把点特征扔掉，我们是结合起来使用，中间是点，下面才是线条。所以，优化量由三部分组成，分别是相机位置位姿和点特征，然后还有结构线条特征。
StructSLAM遵循经典滤波SLAM方法流程。我们回顾一下经典的卡尔曼滤波。第一步首先是初始化，初始化这里有三个步骤，
首先，至少要识别出两个投影的方向去检测主方向。我们首先对这个图像进行线段抽取，使用LSD线条抽取值，用J-Linkage对平行线条进行自动聚类，以获得投影灭点。
有了投影的灭点之后，我们可以用投影几何反推出投影主方向的位置，有了主方向的位置我们就可以进行接下来的主流程。
第三步，主方向找好了之后，还要对初始的结构线条进行参数初始化。图中黄色部分是我们所检测到的线段，这是相机当前的姿态。其初始化过程可以从这个图表示，这里面线段中心是在参数平面的投影。反深度是事先给的一个先验，初始化是把这个投影往外延伸一点，最终得到一个焦点。那么，这个线条就可以确定下来了。
当结构线条一旦初始化完毕，就进入真正的程序流程。
首先进行运动状态预测。预测主要是相机运动状态的预测，可以用传统的匀速模型，轮速计或者是结合其它的传感器进行预测。
接下来进行数据关联。点的数据关联只要匹配就可以，一个点是对应另外一个点，属于一对一的关系。但是线条的跟踪要比点的跟踪要难很多，为什么呢？因为线段的检测，比如看到墙角有一根线，但是在图像里面，有时上一帧检测是一根线条，下一帧线条就断了变成三根线段，从而形成一对多的关系。另外，线条外观是非常相似的，所以对它的匹配带来一定的困难。
我们系统里面采取了什么样的方案呢？首先，采用非常简单的描述子，使用图像块作为线条描述子。图像块是怎么来的呢？当这个线条初始化时，被检测出线段，取这个线段的终点的块作为其描述子，这就相当于它一直跟随这个线条。如果想要得到它在当前帧的对应的线段，首先要把线条投影到图像上，然后在图像上做卡方测试，在一个具体范围内进行筛选，在范围内的才认为是候选。在候选的线条里面去挑一个与ZNCC匹配得最好的线段，有可能会选出很多条，比如设置阈值是0.8（0.8一般都会匹配得非常好），在这里能找出一系列的线段，但没有关系，这些线段还是可以拿到下一步去进行处理。
候选线段中Outlier是不可避免的，因此还要采取一种比较保险的方式，那就是加上RANSAC抽取的过程。但是这个抽取的过程与单目双目视觉做五点法或者八点法不一样，这里有单个特征就可以进行RANSAC。为什么呢，因为卡尔曼滤波有一个特殊性，只要有一个点就可以进行状态更新。所以，第一步在所有的特征点和候选里面抽取一个匹配，有可能是点或者有可能是线条。第二步，试探性进行卡尔曼更新，用这一个观测量进行整个状态的更新，然后检查更新后的状态是否与观测一致。
所谓与观测一致，意思是最终观测的残差小于某个域值。这一域值可以自己定义，比如点可以定义4个pixel (像素)作为阈值，线条可能是两个端点与直线的距离，也是几个pixel的距离。设置好域值之后，可以一直重复地运行，找到一个最大的一致特征集合，最后使用一致特征集合统一再进行EKF更新，这样系统对这个outlier非常鲁棒。
上图是一个例子，我们在一些室外或者是不规则的环境，图像中有一些曲线，局部被抽出直线，如果不做一些剔除，会对状况估算值产生比较坏的影响。通过这个方法可以看到紫色的被当作outlier被剔除掉。
接下来，讲讲卡尔曼更新步骤中最关键的步骤，就是观测模型的设计。这个观测模型非常直接，一个结构线条要投影到图像上面，它可以分成两个部分，第一个部分是把主方向投影到图像上面，也即计算投影灭点；第二是在参数平面上把点投影到图像中去。两个点都投影到图像上面之后，就可以通过连线知道最终投影的直线，端点代表焦点。
投影的过程比较复杂，首先是世界坐标系转到相机坐标系，接着在象素坐标系进行叉乘最终得到直线。如果直线如果已经获得的话，观测模型就非常简单，也即是线段两个端点与直线的带符号的距离。
这一算法是经典的EKF的滤波的过程。所以，实现起来并不是特别复杂，用matlab可能花两三个星期就可以实现出来。
我们做了一系列的实验进行StructSLAM对比验证。
第一个实验是仿真试验。上图我们可以看到，有纯用点、纯用线，还有用结构线条，还有点+线等不同方案。实验结果中可以看到，结构线条的优势非常明显。同时我们也注意到，用线条的工作也较多，可以直接看到如果线条不加方向的约束，最终重构出来的线条特征是非常杂乱的，其结果与真正的方向差异很大，而且表现出来的轨迹的精度不是太好。
这是在室内场景的一个例子，我们与早期的Mono-SLAM进行了对比。可以看到，Mono-SLAM的特征点非常少，1-2个特征点，在正常场景下较为鲁棒的MonoSLAM在这种场景下鲁棒性较差，但是StructSLAM可以正常地运行。
我们采集了室外场景数据，验证系统对噪声的耐受能力。比如说不规则的场景或者是纹理比较杂乱的场景可能对StructSLAM系统造成影响，但是利用RANSAC方法之后，可以把这种影响降低到最低。
同时，我们在Rawseeds公开数据集上进行测试。我们只使用了前向低分辨率鱼眼摄像头，同时也用了轮速计（因为它好几个拐点的地方没有任何的纹理，仅仅摄像头很难提取信息，所以一定要辅助其它的传感器，实验中参与对比的系统都加上了轮速计）。实验对比结果中可以看到，区别还是很明显的。在这些结果中，StructSLAM的结果和groundtruth是基本上一致的。
这是我们量化的结果，在两个比较长的数据下，StructSLAM都跑到亚米级的精度，最差的误差不会超过2m。
结果中有意思的是，在随着时间增长的累计误差分析中，StructSLAM基本没有增长，而其它SLAM系统误差随时间增长非常严重。这是因为StructSLAM对方向约束得特别好，对每根线条都是全局进行约束，从而确保角度的稳定准确性，所以对累计误差起到非常大的帮助。
从这个结果来看第一个版本的StructSLAM我们可以得到一些初步的结果。主要有，首先，在纹理缺失的情况下，线条可以增强系统鲁棒性；其次，线条所蕴含的全局方向性，可以明显降低位置累计误差；其三，在于里程计结合之后，可以达到非常好的绝对定位精度。
3.3 StructVIO（视觉惯导紧耦合）
接下来讲我们的改进版本，StructVIO。这个版本试图把视觉与惯导紧耦合，让它运行得更好。实际上，之前主方向是有一些限制的，之前是考虑三个，但是现实场景里面是很复杂的，不可能说所有的建筑都是三个主方向。
所以，我们首先采取一种多曼哈顿滤波模型，处理更加不规则的场景，像斜的走廊或者是圆形，这种环境有更多环境的叠加。第二个就是把滤波框架换成多状态滤波，这种方案可以提高精度，降低计算量。在惯导系统，初始化是比较麻烦的事情，最终，我们加上线条方向先验知识，对快速初始化的帮助非常大。
由于工作尚未完全整理好，这里先讲部分初步的结果。
首先，我们用Tango测试许多场景。第一次测评用15组自采数据用于不同的场景，包括与Tango自身算法，OKVIS, VINS等对比，实现结果是StructVIO是最好的，当然其他方法是只是用点，这个比较不太公平。实际上，SLAM跟SLAM的方法相比很难相比，为什么呢？因为参数太多，没办法说很平等的比，只能说加线条很没有加线条，加了结构特征之后的StructVIO的性能是最好的。
然后，在Euroc数据集上测试。由于这个场景比较小，而且纹理比较丰富，一般基于特征的方法已经表现很好了，加上结构线条之后，我们还是看到了一些提升，大家可以看到StructVIO的RMSE还是能够降低一些，整体都优于其他框架。StructVIO比较适应场景大一点的应用，如大商场和地库。
三维视觉前沿进展与应用
中国图象图形学学会主办，深蓝学院承办的“三维视觉前沿进展与应用”前沿讲习班，将于10月13日-14日（周末）在北京大学举办。邀请8位学术界和企业界专家学者作报告，并交流分享。
报名链接：CSIG图像图形学科前沿讲习班 第7期
注：文章由深蓝学院第二期视觉SLAM课程优秀学员肖麟慧整理，未经允许，禁止以任何形式转载。
深蓝学院（https://www.shenlanxueyuan.com/）是专注于人工智能的在线教育平台，致力于构建前沿科技课程培养体系的业界标准，涵盖人工智能基础、机器学习、计算机视觉、自然语言处理、智能机器人等领域。