CN102938149B - 一种将视觉注册和机械注册相结合的混合三维注册方法 - Google Patents

Abstract

本发明提供一种将视觉注册和机械注册相结合的混合三维注册方法，该方法的核心是将摄像机坐标系转换到Naturalpoint坐标系，再利用左右手坐标系之间的转换和视口与摄像机之间的转换，计算出摄像机坐标系与红外marker坐标系之间的转换从而将虚拟物体注册到场景中。本发明避免了传统方法中对视觉marker的实时跟踪，在相对较大的空间中对摄像机进行注册，抗遮挡效果好，并且注册流程需要人工参与的部分少，适合快速的生成实时场景，可以满足工业级三维注册对精度和速度的要求。

Description

一种将视觉注册和机械注册相结合的混合三维注册方法

技术领域

本发明涉及三维注册技术领域，具体涉及一种将视觉注册和机械注册相结合的混合三维注册方法，其利用Naturalpoint输出6DOF信息的方法从摄像机和红外marker之间的转换矩阵来获得摄像机的实时外部参数，用于三维模型在增强现实环境中的实时三维注册。

背景技术

在增强现实中，用户可以获得一种混合的场景，该场景将真实的背景图像和虚拟的三维模型叠加到一起。三维注册技术是增强现实的关键技术之一。它获取摄像机的位置和姿态并计算虚拟物体在投影平面上的映射位置，这样虚拟物体就会被实时地显示到显示器的正确位置。常见的三维注册方法主要是基于视觉的三维注册方法和机械式三维注册方法。

基于视觉的三维注册方法包含两类，一种是基于marker的三维注册，另一种是基于自然特征点的三维注册。文献1-Real-time 3D registration of stereo-vision based range images usingGPU.In Applications of Computer Vision(WACV),2009 Workshop on,pages 1-6,dec.2009介绍了一种结合GPU的视觉式三维注册方法，该方法通过对预先设置好的marker拍照，通过分析每一帧图像来获取摄像机相对于世界坐标系的姿态，以此定位摄像机的位置。这种方法计算速度较快，可以满足实时性的要求，但是需要对整个marker进行拍摄，否则无法计算出摄像机的姿态，因此这种注册方法容易受遮挡的影响。文献2-Registration for Stereo Vision-basedAugmented Reality Based on Extendible Tracking of Markers and NaturalFeatures.IEEE.2002:1045-1048使用自然特征点和预定义基准marker结合的方法来注册三维物体，首先估计真实世界和虚拟场景之间的投影矩阵，同时从初始化图像中检测和跟踪自然特征点，当整个marker出现在图像帧时，自然特征点用来修正投影矩阵。这种方法结合了自然特征点和marker，自然特征点可以对marker的易遮挡做出补偿，而marker可以对自然特征点的跟踪精度较差做出补偿。

另一种三维注册方法为机械式三维注册方法，这种方法稳定，不易受遮挡的影响，可以跟踪注册多个位置，适用于较大区域内的三维立体注册。同时这种方法的缺陷是不易融入增强现实系统中，需要借助其他方法将其坐标系统一到世界坐标系中。文献2-Hybrid Inertial andVision Tracking for Augmented Reality Registration提出了一种将惯导跟踪和视觉式注册相结合的三维注册方法。

发明内容

本发明的技术解决问题：克服现有技术的不足，提供一种将视觉注册和机械注册相结合的混合三维注册方法，该方法在实时运行时，不需要额外的标定marker，不需要额外的相机对标定marker进行实时摄像，综合视觉注册的精确性和机械注册的快速性，同时避免视觉注册易被遮挡和机械注册难以融入系统的缺点，在场景中快速移动头盔显示器的情况下，依然可以快速精准地定位物体。

本发明的技术解决方案：一种将视觉注册和机械注册相结合的混合三维注册方法，其步骤如下：

（1）标定摄像机的内部参数；

（2）摄像机对多marker标定板拍照得到原始图像，去除图像头信息得到图像raw数据，定义多marker配置文件，使用ARToolkitPlus计算摄像机相对于多marker坐标系的外部参数A；

（3）计算Naturalpoint输出矩阵Np，首先计算绕x/y/z三个坐标轴方向的旋转矩阵，然后计算平移矩阵t，按照特定顺序相乘即得矩阵Np；

（4）确定视口坐标系和摄像机坐标系之间的转换矩阵P，该矩阵随着不同的绘制引擎和摄像机而不同；

（5）由于无法测量摄像机坐标系相对于世界坐标系的转换关系，需要借助红外小球确定红外小球坐标系与摄像机坐标系之间的转换矩阵T，将红外marker和摄像机绑定，这样矩阵T固定，可作为初始值载入系统。

所述步骤（1）中，标定摄像机时使用两种不同的方法，第一种方法为opencv的标定方法得到结果R1，第二种方法为matlab的标定方法得到结果R2，当二者误差小于阈值n时，即|R1-R2|<n时，将R2作为标定结果，否则舍弃此次标定，重新标定。

所述步骤（2）中计算摄像机相对于世界坐标系的外部参数时，使用多marker矩阵来定义待跟踪平面，同时定义每一个marker中心的坐标，利用步骤（1）中计算出的摄像机内部参数，使用ARToolkitPlus开源工具计算摄像机的外部参数，其中，多marker矩阵遵循右手坐标系，按照常用图片格式规定x和y方向，x方向规定有3个矩阵块，z方向规定有4个矩阵块，每一个矩阵块的中心位置参考多marker板的中心。

所述步骤（3）中，Natualpoint的原始坐标系为左手坐标系，需要调整x/y/z方向的三个旋转矩阵的相乘次序，使输出矩阵Np符合右手坐标系。

所述步骤（4）中，确定的绘制引擎对应确定的视口朝向，确定的摄像机对应确定的摄像机朝向，确定二者之间的转换矩阵P时，视口坐标系和摄像机坐标系之间的y轴和z轴相反，x轴相同。

所述步骤（5）中，计算摄像机和红外marker之间的转换矩阵T的计算公式为：

T＝P*A*Np。

所述的计算Naturalpoint输出矩阵的过程为：

根据Naturalpoint输出的位移x/y/z，计算位移矩阵 $t=\left[\begin{array}{cccc}1& 0& 0& x\\ 0& 1& 0& y\\ 0& 0& 1& z\\ 0& 0& 0& 1\end{array}\right];$

根据Naturalpoint输出的roll，计算旋转矩阵 $\mathrm{Rx}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos \left(\mathrm{roll}\right)& -\sin \left(\mathrm{roll}\right)& 0\\ 0& \sin \left(\mathrm{roll}\right)& \cos \left(\mathrm{roll}\right)& 0\\ 0& 0& 0& 1\end{array}\right];$

根据Naturalpoint输出的pitch，计算旋转矩阵 $\mathrm{Ry}=\left[\begin{array}{cccc}\cos \left(\mathrm{pitch}\right)& -\sin \left(\mathrm{pitch}\right)& 0& 0\\ \sin \left(\mathrm{pitch}\right)& \cos \left(\mathrm{pitch}\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

根据Naturalpoint输出的pitch，计算旋转矩阵 $\mathrm{Rz}=\left[\begin{array}{cccc}\cos \left(\mathrm{yaw}\right)& 0& \sin \left(\mathrm{yaw}\right)& 0\\ 0& 1& 0& 0\\ -\sin \left(\mathrm{yaw}\right)& 0& \cos \left(\mathrm{yaw}\right)& 0\\ 0& 0& 0& 1\end{array}\right];$

输出矩阵NP＝t*Rx*Rz*Ry。

本发明的原理：本发明的核心是将摄像机坐标系转换到Naturalpoint坐标系，再利用左右手坐标系之间的转换和视口与摄像机之间的转换，计算出摄像机坐标系与红外marker坐标系之间的转换从而将虚拟物体注册到场景中。本发明步骤（1）为步骤（2）计算摄像机外部参数做准备，步骤（2）、（3）和（4）为步骤（5）计算摄像机坐标系和红外marker坐标系之间的转换提供支持。

本发明与现有技术相比的优点在于：

（1）、本发明使用多个红外相机对红外marker进行跟踪，只需3个红外相机捕捉到marker，即可跟踪到，避免了红外marker被移动物体或者障碍物遮挡的情况。

（2）、本发明注册的空间区域大，多个红外相机构成一个弧形场景，头盔显示器可以在场景中自由地漫游，注册的区域可以到达7m²左右。

（3）、本发明支持多个虚拟物体的注册，可以对多个红外marker进行跟踪，每一个红外marker代表一个虚拟物体，可以将多个虚拟物体注册场景中不同的位置，大大的增加了场景的多样性。

（4）、本发明三维注册的速度快，原始Naturalpoint输出帧率为250帧，经过模型视点运算后，帧率可以达到120帧。由于绘制的瓶颈，最终整个场景的帧率可以到达30帧，完全可以满足实时场景绘制的要求。

（5）、本发明三维注册的精度高，由于计算摄像机外参使用了ARToolkitPlus，其精度可以到达mm级别，其他的计算只涉及加法和乘法，因此整个场景的注册精度能到达mm级别。

附图说明

图1为本发明算法流程图；

图2为摄像机内部参数标定示意图；

图3为摄像机外部参数标定步骤示意图；

图4A为红外marker示意图；

图4B为红外marker中心位置示意图；

图5为三维注册坐标系统示意图。

具体实施方式

如图1所示，本发明具体实现步骤如下：

步骤S101，步骤S101中，标定摄像机的内部参数时需要通过两种方法来确定，具体方法是，首先使用opencv的标定方法对摄像机进行标定得到结果R1=（fx1，fy1，cx1,cy1），其中fx1和fy1为焦点距离，cx1和cy1为主点在像平面上的坐标，然后使用matlab的标定方法得到结果R2=（fx2，fy2，cx2,cy2），当二者之间的差|R1-R2|的值小于n=（Δfx,ΔfyΔcx,Δcy）时，将R2作为本次内参标定的结果，否则，重新标定直到满足条件|R1-R2|<n为止。这样做的目的是尽量避免人工操作导致的误差，使得内部参数的标定到达最佳效果。

在图2中，相机坐标系即是以光轴中心O为原点的坐标系，其z轴满足右手法则，成像原点Of所代表平面即为像平面坐标系，实际物体坐标系即为世界坐标系。其中，P在世界坐标系中的坐标为(Xw，Yw，Zw)，Pu是P在像平面坐标系的投影点，其相机坐标系的坐标为(Xu，Yu，Zu)，像平面坐标系的坐标为(u,v，1)。θ是相机坐标系Z轴与像平面夹角，一般情况下Z轴与像平面垂直，θ值为90。。且相机坐标系的XcOcYc平面与像平面坐标系XfOfYf平行，f为相机的焦距。

对于从相机坐标系到像平面坐标系的转换，像平面坐标系是用像素单位来表示的，而相机坐标系则是以毫米为单位来表示的，因此，需要先得到像平面的像素单位与毫米单位之间的线性关系。在图2中，相机光轴中心沿Zc轴与像平面的交点O’为投影中心，坐标为(Cx，Cy)，以像素为单位，而每个像素在Xf与Yf方向的物理尺寸为sx=1/dx和sy=1/dy，单位为像素/毫米，则在XfOfYf平面，根据Pu的像素坐标(u，v，1)到毫米坐标的转换，有：

$\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}{s}_x& 0& c_x\\ 0& s_y& c_y\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}x\\ y\\ 1\end{array}\right)$

根据小孔成像模型，像平面的物理坐标(x，y，1)和相机坐标系的坐标(Xu，Yu，Zu)满足：

$\left(\begin{array}{c}x\\ y\\ 1\end{array}\right)=\frac{1}{z_u}\left(\begin{array}{cccc}f& -{f\cos }^{-1}\mathrm{\&theta;}& 0& 0\\ 0& f{\sin }^{-1}\mathrm{\&theta;}& 0& 0\\ 0& 0& 1& 0\end{array}\right)\left(\begin{array}{c}{X}_u\\ Y_u\\ Z_u\\ 1\end{array}\right)$

联立公式(1)和公式(2)，即可得相机坐标系与像平面坐标系变换矩阵。如公式(3)所示：

$\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\frac{1}{z_u}\left(\begin{array}{cccc}{s}_{x}f& -{f\cos }^{-1}\mathrm{\&theta;}& c_x& 0\\ 0& s_{y}f{\sin }^{-1}\mathrm{\&theta;}& c_y& 0\\ 0& 0& 1& 0\end{array}\right)\left(\begin{array}{c}{X}_u\\ Y{u}_{}\\ Z_u\\ 1\end{array}\right)$

其中，(1/sx，1/sy，cx，cy，f，θ)即为所求的相机的6个内部参数。利用开源的opencv库或者matlab标定库，可以对摄像机的6个内部参数进行标定。

步骤S102计算摄像机外部参数矩阵，使用摄像机对多marker矩阵拍照得到原始图像，去掉原始图像的头信息，利用S101中计算的内部参数和多marker配置文件，使用ARToolkitPlus计算出摄像机的外部参数。

根据刚体变换，从相机坐标系到世界坐标系的变换如下式，R为旋转矩阵，T为平移矩阵。

$\left(\begin{array}{c}{X}_u\\ Y_u\\ Z_u\\ 1\end{array}\right)=\left(\begin{array}{cc}R& T\\ 0& 1\end{array}\right)\left(\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right)$

利用S101中内部参数公式，可以求得像平面坐标系与世界坐标系之间的转换关系如下式：

$\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\frac{1}{z_u}\left(\begin{array}{cccc}{s}_{x}f& -f{\cos }^{-1}\mathrm{\&theta;}& c_x& 0\\ 0& s_{y}f{\sin }^{-1}\mathrm{\&theta;}& c_y& 0\\ 0& 0& 1& 0\end{array}\right)\left(\begin{array}{cc}R& T\\ 0& 1\end{array}\right)\left(\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right)$

矩阵 $\left(\begin{array}{cc}R& T\\ 0& 1\end{array}\right)$ 即为要求的外部参数矩阵。

图3为计算外部参数的步骤。S301为摄像机对多marker矩阵捕获一帧图像，S302去掉图像头信息，S303读入多marker矩阵配置文件及S101中计算的内部参数，S304计算外部参数。

步骤S103计算Naturalpoint输出矩阵Np，图4A为红外marker的实物，图4B示意图为红外marker的中心位置，Naturalpoint输出的6DOF信息包括x/y/z/yaw/pitch/roll，下式计算红外小球的位置矩阵。

$t=\left[\begin{array}{cccc}1& 0& 0& x\\ 0& 1& 0& y\\ 0& 0& 1& z\\ 0& 0& 0& 1\end{array}\right]$

以下三个公式分别计算红外小球绕x/y/z方向的旋转矩阵。

$\mathrm{Rx}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos \left(\mathrm{roll}\right)& -\sin \left(\mathrm{roll}\right)& 0\\ 0& \sin \left(\mathrm{roll}\right)& \cos \left(\mathrm{roll}\right)& 0\\ 0& 0& 0& 1\end{array}\right]$

$\mathrm{Ry}=\left[\begin{array}{cccc}\cos \left(\mathrm{pitch}\right)& -\sin \left(\mathrm{pitch}\right)& 0& 0\\ \sin \left(\mathrm{pitch}\right)& \cos \left(\mathrm{pitch}\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

$\mathrm{Rz}=\left[\begin{array}{cccc}\cos \left(\mathrm{yaw}\right)& 0& \sin \left(\mathrm{yaw}\right)& 0\\ 0& 1& 0& 0\\ -\sin \left(\mathrm{yaw}\right)& 0& \cos \left(\mathrm{yaw}\right)& 0\\ 0& 0& 0& 1\end{array}\right]$

旋转矩阵通过公式NP＝t*Rx*Rz*Ry来计算。

步骤S104确定视口坐标系和摄像机坐标系之间的转换矩阵P，在图5中，绘制引擎的视口坐标系代表场景的虚拟观察点，而摄像机坐标系代表场景的实际观察点，即相机光心位置处。二者之间的转换矩阵P适用于下式：

(Xc Yc Zc 1)^T＝P(Xv Yv Zv 1)^T

在本系统中，摄像机坐标系和视口坐标系有相同的X轴，而Y轴和Z轴相反。因此常量矩阵P可以直接写出：

$P=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& -1& 0& 0\\ 0& 0& -1& 0\\ 0& 0& 0& 1\end{array}\right]$

步骤S105计算摄像机和红外marker之间的转换矩阵T，红外marker坐标系和摄像机坐标系之间的转换矩阵T可以通过T＝P*A*NP来计算：

在实时计算阶段，模型视点矩阵Modelview可以通过Naturalpoint的实时输出矩阵M和转换矩阵T计算出来：

Modelview＝inv(M)*T

其中，inv(M)代表M的逆矩阵。

Claims (6)

1.一种将视觉注册和机械注册相结合的混合三维注册方法，其特征在于：该方法步骤如下：

步骤(1)、标定摄像机的内部参数；

所述步骤(1)中，标定摄像机时使用两种不同的方法，第一种方法为opencv的标定方法得到结果R1，第二种方法为matlab的标定方法得到结果R2，当二者误差小于阈值n时，即|R1-R2|<n时，将R2作为标定结果，否则舍弃此次标定，重新标定；

步骤(2)、摄像机捕获多marker标定板的一帧图像，去除原始头信息得到图像的raw数据，定义多marker配置文件，使用ARToolkitPlus开源工具计算摄像机相对于多marker坐标系的外部参数A；

步骤(3)、计算Naturalpoint输出矩阵Np，首先计算绕x/y/z三个坐标轴方向的旋转矩阵，然后计算平移矩阵t，按照特定顺序相乘即得矩阵Np；

步骤(4)、确定视口坐标系和摄像机坐标系之间的转换矩阵P，该矩阵随着不同的绘制引擎和摄像机而不同；

步骤(5)、由于无法测量摄像机坐标系相对于世界坐标系的转换关系，需要借助红外小球确定红外小球坐标系与摄像机坐标系之间的转换矩阵T，将红外marker和摄像机绑定，这样矩阵T固定，可作为初始值载入系统。

2.根据权利要求1所述的一种将视觉注册和机械注册相结合的混合三维注册方法，其特征在于：所述步骤(2)中计算摄像机相对于世界坐标系的外部参数时，使用多marker矩阵来定义待跟踪平面，同时定义每一个marker中心的坐标，利用步骤(1)中计算出的摄像机内部参数，使用ARToolkitPlus开源工具计算摄像机的外部参数，其中，多marker矩阵遵循右手坐标系，按照常用图片格式规定x和y方向，x方向规定有3个矩阵块，z方向规定有4个矩阵块，每一个矩阵块的中心位置参考多marker板的中心。

3.根据权利要求1所述的一种将视觉注册和机械注册相结合的混合三维注册方法，其特征在于：所述步骤(3)中，Natualpoint的原始坐标系为左手坐标系，需要调整x/y/z方向的三个旋转矩阵的相乘次序，使输出矩阵Np符合右手坐标系。

4.根据权利要求1所述的一种将视觉注册和机械注册相结合的混合三维注册方法，其特征在于：所述步骤(4)中，确定的绘制引擎对应确定的视口朝向，确定的摄像机对应确定的摄像机朝向，确定二者之间的转换矩阵P时，视口坐标系和摄像机坐标系之间的y轴和z轴相反，x轴相同。

5.根据权利要求1所述的一种将视觉注册和机械注册相结合的混合三维注册方法，其特征在于：所述步骤(5)中，计算摄像机和红外marker之间的转换矩阵T的计算公式为：

T＝P*A*Np。

6.根据权利要求3所述的一种将视觉注册和机械注册相结合的混合三维注册方法，其特征在于：计算Naturalpoint输出矩阵的过程为：

根据Naturalpoint输出的位移x/y/z，计算位移矩阵 $t=\left[\begin{array}{cccc}1& 0& 0& x\\ 0& 1& 0& y\\ 0& 0& 1& z\\ 0& 0& 0& 1\end{array}\right];$

根据Naturalpoint输出的roll，计算旋转矩阵 $\mathrm{Rx}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos \left(\mathrm{roll}\right)& -\sin \left(\mathrm{roll}\right)& 0\\ 0& \sin \left(\mathrm{roll}\right)& \cos \left(\mathrm{roll}\right)& 0\\ 0& 0& 0& 1\end{array}\right];$

根据Naturalpoint输出的pitch，计算旋转矩阵 $\mathrm{Ry}=\left[\begin{array}{cccc}\cos \left(\mathrm{piteh}\right)& -\sin \left(\mathrm{piteh}\right)& 0& 0\\ \sin \left(\mathrm{piteh}\right)& \cos \left(\mathrm{piteh}\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

根据Naturalpoint输出的pitch，计算旋转矩阵 $\mathrm{Rz}=\left[\begin{array}{cccc}\cos \left(\mathrm{yaw}\right)& 0& \sin \left(\mathrm{yaw}\right)& 0\\ 0& 1& 0& 0\\ -\sin \left(\mathrm{yaw}\right)& 0& \cos \left(\mathrm{yaw}\right)& 0\\ 0& 0& 0& 1\end{array}\right];$

输出矩阵Np＝t*Rx*Rz*Ry。