CN101718548A - 基于平面标志物的位姿处理方法 - Google Patents

Abstract

本发明公开了一种基于平面标志物的位姿处理方法，用于解决现有技术对于平面标志物的位姿处理正确率低的技术问题。本发明依据标志物检测和识别方法得到标志物的相关信息，以三维空间中的共线误差作为优化函数，由得到的2D和3D点之间的对应关系确定相机的位置和姿态，运用RPP算法获得使OI算法中的共线误差函数取得局部极值的另一解，比较得到的两个局部极值解，取共线误差较小的解作为最终处理结果。由于采用RPP算法，解决了姿态不确定性问题，将平面标志物的位姿处理正确率由现有技术的50％左右提高到95％以上。

Description

基于平面标志物的位姿处理方法

技术领域

本发明涉及一种位姿处理方法，特别是基于平面标志物的位姿处理方法。

背景技术

文献“Fast and globally convergent pose estimation from video images，IEEE Transactions onPattern Analysis And Machine Intelligence，2000，Vol.22(6)，p610-622.”公开了一种全局收敛的位姿估计方法，即正交迭代(OI)方法。该方法以目标空间中的共线误差作为优化函数，在每步的迭代过程中，首先估计相机坐标系到世界坐标系的旋转矩阵R，然后计算相应的平移向量t。这种迭代方法用基于奇异值(SVD)分解的方法解决了旋转矩阵的单位正交约束问题，而没有运用将其参数化为欧拉角的方法。但对于位姿跟踪系统中常用的平面标志物而言，该方法没有考虑到位姿的不确定问题。实际上，对于平面标志物，误差函数可能存在两个局部极小值，如果不考虑位姿的不确定问题，得到正确解的概率只有50％左右。

发明内容

为了克服现有技术对于平面标志物的位姿处理正确率低的不足，本发明提供一种基于平面标志物的位姿处理方法，采用RPP算法，解决了姿态不确定性问题，可以提高平面标志物的位姿处理正确率。

本发明解决其技术问题所采用的技术方案：一种基于平面标志物的位姿处理方法，其特点是包括下述步骤：

(a)选取ARToolkitplus提供的Simple ID标志物作为增强现实系统的平面标志物，实时处理过程中，依据标志物检测和识别方法得到标志物的相关信息，以三维空间中的共线误差作为优化函数，OI算法由得到的2D和3D点之间的对应关系确定相机的位置和姿态；

(b)由上述OI算法得到的初始值 ${\hat{P}}_1=({\hat{R}}_1,{\hat{t}}_1),$ 运用RPP算法获得使OI算法中的共线误差函数取得局部极值的另一解，比较得到的两个局部极值解，取共线误差较小的解作为最终处理结果；

(c)由 ${\tilde{v}}_i=R_t{\hat{v}}_i,$ ${\tilde{t}}_1=R_t{\hat{t}}_1,$ ${\tilde{R}}_1=R_t{\hat{R}}_1,$ 变换坐标系得到 ${\tilde{P}}_1=({\tilde{R}}_1,{\tilde{t}}_1);$

(d)根据 $E_{\mathrm{os}}(\tilde{R},\tilde{t})=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|(I-\frac{{\tilde{v}}_i{{\tilde{v}}_i}^t}{{{\tilde{v}}_i}^t{\tilde{v}}_i})(\tilde{R}{\tilde{R}}_z{\tilde{R}}_z^{-1}{p}_i+\tilde{t})\left|\right|}^2,$ ${\tilde{R}}_1{\tilde{R}}_z=R_z\left({\widetilde{\mathrm{\γ}}}_1\right)R_y\left({\widetilde{\mathrm{\β}}}_1\right)R_z\left({\widetilde{\mathrm{\α}}}_1\right)$ 以及 ${\tilde{v}}_i\∝R_z\left(\mathrm{\γ}\right)R_y\left(\mathrm{\β}\right){\tilde{p}}_i+\tilde{t}$ 计算

并得到

和；

(e)固定 $\mathrm{\γ}={\widetilde{\mathrm{\γ}}}_1,$ 计算与 $E_{\mathrm{os}}(\mathrm{\γ},\mathrm{\β},\tilde{t})=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|(I-\frac{{\tilde{v}}_i{\tilde{v}}_i^t}{{\tilde{v}}_i^t{\tilde{v}}_i})(R_z\left(\mathrm{\γ}\right)R_y\left(\mathrm{\β}\right)p_i+\tilde{t})\left|\right|}^2$ 式的所有局部极值相对应的参数β和

(f)对所有局部极值，做逆变换得到

(g)对所有的

运用位姿处理方法，得到最终的P_i ^*；

(h)选取使E_os最小的解作为处理结果。

本发明的有益效果是：由于采用RPP算法，解决了姿态不确定性问题，将平面标志物的位姿处理正确率由现有技术的50％左右提高到95％以上。

下面结合具体实施方式对本发明作详细说明。

具体实施方式

本发明具体方法步骤如下：

(1)初始值的获取。

采用ARToolkitplus中的Simple ID平面标志物，输入平面标志物的图像，由标志物检测和识别算法获得平面标志物的相关信息，例如，图像中标志物的四个顶点坐标。设标志物的四个顶点在相机坐标系中的坐标为q_i＝(x′_i，y_i′，z_i′)^t，i＝1....n，n＝4，而在世界坐标系中的坐标为p_i＝(x_i，y_i，z_i)^t，i＝1....n，n＝4，参考点p_i在相机单位成像平面上的齐次坐标为v_i＝(u_i，v_i，1)^t，世界坐标系到相机坐标系的旋转矩阵和平移向量分别为R和t，则有：

q_i＝Rp_i+t                                     (1)

$v_i=\frac{1}{r_3^t{p}_i+t_z}(R{p}_i+t)---\left(2\right)$

若设实际观测到的参考点在单位成像平面上的坐标为 ${\hat{v}}_i={\left({\hat{u}}_i{\hat{v}}_{i}1\right)}^t,$ 则三维空间中的共线

误差表述为下式：

$E(R,t)=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|e_i\left|\right|}^2=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|(I-{\hat{V}}_i)({\mathrm{Rp}}_i+t)\left|\right|}^2---\left(3\right)$

其中， ${\hat{V}}_i=\frac{{\hat{v}}_i{\hat{v}}_i^t}{{\hat{v}}_i^t{\hat{v}}_i}.$ 在给定旋转矩阵R的情况下，得到使目标函数最小的t的闭式解如下：

$t\left(R\right)=\frac{1}{n}{(I-\frac{1}{n}\underset{j}{\mathrm{\Σ}}{\hat{V}}_j)}^{-1}\underset{j}{\mathrm{\Σ}}({\hat{V}}_j-I){\mathrm{Rp}}_j---\left(4\right)$

将t写作R的函数t(R)，并令 $q_i\left(R\right)={\hat{V}}_i({\mathrm{Rp}}_i+t\left(R\right));$ 则(4)式改写为

$E\left(R\right)=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|e_i\left|\right|}^2=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|{\mathrm{Rp}}_i+t\left(R\right)-q_i\left(R\right)\left|\right|}^2---\left(5\right)$

迭代计算R，首先，假设第k次估计值分别为R^(k)，t^(k)＝t(R^(k))，q_i ^(k)＝R^(k)p_i+t^(k)，则R的k+1次估计值通过解下述的绝对定向问题得到：

$R^{(k+1)}=\arg {\min }_R\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|{\mathrm{Rp}}_i+t-{\hat{V}}_i{q_i}^{\left(k\right)}\left|\right|}^2=\arg {\max }_R\mathrm{tr}\left(R^{t}M\left(R^{\left(k\right)}\right)\right)---\left(6\right)$

进而t的k+1次估计值为t^(k+1)＝t(R^(k+1))，这样持续迭代下去最终得到旋转矩阵R^*，即 $R^{*}=\arg {\min }_R\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|{\mathrm{Rp}}_i+t-{\hat{V}}_i(R^{*}{p}_i+t\left(R^{*}\right))\left|\right|}^2.$

(2)另一局部极值及最终解的获取。

设C_C表示相机的光心，P_i和P_j为标志物平面上的参考点，C_M表示标志物坐标系中的原点，C_C到C_M的向量用t表示。将标志物沿y轴旋转α角，这时参考点变为P_iα和P_jα，投影到图像平面上的点在相机坐标系中的坐标分别为v_i和v_j，它们的实际观测值分别为

和

由OI算法，目标空间的误差函数写作：

$E_{\mathrm{os}}(R,t)=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|(I-\frac{{\tilde{v}}_i{\tilde{v}}_i^t}{{\tilde{v}}_i^t{\tilde{v}}_i})({\mathrm{Rp}}_i+t)\left|\right|}^2---\left(7\right)$

对于每一特定的α值，可能存在另一个导致误差函数E_os取得局部极小值的角β。虽然由于噪声α角和β角对应的E_os均大于0，但仍可认为正确解α对应的E_os值小于错误解β所对应的值。

为了得到正确解，选取从其它位姿估计算法得到的解

作为RPP算法的初始估计值。RPP算法的核心在于由该初始值

推出使误差函数E_os最小的另一解首先，假设目标坐标系到相机坐标系的变换分别为R和t，则

${\hat{v}}_i\≈v_i\∝{\mathrm{Rp}}_i+t---\left(8\right)$

不失一般性，在(8)式两边同乘以R_t：

$R_t{\hat{v}}_i\≈R_t{v}_i\∝{R_t\mathrm{Rp}}_i+R_{t}t---\left(9\right)$

使得 $R_t{\hat{t}}_1={\left[00\right|\left|{\hat{t}}_1\right|\left|\right]}^t,$ 这样坐标变换的结果是目标坐标系中的(0 0 0)^t将被投影到图像空间中的

令

${\tilde{v}}_i=R_t{\hat{v}}_i,{\tilde{t}}_1=R_t{\hat{t}}_1,{\tilde{R}}_1=R_t{\hat{R}}_1---\left(10\right)$

$E_{\mathrm{os}}(\tilde{R},\tilde{t})=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|(I-\frac{{\tilde{v}}_i{{\tilde{v}}_i}^t}{{{\tilde{v}}_i}^t{\tilde{v}}_i})(\tilde{R}{p}_i+\tilde{t})\left|\right|}^2---\left(11\right)$

这样

使误差函数

最小，不失一般性引入旋转矩阵

这样

$E_{\mathrm{os}}(\tilde{R},\tilde{t})=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|(I-\frac{{\tilde{v}}_i{{\tilde{v}}_i}^t}{{{\tilde{v}}_i}^t{\tilde{v}}_i})(\tilde{R}{\tilde{R}}_z{\tilde{R}}_z^{-1}{p}_i+\tilde{t})\left|\right|}^2---\left(12\right)$

旋转矩阵

仅绕着z轴旋转平面标志物，所以 ${\tilde{p}}_i={\tilde{R}}_z^{-1}{p}_i$ 仍在z＝0的平面上。旋转矩阵

分解为三个旋转矩阵的乘积：

${\tilde{R}}_1{\tilde{R}}_z{=R}_z\left({\widetilde{\mathrm{\γ}}}_1\right)R_y\left({\widetilde{\mathrm{\β}}}_1\right)R_z\left({\widetilde{\mathrm{\α}}}_1\right)--\left(13\right)$

这里，R_i(φ)表示绕i轴旋转φ角。选取适当的使得 ${\widetilde{\mathrm{\α}}}_1=0,$ 这样，

${\tilde{v}}_i\∝R_z\left(\mathrm{\γ}\right)R_y\left(\mathrm{\β}\right){\tilde{p}}_i+\tilde{t}---\left(14\right)$

相应的误差函数为

$E_{\mathrm{os}}(\mathrm{\γ},\mathrm{\β},\tilde{t})=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|(I-\frac{{\tilde{v}}_i{\tilde{v}}_i^t}{{\tilde{v}}_i^t{\tilde{v}}_i}(R_z\left(\mathrm{\γ}\right)R_y\left(\mathrm{\β}\right)p_i+\tilde{t}))\left|\right|}^2---\left(15\right)$

由于 ${\tilde{t}}_1={\left[00\right|\left|{\hat{t}}_1\right|\left|\right]}^t,$ $R_z\left(\mathrm{\γ}\right){\tilde{t}}_1={\tilde{t}}_1,$ 即旋转矩阵R_z(γ)是一个绕相机光轴旋转的矩阵，该矩阵并不影响成像平面和目标平面之间的几何关系。因而可以寻求目标函数的最小值时只需考虑β和

综上所述，RPP算法归纳如下：

运用一般的位姿估计算法得到一个解 ${\hat{P}}_1=({\hat{R}}_1,{\hat{t}}_1).$

是E_os的一个局部极值解，RPP算法由此推导出另一个使E_os取得局部极值的解，如果E_os存在两个极值的话；

根据(10)式变换坐标系得到 ${\tilde{P}}_1=({\tilde{R}}_1,{\tilde{t}}_1);$

根据(12)到(14)式计算

并得到

和；

固定 $\mathrm{\γ}={\widetilde{\mathrm{\γ}}}_1,$ 计算与式(15)的所有局部极值相对应的参数β和

对所有局部极值，做逆变换得到

对所有的运用普通的位姿估计算法，例如OI算法得到最终的P_i ^*；

选取使E_os最小的解作为最终的解。

为了测试本发明方法的鲁棒性，随机生成1000组数据，并添加不同的噪声以仿真真实的图像，统计结果表明算法的鲁棒性很强，位姿处理的正确率达95％以上。

Claims (1)

1.一种基于平面标志物的位姿处理方法，其特征在于包括下述步骤：

(a)选取ARToolkitplus提供的Simple ID标志物作为增强现实系统的平面标志物，实时处理过程中，依据标志物检测和识别方法得到标志物的相关信息，以三维空间中的共线误差作为优化函数，OI算法由得到的2D和3D点之间的对应关系确定相机的位置和姿态；

(b)由上述OI算法得到的初始值 ${\hat{P}}_1=({\hat{R}}_1,{\hat{t}}_1),$ 运用RPP算法获得使OI算法中的共线误差函数取得局部极值的另一解，比较得到的两个局部极值解，取共线误差较小的解作为最终处理结果；

(c)由 ${\tilde{v}}_i=R_t{\hat{v}}_i,$ ${\tilde{t}}_1=R_t{\hat{t}}_1,$ ${\tilde{R}}_1=R_t{\hat{R}}_1$ 变换坐标系得到 ${\tilde{P}}_1=({\tilde{R}}_1,{\tilde{t}}_1);$

(d)根据 $E_{\mathrm{os}}(\tilde{R},\tilde{t})=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|(I-\frac{{\tilde{v}}_i{\tilde{v}}_i^t}{{\tilde{v}}_i^t{\tilde{v}}_i})(\tilde{R}{\tilde{R}}_z{\tilde{R}}_z^{-1}{p}_i+\tilde{t})\left|\right|}^2,$ ${\tilde{R}}_1{\tilde{R}}_z=R_z\left({\widetilde{\mathrm{\γ}}}_1\right)R_y\left({\widetilde{\mathrm{\β}}}_1\right)R_z\left({\widetilde{\mathrm{\α}}}_1\right)$ 以及 ${\tilde{v}}_i\∝R_z\left(\mathrm{\γ}\right)R_y\left(\mathrm{\β}\right){\tilde{p}}_i+\tilde{t}$ 计算

并得到和

(e)固定 $\mathrm{\γ}={\widetilde{\mathrm{\γ}}}_1,$ 计算与 $E_{\mathrm{os}}(\mathrm{\γ},\mathrm{\β},\tilde{t})=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|(I-\frac{{\tilde{v}}_i{\tilde{v}}_i^t}{{\tilde{v}}_i^t{\tilde{v}}_i})(R_z\left(\mathrm{\γ}\right)R_y\left(\mathrm{\β}\right)p_i+\tilde{t})\left|\right|}^2$ 式的所有局部极值相对应的参数β和

(f)对所有局部极值，做逆变换得到

(g)对所有的

运用位姿处理方法，得到最终的P_i ^*；

(h)选取使E_os最小的解作为处理结果。