基于ArUco的视觉定位（四）

  这篇博客专门介绍一下MarkerMapper的实现原理，不了解MarkerMapper的可以先看一下我的上一篇博客：基于ArUco的视觉定位（三）。

Paper：Mapping and localization from planar markers （很重要）

  这不是一篇翻译，而是博主对上述论文的总结，理解之中难免有不到位的地方，不足之处还请参看原文。

---

〇、MarkerMapper干了啥？

  这里再陈述一遍MarkerMapper到底干了些什么事：

• 根据我们指定的空间中任意一个ID的marker，MakerMapper将整个空间中任意分布的所有marker以指定marker为参考坐标系构建marker的分布图，生成一个二维码分布文件。二维码的分布体现在每个marker的四个角点相对指定marker坐标系的三维坐标，即得到所有marker在参考世界坐标系下的位姿分布。其实最重要的也是我们最想得到的，就是所有marker的角点在世界坐标系下的三维坐标，因为有了这些角点的三维坐标，我们就可以用PnP算法估计相机的位姿。
• 在构建二维码分布图的过程中，MarkerMapper同时对相机的运行轨迹进行了估计，记录了关键帧在参考世界坐标系下的位姿。这里的关键帧是指那些至少观察到两个marker的帧。
• 除了优化了二维码的位姿分布和相机的运行轨迹，MarkerMapper还优化了相机的内参数。
• MakerMapper为我们生成了一个包含二维码位姿分布以及关键帧轨迹的可视化点云图。

一、基本原理

1、基本思路

  如上图(a)所示，当第$f{}^6$帧图像中同时检测到marker1和marker2时，我们可以得到相机当前与marker1之间的位姿关系$\gamma {}_1^6$、与marker2之间的位姿关系$\gamma {}_2^6$，由此我们就可以在$f{}^6$帧下求得marker1与marker2之间的位姿变换矩阵$\gamma {}_{2,1}^6=(\gamma {}_2^6{)}^{-1}\gamma {}_1^6$；当第$f{}^7$帧图像中同时检测到marker1、marker2和marker3时，我们可以得到相机当前与marker1之间的位姿关系$\gamma {}_1^7$、与marker2之间的位姿关系$\gamma {}_2^7$、以及与marker3之间的位姿关系$\gamma {}_3^7$，从而我们可以在$f{}^7$帧下求得marker1与marker2之间的位姿变换矩阵$\gamma {}_{2,1}^7$、marker1与marker3之间的位姿变换矩阵$\gamma {}_{3,1}^7$、marker2与marker3之间的位姿变换矩阵$\gamma {}_{3,2}^7$。同理，当第$f{}^t$帧图像中同时检测到marker{$i$,$j$,$k$,$...$}时，我们可以求得在当前帧下各marker之间的位姿变换矩阵$\gamma {}_{j,i}^t$、$\gamma {}_{k,i}^t$、$\gamma {}_{k,j}^t$、$...$。

  按照上面的思路，我们可以得到空间中所有二维码任意两两之间的相对位姿关系。指定其中一个二维码坐标系作为参考世界坐标系，就可以求得所有二维码相对于参考坐标系的位姿，进而得到所有二维码的角点在世界坐标系下的三维坐标。

  道理我们都懂，但实现起来远非这么简单。
　　
  理论上，在相机经过若干帧观察到所有marker后，我们就可以推算出任意marker间的相对位姿关系$\gamma {}_{j,i}$，但由于光线不佳、相机移动过快、分辨率较低、对焦不清等等原因，二维码角点的像素坐标是不准的，由此计算出来的${\gamma }_i^t$也是不准的，$\gamma {}_{j,i}^t$就更不准。总而言之，由于观测噪声的存在，这种简单地推算出来的关系是非常不准确的，存在极大的累积误差。所以，如何在有噪声的观测数据下精确地计算出每个marker在世界坐标系下的角点坐标，才是我们真正要解决的问题。

2、核心问题

  经过上面的分析，我们知道，问题的关键在于如何在相机不准确的观测数据下估计出二维码准确的位姿分布。了解SLAM的同学应该都知道，要解决这个问题最终不得不使出我们的杀手锏——非线性优化。好吧，MarkerMapper最终也要归结到一个优化问题上，基本手段就是最小化重投影误差：
$$e_i^t=\sum _j[\Psi (\delta ,{\gamma }^t,{\gamma }_i\cdot c_j)-{\mathbf{u}}_{i,j}^t{]}^2\text{\hspace{1em}(非常重要)}$$

解释一下这个重要的公式：

• $c_j$为已知量，表示marker$i$在该marker坐标系下角点$j$的三维坐标。通常，在marker边长已知为$s$的情况下，每个marker的4个角点在自身坐标系下的坐标分别为$c_0=(s/2,-s/2,0),c_1=(s/2,s/2,0),c_2=(-s/2,s/2,0),c_3=(-s/2,-s/2,0)$。
• ${\gamma }_i$为待估计量，表示从marker坐标系到世界坐标系的齐次变换矩阵，这里的世界坐标系是指参考的marker坐标系。${\gamma }_i\cdot c_j$是将marker$i$的第$j$个角点有marker坐标系转换到世界坐标系下表示。
• ${\gamma }^t$为待估计量，表示从世界坐标系到相机坐标系的齐次变换矩阵，也即相机的外参。
• $\delta$为待估计量，表示相机的内参矩阵。
• 函数$\Psi (\delta ,{\gamma }^t,{\gamma }_i\cdot c_j)$表示marker$i$的角点$j$从三维空间坐标到像素坐标的一种投影，它先将角点$c_j$转换到世界坐标系，再转换到相机坐标系，最后转换到像素坐标系。所以，$\Psi$表示的角点像素坐标是根据待估计量计算出来的，称为重投影。
• $u_{i,j}^t$为观测量，表示在第$f^t$帧下观察到的marker$i$第$j$个角点真实的像素坐标，由于各种原因产生的观测噪声，真实的并不代表就是准确的。

所以，$e_i^t={\sum }_j[\Psi -{\mathbf{u}}_{i,j}^t{]}^2$表示在第$f^t$帧下marker$i$ 4个角点的重投影误差平方和，我们的最终目的是估计出最优的 ${\gamma }_i$、${\gamma }^t$、$\delta$ 使得所有的$e_i^t$和最小，即：
$$\mathbf{e}({\gamma }_1,\cdot \cdot \cdot ,{\gamma }_M,{\gamma }^1,\cdot \cdot \cdot ,{\gamma }^N,\delta )=\sum _t\sum _{i\in f^t}{e}_i^t\text{\hspace{1em}(1)}$$

将变换矩阵$\gamma$用它的李代数形式$\zeta$代替，写成如下形式：
$$f({\gamma }_i,{\gamma }^t,\delta )=\mathbf{e}({\zeta }_1,\cdot \cdot \cdot ,{\zeta }_M,{\zeta }^1,\cdot \cdot \cdot ,{\zeta }^N,\delta )=\sum _t\sum _{i\in f^t}\left\{\sum _j[\Psi (\delta ,\Gamma ({\zeta }^t),\Gamma ({\zeta }_i)\cdot c_j)-{\mathbf{u}}_{i,j}^t{]}^2\right\}\text{\hspace{1em}(2)}$$

其中，$\zeta =(\mathbf{r},\mathbf{t})|\mathbf{r},\mathbf{t}\in {\mathbb{R}}^3$，$\gamma =\Gamma (\zeta )=\left[\begin{array}{cc}\mathbf{R}& {\mathbf{t}}^{\text{T}}\\ 0& 1\end{array}\right]$，这里的$\mathbf{r}$、$\mathbf{t}$均为行向量。$f^t$为所有观察到两个及两个以上marker的帧组成的集合，我在这里称之为关键帧。
　　好了，接下来的问题就是求上述非线性函数$f({\gamma }_i,{\gamma }^t,\delta )$的最小二乘解，它得到的最优解就是${\gamma }_i$、${\gamma }^t$、$\delta$的最大似然估计值。那么，如何求解这个最小二乘问题？有以下两个要点：

• （1）给定待估计量一个合适的初始值
• （2）选择合适的迭代算法

二、确定优化变量的初始值

  我们要优化的变量有三个：${\gamma }_i$、${\gamma }^t$、$\delta$，其中$\delta$的初始值是通过各种相机标定方法在线下获得的，属于MakerMapper的输入参数，所以对$\delta$而言直接优化即可，无需再确定它的初始值。下面详细介绍如何确定${\gamma }_i$、${\gamma }^t$的初始值。

1、如何获得每个marker在参考世界坐标系下的最优初始位姿${\tilde{\gamma }}_i$

1.1、创建位姿箭图$Q$，找出marker间最优的位姿变换矩阵${\hat{\gamma }}_{j,i}$

  上图（b）即是一个位姿箭图$Q$，它由节点和边组成。节点表示不同ID的marker，节点之间相连的边表示所对应marker与marker之间的相对位姿关系${\gamma }_{j,i}^t$。不是所有节点之间都有直接相连的边，节点之间也可能存在多条边，任意两个节点之间所有的边组成的集合记为${\chi }_{j,i}$。

  我们首先要解决的，是从任意两两节点之间所有的边${\chi }_{j,i}$中选出最优的${\hat{\gamma }}_{j,i}$，用来创建初始位姿图。定义下式：
$$\epsilon ({\gamma }_{j,i}^{t^{\prime }},f^t)=\sum _k{\left[\Phi (\delta ,{\gamma }_j^t,{\gamma }_{j,i}^{t^{\prime }}\cdot c_k)-{\mathbf{u}}_{i,k}^t\right]}^2，{\gamma }_{j,i}^{t^{\prime }}\in {\chi }_{j,i}\text{\hspace{1em}(3)}$$

  这个式子的意思是，利用在第$f^{t^{\prime }}$帧算出的${\gamma }_{j,i}^{t^{\prime }}$将marker$i$四个角点的三维坐标转换到marker$j$下表示，然后通过第$f^t$帧中的${\gamma }_j^t$将marker$j$坐标系下表示的marker$i$的四个角点投影到$f^t$帧的像素坐标中，最后求marker$i$在$f^t$中四个角点的重投影误差。其中，$t,t^{\prime }\in {\mathcal{F}}_{i,j}$，${\mathcal{F}}_{i,j}$表示所有包含marker$i$与marker$j$的帧。

  如果$\epsilon ({\gamma }_{j,i}^{t^{\prime }},f^t)<\epsilon ({\gamma }_{j,i}^t,f^{t^{\prime }})$，即认为${\gamma }_{j,i}^{t^{\prime }}$优于${\gamma }_{j,i}^t$，据此，我们就可以从节点$i$与节点$j$的所有边中找到marker$i$与marker$j$之间最优的位姿矩阵${\hat{\gamma }}_{j,i}$：
$${\hat{\gamma }}_{j,i}=\underset{{\gamma }_{j,i}\in {\chi }_{j,i}}{\mathrm{argmin}}{\mathbf{e}}_{j,i}({\gamma }_{j,i})=\underset{{\gamma }_{j,i}\in {\chi }_{j,i}}{\mathrm{argmin}}\sum _{t,t^{\prime }\in {\mathcal{F}}_{i,j}}\epsilon ({\gamma }_{j,i}^{t^{\prime }},f^t)\text{\hspace{1em}(4)}$$

上式表示从节点$i,j$之间所有的边${\chi }_{j,i}$中找到一条${\gamma }_{j,i}$，使得在包含所有marker$i,j$的帧${\mathcal{F}}_{i,j}$中由${\gamma }_{j,i}$得到的重投影误差平方和最小，这条边就记作${\hat{\gamma }}_{j,i}$

1.2、创建初始位姿图$G$，计算位姿图$G$的最小生成树***mst***

  有了${\hat{\gamma }}_{j,i}$，现在$Q$中的所有节点之间就只剩下了唯一的一条边，当然有的节点之间也可能没有边，但这并不代表它们之间是没有联系的。接下来，我们要利用${\hat{\gamma }}_{j,i}$创建一个位姿图$G$，如上图（c）所示。它的节点也代表不同ID的marker，但节点与节点之间有两条边，分别为${\hat{\gamma }}_{j,i}$和${\hat{\gamma }}_{i,j}$，两者是互逆的关系。

  根据位姿图$G$，我们可以指定其中任意一个节点代表的marker$a$作为参考，利用节点之间连接的边将所有marker的位姿都转换到参考marker$a$坐标系下表示：
$${\hat{\gamma }}_i={\hat{\gamma }}_{a,b}\cdot \cdot \cdot {\hat{\gamma }}_{k,h}{\hat{\gamma }}_{h,i}={\hat{\gamma }}_{a,i}\text{\hspace{1em}(5)}$$

  在计算${\hat{\gamma }}_i$之前要先计算位姿图$G$的最小生成树（mst），即找到可以连通所有节点的最优路径，路径是由节点与节点之间的边连接而成。当然，任意一个节点$i$到参考节点$a$的路径都可能不止一条，这里寻找最优路径的原则是使得组成该路径的所有边的重投影误差和最小，即找到一条最佳路径使得${\mathbf{e}}_{a,i}({\hat{\gamma }}_{a,i})$最小：

( a , b , ⋅ ⋅ ⋅ , k , h , i ) = argmin   e a , i ( γ ^ a , i ) = argmin   { e a , b ( γ ^ a , b ) + ⋅ ⋅ ⋅ + e k , h ( γ ^ k , h ) + e h , i ( γ ^ h , i ) } (6) (a,b,\cdot\cdot\cdot ,k,h,i)={\text{argmin}}\,{\bf e}_{a,i}(\hat\gamma_{a,i})={\text{argmin}}\,\{{\bf e}_{a,b}(\hat\gamma_{a,b})+\cdot\cdot\cdot+{\bf e}_{k,h}(\hat\gamma_{k,h})+{\bf e}_{h,i}(\hat\gamma_{h,i})\}\tag{6} (a,b,⋅⋅⋅,k,h,i)=argminea,i​(γ^​a,i​)=argmin{ea,b​(γ^​a,b​)+⋅⋅⋅+ek,h​(γ^​k,h​)+eh,i​(γ^​h,i​)}(6)

  上式在论文中并没有出现，是本人自己的理解，其中${\mathbf{e}}_{j,i}({\hat{\gamma }}_{j,i})$就是在上一步式（4）中计算的${\hat{\gamma }}_{j,i}$所对应的最小重投影误差平方和。最后，位姿图$G$会变成类似下图所示的样子（加粗的黑线就是 mst ），连通节点$i$与节点$j$的路径都只剩下了唯一一条，但节点与节点之间依然有两条互逆的边（表示往返）：

  其实这一步的最终目的并不是计算${\hat{\gamma }}_i$，而是要得到满足上述条件的最小生成树 mst 。另外，如果我们在MarkerMapper的入口参数中没有指定参考marker，那么程序就会根据上述原则利用弗洛伊德算法从所有的marker中选择一个合适的参考节点，使得生成的 mst 最优。

1.3、优化位姿图$G$，并得到所有marker在参考坐标系下的初始位姿${\tilde{\gamma }}_i$

  这一步是论文中最复杂的部分。首先，为了防止outliers（位姿图$G$中某些不太靠谱的边）败坏优化的过程，我们先计算 mst 中所有边的权重$\varpi (e)={\mathbf{e}}_{j,i}({\hat{\gamma }}_{j,i})$的均值和标准差，对于不在 mst 中的边，凡是权重大于均值2.58倍的都去掉，剩下的所有边和节点组成的位姿图记作$G^{\prime }$。然后对$G^{\prime }$做进一步优化。
　　
  考虑$G^{\prime }$中的一组循环节点$c=(1,2,3,\cdot \cdot \cdot ,n-1,n,1)$，节点之间最优的边${\tilde{\gamma }}_{j,i}$应该满足下式：
$${\mathbf{I}}_{4\times 4}={\tilde{\gamma }}_{1,2}{\tilde{\gamma }}_{2,3}\cdot \cdot \cdot {\tilde{\gamma }}_{n-1,n}{\tilde{\gamma }}_{n,1}$$

  即经过一个循环，节点$i$仍能投影到原处。假设节点之间的初始位姿${\hat{\gamma }}_{j,i}$都是相对正确的，那我们应该最小化${\tilde{\gamma }}_{j,i}$与${\hat{\gamma }}_{j,i}$之间的加权距离，即：

$$\text{min}\sum _{k\in c}{w}_{k,k+1}\text{d}({\tilde{\gamma }}_{k,k+1},{\hat{\gamma }}_{k,k+1})$$

其中$w_{k,k+1}=\frac{1}{{\mathbf{e}}_{k,k+1}\sum _{k\in c}\frac{1}{{\mathbf{e}}_{k,k+1}}}$，$w_{k,k+1}$的值域为(0,1)，并且$\sum _{k\in c}{w}_{k,k+1}=1$，下面对$w_{k,k+1}$的这两个性质进行推导：

$$\begin{gathered} 令{\mathbf{e}}^{\prime }(c)=\sum _{k\in c}\frac{1}{{\mathbf{e}}_{k,k+1}}\in (0,+\infty ),{\mathbf{e}}^{\prime \prime }(c)=\prod _{k\in c}{\mathbf{e}}_{k,k+1} \\ {w}_{k,k+1}=\frac{1}{{\mathbf{e}}_{k,k+1}{\mathbf{e}}^{\prime }(c)}=\frac{1}{1+{\mathbf{e}}^{\prime }(c-k)}\in (0,1),k\in c;{\mathbf{e}}^{\prime }(c-k)\in (0,+\infty )表示{\mathbf{e}}^{\prime }(c)中没有\frac{1}{{\mathbf{e}}_{k,k+1}}这一项 \\ \sum _{k\in c}{w}_{k,k+1}=\frac{{\mathbf{e}}^{\prime \prime }(c-1)+\cdot \cdot \cdot +{\mathbf{e}}^{\prime \prime }(c-k)+\cdot \cdot \cdot +{\mathbf{e}}^{\prime \prime }(c-n)}{\left\{\prod _{k\in c}{\mathbf{e}}_{k,k+1}\right\}{\mathbf{e}}^{\prime }(c)}=\frac{(\frac{1}{{\mathbf{e}}_{1,2}}+\cdot \cdot \cdot +\frac{1}{{\mathbf{e}}_{k,k+1}}+\cdot \cdot \cdot \frac{1}{{\mathbf{e}}_{n,1}}){\mathbf{e}}^{\prime \prime }(c)}{\left\{\prod _{k\in c}{\mathbf{e}}_{k,k+1}\right\}{\mathbf{e}}^{\prime }(c)}=\frac{e^{\prime \prime }(c)\left\{\sum _{k\in c}\frac{1}{{\mathbf{e}}_{k,k+1}}\right\}}{\left\{\prod _{k\in c}{\mathbf{e}}_{k,k+1}\right\}{\mathbf{e}}^{\prime }(c)}=1 \end{gathered}$$

  对于$\tilde{\gamma }=\left[\begin{array}{cc}\tilde{\mathbf{R}}& \tilde{\mathbf{t}}\\ 0& 1\end{array}\right]$和$\hat{\gamma }=\left[\begin{array}{cc}\hat{\mathbf{R}}& \hat{\mathbf{t}}\\ 0& 1\end{array}\right]$，有：
$${\tilde{\mathbf{R}}}_{1,2}\cdot \cdot \cdot {\tilde{\mathbf{R}}}_{k,k+1}\cdot \cdot \cdot {\tilde{\mathbf{R}}}_{n,1}={\hat{\mathbf{R}}}_{1,2}{\mathbf{E}}_{1,2}^{{\alpha }_{1,2}}\cdot \cdot \cdot {\hat{\mathbf{R}}}_{k,k+1}{\mathbf{E}}_{k,k+1}^{{\alpha }_{k,k+1}}\cdot \cdot \cdot {\hat{\mathbf{R}}}_{n,1}{\mathbf{E}}_{n,1}^{{\alpha }_{n,1}}={\mathbf{I}}_{3\times 3}\text{\hspace{1em}(7)}$$

其中， E k , k + 1 α k , k + 1 = exp { α k , k + 1   ln E k , k + 1 } {\bf E}_{k,k+1}^{\alpha_{k,k+1}}=\text{exp}\{\alpha_{k,k+1}\,\text{ln}{\bf E}_{k,k+1}\} Ek,k+1αk,k+1​​=exp{αk,k+1​lnEk,k+1​}，${\alpha }_{k,k+1}=\frac{1/w_{k,k+1}}{\sum _{j\in c}1/w_{j,j+1}}=\frac{{\mathbf{e}}_{k,k+1}}{\sum _{j\in c}{\mathbf{e}}_{j,j+1}}\in [0,1]$，$\sum _{k\in c}{\alpha }_{k,k+1}=1$，$$\begin{gathered} {\mathbf{e}}_{j,i}({\gamma }_{j,i})=\sum _{t,t^{\prime }\in {\mathcal{F}}_{i,j} \\ {\gamma }_{j,i}\in {\chi }_{j,i}}\epsilon ({\gamma }_{j,i}^{t^{\prime }},f^t) \end{gathered}$$。注意这里${\mathbf{E}}_{k,k+1}^{{\alpha }_{k,k+1}}$中的${\alpha }_{k,k+1}$并不是一个普通的右上标，而是代表矩阵的指数，可以理解为旋转矩阵的幂运算，即有：
$${\mathbf{E}}_{k,k+1}^{{\alpha }_{1,2}}\cdot \cdot \cdot {\mathbf{E}}_{k,k+1}^{{\alpha }_{k,k+1}}\cdot \cdot \cdot {\mathbf{E}}_{k,k+1}^{{\alpha }_{n,1}}={\mathbf{E}}_{k,k+1}^{{\alpha }_{1,2}+\cdot \cdot \cdot +{\alpha }_{k,k+1}+\cdot \cdot \cdot +{\alpha }_{n,1}}={\mathbf{E}}_{k,k+1}$$

其中每一个${\mathbf{E}}_{k,k+1}^{{\alpha }_{k,k+1}}$都是与${\mathbf{E}}_{k,k+1}$同轴的旋转矩阵，即旋转${\mathbf{E}}_{k,k+1}$由这些微小的旋转${\mathbf{E}}_{k,k+1}^{{\alpha }_{k,k+1}}$组成。

  那么（7）式表示的到底是什么意思？就是说，比如在上图所示的一个循环$c=(1,2,4,3)$中，我们可以给每个边添加一个微小的误差项，使得这个循环满足（7）式。同样对于循环$c=(1,2,5,3)$，我们也可以给每个边添加一个微小的误差项，使得这个循环满足（7）式。即

$$\{\begin{array}{l}{\hat{\mathbf{R}}}_{1,2}{\mathbf{E}}_{1,2}^{{\alpha }_{1,2}}{\hat{\mathbf{R}}}_{2,4}{\mathbf{E}}_{2,4}^{{\alpha }_{2,4}}{\hat{\mathbf{R}}}_{4,3}{\mathbf{E}}_{4,3}^{{\alpha }_{4,3}}{\hat{\mathbf{R}}}_{3,1}{\mathbf{E}}_{3,1}^{{\alpha }_{3,1}}={\mathbf{I}}_{3\times 3}\\ {\hat{\mathbf{R}}}_{1,2}{\mathbf{E}}_{1,2}^{{\alpha }_{1,2}}{\hat{\mathbf{R}}}_{2,5}{\mathbf{E}}_{2,5}^{{\alpha }_{2,5}}{\hat{\mathbf{R}}}_{5,3}{\mathbf{E}}_{5,3}^{{\alpha }_{5,3}}{\hat{\mathbf{R}}}_{3,1}{\mathbf{E}}_{3,1}^{{\alpha }_{3,1}}={\mathbf{I}}_{3\times 3}\end{array}$$

对于旋转矩阵${\mathbf{R}}_{k,k+1}$，它的误差项是一个与之同轴的旋转矩阵${\mathbf{E}}_{k,k+1}^{{\alpha }_{k,k+1}}$，这个旋转矩阵的旋转角度由${\alpha }_{k,k+1}$确定，${\alpha }_{k,k+1}$与节点$k$到节点$k+1$之间的重投影误差有关。（这一部分我不是特别理解，待修正）

2、如何获得关键帧在参考世界坐标系下的最优初始位姿${\tilde{\gamma }}^t$

未完成。。。