CN102506757B - 双目立体测量***多视角测量中的自定位方法 - Google Patents

Abstract

本发明涉及一种自动确定双目立体传感器在不同视角测量时的相对位置和姿态的方法。本方法利用双目立体测量***在每个视角测量时，左、右相机的相对位姿是已知且保持不变这一内在约束条件，联合了左、右相机在不同次测量中各自产生的匹配点对，构造多视图几何约束关系，并自动解算两次测量时立体传感器相对位姿的初值；然后通过多视角测量中重建出的三维特征点及其在各幅图像中的像点与立体传感器相对位姿之间的约束关系，优化确定各个测量视角的相对位姿。本方法提高立体传感器自身定位及数据拼合的精度与可靠性，并降低多视角测量数据的可拼合条件。

Description

双目立体测量***多视角测量中的自定位方法

技术领域

本发明属于三维测量领域，特别涉及一种双目立体测量***在多视角测量时的自定位方法。 

背景技术

三维轮廓数据的测量在航空航天、车辆船舶、机械制造、生物医学、纺织服装、工业设计等领域有着广泛的应用需求。基于机械、光学、声学、电磁学原理的各种测量方法均取得了巨大进展，尤其是以激光扫描法、干涉法和结构光方法为代表的光学非接触测量方法在许多领域中实现了商业应用。然而，各种三维测量方法一次测量都只能获得有限区域的表面数据，大型物体或者全方位轮廓的测量要通过改变视角进行多次测量，这样就需要确定各个视角下三维测量***（立体传感器）的相对位置和姿态（简称位姿），从而将每次测量的数据拼合到一个统一的坐标系下。 

目前解决这一问题主要有以下方法：一是在被测物体表面黏贴球形或者圆形的人工标记点，通过两次测量间至少三个以上的共同标记点，实现两个测量数据集之间的坐标变换。这种方法增加了测量前的准备工作，而且黏贴标记点的地方模型本身的测量数据会缺失，对于小型的和细节丰富的物体这个问题尤其突出。第二种方法是通过机械机构控制不同次测量中立体传感器与被测物体之间的相对运动，由运动参数直接计算测量点集间的坐标变换关系，目前常用的机械机构有旋转台，五轴联动装置等，这些装置的增加使得测量***的灵活性和对被测物体尺寸大小的适应性明显降低。专利“基于姿态传感器的多视角测量数据自拼合方法（ZL200910029324）”将微小型姿态传感器固定在点云测量设备上，以确定不同测量视角下的相对旋转变换，再利用软件算法确定平移变换。这一方法中的辅助装置十分小巧，便于实现，但只适用于物体不动立体传感器改变方位的多视角测量情况。另一类方法是完全通过后续处理算法对不同视角下输出的三维测量数据进行分析和特征点匹配，进而实现数据拼合。这种从三维测量数据中提取特征进行拼合的方法对于双目立体测量***的两个测量视角而言，要求至少有三个点以上的点在全部4幅图像中同时被定位识别，数据的可拼合条件相对较高，且受不同被测物体的表面几何形状影响严重。 

发明内容

技术问题：本发明的目的在于提供一种新的双目立体多视角测量中的设备自定位方法，从而提高立体传感器自身定位及数据拼合的精度与可靠性，并降低多视角测量数据的可拼合条件。 

一种双目立体测量***多视角测量中的自定位方法，其特征在于包括以下过程： 

步骤1：两测量视角之间图像特征匹配集的建立 

双目立体测量***在两个不同视角进行测量时，左右相机共拍摄4幅图像，通过对两两图像进行特征点提取和匹配，获取相应的同名对应点集合，其中同名对应点表示同一空间点在不同图像上的投影点； 

对于多视角测量中的两个视角，不失一般性称为视角1和视角2，记在视角1测量时左、右相机拍摄的图像分别为

和

在视角2测量时左、右相机拍摄的图像为

和

这里上标代表视角，下标l、r代表左、右相机； 

建立

与

之间的同名对应点集合与之间的同名对应点集合为 

和

之间的同名对应点集合

和

之间的同名对应点集合

和

之间的同名对应点集合

以及

和

之间的同名对应点集合

步骤2：两测量视角之间几何变换的初始估计 

双目立体测量***进行两视角测量时，双目立体结构中两个相机的内部参数和相对位姿已经过标定，而双目立体结构在两个不同测量视角之间的位姿是未知的，需要计算能表征相应位姿关系的几何变换矩阵； 

集合

中的一个元素即一个同名匹配点对记为

集合

中的元素记为

已标定的左、右摄像机的内部参数矩阵分别记为A_l、A_r，则在图像畸变已经做过校正的条件下，根据现有的针孔成像模型，可得图像特征点在各自摄像机坐标系下的三维坐标： 

$q_l^1={\mathrm{\κ}}_l{A}_l^{-1}{\tilde{p}}_l^1,q_l^2={\mathrm{\κ}}_l{A}_l^{-1}{\tilde{p}}_l^2---\left(1\right)$

$q_r^1={\mathrm{\κ}}_r{A}_r^{-1}{\tilde{p}}_r^1,q_r^2={\mathrm{\κ}}_r{A}_r^{-1}{\tilde{p}}_r^2---\left(2\right)$

这里

代表特征点

在左摄像机坐标系取得三维坐标，

代表特征点

在左摄像机坐标系取得三维坐标，

代表特征点在右摄像机坐标系取得三维坐标，

代表特征点

在右摄像机坐标系取得三维坐标，

分别表示图像点 

的齐次坐标，κ_l和κ_r为非零尺度因子；为方便阐述

也表述为 $(q_l^1\⇔q_l^2)\∈M_{l_1-l_2};p_r^1\⇔p_r^2$ 也表述为 $(q_r^1\⇔q_r^2)\∈M_{r_1-r_2};$ 根据多视图几何理论， 左相机在两个视角处拍摄的同名匹配点

满足： 

${\left(q_l^2\right)}^T{E}_{l-l}{q}_l^1=0---\left(3\right)$

其中E_l-l为表征左相机在两个测量视角之间相对位姿的本质矩阵；同理，右相机在两个测量视角下拍摄的同名匹配点

满足： 

${\left(q_r^2\right)}^T{E}_{r-r}{q}_r^1=0---\left(4\right)$

其中E_r-r为表征右相机在两个测量视角之间的位姿关系的本质矩阵；由于双目立体测量***在不同视角进行测量时，两个相机的相对位姿保持不变，因此两个相机在两次测量中的几何变换关系是一致的，即可以记为： 

E＝E_l-l=E_r-r      (5) 

因此，可以联立式（3）和式（4）中的两个方程，得到： 

(q²)^TEq¹=0     (6) 

其中q¹和q²为同名匹配点对，且

的每个匹配点对都通过（6）式共同构成关于E矩阵的方程组；上述q¹代表和

q²代表

和

p¹代表和

p²代表和

由于这里的本质矩阵E可由两个视角间的旋转矩阵R和平移向量T＝[t_x t_y t_z]^T来表示： 

$E=R\·\left[\begin{array}{ccc}0& {-t}_z& t_y\\ t_z& 0& {-t}_x\\ {-t}_y& t_x& 0\end{array}\right]---\left(7\right)$

因此只要有5个以上的同名匹配点对即可求解，即只要

就可以求解出E矩阵，这里|·|表示集合所含元素的个数； 

对求得的E矩阵进行分解，得到立体传感器在测量视角1和视角2之间的旋转矩阵R和归一化的平移向量T′； 

步骤3：两测量视角之间平移尺度求解 

步骤2只得到了归一化的平移向量T′，本步骤确定它与实际的平移向量T之间相差的固定比例因子s；对于集合中的匹配点对

若集合

中存在

即存在

则根据双目立体***的标定参数，由

重建实际场景中的三维点坐标,记为

同时根据上面求出的位姿R、T′，重建归一化的三维坐标

同样，若集合

和

中的特征点对存在

则根据双目立体***的标定参数，由

重建实际场景中的三维点坐标

同时根据上面求出的位姿R、T′，重建归一化的三维坐标

上述

和

是建立在左摄像机

坐标系下的三维坐 标，而对于摄像机

坐标系下的

也可由双目立体结构的外部几何参数信息转换到

坐标系下，以下不做特别说明

也表示

坐标系下的三维坐标；这样，可以得到实际场景中的三维点集

和归一化三维点集{X′_i|i＝1,…,m}＝{X′_l-l}∪{X′_r-r}，这两个点集之间的尺度同样相差比例因子s；因此，s可以由式（8）确定： 

$s=\frac{\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\left|\right|X_i-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{X}_i\left|\right|}{\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\left|\right|X_i^{\′}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{X}_i^{\′}\left|\right|}---\left(8\right)$

这里||·||表示求向量的模长，m是并集

中元素的个数；要使式（8）有意义，需满足

至此，将s与归一化平移向量T′相乘，就可获得不同测量视角之间立体传感器的绝对平移向量 

T=sT′                            (9) 

步骤4：两视角立体传感器相对位姿优化 

本步骤进一步利用多视角测量中重建出的三维特征点及其在各幅图像中的像点与立体传感器相对位姿之间的几何约束关系，以步骤2和步骤3中获得的结果R、T作为初值，两步优化确定立体传感器的最终相对位姿； 

第一步优化以集合

中的所有匹配点对建立最小化目标函数： 

$\min \left(\underset{i=1}{\overset{n_{\mathrm{ll}}}{\mathrm{\Σ}}}\right||p_{l-l}^{2i}-{\hat{p}}_{l-l}^{2i}(R,T)||+\underset{i=1}{\overset{n_{\mathrm{rr}}}{\mathrm{\Σ}}}||p_{r-r}^{2i}-{\hat{p}}_{r-r}^{2i}(R,T)|\left|\right)---\left(10\right)$

其中，

分别是

中的匹配像点经双目立体模型重建出的三维点再投影到图像

上的图像坐标，

和

是在图像上识别出的相应特征点的真实图像坐标，n_ll、n_rr为集合

中元素的个数，优化变量R、T的初值已由步骤3得到； 

第二步优化是为了进一步提高测量***的拼合精度，将特征点的三维坐标也松弛为优化变量，同时为了尽量利用更多的约束提高迭代结果的可靠性，将能够对两个测量视角之间的相对位姿起有效约束作用的集合中的同名匹配点对均重建出三维坐标点，并将这些三维坐标点分别投影到 和图像上，相应的再投影误差都计入本次优化过程，最小化目标函数 

$\min \left(\underset{j=1}{\overset{4}{\mathrm{\Σ}}}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\right||p_j^i-{\hat{p}}_j^i(R,T,X^i)|\left|\right)---\left(11\right)$

其中Xⁱ,i=1,2,…,N是视角1和视角2下重建出的所有三维特征点，

是Xⁱ再投影到第j幅图像上的图像坐标，这里的第j幅图像（j=1,…,4）依次代表图像

和

（11）式的目标函数中R,T,Xⁱ是优化变量，其中R和T的初 始值由上一步优化的结果得到，而各三维特征点坐标的初始值Xⁱ由相应的匹配点对重建所得；如果Xⁱ点在第j幅图像上未提取出相应的图像点，即

不存在，则（11）式中取

也就是该点在第j幅图像上的再投影误差不计入目标函数； 

目标函数式（10）和式（11）采用非线性优化方法迭代求解最终得到优化后的旋转矩阵R和平移向量T； 

步骤5：多视角立体传感器相对位姿的整体优化 

在利用步骤1～步骤4的方法确定了两两视角之间相对位姿参数后，进行k个视角相对位姿的整体优化，以进一步减小拼合的累积误差，整体优化的目标函数为 

$\min \left(\underset{j=1}{\overset{2k}{\mathrm{\Σ}}}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\right||p_j^i-{\hat{p}}_j^i(R^{2-1},T^{2-1},R^{3-1},T^{3-1},\·\·\·,R^{k-1},T^{k-1},X^i)|\left|\right)---\left(12\right)$

其中Xⁱ,i=1,2,…,N是各个视角下重建出的所有三维特征点，是Xⁱ再投影到第j(j=1,2,…,2k)幅图像上的图像坐标，

是Xⁱ点在图像j上识别出的真实图像坐标，R^2-1,T^2-1,R^3-1,T^3-1,…,R^k-1,T^k-1分别是视角2,3,…,k相对于视角1的旋转矩阵和平移向量。（12）式目标函数中的R^2-1,T^2-1,R^3-1,T^3-1,…,R^k-1,T^k-1和Xⁱ是优化变量，其中R^2-1,T^2-1,R^3-1,T^3-1,…,R^k-1,T^k-1的初始值已由两视角拼合步骤得到，而各三维特征点坐标的初始值Xⁱ也已由相应的匹配点对重建所得。如果Xⁱ点在第j幅图像上未提取出相应的图像点，即不存在，则（12）式中取  $p_j^i={\hat{p}}_j^i(R^{2-1},T^{2-1},R^{3-1},T^{3-1},\·\·\·,R^{k-1},T^{k-1},X^i).$

目标函数式（12）采用非线性优化方法迭代求解。 

本发明不同于从三维测量数据中提取特征进行设备自身定位和数据拼合的方法，本方法将双目立体测量***多视角拍摄的各幅图像，构建成多视图几何问题，在建立各图像对之间的匹配特征点对集合的基础上，将双目立体结构的内在约束引入多视图几何求解过程，从而确定各测量视角之间立体传感器的相对位姿。对于两个测量视角而言，本方法并不需要特征点在左右相机拍摄的共4幅图像中被同时提取并匹配才可以参与问题的求解，在3幅或者2幅图像中被提取和匹配的点也可参与求解。更重要的是，对于已经标定的双目立体测量***而言，在每个视角测量时，左、右相机的相对位姿是已知且保持不变的，因此4幅图像中只有两个测量视角之间的相对位姿是未知的。本方法充分利用了双目立体多视角测量的这一内在属性，一方面尽可能地松弛了问题的可解条件，另一方面使尽可能多的约束参与问题的求解，明显提高了数据拼合的精度和鲁棒性。 

有益效果：本发明与现有技术相比，具有如下特色和优点： 

1．由于直接利用双目立体测量中的图像特征信息，并联合了左、右相机在不同次测量中各自产生的匹配点对，求解两次测量之间立体传感器的相对位姿（即自定位），从而松弛了两次测量数据之间的可拼合条件，使得某些原本无法拼合的情况可以得到拼合； 

2.由于将双目立体结构的内在约束融入不同测量视角下的图像匹配关系查找和多视图几何的求解，因此能够明显提高数据拼合的可靠性和拼合精度。 

附图及附表说明 

附图1是两视角测量立体传感器自定位的方法流程示意图； 

附图2是双目立体测量***的多视角测量示意图； 

附图3是本发明实施例1的立体传感器自定位和数据拼合结果示意图； 

附图4是本发明的实施例2的实施对象和实施结果图，其中图4(a)为待测对象，图4(b)为视角1测量的点云，4(c)为视角2测量的点云；4(c)为点云拼合结果。 

图5是实施例1中的标记点匹配和重建结果 

图6是实施例1中的图像间的匹配点对 

图7是实施例2中的拼合误差对比统计结果 

具体实施方式

本发明的具体实施方式结合附图详述如下。 

参见附图1所示，本发明由以下步骤组成： 

步骤1：两测量视角之间图像特征匹配集的建立 

双目立体测量***在两个不同视角进行测量时，左右相机共拍摄4幅图像，通过对两两图像进行特征点提取和匹配，获取相应的特征匹配集（又称同名对应点集合），其中同名对应点表示同一空间点在不同图像上的投影点。 

对于多视角测量中的两个视角，不失一般性称为视角1和视角2。记在视角1测量时左、右相机拍摄的图像分别为

和

在视角2测量时左、右相机拍摄的图像为

和这里上标代表视角，下标代表左（l）、右（r）相机。 

建立

与

之间的同名对应点集合

与

之间的同名对应点集合为 

和

之间的同名对应点集合

和

之间的同名对应点集合

和

之间的同名对应点集合

以及

和

之间的同名对应点集合建立同名对应点集合的方法可以采用任何已有的图像特征点提取和匹配方法，其中特征点提取方法包括Harris角点探测法、Harris-Laplace、Hessian-Laplace和SIFT等，特征点匹配方法包括图割法、动态规划法、迭代松弛法等。 

步骤2：两测量视角之间几何变换的初始估计 

双目立体测量***进行两视角测量时，双目立体结构中两个相机的内部参数和相对位姿已经过标定，而双目立体结构在两个不同测量视角之间的位姿是未知的，需要计算能表征相应位姿关系的几何变换矩阵。 

集合

中的一个元素（同名匹配点对）记为

集合

中的元素记为

已标定的左、右摄像机的内部参数矩阵分别记为A_l、A_r，则在图像畸变已经做过校正的条件下，根据现有的针孔成像模型，可得图像特征点在各自摄像机坐标系下的三维坐标： 

$q_l^1={\mathrm{\κ}}_l{A}_l^{-1}{\tilde{p}}_l^1,q_l^2={\mathrm{\κ}}_l{A}_l^{-1}{\tilde{p}}_l^2---\left(1\right)$

$q_r^1={\mathrm{\κ}}_r{A}_r^{-1}{\tilde{p}}_r^1,q_r^2={\mathrm{\κ}}_r{A}_r^{-1}{\tilde{p}}_r^2---\left(2\right)$

这里代表特征点在左摄像机坐标系取得三维坐标，

代表特征点

在左摄像机坐标系取得三维坐标，

代表特征点

在右摄像机坐标系取得三维坐标，

代表特征点在右摄像机坐标系取得三维坐标，

分别表示图像点 

的齐次坐标，κ_l和κ_r为非零尺度因子。为方便阐述

也表述为 $(q_l^1\⇔q_l^2)\∈M_{l_1-l_2};p_r^1\⇔p_r^2$ 也表述为 $(q_r^1\⇔q_r^2)\∈M_{r_1-r_2}.$ 根据多视图几何理论（参见：Richard Hartley,Andrew Zisserman著.Multiple View Geometry in Computer Vision,Cambridge University Press,2004），左相机在两个视角处拍摄的同名匹配点

满足： 

${\left(q_l^2\right)}^T{E}_{l-l}{q}_l^1=0---\left(3\right)$

其中E_l-l为表征左相机在两个测量视角之间相对位姿的本质矩阵。同理，右相机在两个测量视角下拍摄的同名匹配点

满足： 

${\left(q_r^2\right)}^T{E}_{r-r}{q}_r^1=0---\left(4\right)$

其中E_r-r为表征右相机在两个测量视角之间的位姿关系的本质矩阵。由于双目立体测量***在不同视角进行测量时，两个相机的相对位姿保持不变，因此两个相机在两次测量中的几何变换关系是一致的，即可以记为： 

E＝E_l-l=E_r-r    (5) 

因此，可以联立式（3）和式（4）中的两个方程，得到： 

(q²)^TEq¹=0      (6) 

其中q¹和q²为同名匹配点对，且

的每个匹配点对都通过（6）式共同构成关于E矩阵的方程组。上述q¹代表

和q²代表

和

p¹代表和

p²代表

和

由于这里的本质矩阵E可由两个视角间的旋转矩阵R和平移向量T＝[t_x t_y t_z]^T来表示： 

$E=R\·\left[\begin{array}{ccc}0& {-t}_z& t_y\\ t_z& 0& {-t}_x\\ {-t}_y& t_x& 0\end{array}\right]---\left(7\right)$

因此只要有5个以上的同名匹配点对

即可求解，即只要

就可以求解出E矩阵，这里|·|表示集合所含元素的个数。 

对求得的E矩阵进行分解，得到立体传感器在测量视角1和视角2之间的旋转矩阵R和归一化的平移向量T′。 

步骤3：两测量视角之间平移尺度求解 

步骤2只得到了归一化的平移向量T′，本步骤确定它与实际的平移向量T之间相差的固定比例因子s。对于集合中的匹配点对

若集合

中存在

即存在

则根据双目立体***的标定参数，由

重建实际场景中的三维点坐标,记为

同时根据上面求出的位姿R、T′，重建归一化的三维坐标

同样，若集合

和

中的特征点对存在

则根据双目立体***的标定参数，由

重建实际场景中的三维点坐标

同时根据上面求出的位姿R、T′，重建归一化的三维坐标

上述

和

是建立在左摄像机

坐标系下的三维坐标，而对于摄像机坐标系下的也可由双目立体结构的外部几何参数信息转换到

坐标系下，以下不做特别说明

也表示

坐标系下的三维坐标。这样，可以得到实际场景中的三维点集

和归一化三维点集{X′_i|i＝1,…,m}＝{X′_l-l}∪{X′_r-r}，这两个点集之间的尺度同样相差比例因子s。因此，s可以由式（8）确定： 

$s=\frac{\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\left|\right|X_i-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{X}_i\left|\right|}{\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\left|\right|X_i^{\′}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{X}_i^{\′}\left|\right|}---\left(8\right)$

这里||·||表示求向量的模长，m是并集中元素的个数。要使式（8）有意义，需满足至此，将s与归一化平移向量T′相乘，就可获 得不同测量视角之间立体传感器的绝对平移向量 

T=sT′        (9) 

步骤4：两视角立体传感器相对位姿优化 

本步骤进一步利用多视角测量中重建出的三维特征点及其在各幅图像中的像点与立体传感器相对位姿之间的几何约束关系，以步骤2和步骤3中获得的结果R、T作为初值，两步优化确定立体传感器的最终相对位姿。 

第一步优化以集合

中的所有匹配点对建立最小化目标函数： 

$\min \left(\underset{i=1}{\overset{n_{\mathrm{ll}}}{\mathrm{\Σ}}}\right||p_{l-l}^{2i}-{\hat{p}}_{l-l}^{2i}(R,T)||+\underset{i=1}{\overset{n_{\mathrm{rr}}}{\mathrm{\Σ}}}||p_{r-r}^{2i}-{\hat{p}}_{r-r}^{2i}(R,T)|\left|\right)---\left(10\right)$

其中，

分别是

中的匹配像点经双目立体模型重建出的三维点再投影到图像

上的图像坐标，和是在图像

上识别出的相应特征点的真实图像坐标，n_ll、n_rr为集合

中元素的个数，优化变量R、T的初值已由步骤3得到。 

第二步优化是为了进一步提高测量***的拼合精度，将特征点的三维坐标也松弛为优化变量，同时为了尽量利用更多的约束提高迭代结果的可靠性，将能够对两个测量视角之间的相对位姿起有效约束作用的集合

中的同名匹配点对均重建出三维坐标点，并将这些三维坐标点分别投影到

和

图像上，相应的再投影误差都计入本次优化过程，最小化目标函数 

$\min \left(\underset{j=1}{\overset{4}{\mathrm{\Σ}}}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\right||p_j^i-{\hat{p}}_j^i(R,T,X^i)|\left|\right)---\left(11\right)$

其中Xⁱ,i=1,2,…,N是视角1和视角2下重建出的所有三维特征点，

是Xⁱ再投影到第j幅图像上的图像坐标，这里的第j幅图像（j=1,…,4）依次代表图像

和

（11）式的目标函数中R,T,Xⁱ是优化变量，其中R和T的初始值由上一步优化的结果得到，而各三维特征点坐标的初始值Xⁱ由相应的匹配点对重建所得。如果Xⁱ点在第j幅图像上未提取出相应的图像点，即

不存在，则（11）式中取

也就是该点在第j幅图像上的再投影误差不计入目标函数。 

目标函数式（10）和式（11）可以采用Levenberg-Marquardt等非线性优化方法迭代求解。最终得到优化后的旋转矩阵R和平移向量T。 

步骤5：多视角立体传感器相对位姿的整体优化 

在利用步骤1～步骤4的方法确定了两两视角之间相对位姿参数后，进行k个视角相对位姿的整体优化，以进一步减小拼合的累积误差，整体优化的目标函数为 

$\min \left(\underset{j=1}{\overset{2k}{\mathrm{\Σ}}}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\right||p_j^i-{\hat{p}}_j^i(R^{2-1},T^{2-1},R^{3-1},T^{3-1},\·\·\·,R^{k-1},T^{k-1},X^i)|\left|\right)---\left(12\right)$

其中Xⁱ,i=1,2,…,N是各个视角下重建出的所有三维特征点，是Xⁱ再投影到第j(j=1,2,…,2k)幅图像上的图像坐标，

是Xⁱ点在图像j上识别出的真实图像坐标，R^2-1,T^2-1,R^3-1,T^3-1,…,R^k-1,T^k-1分别是视角2,3,…,k相对于视角1的旋转矩阵和平移向量。（12）式目标函数中的R^2-1,T^2-1,R^3-1,T^3-1,…,R^k-1,T^k-1和Xⁱ是优化变量，其中R^2-1,T^2-1,R^3-1,T^3-1,…,R^k-1,T^k-1的初始值已由两视角拼合步骤得到，而各三维特征点坐标的初始值Xⁱ也已由相应的匹配点对重建所得。如果Xⁱ点在第j幅图像上未提取出相应的图像点，即

不存在，则（12）式中取  $p_j^i={\hat{p}}_j^i(R^{2-1},T^{2-1},R^{3-1},T^{3-1},\·\·\·,R^{k-1},T^{k-1},X^i).$

目标函数式（12）同样可采用Levenberg-Marquardt等非线性优化方法迭代求解。 

可以理解对于本领域的技术人员来说，在本发明方法基础上可能存在多种的改进类型。因而上述描述和相关的附图只是本发明的示意而不局限于此。也可以理解本发明覆盖任何基于本发明的改进，适用和调整。通常来说，本发明的原理以及本申请所公开的实施例和公知的或惯用技术的其他改进方法或者可以应用前述方法的本质特征的其他***都受到所附权利要求书的保护。 

实施例1： 

本实施例采用发明专利“基于瞬时随机光照的三维曲面测量装置及方法（ZL200810122905.0）”所述的双目立体测量***。为了验证本发明提出的方法能够在更松弛的条件下实现设备自定位，本例中在物体表面粘贴圆形标记点，由双目立体传感器从两个不同角度拍摄立体图像对，对其表面上不规则分布的目标点进行识别，并提取相应的目标点中心坐标，如图5所示。需要指出的是，本发明方法并不限于这种离散标记点，对于任何其它方法获得的同名对应点，都可以应用本发明方法进行多视角测量的自定位。 

通过两视角双目立体测量图像之间的两两匹配，分别获取图像对上彼此可见的目标像点坐标，见图6。其中，同视角摄取的两幅图像上的匹配目标点，可以直接重建相应的三维坐标。从重建结果可以看到，本例中仅有两个标记点（目标点序号为1和4）在两次测量中均被重建出来，这不满足两次测量输出中至少具有三个共同点的三维数据拼合基本条件，因此无法根据测量出的三维数据来实现立体传感器定位和测量数据拼合。而由于在两次测量中左、右相机各自产生的匹 配点对之和n≥5，且在任意三幅图像中同时可见的特征点个数m≥2，因此满足本发明方法的可解条件，用本发明的方法可以进行立体传感器定位和数据拼合。 

具体应用本发明方法的拼合过程如下： 

(1)建立两测量视角两两图像之间的特征匹配集，结果见图6。 

(2)联合左、右相机在不同次测量中各自产生的匹配点对和

利用公式(6)自动解算本质矩阵E。对E进行奇异值分解： 

E=Udiag(1,1,0)V^T     (13) 

其中U为3×3上三角矩阵，V为3×3下三角矩阵，且满足det(U)＞0，det(V)＞0，这里det(□)表示矩阵的行列式值。记矩阵U的最后一列[u₁₃ u₂₃ u₃₃]^T＝T′，则T′与实际的平移向量T(t_x,t_y,t_z)仅相差一个比例因子s。旋转矩阵R为UWV^T或UW^TV^T，这里的W是正交矩阵： 

$W=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]---\left(14\right)$

由于分解出的旋转矩阵R和归一化的平移向量T′都具有二义性，测量***在视角1和视角2之间的相对位姿的齐次变换矩阵M＝[R|T]实际上有以下四种可能： 

M₁＝[UWV^T|T′]；M₂＝[UWV^T|-T′]； 

(15) 

M₃＝[UW^TV^T|T′]；M₄＝[UW^TV^T|-T′]； 

为了从中选出唯一正确的位姿，依次假设M_i,i＝1,2,3,4为正确位姿变换矩阵，并利用该变换矩阵重建集合M_l-l、M_r-r中所有同名匹配点对的三维坐标，若重建出的三维点均在两个摄像机前面，则该位姿是正确解，否则是错误解，予以剔除。这样获取两次测量时立体传感器相对位姿的初值R和T′。 

(3)由公式(8)获得比例因子s，再将s代入式(9)，得到两测量视角间的实际平移向量T。 

(4)将步骤2和步骤3中获得的结果R、T代入式（10）和式（11）进行优化，得到两个测量视角间几何变换R、T的最终优化结果。 

因本例只进行了两个视角的测量，因此不需要步骤5。图3给出了本例定位出的这两测量视角下立体传感器的相对位姿及拼合后的标记点空间位置，可以直 接看出数据拼合结果的正确性。 

实施例2： 

本实施例仍然采用发明专利“基于瞬时随机光照的三维曲面测量装置及方法（ZL200810122905.0）”所述的双目立体测量***，对如图4(a)所示的车门钣金件进行双目立体表面点云测量。双目立体测量***每次测量时，除了拍摄随机光场照射下的用于三维点云生成的立体图像对之外，左、右相机还同时再拍摄一幅没有光场照射的物体自然纹理图像。随机光场照射下的立体图像用于重建三维点云，两个不同视角下的三维钣金件表面点云测量结果如图4(b)、图4(c)。利用这两个视角下拍摄的4幅自然纹理图像，通过本发明方法进行两视角测量的设备自定位，进而对点云数据进行拼合。图4(d)显示了用本发明方法获得的数据拼合结果，从中可以看出点云重叠区域相互渗透现象较为明显，直观地说明了本发明方法的有效性。 

为了定量对比验证本发明方法对提高拼合精度的效果，本例除了用本发明方法获得拼合实验结果以外，还另将4幅纹理图像中共同可见的特征点重建出它们的三维坐标，利用两次测量输出的共同特征点的三维坐标估计测量视角间的相对位姿，也得到两幅点云数据的拼合结果。定量分析以上两种方法的拼合误差，从图7的统计结果可见，本发明方法得出的平均拼合误差约降低为基于三维特征点的拼合方法的20%，而标准差只约为后者的10%，拼合精度提高的效果是非常明显的。 

Claims (1)

1.一种双目立体测量***多视角测量中的自定位方法，其特征在于包括以下过程：

步骤1：两测量视角之间图像特征匹配集的建立

双目立体测量***在两个不同视角进行测量时，左右相机共拍摄4幅图像，通过对两两图像进行特征点提取和匹配，获取相应的同名对应点集合，其中同名对应点表示同一空间点在不同图像上的投影点；

对于多视角测量中的两个视角，不失一般性称为视角1和视角2，记在视角1测量时左、右相机拍摄的图像分别为

和

在视角2测量时左、右相机拍摄的图像为

和

这里上标代表视角，下标l、r代表左、右相机；

建立

与

之间的同名对应点集合与

之间的同名对应点集合为和之间的同名对应点集合

和

之间的同名对应点集合和

之间的同名对应点集合

以及

和

之间的同名对应点集合

步骤2：两测量视角之间几何变换的初始估计

双目立体测量***进行两视角测量时，双目立体结构中两个相机的内部参数和相对位姿已经过标定，而双目立体结构在两个不同测量视角之间的位姿是未知的，需要计算能表征相应位姿关系的几何变换矩阵；

集合

中的一个元素即一个同名匹配点对记为

集合

中的元素记为

已标定的左、右摄像机的内部参数矩阵分别记为A_l、A_r，则在图像畸变已经做过校正的条件下，根据现有的针孔成像模型，可得图像特征点在各自摄像机坐标系下的三维坐标：

$q_l^1={\mathrm{\κ}}_l{A}_l^{-1}{\tilde{p}}_l^1,q_l^2={\mathrm{\κ}}_l{A}_l^{-1}{\tilde{p}}_l^2---\left(1\right)$

$q_r^l={\mathrm{\κ}}_r{A}_r^{-1}{\tilde{p}}_r^1,q_r^2={\mathrm{\κ}}_r{A}_r^{-1}{\tilde{p}}_r^2---\left(2\right)$ 这里

代表特征点

在左摄像机坐标系取得三维坐标，

代表特征点

在左摄像机坐标系取得三维坐标，

代表特征点

在右摄像机坐标系取得三维坐标，

代表特征点

在右摄像机坐标系取得三维坐标，

分别表示图像点

的齐次坐标，κ_l和κ_r为非零尺度因子；为方便阐述

也表述为

也表述为根据多视图几何理论，左相机在两个视角处拍摄的同名匹配点

满足：

${\left(q_l^2\right)}^T{E}_{l-l}{q}_l^l=0---\left(3\right)$ 其中E_l-l为表征左相机在两个测量视角之间相对位姿的本质矩阵；同理，右相机在两个测量视角下拍摄的同名匹配点

满足：

${\left(q_r^2\right)}^T{E}_{r-r}{q}_r^1=0---\left(4\right)$ 其中E_r-r为表征右相机在两个测量视角之间的位姿关系的本质矩阵；由于双目立体测量***在不同视角进行测量时，两个相机的相对位姿保持不变，因此两个相机在两次测量中的几何变换关系是一致的，即可以记为：

E＝E_l-l=E_r-r  (5)因此，可以联立式（3）和式（4）中的两个方程，得到：

(q²)^TEq¹＝0  (6)其中q¹和q²为同名匹配点对，且

的每个匹配点对都通过（6）式共同构成关于E矩阵的方程组；上述q1代表

和

代表和

p1代表

和 $p_r^1,p^2$ 代表 $p_l^2$ 和 $p_r^2;$

由于这里的本质矩阵E由两个视角间的旋转矩阵R和平移向量T＝[t_xt_yt_z]^T来表示：

$E=R\·\left[\begin{array}{ccc}0& -t_z& t_y\\ t_z& 0& -t_z\\ -t_y& t_x& 0\end{array}\right]---\left(7\right)$ 因此只要有5个以上的同名匹配点对

即可求解，即只要

就可以求解出E矩阵；

对求得的E矩阵进行分解，得到立体传感器在测量视角1和视角2之间的旋转矩阵R和归一化的平移向量T′；

步骤3：两测量视角之间平移尺度求解

步骤2只得到了归一化的平移向量T′，本步骤确定它与实际的平移向量T之间相差的固定比例因子s；对于集合中的匹配点对

若集合

中存在

即存在则根据双目立体***的标定参数，由

重建实际场景中的三维点坐标,记为

同时根据上面求出的位姿R、T′，由

重建归一化的三维坐标

同样，若集合

和

中的特征点对存在

则根据双目立体***的标定参数，由

重建实际场景中的三维点坐标

同时根据上面求出的位姿R、T′，由

重建归一化的三维坐标X′_r-r；上述

和X′_l-l是建立在左摄像机坐标系下的三维坐标，而对于摄像机

坐标系下的X′_r-r由双目立体结构的外部几何参数信息转换到

坐标系下，以下不做特别说明X′_r-r表示变换到

坐标系下的三维坐标；这样，可以得到实际场景中的三维点集

和归一化三维点集

这两个点集之间的尺度同样相差比例因子s；因此，

s可以由式（8）确定：

$\frac{\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\left|\right|X_i-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{X}_i\left|\right|}{\left|\right|X_i^{\′}-\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{X}_i^{\′}\left|\right|}\left(8\right)$

这里||·||表示求向量的模长，m是并集

中元素的个数；要使式（8）有意义，需满足

至此，将s与归一化平移向量T′相乘，就可获得不同测量视角之间立体传感器的绝对平移向量

T=sT′  (9)

步骤4：两视角立体传感器相对位姿优化

本步骤进一步利用多视角测量中重建出的三维特征点及其在各幅图像中的像点与立体传感器相对位姿之间的几何约束关系，以步骤2和步骤3中获得的结果R、T作为初值，两步优化确定立体传感器的最终相对位姿；

第一步优化以集合中的所有匹配点对建立最小化目标函数：

$\left(\underset{i=1}{\overset{n_U}{\mathrm{\Σ}}}\right||p_{l-l}^{2i}-{\hat{p}}_{l-l}^{2i}(R,T)||+\underset{i=1}{\overset{n_{\mathrm{rr}}}{\mathrm{\Σ}}}||p_{r-r}^{2i}-{\hat{p}}_{r-r}^{2i}(R,T)|\left|\right)---\left(10\right)$ 其中，分别是

中的匹配像点经双目立体模型重建出的三维点再投影到图像

上的图像坐标，

和

是在图像

上识别出的相应特征点的真实图像坐标，n_ll、n_rr为集合

中元素的个数，优化变量R、T的初值已由步骤3得到；

第二步优化是为了进一步提高测量***的拼合精度，将特征点的三维坐标也松弛为优化变量，同时为了尽量利用更多的约束提高迭代结果的可靠性，将能够对两个测量视角之间的相对位姿起有效约束作用的集合

中的同名匹配点对均重建出三维坐标点，并将这些三维坐标点分别投影到

和

图像上，相应的再投影误差都计入本次优化过程，最小化目标函数

$\left(\underset{j=1}{\overset{4}{\mathrm{\Σ}}}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\right||p_j^i-{\hat{p}}_j^i(R,T,X^i)|\left|\right)---\left(11\right)$ 其中Xⁱ,i=1,2,...,N是视角1和视角2下重建出的所有三维特征点，是Xi再投影到第j幅图像上的图像坐标，当j分别为j=1,...,4时，这里的第j幅图像依次代表图像和

是Xⁱ点在图像j上识别出的真实图像坐标；（11）式的目标函数中R,T,Xi是优化变量，其中R和T的初始值由上一步优化的结果得到，而各三维特征点坐标的初始值Xⁱ由相应的匹配点对重建所得；如果Xⁱ点在第j幅图像上未提取出相应的图像点，即不存在，则（11）式中取

也就是该点在第j幅图像上的再投影误差不计入目标函数；

目标函数式（10）和式（11）采用非线性优化方法迭代求解最终得到优化后的旋转矩阵R和平移向量T；

步骤5：多视角立体传感器相对位姿的整体优化

在利用步骤1～步骤4的方法确定了两两视角之间相对位姿参数后，进行k个视角相对位姿的整体优化，以进一步减小拼合的累积误差，整体优化的目标函数为

$\min \left(\underset{j=1}{\overset{2k}{\mathrm{\Σ}}}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\right||p_j^i-{\hat{p}}_j^i(R^{2-1},T^{2-1},R^{3-1},T^{3-1},...,R^{k-1},T^{k-1},X^i)|\left|\right)---\left(12\right)$ 其中Xⁱ,i=1,2,...,N是各个视角下重建出的所有三维特征点，

是Xi再投影到第j幅图像上的图像坐标，其中j=1，2...，2k，

是Xⁱ点在图像j上识别出的真实图像坐标，R^2-1,T^2-1,R^3-1,T^3-1,...,R^k-1,T^k-1分别是视角2,3,…,k相对于视角1的旋转矩阵和平移向量，（12）式目标函数中的R^2-1,T^2-1,R^3-1,T^3-1,...,R^k-1,T^k-1和Xⁱ是优化变量，其中R^2-1,T^2-1,R^3-1,T^3-1,...,R^k-1,T^k-1的初始值已由两视角拼合步骤得到，而各三维特征点坐标的初始值Xi也已由相应的匹配点对重建所得，如果Xi点在第j幅图像上未提取出相应的图像点，即

不存在，则（12）式中取

目标函数式（12）采用非线性优化方法迭代求解。

Images