Robust Pose Estimation from a Planar Target

Abstract

理论上，已经标定的相机pose可以用最小4个共面不共线点唯一确定。实践中，平面目标的相机pose跟踪有很多的应用，最近也有很多实时完成pose估计算法，但是这些算法都遇到pose歧义问题。这篇文章调研了相机平面目标pose歧义性。我们证明了pose的歧义性，即以误差函数为基础的求解方法将得到两个不同的局部最小值，即使在广角镜头和近距离目标的情况下也存在该问题。我们给出两个最小值的详尽的解释，开发了唯一的，鲁棒的新算法求解平面目标的pose估计问题。
paper: Robust Pose Estimation from a Planar Targe
author: Gerald Schweighofer and Axel Pin

Introduction

pose估计的很多应用中，需要估计6自由度的相机pose,它使用已知场景结构的匹配来计算，可以利用单张图像或者图像序列来做。在摄影测量学中，这个问题被称为空间划分（space resectioning），通常通过线束调整(bundle ajustment)技术来离线解决，从而获得很高的精度，同时还具有针对异常值的高鲁棒性。通常，有多种方法可以解决此问题，只要可以使用许多点，并且可以离线计算pose（例如，考虑帧n + k中的信息以计算帧n的pose）。线上pose跟踪要求实时的计算每一帧的pose。通常，从目标中提取兴趣点，然后计算相对场景目标的相机pose。理论上，在相机的内参已知的情况下，pose可以被4个或者更多个共面不贡献的点来计算¹。在计算机视觉文献中，存在一些已知的相机pose估计的方法，大多数的工作来自于共面点^{2 3}，一些点线的扩展工作^{4 5},以及一些3D目标配准的工作⁶。最近出来了基于在线(online)结构和运动估计的报告⁷，它提取许多的兴趣点，帧到帧的匹配也很容易，不需要预先知道场景坐标系（与此类似的更早的工作看这里⁸）。

以平面目标为基础的视觉跟踪系统的例子包括ARToolkit⁹ (在增强现实中广泛使用)， Mali等¹⁰ (他们通过改进目标板设计，声称精度比ARToolkit更高)，我们自己的开发¹¹ (融合惯导和视觉来增加鲁棒性)。 Kawan等人¹² 进一步讨论大量的针对AR的marker，提出了自己的平面目标编码。使用这种系统的用户会发现基于视觉的pose不是很精确，这会导致明显的抖动，并且不会因pose跳变和总体pose离群值而引起很大的痛苦。

我们完成很多对比实验，比较了一些pose算法和平面配置。即使我们很好的标定以及避免掌握的关键性配置，但仍然观察到不应该出现的pose跳变。

这些pose的歧义性 Oberkamp等人¹³也讨论过,他们直接给出了那种情况下正交投影的直接解释，开发了POSIT 算法，即在每次迭代时使用带尺度的正交投影(scaled orthographic projection)。POSIT保留两个解，最后根据更好的距离测量$E$来确定最终的解。然而他们的方法仅仅在距离非常大的深度场景中有效，那种情况下，透视投影可以用带尺度的正交投影近似。

这篇paper解决了透视投影的一般性问题。我们证明pose的歧义性在广角镜头和近范围的情况也存在。我们给出了详尽的解释，开发了唯一的，鲁棒的新算法求解平面目标的pose估计问题。

Pose Ambiguity

我们这里讨论已标定相机（已知内参）的pose估计问题，即找相机的6个外部参数问题：相机和场景坐标系之间的方向$R=f(\alpha ,\beta ,\gamma )$和位置$\mathbf{t}=[t_x,t_y,t_z{]}^T$。图1坐标是相机的投影中心$C_C$(Camera center)和图像平面（image plane），右边看到平面II上的mode点$P_i,P_j$。$C_C$是坐标系的原点，model坐标系的原点是model的中心$C_M$(model center)，$C_C$和$C_M$之间的距离用$\mathbf{t}$表示。绕着model的y轴旋转$\alpha$角度，我们得到位于$I{I}_{\alpha }$平面上的model点$P_{i\alpha }$和$P_{j\alpha }$。这些点被投影到图像，表示为${\mathbf{v}}_{\mathbf{i}},{\mathbf{v}}_{\mathbf{j}}$。

我们假设n个场景坐标点${\mathbf{p}}_{\mathbf{i}}$变换到相机坐标点${\mathbf{v}}_{\mathbf{i}}$
$${\mathbf{v}}_{\mathbf{i}}\propto R{\mathbf{p}}_{\mathbf{i}}+\mathbf{t}\text{\hspace{1em}(1)}$$
其中$\propto$表示比例（或者正比于的意思）因此测量的${\mathbf{v}}_{\mathbf{i}}$带有未知的尺度。我们进一步将${\mathbf{v}}_{\mathbf{i}}$作为相机中的坐标点，$\widehat{{\mathbf{v}}_{\mathbf{i}}}$作为图像测量点（也是以相机坐标系点，但是由于噪音存在而不精确）。一个pose估计算法需要去找$\hat{R},\hat{t}$使得误差函数最小，在这篇文章中我们使用对象空间(object-space)误差函数²
$$E_{os}(\hat{R},\hat{\mathbf{t}})=\sum _{i=1}^n\Vert \left(I-\frac{\widehat{{\mathbf{v}}_{\mathbf{i}}}{\hat{\mathbf{v}}}_{\mathbf{i}}^{\mathbf{t}}}{{\hat{\mathbf{v}}}_{\mathbf{i}}^{\mathbf{t}}{\hat{\mathbf{v}}}_{\mathbf{i}}}\right)(\hat{R}{\mathbf{p}}_{\mathbf{i}}+\hat{\mathbf{t}}){\Vert }^2\text{\hspace{1em}(2)}$$
note:一个向量投影到与它平行的向量上所得的向量与它自身向量相等
图1中，我们的例子想要指出的是对于每一个选择的$\alpha$存在不同的角度$\beta$同样使得误差函数$E_{os}$局部最小。那样最小的存在性依赖于等式（2）中的所有的参数。图二展示了误差函数$E_{os}$在不同距离$\mathbf{t}$下的变换,其中$\mathbf{t}$表示$C_M$与$C_C$之间的距离。图一展示的$R$仅仅是一个关于y轴旋转，且$\alpha =60^{\circ }$，z噪声的情况（$\widehat{{\mathbf{v}}_{\mathbf{i}}}={\mathbf{v}}_{\mathbf{i}}$ ）。所有的可视化曲线在$\alpha =60^{\circ }$时取得局部最小，误差函数$E_{os}$取得最小值0。对于$\Vert \mathbf{t}\Vert =2,2.5,3,5$，在$\beta =-38.8^{\circ },-49.9^{\circ },-53.7^{\circ },-58.0^{\circ }$时，我们可以看到增加有意义的（即指$E_{os}$下降）局部最小。随着model和相机距离的增加透视投影的影响减少。当$\Vert \mathbf{t}\Vert =\infty$时，我们将要在$\beta =-60^{\circ }$得到第二个最小值使得$E_{os}=0$。对于$\Vert \mathbf{t}\Vert <1.86$仅仅存在一个最小的误差函数($E_{os}=0$, where $\alpha =60^{\circ }$)。
图3描述了$E_{os}$对变化的角$\alpha$的依赖性。图三是$\Vert \mathbf{t}\Vert =3$时的情况，再一次，我们假设无噪声的情况，对于每一个$\alpha$是得$E_{os}=0$，对于$\alpha =50^{\circ },60^{\circ },70^{\circ },80^{\circ }$时，我们将要看到在$\beta =-26.0^{\circ },-36.4^{\circ },-51.5^{\circ },-64.8^{\circ }$时的第二个最小值$E_{os}$。对于$\alpha <43.4^{\circ }$时不存在第二个最小值$E_{os}$。

Robust pose estimation algorithm

前一节演示了即使在没有噪声的情况下依然存在两个不同的局部最小值$E_{os}$。从现在起，我们讨论真实的图像情况，一般情况下${\mathbf{v}}_{\mathbf{i}}\ne \widehat{{\mathbf{v}}_{\mathbf{i}}}$。我们可能认为比无噪声的情况相比，可能会更多的出现两个局部最小，但是$E_{os}=0$就两种情况，即正确的$\alpha$,错误的$\beta$。我们的新算法以这两个为基础：

1. 有时候有两个局部最小，这依赖于实际的参数配置($\hat{R},\hat{\mathbf{t}},{\mathbf{p}}_{\mathbf{i}},\widehat{{\mathbf{v}}_{\mathbf{i}}}$)
2. 正确的pose估计的解应该有更低的误差值$E_{os}$。

为了开发算法，我们首先假设一个已知的pose($\hat{R},\hat{\mathbf{t}}$)（它可以通过任意的pose估计算法获取）。正如第II节描述的那样，可能有两个局部的最小解（值）。当pose估计算法返回一个错误结果的时候也可能出现。IV实验结果表明对于一些常见的算法几乎有50%的可能出现。

我们现在展示如何利用初始化猜想的pose($\hat{R},\hat{\mathbf{t}}$）估计第二个最小化误差函数$E_{os}$对应的pose。

我们使用连续变换等式（1）的方法使得最终的pose仅仅依赖绕y轴$R_y(\beta )$的旋转和$\mathbf{t}$。因此我们在等式（3），（5），（9）和根据初始化的(${\hat{R}}_1,\widehat{{\mathbf{t}}_1}$)得到的特定的计算（$R_x和R_z$）来切换。

假设mode 点 ${\mathbf{p}}_{\mathbf{i}}$在图像中的测量为$\widehat{{\mathbf{v}}_{\mathbf{i}}}$使得
$$\widehat{{\mathbf{v}}_{\mathbf{i}}}\approx {\mathbf{v}}_{\mathbf{i}}\propto R{\mathbf{p}}_{\mathbf{i}}+\mathbf{t}\text{\hspace{1em}(3)}$$
为了不失一般性，我们将等式（3）的两边乘以$R_t$得到：
$$R_t\widehat{{\mathbf{v}}_{\mathbf{i}}}\approx R_t{\mathbf{v}}_{\mathbf{i}}\propto R_{t}R{\mathbf{p}}_{\mathbf{i}}+R_t\mathbf{t}\text{\hspace{1em}(4)}$$
使得$R_t\widehat{{\mathbf{t}}_1}=[0,0,\Vert \hat{\mathbf{t}}\Vert {]}^T$，坐标系这样变换的结果是把mode($[0,0,0{]}^T$)投影到图像的$[0,0,\Vert \mathbf{t}\Vert ]$。
note: 这样变换并不影响误差函数的最小化的结果
让
$${\tilde{\mathbf{v}}}_{\mathbf{i}}=R_t\widehat{{\mathbf{v}}_{\mathbf{i}}},{\tilde{\mathbf{t}}}_1=R_t\mathbf{t},{\tilde{R}}_1=R_t{\hat{R}}_1\text{\hspace{1em}(5)}$$
最小化pose(${\tilde{R}}_1,{\tilde{\mathbf{t}}}_1$)
$$E_{os}(\tilde{R},\tilde{\mathbf{t}})=\sum _{i=1}^n\Vert \left(I-\frac{{\tilde{\mathbf{v}}}_{\mathbf{i}}{\tilde{\mathbf{v}}}_{\mathbf{i}}^{\mathbf{t}}}{{\tilde{\mathbf{v}}}_{\mathbf{i}}^{\mathbf{t}}{\tilde{\mathbf{v}}}_{\mathbf{i}}}\right)(\tilde{R}{\mathbf{p}}_{\mathbf{i}}+\tilde{\mathbf{t}}){\Vert }^2\text{\hspace{1em}(6)}$$
为了不失一般性，我们引入旋转矩阵${\tilde{R}}_z$，使得
$$E_{os}(\tilde{R},\tilde{\mathbf{t}})=\sum _{i=1}^n\Vert \left(I-\frac{{\tilde{\mathbf{v}}}_{\mathbf{i}}{\tilde{\mathbf{v}}}_{\mathbf{i}}^{\mathbf{t}}}{{\tilde{\mathbf{v}}}_{\mathbf{i}}^{\mathbf{t}}{\tilde{\mathbf{v}}}_{\mathbf{i}}}\right)(\tilde{R}\underset{\mathbf{I}}{{\tilde{R}}_z{\tilde{R}}_z^{-1}}{\mathbf{p}}_{\mathbf{i}}+\tilde{\mathbf{t}}){\Vert }^2\text{\hspace{1em}(7)}$$
旋转${\tilde{R}}_z^{-1}$仅仅关于z轴旋转平面mode点${\mathbf{p}}_{\mathbf{i}}$使得旋转model点${\tilde{\mathbf{p}}}_{\mathbf{i}}={\tilde{R}}_z^{-1}{\mathbf{p}}_{\mathbf{i}}$的$z=0$。旋转矩阵${\tilde{R}}_1{\tilde{R}}_z$可分解为三个旋转的乘积
$${\tilde{R}}_1{\tilde{R}}_z=R_z({\tilde{\gamma }}_1)R_y({\tilde{\beta }}_1)R_x({\tilde{\alpha }}_1)\text{\hspace{1em}(8)}$$
其中$R_i(\varphi )$表示关于i轴旋转了$\varphi$度。通过选择合适的${\tilde{R}}_z$使得${\tilde{\alpha }}_1=0$,我们将要得到另外一个变化系统
$${\tilde{\mathbf{v}}}_{\mathbf{i}}\approx R_z(\gamma )R_y(\beta ){\tilde{\mathbf{p}}}_{\mathbf{i}}+\tilde{\mathbf{t}}\text{\hspace{1em}(9)}$$
对应的误差函数
$$E_{os}(\gamma ,\beta ,\tilde{\mathbf{t}})=\sum _{i=1}^n\Vert \left(I-\frac{{\tilde{\mathbf{v}}}_{\mathbf{i}}{\tilde{\mathbf{v}}}_{\mathbf{i}}^{\mathbf{t}}}{{\tilde{\mathbf{v}}}_{\mathbf{i}}^{\mathbf{t}}{\tilde{\mathbf{v}}}_{\mathbf{i}}}\right)(R_z(\gamma )R_y(\beta ){\tilde{\mathbf{p}}}_{\mathbf{i}}+\tilde{\mathbf{t}}){\Vert }^2\text{\hspace{1em}(10)}$$
利用已知的pose(${\tilde{R}}_1,{\tilde{\mathbf{t}}}_1$)或者($f(0,\tilde{\beta },{\tilde{\gamma }}_1),{\mathbf{t}}_1$)情况，则等式（9）和（10）等价于（4）和（6）。

从$II$节中我们知道误差函数$E_{os}$相对一个轴的旋转（y轴， 依赖参数）可能有两个最小值。在我们的变换系统等式（9）中，我们有一个关于y轴和z轴的旋转。

让我们讨论旋转$R_z(\gamma )$的影响，从步骤（5）可以知道${\tilde{\mathbf{t}}}_1=[0,0,\Vert {\mathbf{t}}_1\Vert ]$，我们重写等式（9）为
$${\tilde{\mathbf{v}}}_{\mathbf{i}}\approx R_z(\gamma )(R_y(\beta ){\tilde{\mathbf{p}}}_{\mathbf{i}}+{\tilde{\mathbf{t}}}_1)\text{\hspace{1em}(11)}$$
因为$R_z(\gamma ){\tilde{\mathbf{t}}}_1={\tilde{\mathbf{t}}}_1$。因此旋转$R_z(\gamma )$变成了仅仅绕着光轴的旋转，这就保证了图像平面和model平面几何关系不变，仅仅影响了图像的坐标。因此，与图一相关的所有的观测是有效的，我们仅仅需要找关于$\beta$的$E_{os}$的最小值。

因此我们提出了下面的基于平面的鲁棒性pose估计：

1. 首先，利用存在的任何一个迭代pose估计方法估计pose ${\hat{P}}_1=({\hat{R}}_1,{\hat{\mathbf{t}}}_1)$。在我们的实验中使用[2]²的估计方法。${\hat{P}}_1$是$E_{os}$一个局部最小，我们的目的是分析推导第二个局部最小，如果第二个局部最小存在的话。
2. 根据等式（5）变换坐标系统，得到${\tilde{P}}_1=({\tilde{R}}_1,{\tilde{\mathbf{t}}}_1)$
3. 根据等式（7）到（9）的描述估计${\tilde{R}}_z$得到变换的系统和初始参数pose(${\tilde{\gamma }}_1$和${\tilde{\beta }}_1$)
4. 固定$\gamma ={\tilde{\gamma }}_1$估计使得等式(10)局部最小的参数$\beta$和$\tilde{\mathbf{t}}$，细节见III-A节
5. 重复步骤1,2来获取局部最小的pose${\tilde{P}}_i$
6. 将估计出来的pose ${\tilde{P}}_i$作为初始值，利用迭代pose估计算法²获取最终的pose $P_i^{\ast }$
7. 根据最小的$E_{os}$确定最终的和正确的pose。

A. Estimation of the local minima

我们这里讨论根据给定的${\tilde{\mathbf{p}}}_{\mathbf{i}},{\tilde{\mathbf{v}}}_{\mathbf{i}},R_z(\gamma )$如何估计等式（10）的局部最小。为了简化等式，设
$${\tilde{V}}_i=\frac{{\tilde{\mathbf{v}}}_{\mathbf{i}}{\tilde{\mathbf{v}}}_{\mathbf{i}}^{\mathbf{t}}}{{\tilde{\mathbf{v}}}_{\mathbf{i}}^{\mathbf{t}}{\tilde{\mathbf{v}}}_{\mathbf{i}}}$$
在等式(10)中，为求解最优(optimal)的平移${\tilde{\mathbf{t}}}_{\mathbf{o}\mathbf{p}\mathbf{t}}$，$E_{os}$对$\tilde{\mathbf{t}}$的导数²:
$$\begin{array}{rl}\frac{\partial E_{os}}{\partial \tilde{\mathbf{t}}}& =0⟹\\ {\tilde{\mathbf{t}}}_{opt}(\beta )& =\frac{1}{n}\left(I-\frac{1}{n}\sum _j{\tilde{V}}_j\right)\sum _j\left({\tilde{V}}_j-I\right)R_z(\gamma )R_y(\beta ){\tilde{\mathbf{p}}}_{\mathbf{i}}\\ & =G\sum _j\left({\tilde{V}}_j-I\right)R_z(\gamma )R_y(\beta ){\tilde{\mathbf{p}}}_{\mathbf{i}}\end{array}\text{\hspace{1em}(12)}$$
其中G是不变的且仅仅依赖于测量的图像位置${\tilde{\mathbf{v}}}_{\mathbf{i}}$。将$\tilde{\mathbf{t}}(R)$带入等式(10)得到仅仅依赖$\beta$（关于y轴的旋转）：
$$\begin{gathered} E_{os}(\beta )=\sum _{i=1}^n\Vert \left(I-{\tilde{V}}_i\right)(R_z(\gamma )R_y(\beta ){\tilde{\mathbf{p}}}_{\mathbf{i}} \\ + \\ G\sum _{j=1}^n\left({\tilde{V}}_j-I\right)R_z(\gamma )R_y(\beta ){\tilde{\mathbf{p}}}_{\mathbf{i}}{\Vert }^2\text{\hspace{1em}(13)} \end{gathered}$$
为了简化$R_y(\beta )$，替换(${\beta }_t=tan\frac{1}{2\beta }$),得到
$$\begin{gathered} R_y(\beta )=\left[\begin{array}{ccc}\cos (\beta )& 0& \sin (\beta )\\ 0& 1& 0\\ -\sin (\beta )& 0& \cos (\beta )\end{array}\right]; \\ {R}_y({\beta }_t)=\frac{1}{1+{\beta }_t^2}\left[\begin{array}{ccc}1-{\beta }_t^2& 0& 2{\beta }_t\\ 0& 1+{\beta }_t^2& 0\\ -2{\beta }_t& 0& 1-{\beta }_t^2\end{array}\right]\text{\hspace{1em}(14)} \end{gathered}$$
将它带入等式(13)，我们将要得到仅仅依赖${\beta }_t$函数$E_{os}({\beta }_t)$，有
$$\frac{\partial E_{os}({\beta }_t)}{\partial {\beta }_t}=0$$
这是一个自由度为4的多项式，很容易求解。一般情况下，我们将要获得两个最小和两个最小。两个最小由极值点$\frac{{\partial }^2{E}_{os}({\beta }_t)}{{\partial }^2{\beta }_t}=0$获得。

IV Experimental result

reference

---

1. B. Wrobel, “Minimum solutions for orientation,” in Calibration and Orientation of Cameras in Computer Vision, Gruen and Huang, Eds. Springer-Verlag, 2001, ch. 2 ↩︎

2. C. Lu, G. Hager, and E. Mjolsness, “Fast and globally convergent pose estimation from video images,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 22, no. 6, pp. 610–622, June 2000 ↩︎ ↩︎ ↩︎ ↩︎ ↩︎

3. M. Fischler and R. Bolles, “The random sample consensus set: a paradigm for model fitting with applications to image analysis and automated cartography,” Communications of the ACM, vol. 24, no. 6, pp. 381–395, 1981 ↩︎

4. A. Ansar and K. Daniilidis, “Linear pose estimation from points or lines,” in European Conference on Computer Vision, A. H. et al., Ed., vol. 4. Copenhagen, Denmark: Springer, May 2002, pp. 282–296 ↩︎

5. P. Wunsch and G. Hirzinger, “Registration of cad-models to images by iterative inverse perspective matching,” Pattern Recognition. Proceedings of the 13th International Conference on, vol. 1, pp. 78–83, August 1996 ↩︎

6. H. Araujo, ´ R. Carceroni, , and C. Brown, “A fully projective formulation to improve the accuracy of lowe’s pose-estimation algorithm,” Computer Vision and Image Understanding, vol. 71, no. 2, pp. 227–238, 1998 ↩︎

7. D. Nister, O. Naroditsky, and J. Bergen, “Visual odometry,” IEEE Conference on Computer Vision and Pattern Recognition, vol. 1, pp. 652–659, 2004. ↩︎

8. C. Harris, “Geometry from visual motion,” in Active Vision, A. Blake and A. Yuille, Eds. MIT Press, 1992, ch. 1 ↩︎

9. H. Kato and M. Billinghurst, “Marker tracking and hmd calibration for a video-based augmented reality conferencing system,” (IWAR 99) Proceedings. 2nd IEEE and ACM International Workshop on Augmented Reality, pp. 85–94, 1999 ↩︎

10. S. Malik, G. Roth, and C. McDonald, “Robust 2d tracking for real-time augmented reality,” Proceedings. Vision Interface 2002, 2002 ↩︎

11. M. K. Chandraker, C. Stock, and A. Pinz, “Real-time camera pose in a room,” 3rd Int Conf. Comp. Vision Systems ICVS, pp. 98–110, 2003 ↩︎

12. T. Kawano, Y. Ban, and K. Uehara, “A coded visual marker for video tracking system based on structured image analysis,” (ISMAR03) Proceedings. 2nd IEEE and ACM International Symposium on Mixed and Augmented Reality, 2003 ↩︎

13. D. Oberkampf, D. F. DeMenthon, and L. S. Davis, “Iterative pose estimation using coplanar feature points,” Computer Vision and Image Understanding, vol. 63, no. 3, pp. 495–511, May 199 ↩︎