本发明属于计算机视觉与计算机图形学领域，具体涉及一种快速鲁棒的摄像机绝对姿态估计方法。
背景技术：
摄像机绝对姿态估计的目的是计算摄像机的外部参数，在计算机视觉和图形学领域具有广泛的应用，例如三维重建、虚拟现实、增强现实、运动推断结构、实时定位与地图构建和无人驾驶等。由于摄像机的种类繁多，导致出现数据的多元化问题，导致现有算法求解出的相机姿态信息的精度较低。例如论文《accuratenon-iterativeo(n)solutiontothepnpproblem》中提出的摄像机姿态估计方法依赖于深度信息，在深度信息不可用的情况下，此方法不能够求解出摄像机姿态信息(moreno-noguerf，lepetitv，fuap.accuratenon-iterativeo(n)solutiontothepnpproblem.proceedings/ieeeinternationalconferenceoncomputervision.ieeeinternationalconferenceoncomputervision，2007.)。
论文《pnpproblemrevisited》提出的方法仅针对与三个非共面点情况下的摄像机姿态求解方法，对二维点和三维点的数量有严格的要求，然而在实践中这些限制条件是难以满足的(wuy，huz.pnpproblemrevisited.journalofmathematicalimagingandvision，2006，24(1)：131-141.)。论文《aversatileapproachforsolvingpnp，pnpf，andpnpfrproblems》中的方法针对不同的应用场景需要不同的二维点与三维点数量；例如，当求解已标定的摄像机姿态时，需要3个二维与三维点对；当求解未标定摄像机姿态时，至少需要5个二维与三维点对，不具有通用性(nakanog.aversatileapproachforsolvingpnp，pnpf，andpnpfrproblems.europeanconferenceoncomputervision.springer，cham，2016.)。
因此，根据现有技术分析，现有技术在估计摄像机的姿态信息时，存在着对深度信息依赖较大，通用性差，并且对给定的二维点和三维点信息要求严格，实践中难以满足需求等缺点，因此很难快速估计出可变焦距情况下精确的摄像机外部姿态信息。
技术实现要素：
技术问题：本发明提供一种快速鲁棒的摄像机绝对姿态估计方法，该方法能够在噪声数据情况下，快速精确地求解出摄像机的绝对姿态信息，同时避免三维点的深度变化的影响，通用性和鲁棒性较强。
技术方案：本发明一种快速鲁棒的摄像机绝对姿态估计方法，包括以下步骤：
s1：确定应用场景的二维点、三维点以及所采用的摄像机参数，其中的二维点是指图像上的特征点，三维点是指与所述二维点对应的世界坐标系中的点，摄相机参数包括摄像机的焦距、主点和畸变系数；
s2：对于给定的二维点、三维点和摄像机参数，根据摄像机的成像原理，建立摄像机的投影方程；
s3：将摄像机的焦距代入步骤s2中建立的摄像机的投影方程，得到简化的摄像机的投影方程；
s4：将给定的二维点和三维点代入步骤s3中的简化后的摄像机投影方程中，计算得出简化后的摄像机投影方程的系数矩阵；
s5：对步骤s4得到的系数矩阵进行分解，求解得出初始的摄像机绝对姿态信息；
s6：对步骤s5得到的初始的摄像机绝对姿态信息进行优化，得到精确的摄像机绝对姿态信息。
进一步地，所述步骤s2中，在核空间下建立摄像机的投影方程。
进一步地，所述步骤s2中，所建立的摄像机的投影方程为：
进一步地，所述步骤s3中，简化的摄像机投影方程为：
u1f(r11mx r12my r13mz t1) u2f(r21mx r22my r23mz t2)u3f(r31mx r32my r33mz t3)＝0
和
v1f(r11mx r12my r13mz t1) v2f(r11mx r12my r13mz t2)v3f(r11mx r12my r13mz t3)＝0
其中，u1、u2、u3、v1、v2、v3为连接摄像机中心与二维点mi形成的向量在核空间的坐标，r11、r12、r13、r21、r22、r23、r31、r32、r33表示旋转矩阵中的元素，t1、t2、t3表示平移向量中的元素，mx、my、mz表示三维点mi的坐标。
进一步地，所述步骤s5在对系数矩阵进行分解时采用奇异值分解法，将简化后的摄像机投影方程的系数矩阵的增广矩阵进行分解，从而计算出初始的摄像机绝对姿态信息，所述绝对姿态信息包括摄像机的平移向量和旋转矩阵。
进一步地，所述旋转矩阵根据三维点的共面和非共面两种情况分别计算，对于三维点共面情况下，摄像机旋转矩阵的计算方法为：
进一步地，所述步骤s6中，采用非线性优化方法对初始的摄像机绝对姿态信息进行优化，得到精确的摄像机绝对姿态信息，所述非线性计算方法为：
进一步地，所述ri(x)的计算方法为：ri(x)＝mi-k[ri|ti]mi，其中ri表示旋转矩阵、ti表示平移向量、mi表示三维点、mi表示二维点、k表示摄像机的内部参数矩阵。
有益效果：本发明与现有技术相比，具有以下优点：
(1)本发明所提出的快速鲁棒的摄像机绝对姿态估计方法，在核空间内建立摄像机的投影方程，求解摄像机的姿态信息最少只需要6组二维和三维的点对即可，因此减少了求解摄像机绝对姿态所需要的二维点与三维点数目，因此只需从噪声数据中提取少量的数据点，即能完成绝对姿态估计，从而极大地提高了算法的时间效率。
(2)本发明所提出的快速鲁棒的摄像机绝对姿态估计方法，在摄像机焦距已知的情况下，能够消除摄像机投影方程中三维点的深度信息，从而等够得到简化的摄像机投影方程。因此避免了在实际工程应用时过程中，当无法获得深度信息时，摄像机姿态信息求解失败的情况，而本发明的方法不依赖深度信息，即能有效地估计出摄像机的姿态信息，从而，能够适用具体实践中无法获取深度信息的场景，具有较高的通用性和鲁棒性。
(3)本发明所提出的快速鲁棒的摄像机绝对姿态估计方法，采用奇异值分解方法计算出摄像机的初始姿态信息，同时采用非线性优化方法计算出精确的摄像机绝对姿态信息；并结合在核空间建立的摄像机投影方程，既能提高时间效率又能增强算法的鲁棒性，从而使得本发明的方法能够快速准确地估计出摄像机的绝对姿态，促进了本发明的方法在实践中的应用。
附图说明
图1为本发明一种快速鲁棒的摄像机绝对姿态估计方法的原理图；
图2为采用本发明的方法进行三维重建时与现有方法的对比结果图。
具体实施方式
下面结合实施例和说明书附图对本发明作进一步的说明。
本发明提供一种快速鲁棒的摄像机绝对姿态估计方法，通过该方法可以快速的求解出噪声数据情况下精确的摄像机绝对姿态信息，从而促进摄像机绝对姿态估计方法在实践中的应用，其应用场景包括：三维重建、虚拟现实、增强现实、运动推断结构、实时定位与地图构建和无人驾驶等。
如图1所示，为本发明一种快速鲁棒的摄像机绝对姿态估计方法的原理图，该方法的步骤包括：
s1：确定应用场景的二维点、三维点以及摄像机参数。
其中的二维点是指图像上的特征点，三维点是指与所述二维点对应的世界坐标系中的点，在实践中，要给定二维点和对应三维点的数量，在本发明的一个实施例中，仅需要6个二维点和对应的三维点，二维点和三维点是一一对应的，当然，也可以更多。对于摄相机参数，需要给出摄像机的焦距、主点和畸变系数。
s2：对于给定的二维点、三维点和摄像机参数，根据摄像机的成像原理，建立摄像机的投影方程。
对于给定的二维点和三维点集合(m，m)，其中m表示二维点，m表示三维点，摄像机绝对姿态估计的目的是寻找相应的旋转矩阵r和平移向量t，使得能够将世界坐标系下的三维点m映射到图像坐标系下的二维点m，例如图1所示三维点mi被投影为图像坐标系下的二维点mi，其中i是序号。因此，根据摄像机成像原理可知，mi和mi之间的投影关系可被定义为：
λimi＝k(rmi t)(1)
其中，λi表示每个三维点的深度；k表示摄像机的内部参数矩阵，在不考虑扭曲参数的情况下，k可以表示为：
其中，f表示摄像机的焦距。
由图1可知，对于给定的二维点mi，连接摄像机的中心与mi可以形成向量vi，向量vi的核空间是一个二维的坐标系统，可以表示为u和v。在图1的左上角，展示了向量vi与其核空间的关系，即vi垂直于u和v，因此，向量vi的核空间的计算方法如式(3)所示。
其中，u1、u2、u3、v1、v2、v3为连接摄像机中心与二维点mi形成的向量在核空间的坐标，null(g)表示函数，g表示变量，用于计算向量
为了便于描述问题，定义向量vi与它的核空间kernel(vi)之间的运算关系，如式(4)所示。
由于kernel(vi)tvi的值接近于零，因此，可以获得式(5)所示的等价关系。
在图1中，假设摄像机在坐标原点，在数值上向量vi的模长与mi相等，因此，将mi带入式(5)可以获得式(6)。
同理，可以获得mi的核空间，计算方法如式(7)所示。
将式(7)代入式(6)可以获得式(8)。
此外，当λi≠0时，由式(1)可以获得式(9)。
所得的式(9)即为建立的摄影机的投影方程。根据上述过程，本发明中的摄像机的投影方程是在核空间内建立的，能够减少所需的二维点和三维点的数目，因为所需的二维点和三维点的数目少，因此只需从噪声数据中提取少量的数据点，即能完成绝对姿态估计，从而避免了复杂的运算，从而有效地提高了运算速度，极大地提高了算法的时间效率。
s3：将摄像机焦距代入步骤s2中建立的摄像机的投影方程，消除三维点的深度信息，得到简化的摄像机的投影方程。
将式(9)代入式(8)可以获得式(10)。
记r表示旋转矩阵，
其中r11、r12、r13、r21、r22、r23、r31、r32、r33表示旋转矩阵中的元素。
记t表示平移向量，则
t＝[t1t2t3]t(12)
其中t1、t2、t3表示平移向量中的元素。
将式(11)和(12)代入式(10)并展开，可以获得两个独立的方程，分别如式(13)和(14)所示。
mx、my、mz表示三维点mi的坐标，式(13)和(14)即为简化后的摄像机的投影方程。
本发明中，摄像机的焦距是已知的，从而消除了摄像机投影方程中三维点的深度信息，得到简化的摄像机投影方程，使得本发明的方法不依赖深度信息，即能有效地估计出摄像机的姿态信息，从而避免了在深度信息无法获取时，出现摄像机姿态信息求解失败的情况，因此能够适用具体实践中无法获取深度信息的场景，具有较高的适用性和鲁棒性。
s4：将给定的二维点和三维点代入步骤s3中的简化后的摄像机投影方程，计算得出简化后的摄像机投影方程的系数矩阵。
为了计算摄像机绝对姿态信息，采用向量x表示所求未知参数的值，如式(15)所示。
x＝[r11r12…r33t1t2t3](15)
对于给定的n(大于或等于6)个二维点和三维点对，由式(13)和(14)可以获得如式(16)所示的齐次方程，
ax＝0(16)
其中，a是2n×12的系数矩阵。
为了获得方程ax＝0的最优解，结合摄像机绝对姿态信息的固有属性，采用最优化方法表示姿态信息的求解过程，如式(17)所示。
其中，f表示矩阵的frobenius范数。
s5：对步骤s4得到的系数矩阵进行分解，求解得出初始的摄像机绝对姿态信息。
对式(17)的最直接求解方法是奇异值分解，因此，为了处理矩阵的过约束问题，令p＝ata，然后对矩阵p进行奇异值分解，计算方法如式(18)所示。
p＝udvt(18)
其中u、d、vt为对称矩阵，根据奇异值分解原理可知，未知量x的值是矩阵p的最小奇异值所对应的奇异向量vit。以列为单位，将vit的前9个元素设置为摄像机的旋转矩阵，即可获得摄像机旋转矩阵的初值r，结果如式(19)所示，
其中
将向量vit的最后3个元素作为摄像机平移向量的初值
其中
为了避免摄像机旋转尺度变化对平移向量的影响，需要对预先计算的摄像机平移向量
其中，
由于式(17)中的约束条件要求||x||＝1，因此，9个参数不能够确定一个正确的旋转矩阵。为了获得正确的摄像机旋转矩阵，需要对r再次进行奇异值分解，计算方法如式(22)所示。
其中，ur、dr、
然而，对于三维点共面条件下的摄像机旋转矩阵的计算方法如式(24)所示。
至此，采用奇异值分解方法能够有效的在核空间内为给定的二维点和三维点集(m，m)计算出摄像机绝对姿态的初始信息，然而初始摄像机姿态还需进一步优化才能获得更为精确的结果。
s6：对步骤s5得到的初始的摄像机绝对姿态信息进行优化，得到精确的摄像机绝对姿态信息。
为了获得精确的摄像机姿态信息，需要对采用奇异值分解获得的初始姿态做进一步优化。然而，选取合适的优化方法对算法的求解效率和精度都会产生至关重要的影响。在保持二维点和三维点坐标不变的情况下，优化摄像机的外部参数r和t，使得反投影误差最小。假设对于每一个二维点和三维点对(mi，mi)具有相同的绝对姿态信息，即旋转矩阵ri＝r和ti＝t，因此，可以建立反投影误差方程，如式(25)所示。
其中，p表示目标函数的定义域，r(x)表示由二维点mi与三维点mi的反投影之前的误差ri(x)所构成的残差矩阵；ri(x)的计算方法如式(26)所示。
ri(x)＝mi-k[ri|ti]mi(26)
根据泰勒展开原理将式(25)展开可以获得如式(27)所示的雅克比方程。
(jtj λdiag(jtj))δi＝-jtri(27)
其中，j表示包含摄像机旋转矩阵ri、平移向量ti和三维点mi的雅克比矩阵，δi表示增量因子。
由于优化对象是残差矩阵，因此，为了便于问题描述，将式(27)改为矩阵形式，如式(28)所示。
其中，u和v是分块对角矩阵；
由式(28)可知，绝对姿态优化的目标是获得摄像机矩阵的残差信息δpi，因此，需求计算式(3.28)所表示的矩阵方程组的解。在式(28)两边同时乘以分块矩阵a，
其中，i为单位矩阵，o为零矩阵，可以获得式(30)所示的矩阵方程，
展开式(30)可以获得式(31)所示的关于摄像机姿态信息的矩阵方程。
(u-wv-1wt)δpi＝rpi-wv-1rmi(31)
由于(u-wv-1wt)是分块对称矩阵，因此，采用快速的矩阵分解算法求解式(31)的解δp，
δp＝{δp1，…，δpn}(32)
其中，δpj＝[rjtj]t，j为序号。
为了获得精确的摄像机姿态信息，采用矩阵的l2范数计算每个旋转矩阵rj和平移向量tj的权值，最终获得的摄像机旋转矩阵r和平移向量t如式(33)所示。
至此，通过非线性优化技术对直接计算获得的摄像机姿态进行优化，即可获得最精确的摄像机姿态信息，同时具有较高的时间效率。
由此，本发明的方法，通过在核空间中建立摄像机的投影方程，同时采用奇异值分解法计算摄像机的初始姿态矩阵，并采用非线性优化方法最终计算出精确的摄像机绝对姿态信息，使得本发明的方法既具有较高的时间效率，并且具有较强的鲁棒性，从而能够快速鲁棒地估计出摄像机的绝对姿态信息，从而更适用于三维系统的重建。
将本发明的摄像机姿态估计方法应用在三维重建中，并与采用论文《accuratenon-iterativeo(n)solutiontothepnpproblem》中的方法所获得的结果进行对比，比较结果如图2所示。图2中，第一行为输入的图像序列，第二行的左侧的第一个子图是使用本发明的摄像机姿态求解方法获得的结果，可以看出由于能够本专利的方法能够正确的计算出摄像机姿态信息，所以重建出的三维点云模型与真实场景具有较高的几何一致性。右侧的结果是采用论文《accuratenon-iterativeo(n)solutiontothepnpproblem》中的方法所获得的结果，可以看出由于此论文中描述的方法在深度信息获取失败时，导致在三维重建应用中获得的三维模型与真实场景的几何外形具有较大差异。
上述实施例仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和等同替换，这些对本发明权利要求进行改进和等同替换后的技术方案，均落入本发明的保护范围。
1.一种快速鲁棒的摄像机绝对姿态估计方法，其特征在于，包括以下步骤：
s1：确定应用场景的二维点、三维点以及所采用的摄像机参数，其中的二维点是指图像上的特征点，三维点是指与所述二维点对应的世界坐标系中的点，摄相机参数包括摄像机的焦距、主点和畸变系数；
s2：对于给定的二维点、三维点和摄像机参数，根据摄像机的成像原理，建立摄像机的投影方程；
s3：将摄像机的焦距代入步骤s2中建立的摄像机的投影方程，得到简化的摄像机的投影方程；
s4：将给定的二维点和三维点代入步骤s3中的简化后的摄像机投影方程中，计算得出简化后的摄像机投影方程的系数矩阵；
s5：对步骤s4得到的系数矩阵进行分解，求解得出初始的摄像机绝对姿态信息；
s6：对步骤s5得到的初始的摄像机绝对姿态信息进行优化，得到精确的摄像机绝对姿态信息。
2.根据权利要求1所述的一种快速鲁棒的摄像机绝对姿态估计方法，其特征在于，所述步骤s2中，在核空间下建立摄像机的投影方程。
3.根据权利要求2所述的一种快速鲁棒的摄像机绝对姿态估计方法，其特征在于，所述步骤s2中，所建立的摄像机的投影方程为：
4.根据权利要求1所述的一种快速鲁棒的摄像机绝对姿态估计方法，其特征在于，所述步骤s3中，简化的摄像机投影方程为：
u1f(r11mx r12my r13mz t1) u2f(r21mx r22my r23mz t2)
u3f(r31mx r32my r33mz t3)＝0
和
v1f(r11mx r12my r13mz t1) v2f(r11mx r12my r13mz t2)
v3f(r11mx r12my r13mz t3)＝0
其中，u1、u2、u3、v1、v2、v3为连接摄像机中心与二维点mi形成的向量在核空间的坐标，r11、r12、r13、r21、r22、r23、r31、r32、r33表示旋转矩阵中的元素，t1、t2、t3表示平移向量中的元素，mx、my、mz表示三维点mi的坐标。
5.根据权利要求1所述的一种快速鲁棒的摄像机绝对姿态估计方法，其特征在于，所述步骤s5在对系数矩阵进行分解时采用奇异值分解法，将简化后的摄像机投影方程的系数矩阵的增广矩阵进行分解，从而计算出初始的摄像机绝对姿态信息，所述绝对姿态信息包括摄像机的平移向量和旋转矩阵。
6.根据权利要求5所述的一种快速鲁棒的摄像机绝对姿态估计方法，其特征在于，所述旋转矩阵根据三维点的共面和非共面两种情况分别计算，对于三维点共面情况下，摄像机旋转矩阵的计算方法为：
7.根据权利要求1～6任一项所述的一种快速鲁棒的摄像机绝对姿态估计方法，其特征在于，所述步骤s6中，采用非线性优化方法对初始的摄像机绝对姿态信息进行优化，得到精确的摄像机绝对姿态信息，所述非线性计算方法为：
8.根据权利要求7所述的一种快速鲁棒的摄像机绝对姿态估计方法，其特征在于，所述ri(x)的计算方法为：ri(x)＝mi-k[ri|ti]mi，其中ri表示旋转矩阵、ti表示平移向量、mi表示三维点、mi表示二维点、k表示摄像机的内部参数矩阵。
技术总结