【论文】TagSLAM: Robust SLAM with Fiducial Markers

I. INTRODUCTION

很多的SLAM框架都在努力地解决一些很难的问题。像重新辨认之前见过的landmark，另一个是如何保存与维护landmark（这个回很费内存空间）。维护这些点都是为了能够实现loop closeure 。
但是在很多的估计实验中是可以使用一些方法来温和地避开一些与环境的交互问题的。
通过放置一些Tag是可以创建一小撮的landmark，特别是在特征点小于100个时候尤其有用。TagSLAM是专门对一些特征点不多的场景，内存与计算速度的要求不高，而且对于回环检测的要求很高的场景。如果有至少一个Tag被camera观察到，就可以进行一次回环处理了。

Tag可以为观察到的实体提供几何上的约束。比如说可以预先确定好的放置的Tag的位置。或者说可以使用一些laser工具来测算实体与tag之间的距离。当一个tag被移动的相机认出来之后，会在相应的位置创建一个轨迹。这个tag的方法可以大大增强现有的SLAM系统和VIO系统。

这个TagSLAM是使用GTSAM来实现优化的，需要估计一个良好的初值，不然的话可能会收敛失败。

值得注意的是，SLAM是一个想当通用的方法。如果你知道了相机的位姿，那么相对的，你就可以知道landmark的位置，同理如果你知道了landmark 的位置，那么就可以知道相机的位姿。TagSlam继承了这个特征，可以实现相机是静止的情况下，可以估计出移动Tag 的位置。

II. RELATED WORK

重点提到了一个SPM-SLAM作用与其相近。
SPM-SLAM是和一般的SLAM一样使用过的是关键帧模式，而TagSLAM是所有帧都是关键帧然后再利用iSAM2进行稀疏化（这里不是很懂）。

对于长路程的话SPM-SLAM是优于TagSLAM，TagSLAM可以满足更加多的场景需求。
SPM-SLAM不可以包含关键点的测量，但是这个缺点在后来UcoSLAM中被解决了。
SPM-SLAM使用两帧之间的关系测量出Tag 的位姿，TagSLAM则是不会立刻使用Tag 的关系，直到没有二义性地确定了Tag 的位姿。

SPM-SLAM只注意他的模糊的测量，TagSLAM则是会额外考虑Tag 的观测形状等。

III. MODEL SETUP

符号表示：

A坐标系下的X，乘上A坐标系下到B坐标的变换矩阵，得到B坐标系下的X坐标

body：是与tag和camera所相联系的。body的位姿可以是静态的，也可以是动态的。
Tag：必须是独一无二的，同时每一个Tag必须与body相联系，没有相联系的tag将会无视它。不同于body，tag的位姿只能是静态的。
camera pose：就像tag位姿是静态的。是来源于body所链接的相机位姿的（应该是先验的意思）。所来源于的body，对于camera来说被称为rig。

IV. FACTOR GRAPH

这个优化是才用贝叶斯公式进行最大似然估计的方法来优化的，利用最大似然估计的值作为优化值。默认所有的概率分布都是高斯分布。

优化的对象是各种位姿，一共包括了4种约束：

每一帧相机位姿及其相连的body之间的约束。（body类似于关键帧，但是body只有在遇到了tag才有）
每个tag位姿及其相连的body之间的约束。
body和师姐坐标之间的约束（这个不是很懂）
动态的body坐标和世界坐标之间的约束。（这个也不懂）

Absolute Pose Prior

利用一个特定的T0帧（可能就是前面说的body）来形成约束：

Relative Pose Prior.

利用相邻的AB帧之间重投影误差来形成约束。
动态的body帧之间的也是这样形成约束：

Tag Projection Factor.

利用tag的四个顶点来重投影误差来确定相机的位姿。S是tag角点坐标，c表示的是4个角点。

图优化表示：

方块是约束，圆是优化点。PT是4元边，Pr是二元边。

V. ROBUST INITIALIZATION

非线性优化使用的是GSTAM库。
很多的认为因素都会导致优化出错。所以初始化的过程是很重要的。

重要：实验表明，如果只能看到一个tag，其实对于确定位姿是很不准的。至少在画面中需要出现两个tag或者是多个相机观察到tag并且直到他们之间的内参的时候，这个约束才是有用的。（所以定位起码需要两个tag）。

单纯的PnP得出来的结果是没有鲁棒性的。优于这个原因，论文中所有的基于单位来获取位姿初始化都只能来自于一组最小的tag，也就是需要有最小的tag数，并且需要考虑这些tag的是使用与他的使用顺序。（这里的初始化表示的是某一个位姿的创建）

为了尽可能地利用历史的观测，这个系统维护了两个graph：保存了所有factor的完全图和直到当前时间变量使用的所有测量值，也就是说是只保存满足当前变量优化约束的factor的优化图。

A. Subgraph discovery

当新测量值来到的时候可能会导致当前的某个位姿可以确定下来，也可能会导致以前的某个位姿可以确定下来。所以可以确定产生确定下来的位姿的fator和变量都会加入到subgraph中。并且按照这些约束的产生顺序，加入到initialization list中。这个顺学在后面的处理中将会有很大的作用。

已经发现的subgraph是会依赖于新发现的factor的，一个新的factor 会可以加入到subgraph中。在通常情况下，subgraph是相连的，一个组新的factor 会仅产生一个subgraph。

本文的方法中会严格控制tag的位姿约束进入到subgraph中，除非是tag的位姿已经确定了或者是已经给了一个位姿先验。，亦或者是观察他的角度小于60度（这样的重投影误差会比较明显）。

所有进入到subgraph中过的变量都应该是已经确定的（有初值），对于那栋动态的位姿，也应该有个约束或者是先验信息。上面这个图就是和Fig2对比，t时刻的动态位姿虽然没有确定，但是也会有先验位姿。

B. Order of subgraph discovery

进入到subgraph是会按照下面的顺序的：
1、任何与先验位姿相关联的factor
2、任何与tag的重投影相关联的factor
3、任何不建立位姿的factor

读后感想

相较于常规的SLAM系统，用了一些新的设计思路。
1、subgraph系统，tag只会在确定了位姿之后才会加进去因子图中。
2、所有帧都会加入进去，同时也是用了body节点的概念。
3、优化的思想是基于最大似然估计，而不是常规的最小二乘。
4、参考系可以相互转转化，可以在相机不动的情况下，获得观察的tag的位姿。
缺点：
1、因为架构的不同，加入到别的SLAM框架里面的难度加大。
2、所有帧都会参与优化，计算量比较大，不适合于移动平台。