一种使用少数共面点精确定位多摄像头系统位姿的方法

摘要

本发明提供一种使用少数共面点精确定位多摄像头系统位姿的方法，它可描述为以下技术方案：多摄像头系统由N个相对位姿关系固定的摄像头组成，N为2或大于2的自然数，其中每一个摄像头具有唯一的编号；在多摄像头系统中定义第j个摄像头的内参矩阵为Kj。特殊的3D点在第j个摄像头上的图像记为mj＝[uj vj]T。可以由已知的摄像头间的刚性转换定位每个摄像头的Rj，tj。本发明提出了一种利用单应性的方法提出了一种新的多摄像头定位方法，能够使用3D共面参考点映射图像点精确定位摄像头的位置。相对于原有方法，本发明提出的方法更加精确，更具鲁棒性，可用于对视角有要求、共面点很少的情况。

说明书

技术领域

本发明涉及多摄像头系统中定位所有摄像头位姿的方法，尤其是涉及一种利用单应性使用少数共面点的精确定位方法。

背景技术

当前通用的位姿定位理论和方法：

1、高斯牛顿(Gauss-Newton)方法：

Gauss-Newton方法是用来解决最小二乘问题的经典解法。它通过在当前估计值用一节泰勒级数迭代线性化共线性方程来实现。如果初始估计值比较好，那么很快就可以收敛得到精确解。然而当当前解与实际的精确值差距较大，那么这种方法收敛较慢并很有可能不收敛。

2、Levenberg-Marquardt方法：

Levenberg-Marquardt方法则可以被看做是最速下降法和Gauss-Newton方法的叠加。当当前的解和精确解差距较大时候，这个方法比较慢，但是保证收敛，表现为最速下降法；当当前解接近于真实解时候，它表现为Gauss-Newton方法。Levenberg-Marquardt方法已经成为了非线性最小二乘问题的一种标准解法，在计算机视觉和计算机图形学的领域内被广泛使用。

3、正交迭代(Orthogonal Iteration)算法：

正交迭代算法采用迭代求解方法，具有迭代次数少，计算精度高，计算速度快且全局收敛的特点，可用于实时位姿估计。算法达到了与Levenberg-Marquardt方法相当的精度，并提高了计算效率，受初始影响较小，对错误数据的鲁棒性更高。Chien-Ping Lu，Gregory D.Hager，Eric Mjolsness.Fast and Globally Convergent Pose Estimation from Video Images.IEEETransactions on Pattern Analysis and Machine Intelligence，2000，22(6)：610～622

大多数存在的方法试图通过最优化或者迭代来直接解出方向和平移。

发明内容

本发明所要解决的技术问题是提供一种使用少数共面点精确定位多摄像头系统位姿的方法，能够对于多摄像系统内的摄像头的定位更加精确，更具鲁棒性。

为此本发明采用以下技术方案：

所述多摄像头系统由N个相对位姿关系固定的摄像头组成，N为2或大于2的自然数，其中每一个摄像头具有唯一的编号；其特征在于包括以下步骤：

1)、在多摄像头系统中的N个摄像头中选择一个摄像头作为参照，该摄像头的编号为i，使用张正友标定方法测量多摄像头系统内其它摄像头相对摄像头i的位姿关系，所述位姿关系包括：

a)、平移向量t＝[X，Y，Z]，X、Y、Z分别表示X轴，Y轴和Z轴的偏移量，b)、旋转矩阵R，R为3×3矩阵，表示两个摄像头之间的旋转关系；

2)、在参考平面上放置n个标记点，n为2或大于2的自然数，并测量标记点之间的二维坐标值，以垂直参考平面的方向为Z轴，且该参考平面为Z＝1建立坐标系，指定其中一个标记点为参考原点，得到标记点的坐标M％＝[X，Y，1]^T；

3)、用图像分割方法识别出步骤2)中所放置的标记点在每一个摄像头中成像的图像坐标值，并对每一个摄像头建立得到步骤2)中所放置的标记点的坐标值与其在摄像头上的成像点坐标值之间的一一对应关系；

4)、通过标记点坐标M％＝[X，Y，1]^T和其在摄像头i上的成像点m％＝[u，v，1]^T，计算摄像头i的单应矩阵H，H为3×3矩阵，sm％＝HM％，m％表示点M％在摄像头i上的成像点坐标值，s为一实常数，表示比例因子；通过多个成像点联立方程组解得s和H；

5)、建立摄像头i的单应矩阵与多摄像头系统中其它摄像头的单应矩阵之间的一对一对应关系，它包括：

测量所述被选择的摄像头i的单应矩阵H和其它摄像头相对于摄像头i的平移向量t和旋转矩阵R，

根据所述摄像头i的单应矩阵H和其它摄像头相对于摄像头i的平移向量t和旋转矩阵R，将其它摄像头的单应矩阵表示为与所述被选择的摄像头i的单应矩阵H和t，R相关的等式；

6)、采用非线性优化方法优化摄像头i的单应矩阵H，使得所有摄像头实际获得的成像点坐标值和通过优化后的单应矩阵H’计算出来的像点坐标值的欧氏距离误差最小；

7)、通过优化后的各摄像头的单应矩阵，计算每一个摄像头的位姿参数，所述位姿关系参数包括：旋转矩阵R、平移向量t。

本发明所解决问题的数学化描述为：

在多摄像头系统中定义第j个摄像头的内参矩阵为K_j。特殊的3D点P＝[X，Y，Z]^T在第j个摄像头上的图像记为m^j＝[u^j v^j]^T。摄像头的绝对6DOF(自由度，三个旋转自由度和三个平移自由度)记为[R_j t_j]_3×4，由于多摄像头系统是刚性的所以这些可以预先精确的定标。摄像头i和j之间的转换可以计算如下：

$\left(\begin{array}{cc}{R}_i& t_i\\ 0^T& 1\end{array}\right)=\left(\begin{array}{cc}{R}_{\mathrm{ij}}& t_{\mathrm{ij}}\\ 0^T& 1\end{array}\right)\left(\begin{array}{cc}{R}_j& t_j\\ 0^T& 1\end{array}\right)$

任务是来测量多摄像头系统的绝对位姿，可以描述如下：一组共面不共线的3D参考点坐标，P_i，i＝1，…，n，n≥3，在多摄像头系统中的相关图像记为i＝1，…，n，n≥3，且j＝1，…，k，k≥2，为摄像头数。基于以上的相关点，可以由已知的摄像头间的刚性转换定位每个摄像头的R_j，t_j，[R_ij t_ij]，i，j＝1，…，k。

由于采用本发明的技术方案，与背景技术直接的解出方向和平移不同，本发明提出了一种利用单应性的方法提出了一种新的多摄像头定位方法，能够使用3D共面参考点映射图像点精确定位摄像头的位置。相对于原有方法，本发明提出的方法更加精确，更具鲁棒性，可用于对视角有要求、共面点很少的情况。

附图说明

图1是本发明方法在两个摄像头情况下的示例说明图

图2是本发明方法的步骤流程图

具体实施方式

下面结合图示附图和实例对本发明进一步说明。

本发明公开了一种多摄像头系统中定位所有摄像头位姿的新方法，其中摄像头均刚性放置，只有极少同时共面的点。在本发明中，首先建立了每个摄像头之间欧式几何的相互关系，以此建立多个摄像头的单应矩阵相对其中某一个摄像头C的单应矩阵关系，优化获得摄像头C单应矩阵，再计算其它摄像头的单应矩阵，然后从每一个摄像头对应的单应矩阵中解出对应摄像头的方向旋转矩阵R和平移向量T。

首先介绍下单应矩阵：

参考点的齐次坐标和摄像头j的相关图像点的齐次坐标为有一个射影相关，且这里3D相关点是共面的，所以可以假设Z＝0，这样有：

$s\tilde{m}=K_j\left(\begin{array}{cc}{R}_j& t_j\end{array}\right),\tilde{M}=K_j\left(\begin{array}{cccc}{r}_{1j}& r_{2j}& r_{3j}& t_j\end{array}\right),\tilde{M}=K_j\left(\begin{array}{ccc}{r}_{1j}& r_{2j}& t_j\end{array}\right){\left(\begin{array}{ccc}X& Y& 1\end{array}\right)}^T---\left(1\right)$

这里，s是一个任意的尺度因子，K_j是第j个摄像头的内参矩阵，R_j和t_j是相关摄像头j的旋转和平移。在接下来，仍使用代表M的齐次坐标，但

$H_j={\lambda }_j{K}_j\left(\begin{array}{ccc}{r}_{1j}& r_{2j}& t_j\end{array}\right)\mathrm{with}{s}_j\tilde{m}=H_j\tilde{M}---\left(2\right)$

H是一个3×3矩阵，同样定义了一个尺度因子s_j。为了计算的方便，这里由H(3，3)＝1定义尺度因子。H_j＝[h_1j h_2j h_3j]，然后从方程(2)，可以计算旋转和平移如下：

r_1j＝λ_jK_j^-1h_1j

r_2j＝λ_jK_j^-1h_2j

                                                   (3)

r_3j＝r_1j×r_2j

t_j＝λ_jK_j^-1h_3j

且λ_j＝1/‖K_j^-1h_1j‖＝1/‖K_j^-1h_2j‖，这样待测量摄像头6DOF的目标可以转换为测量相关摄像头的单应性。

以两个摄像头为例，示意图如图1所示，多个摄像头的方法以此类推。

在两摄像头系统中定义第i个摄像头的内参矩阵为K_j。特殊的3D点M＝[X Y Z]^T，第i个点记为M_i，位置如101所示，在第j个摄像头上的图像记为m^j＝[u^j v^j]^T，如102和103所示，两个摄像头的图像平面104和105分别有自己的坐标系106和107。摄像头的绝对6DOF(自由度，三个旋转自由度和三个平移自由度)记为[R_j t_j]_3×4，由于多摄像头系统是刚性的所以这些可以预先精确的定标。摄像头i和j之间的转换可以计算如下：

$\left(\begin{array}{cc}{R}_i& t_i\\ 0^T& 1\end{array}\right)=\left(\begin{array}{cc}{R}_{\mathrm{ij}}& t_{\mathrm{ij}}\\ 0^T& 1\end{array}\right)\left(\begin{array}{cc}{R}_j& t_j\\ 0^T& 1\end{array}\right)$

对于测量多摄像头系统的绝对位姿的任务，可以描述如下：一组共面不共线的3D参考点坐标，M_i，i＝1，…，n，n≥3，在多摄像头系统中的相关图像记为i＝1，…，n，n≥3，且j＝1，…，k，k＝2，为摄像头数。基于以上的相关点，可以由已知的摄像头间的刚性转换测量计算每个摄像头的R_j，t_j，[R_ij t_ij]，i，j＝1，…，2。

具体流程图如图2所示，这里具体介绍每一步的实现细节：

1.步骤201，使用张正友标定方法标定测量多摄像头系统内摄像头的相对某一选定摄像头之间的相对位姿关系：包括旋转矩阵和平移向量[R_ij t_ij]，i，j＝1，…，N。

2.步骤202，参考平面上放置若干个标记点，并测量标记点之间的二维坐标值(以其中某一个点为原点)，这样得到一组共面不共线的3D参考点坐标M_i＝[Xi，Yi，1]，i＝1，…，n，n≥3。

3.步骤203，使用图像分割方法识别出放置的标记点，并对每一个摄像头的二维图像坐标与平面放置的参考点建立一一对应的关系。

4.步骤205，计算摄像头i初始的H_q，用来计算初始单应性H的方法与张正友标定方法相似。首先进行数据标准化，且由方程2有：

$\left(\begin{array}{ccc}{\tilde{M}}^T& 0^T& -u{\tilde{M}}^T\\ 0^T& {\tilde{M}}^T& -v{\tilde{M}}^T\end{array}\right)x=0---\left(4\right)$

因为x定义为一个尺度因子，上式的解可以通过奇异矩阵分解得到，可以重写(4)式为Ax＝0(这里A是一个2n×9的矩阵，n是参考点的数目)，通过奇异矩阵分解方法获得A右面奇异向量及最小的奇异值。

5.步骤205，由摄像头之间的刚性关系得出j摄像头的H₂矩阵。

H₂～H′₂＝K₂[λ₂R₂₁K₁^-1h₁₁ λ₁R₂₁K₁^-1h₂₁ λ₁R₂₁K₁^-1h₃₁+t₂₁]

6.步骤206，最小化所有摄像头关于共面点的图像距离。一共n个参考共面点和k个摄像头，通过最小化下面的函数计算每个摄像头的单应性(H_j，j＝1，…，K)：

$\underset{j=1}{\overset{k}{\Sigma }}\underset{i=1}{\overset{n}{\Sigma }}{\left|\right|{\tilde{m}}_i^j-\frac{1}{s_j}{H}_j{\tilde{M}}_i\left|\right|}^2---\left(5\right)$

上面最优化方程式基于假设每个摄像头的图像点是被高斯噪声扰动的，这是在很多图像噪声消除工作和视觉实验中十分通常的。由于摄像头之间是刚性的组合，每个摄像头的单应性存在固有的限制关系，这将使得只有少数点就得到多摄像头系统的全局优化。

采用一个已知单应性以及刚性旋转和平移计算摄像头单应性的一种计算方法。假设有两个摄像头i和j，以及刚性的旋转矩阵R_ij和平移向量t_ij。基于方程(1)和(3)，在摄像头之间的平移和旋转是：

t_i＝R_ij*t_j+t_ij＝λ_jR_ijK_j^-1h_3j+t_ij

R_i＝R_ij*[λ_jK_j^-1h_2j λ_jK_j^-1h_2j λ_jK_j^-1h_1j×λ_jK_j^-1h_2j]

这样摄像头i的单应性基于单应性在方程(2)的定义有了下面的关系：

H_i～H′_i＝K_i[λ_jR_ijK_j^-1h_2j λ_jR_ijK_j^-1h_2j λ_jR_ijK_j^-1h_3j+t_ij]

在本发明方法中，单应性定义为尺度H(3，3)＝1，所以H_i的单应性可以计算如下：

有了单应性H_i，摄像头i的旋转矩阵和平移向量可以由(3)容易的计算出来。

然后最优化函数(5)可以重写为：

其中尺度因子s_j可以计算如下：

$\frac{1}{s_j}{H}_j{\tilde{M}}_i=\left(\begin{array}{c}{\tilde{u}}_i\\ {\tilde{v}}_i\\ 1\end{array}\right)$

本实例中使用Levenberg-Marquardt方法来解这个非线性最优化问题，分别得到两摄像头的H矩阵。当解最优化方程(6)时，需要一个初始的猜想H_q。

用来计算初始单应性H的方法与张正友方法相似。首先，假设且由方程2有：

$\left(\begin{array}{ccc}{\tilde{M}}^T& 0^T& -u{\tilde{M}}^T\\ 0^T& {\tilde{M}}^T& -v{\tilde{M}}^T\end{array}\right)x=0---\left(7\right)$

因为x定义为一个尺度因子，上式的解可以通过奇异矩阵分解得到，可以重写(7)式为Ax＝0(这里A是一个2n×9的矩阵，n是参考点的数目)，获得A右面奇异向量及最小的奇异值。

在以上方法计算初始猜想单应性之前，应该首先进行数据标准化来获得更加稳定和精确的结果。

此外，如果需要一个更加精确的初始单应性猜想，H的最大可能性估计也可以使用下面方程：

$\underset{i=1}{\overset{n}{\Sigma }}{\left|\right|\left(\begin{array}{c}{u}_i\\ v_i\end{array}\right)-{\tilde{m}}_i||}^2$

这里是H的第i行。上面方程的最优化也可以用Levenberg-Marquardt方法解，且初始猜想可以由方程(7)的解得到。

7.步骤207，由H(3，3)＝1定义了尺度因子，故：

H₁＝[h₁₁ h₂₁ h₃₁]，然后从方程(2)，可以计算摄像头1的旋转和平移如下：

r₁₁＝λ₁K₁^-1h₁₁

r₂₁＝λ₁K₁^-1h₂₁

r₃₁＝r₁₁×r₂₁

t₁＝λ₁K₁^-1h₃₁

这里λ₁＝1/‖K₁^-1h₁₁‖＝1/‖K₁^-1h₂₁‖。

由解出摄像头的旋转矩阵和平移向量，以及摄像头之间的刚性关系，可以解出另外一摄像头的旋转矩阵和平移向量：

$\left(\begin{array}{cc}{R}_2& t_2\\ 0^T& 1\end{array}\right)=\left(\begin{array}{cc}{R}_{21}& t_{21}\\ 0^T& 1\end{array}\right)\left(\begin{array}{cc}{R}_1& t_1\\ 0^T& 1\end{array}\right)$