CN108628808A - 相机采样点的坐标转换方法 - Google Patents

Abstract

本发明实施例公开了一种相机采样点的坐标转换方法，属于数字图像处理和计算机视觉技术领域。技术效果：通过建立相机坐标系与设置参考点，获取由所述局部坐标系中x坐标、y坐标到所述相机坐标系X_C坐标、Y_C坐标的内参矩阵、由所述相机坐标系O_CX_CY_CZ_C中坐标到所述世界坐标系O_WX_WY_WZ_W中坐标的外参矩阵，根据内参矩阵和外参矩阵建立所述局部坐标系任一点坐标转换到所述世界坐标系O_WX_WY_WZ_W坐标的转换公式，根据转换公式将采样点从局部坐标系坐标转换成所述世界坐标系中坐标，降低了坐标转换维度，有效加快了坐标转换的速度。

Description

相机采样点的坐标转换方法

技术领域

本发明涉及数字图像处理与计算机视觉领域，尤其涉及一种相机采样点的坐标转换方法。

背景技术

广义上说振动是指描述系统状态的参量(如位移、电压)在其基准值上下交替变化的过程；狭义的指机械振动，即力学系统中的振动，指物体或质点在其平衡位置附近所作有规律的往复运动。振动测量在微机械系统和精细加工技术中有非常重要的作业，震动的测量方法也包括多种，按照振动信号的转换方式可分为电测法、机械法和光学法。其中，最常见的是光学法，光学法即通过摄影时时获取物体或质点运动图像，通过图像处理获取物体或质点的运动轨迹。光学法属于非接触测量方法，不受电磁干扰，适用于对质量小及不宜安装传感器的试件，在精密测量和传感器、测振仪标定中应用较多。

在实现本发明的过程中，发明人发现至少存在如下问题：

在现有光学法测量过程中，由于相机获取的物体或质点的位置需要经过复杂的坐标转换才能确定物体或质点在实际三维空间中的位置，导致处理速度慢，不利于快速形成物体或质点的运动轨迹，从而影响光学法测量振动在实际生产过程中的应用。

发明内容

本发明实施例提供一种相机采样点的坐标转换方法，通过参考点获得内参矩阵和外参矩阵，根据内参矩阵和外参矩阵建立坐标转换公式，采样点根据坐标转换公式能够快速实现相机局部坐标系坐标到世界坐标系坐标的互换。

根据本发明实施例的一个方面，一种相机采样点的坐标转换方法，包括：

S1，建立相机坐标系O_CX_CY_CZ_C；其中，所述相机坐标系X_C轴与相机局部坐标系x轴平行，且所述相机坐标系Y_C轴与所述局部坐标系y轴平行；

S2，根据参考点，建立由所述局部坐标系中x坐标、y坐标到所述相机坐标系X_C坐标、Y_C坐标的内参矩阵；

S3，根据参考点，建立由所述相机坐标系O_CX_CY_CZ_C中坐标到所述世界坐标系O_WX_WY_WZ_W中坐标的外参矩阵；

S4，根据所述内参矩阵和所述外参矩阵，建立所述局部坐标系任一点坐标转换到所述世界坐标系O_WX_WY_WZ_W坐标的转换公式。

优选的，所述内参矩阵为其中，焦距f为O_C到所述局部坐标系平面距离，o_ux、o_uy为Z_C轴与所述局部坐标系的二维坐标平面的交点o在局部坐标系下坐标，h_x和h_y为像素在相机靶片上的两个物理尺寸，O_C为所述相机坐标系O_CX_CY_CZ_C的原点。

优选的，所述根据参考点，建立由所述局部坐标系中x坐标、y坐标到所述相机坐标系X_C坐标、Y_C坐标的内参矩阵包括：测量获得所述参考点P在所述局部坐标系中坐标(P_ux，P_uy)、所述参考点P在所述相机坐标系中坐标(P_cx，P_cy)；由所述参考点P在所述局部坐标系中坐标进行平移和比例运算得到所述参考点P在所述相机坐标系中坐标，则所述参考点P在所述局部坐标系中坐标(P_ux，P_uy)与所述参考点P在所述相机坐标系中坐标(P_cx，P_cy)对应关系：根据所述对应关系，通过数学运算得到：

优选的，所述外参矩阵为其中，T为平移向量，R为三维旋转矩阵：

优选的，所述根据参考点，建立由所述相机坐标系O_CX_CY_CZ_C中坐标到所述世界坐标系O_WX_WY_WZ_W中坐标的外参矩阵包括：

将所述相机坐标系O_CX_CY_CZ_C的X_CO_CY_C平面投影到所述世界坐标系O_WX_WY_WZ_W的X_WO_WY_W平面获得X₁0₁Y₁平面；

所述参考点P向所述世界坐标系的X_WO_WY_W平面、所述相机坐标系的X_CO_CY_C平面分别投影获得投影点P₁、P₃，直线PP₁与所述相机坐标系的X_CO_CY_C平面相交于P₂点；

在所述相机坐标系内P(x₀,y₀,z₀)、P₃(x₃，y₃，0)，则所述相机坐标系内P与所述投影点P₃的关系：

通过几何关系运算，由所述参考点P和所述投影点P₃得到所述投影点P₂的坐标(x₂，y₂，0)：

根据所述投影点P₂点的坐标，通过几何运算得到所述投影点P₁点在所述X₁0₁Y₁平面中的坐标(x₁₁，y₁₁)：

以所述二维坐标系X₁0₁Y₁原点O₁为中心，在X₁0₁Y₁平面内以一定角度旋转二维坐标系X₁0₁Y₁得到新的二维坐标系X₂0₂Y₂，并通过所述投影点P₁点在X₁0₁Y₁平面中的坐标(x₁₁，y₁₁)求取所述投影点P₁在所述二维坐标系X₂0₂Y₂中的坐标(x₂₁，y₂₁)；其中，在所述二维坐标系X₂0₂Y₂中，X₂轴与所述世界坐标系O_WX_WY_WZ_W中的Y_w轴平行，Y₂轴与所述世界坐标系O_WX_WY_WZ_W中的X_w轴平行，所述旋转角度与所述夹角α相同；

将所述二维坐标系X₂0₂Y₂进行平移得到二维坐标系X₃0₃Y₃，使所述二维坐标系X₃0₃Y₃与所述世界标系O_WX_WY_WZ_W中二维坐标系X_WO_WY_W重合，设所述投影点P₁在所述二维坐标系X₃0₃Y₃中的坐标(x₃₁，y₃₁)，通过测量得到所述参考点P在所述世界坐标系O_WX_WY_WZ_W中坐标值(u₀，v₀，w₀)，则通过几何关系可得到：

设所述二维坐标系X₂0₂Y₂进行平移得到二维坐标系X₃0₃Y₃的平移量(Δx，Δy)，通过几何关系可得到：

将式(5)代入式(6)得到：

通过式(3)、(4)、(5)、(6)、(7)进行代入运算得到：

通过旋转矩阵R与所述参考点P在所述世界坐标系O_WX_WY_WZ_W中的坐标，进行几何运算得到所述投影点P₃在所述世界坐标系O_WX_WY_WZ_W中Z_W轴坐标值：

ω₃＝ω₀-z₀cosβsinθ (9)

根据所述相机坐标系内P₃(x₃，y₃，0)及所述投影点P₃在所述世界坐标系O_WX_WY_WZ_W中Z_W轴坐标值，设所述相机坐标系O_CX_CY_CZ_C经过绕三个坐标轴旋转后平移到所述世界坐标系O_WX_WY_WZ_W中的z轴平移量Δz，经过几何运算得到：

Δz＝ω₀-z₀cosβsinθ(10)

由式(8)、(10)可知平移向量：

优选的，所述根据参考点，建立由所述相机坐标系O_CX_CY_CZ_C中坐标到所述世界坐标系O_WX_WY_WZ_W中坐标的外参矩阵包括：

通过测量获得相机坐标系O_CX_CY_CZ_C中X_C轴与世界坐标系O_WX_WY_WZ_W的O_WX_WY_W平面的夹角α、Y_C轴与世界坐标系O_WX_WY_WZ_W的O_WX_WY_W平面的夹角β、X_C轴投影到世界坐标系O_WX_WY_WZ_W的O_WX_WY_W平面的X_c’轴与Z_W轴夹角θ；通过所述夹角α、β和θ确定三维旋转矩阵R。

优选的，所述通过所述夹角α、β和θ确定三维旋转矩阵R包括：

根据所述夹角α确定绕X_C轴旋转的旋转矩阵:

根据所述夹角β确定绕Y_C轴旋转的旋转矩阵:

根据所述夹角θ确定绕Z_C轴旋转的旋转矩阵：通过所述旋转矩阵R_x(α)、R_y(β)、R_z(θ)进行运算得到所述三维旋转矩阵：

优选的，所述根据所述内参矩阵和所述外参矩阵，建立所述局部坐标系任一点坐标转换到所述世界坐标系O_WX_WY_WZ_W坐标的转换公式包括：

设参考点P在所述相机坐标系中坐标(X_c,Y_c,Z_c)，通过所述参考点P在所述相机坐标系中坐标进行几何运算，得到直线O_CP与所述相机局部坐标系的二维坐标平面的交点p在所述相机坐标系下坐标：p_z＝f；

根据所述相机坐标系O_CX_CY_CZ_C到所述世界坐标系O_WX_WY_WZ_W的坐标转换公式：

与所述交点p在所述相机坐标系下坐标，得到所述相机坐标系O_CX_CY_CZ_C与所述世界坐标系O_WX_WY_WZ_W的坐标关系式：

其中，P_W＝[X_W，Y_W，Z_W]^T为所述参考点P在所述世界坐标系O_WX_WY_WZ_W坐标，旋转矩阵O_w为所述世界坐标系O_WX_WY_WZ_W原点，O_c为所述相机坐标系原点，式②中R_i为旋转矩阵R第i行，即R＝[R_i1,R_i2,R_i3]_3×1；

设所述交点p在所述局部坐标系中坐标(x_u，y_u)，由所述交点p在所述局部坐标系中坐标进行平移和比例运算得到所述交点p在所述相机坐标系中坐标：

其中，所述交点p在所述局部坐标系中坐标与所述参考点P在局部坐标系中坐标相同；

将②代入③得：

将④式扩展为关于齐次方程(x_uh,y_uh,z_uh)三个方程组成的方程组：

其中，x_u＝x_uh/z_uh；y_u＝y_uh/z_uh；

由⑤式可得坐标转换公式：

则由⑥式可得：

p_uh＝M_iM_eP_wh ⑦，

其中，P_wh＝[X_w，Y_w，Z_w，1]^T。

优选的，本发明一种相机采样点的坐标转换方法还包括：由所述参考点P在世界坐标系O_WX_WY_WZ_W坐标P_W＝[X_W，Y_W，Z_W]^T、所述参考点P在所述相机坐标系O_CX_CY_CZ_C的坐标P_C＝[X_C，Y_C，Z_C]^T、在所述局部坐标系的坐标P_u＝[x_u,y_u]^T，将P_W＝[X_W，Y_W，Z_W]^T、P_C＝[X_C，Y_C，Z_C]^T、P_u＝[x_u,y_u]^T通过笛卡尔坐标转化为其次坐标，可得到：P_wh＝[X_wh，Y_wh，Z_wh，1]^T，P_ch＝[X_ch，Y_ch，Z_ch，1]^T和P_uh＝[x_uh，y_uh，z_uh]^T；其中，X_w＝X_wh，Y_w＝Y_wh，Z_w＝Z_wh，X_c＝X_ch，Y_c＝Y_ch，Z_c＝Z_ch；则由式⑦可推知：P_c＝M_eP_wh，p_uh＝M_iP_c，

优选的，本发明一种相机采样点的坐标转换方法还包括：将采样点在所述局部坐标系坐标代入所述转换公式，获得所述采样点在所述世界坐标系中坐标。

本发明通过建立相机坐标系与设置参考点，获取由所述局部坐标系中x坐标、y坐标到所述相机坐标系X_C坐标、Y_C坐标的内参矩阵、由所述相机坐标系O_CX_CY_CZ_C中坐标到所述世界坐标系O_WX_WY_WZ_W中坐标的外参矩阵，根据内参矩阵和外参矩阵建立所述局部坐标系任一点坐标转换到所述世界坐标系O_WX_WY_WZ_W坐标的转换公式，将现有技术中直接在三维空间内进行旋转、平移和计算方法优化为在三维空间内进行投影，并在投影面所在的二维空间内进行旋转、平移和计算，使得坐标转换的计算过程化繁为简，不仅提高了运算速度，而且也保证了转换结果的准确度。

附图说明

图1是本发明的实施例提供的相机采样点的坐标转换方法的流程图；

图2是本发明的另一实施例提供的相机坐标系的位置示意图；

图3是本发明的另一实施例提供的参考点在局部坐标系及相机坐标系中位置关系图；

图4是本发明的另一实施例提供的参考点在相机坐标系及世界坐标系中位置关系图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明了，下面结合具体实施方式并参照附图，对本发明进一步详细说明。应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了对公知结构和技术的描述，以避免不必要地混淆本发明的概念。

在现有技术中，物体的位置和运动轨迹通常采用二维或三位坐标轴来进行描述，通常采用高速相机实时获取物体的图像信息，通过图像处理，最终形成物体的坐标。本发明相机获取的是物体二维坐标，所述二维坐标通过局部坐标系进行描述。所述二维坐标信息包括物体的运动学参数、轨迹信息，用于对目标物体进行路径规划、地理测绘、运动仿真等。

在将物体局部坐标系坐标转换成世界坐标系坐标的过程中，相机的局部坐标系与世界坐标系的相对位置很难测定，采用现有的三维平移和旋转矩阵公式计算方法复杂、处理时间长，不利于相机在实时采样过程中的应用。

实施例一

如图1所示，本发明一种相机采样点的坐标转换方法，包括步骤：S1，建立相机坐标系O_CX_CY_CZ_C；S2，根据参考点，建立由所述局部坐标系中x坐标、y坐标到所述相机坐标系X_C坐标、Y_C坐标的内参矩阵；S3，根据参考点，建立由所述相机坐标系O_CX_CY_CZ_C中坐标到所述世界坐标系O_WX_WY_WZ_W中坐标的外参矩阵；S4，根据所述内参矩阵和所述外参矩阵，建立所述局部坐标系任一点坐标转换到所述世界坐标系O_WX_WY_WZ_W坐标的转换公式。

作为一个优选的实施方式，所述参考点能够直接测量获得所述局部坐标系中坐标值和所述世界坐标系中坐标值，通常采用在在棋盘格标定板上选择参考点，通过高速图像分析软件测得局部坐标系中坐标值。优选的，所述参考点为在所述局部坐标系和世界坐标系中易于测量的点。

在所述步骤S1中，所述相机坐标系X_C轴与相机局部坐标系x轴平行，且所述相机坐标系Y_C轴与所述局部坐标系y轴平行。所述局部坐标系是根据相机拍摄的成像平面来构建的，将相机的成像平面细化成一系列小格子(如图3所示)，光线投射到这些离散小格子上，每一个小格子被称为像素，电子相机以离散的形式实现这些小格子，并通过两个自然数组成的二维坐标对其进行索引。

作为一个优选的实施例，如图2所示，所述相机坐标系O_CX_CY_CZ_C是以相机的镜头中心点为坐标原点O_C，以过原点O_C并平行于相机横向底部边框的直线为X_C轴，X_C轴根据需要选择正方向，以过原点O_C纵向横穿相机镜头并平行于相机底部边框的直线为Y_C轴，以相机安装镜头端作为相机Y_C轴的正方向，以垂直于X_CO_CY_C平面的直线为Z_C轴建立相机坐标系O_CX_CY_CZ_C，以垂直于X_CO_CY_C平面向上的方向为Z_C轴的正方向。所述相机坐标系O_CX_CY_CZ_C和所述世界坐标系O_WX_WY_WZ_W的建立与相机的位置及相机的内部参数无关。

在所述步骤S2中，所述内参矩阵为其中，焦距f为原点O_C到所述局部坐标系平面距离，o_ux、o_uy为Z_C轴与所述局部坐标系的二维坐标平面的交点o在局部坐标系下坐标；，O_C为所述相机坐标系O_CX_CY_CZ_C的原点，h_x和h_y为像素在相机靶片上的两个物理尺寸，即相机成像平面每一个小格子所代表的实际物体大小的尺寸比例，如图3所示，h_x为X_C轴方向上的尺寸比例，h_y为Y_C轴方向的尺寸比例。

本实施例中，外参矩阵为其中，T为平移向量，R为三维旋转矩阵：平移向量T和三维旋转矩阵R用于描述所述相机坐标系O_CX_CY_CZ_C与所述世界坐标系O_WX_WY_WZ_W的相对位置关系。通过测量获得相机坐标系O_CX_CY_CZ_C中X_C轴与世界坐标系O_WX_WY_WZ_W的O_WX_WY_W平面的夹角α、Y_C轴与世界坐标系O_WX_WY_WZ_W的O_WX_WY_W平面的夹角β、X_C轴投影到世界坐标系O_WX_WY_WZ_W的O_WX_WY_W平面的X_c’轴与Z_W轴夹角θ，通过所述夹角α、β和θ确定三维旋转矩阵R。作为一个优选的实施方式，所述夹角α、β和θ自带正负符号，为方便计算，所述夹角θ取正值，可选取X_c’轴逆时针方向旋转与Z_W轴重合时的旋转角度作为夹角θ。

作为本发明优选的实施方式，通过所述夹角α、β和θ确定三维旋转矩阵R包括：

根据所述夹角α确定绕X_C轴旋转的旋转矩阵:根据所述夹角β确定绕Y_C轴旋转的旋转矩阵:根据所述夹角θ确定绕Z_C轴旋转的旋转矩阵：通过所述旋转矩阵R_x(α)、R_y(β)、P_z(θ)进行运算得到所述三维旋转矩阵：

平移向量T描述所述相机坐标系O_CX_CY_CZ_C原点O_C到世界坐标系O_WX_WY_WZ_W原点O_w位置的变化，原点O_C到原点O_w的位置的变化包括在三个坐标轴进行平移，则平移向量：

本发明实施例一种相机采样点的坐标转换方法，还包括步骤：将采样点在所述局部坐标系坐标代入所述转换公式，获得所述采样点在所述世界坐标系中坐标。由于推导公式时采用参考点P作为未知点，坐标转换公式中参考点P可为任意一点，当采样点代入坐标转换公式时，将所述采样点与所述参考点P对应的值带入即可。作为一个优选的实施方式，根据采样点在所述世界坐标系中坐标获得物体的位移改变量；将所述位移改变量，一阶求导后获得物体速度改变量，二阶求导后获得物体加速度改变量。

本实施例中，步骤S2和步骤S3不分先后，即无论先获得外参矩阵还是先获得内参矩阵都不影响本发明的实现。

实施例二

基于实施例一，本发明实施例中，如图3所示，根据参考点建立由所述局部坐标系中x坐标、y坐标到所述相机坐标系X_C坐标、Y_C坐标的内参矩阵包括：测量获得所述参考点P在所述局部坐标系中坐标(P_ux，P_uy)、所述参考点P在所述相机坐标系中坐标(P_cx，P_cy)；由所述参考点P在所述局部坐标系中坐标进行平移和比例运算得到所述参考点P在所述相机坐标系中坐标，则所述参考点P在所述局部坐标系中坐标(P_ux，P_uy)与所述参考点P在所述相机坐标系中坐标(P_cx，P_cy)对应关系：根据所述对应关系，通过数学运算得到：

本实施例中，由于局部坐标系的二维坐标平面与相机坐标系的O_CX_CY_C平面平行，所述参考点在所述局部坐标系中坐标通过平移和比例运算就可得到所述采样点在相机坐标系的O_CX_CY_C面坐标。

实施例三

基于上述实施例，本发明实施例中，如图4所示，所述步骤S3包括如下步骤:

a1)将所述相机坐标系O_CX_CY_CZ_C的X_CO_CY_C平面投影到所述世界坐标系O_WX_WY_WZ_W的X_WO_WY_W平面获得X₁0₁Y₁平面。

a2)所述参考点P向所述世界坐标系的X_WO_WY_W平面、所述相机坐标系的X_CO_CY_C平面分别投影获得投影点P₁、P₃，直线PP₁与所述相机坐标系的X_CO_CY_C平面相交于P₂点。

a3)在所述相机坐标系内P(x₀,y₀,z₀)、P₃(x₃，y₃，0)，则所述相机坐标系内P与所述投影点P₃的关系：

a4)通过几何关系运算，由所述参考点P和所述投影点P₃得到所述投影点P₂的坐标(x₂，y₂，0)：

a5)根据所述投影点P₂点的坐标，通过几何运算得到所述投影点P₁点在所述X₁0₁Y₁平面中的坐标(x₁₁，y₁₁)：

a6)以所述二维坐标系X₁0₁Y₁原点O₁为中心，在X₁0₁Y₁平面内以一定角度旋转二维坐标系X₁0₁Y₁得到新的二维坐标系X₂0₂Y₂，并通过所述投影点P₁点在X₁0₁Y₁平面中的坐标(x₁₁，y₁₁)求取所述投影点P₁在所述二维坐标系X₂0₂Y₂中的坐标(x₂₁，y₂₁)；其中，在所述二维坐标系X₂0₂Y₂中，X₂轴与所述世界坐标系O_WX_WY_WZ_W中的Y_w轴平行，Y₂轴与所述世界坐标系O_WX_WY_WZ_W中的X_w轴平行，所述旋转角度与所述夹角α相同；

a7)将所述二维坐标系X₂0₂Y₂进行平移得到二维坐标系X₃0₃Y₃，使所述二维坐标系X₃0₃Y₃与所述世界标系O_WX_WY_WZ_W中二维坐标系X_WO_WY_W重合，设所述投影点P₁在所述二维坐标系X₃0₃Y₃中的坐标(x₃₁，y₃₁)，通过测量得到所述参考点P在所述世界坐标系O_WX_WY_WZ_W中坐标值(u₀，v₀，w₀)，则通过几何关系可得到：

a8)设所述二维坐标系X₂0₂Y₂进行平移得到二维坐标系X₃0₃Y₃的平移量(Δx，Δy)，通过几何关系可得到：

a9)将式(5)代入式(6)得到：

a10)通过式(3)、(4)、(5)、(6)、(7)进行代入运算得到：

a11)通过旋转矩阵R与所述参考点P在所述世界坐标系O_WX_WY_WZ_W中的坐标，进行几何运算得到所述投影点P₃在所述世界坐标系O_WX_WY_WZ_W中Z_W轴坐标值：

ω₃＝ω₀-z₀cosβsinθ (9)。

a12)根据所述相机坐标系内P₃(x₃，y₃，0)及所述投影点P₃在所述世界坐标系O_WX_WY_WZ_W中Z_W轴坐标值，设所述相机坐标系O_CX_CY_CZ_C经过绕三个坐标轴旋转后平移到所述世界坐标系O_WX_WY_WZ_W中的z轴平移量Δz，经过几何运算得到：

Δz＝ω₀-z₀cosβsinθ (10)。

a13)由式(8)、(10)可知平移向量：

实施例四

基于上述实施例，本发明实施例中，所述步骤S4包括如下步骤：

b1)设参考点P在所述相机坐标系中坐标(X_c,Y_c,Z_c)，通过所述参考点P在所述相机坐标系中坐标进行几何运算，得到直线O_CP与所述相机局部坐标系的二维坐标平面的交点p在所述相机坐标系下坐标：p_z＝f。

本实施例中，通过所述参考点P在所述相机坐标系中坐标(X_c,Y_c,Z_c)，进行几何运算，得到直线O_CP与所述相机局部坐标系的二维坐标平面的交点p在所述相机坐标系下坐标：p_z＝f，具体过程为：如图3所示，Z_C轴垂直于局部坐标系的二维坐标平面，并且与局部坐标系的二维坐标平面相较于o；直线O_CP与所述相机局部坐标系的二维坐标平面的交点p，设p点坐标(p_x，p_y，p_z)，O_C‘是相机的焦点且由相机的内部参数决定，通过几何关系易知△O_cpo和△O_cPO_c‘相似，因此，p_z＝f。本发明实施例使原本复杂的三维坐标转换处理简化成了二维坐标转换处理和一维坐标转换处理，即通过空间几何与相机小孔成像原理相结合使得计算过程化繁为简，不仅提高了运算速度，而且也保证了转换结果的准确度。

b2)根据所述相机坐标系O_CX_CY_CZ_C到所述世界坐标系O_WX_WY_WZ_W的坐标转换公式：

与所述交点p在所述相机坐标系下坐标，得到所述相机坐标系O_CX_CY_CZ_C与所述世界坐标系O_WX_WY_WZ_W的坐标关系式：

其中，P_W＝[X_W，Y_W，Z_W]^T为采样点在所述世界坐标系O_WX_WY_WZ_W坐标，旋转矩阵O_w为世界坐标系下确定的原点，O_c为所述相机坐标系原点，本发明实施例优选采用相机的中心或焦点作为所述相机坐标系原点，式②中R_i为旋转矩阵R第i行，即R＝[R_i1,R_i2,R_i3]_3×1。

b3)设所述交点p在所述局部坐标系中坐标(x_u，y_u)，由所述交点p在所述局部坐标系中坐标进行平移和比例运算得到所述交点p在所述相机坐标系中坐标：

其中，所述交点p在所述局部坐标系中坐标与所述参考点P在局部坐标

系中坐标相同。

将②代入③得：

b4)将④式扩展为关于齐次方程(x_uh,y_uh,z_uh)三个方程组成的方程组：

其中，x_u＝x_uh/z_uh；y_u＝y_uh/z_uh。

b5)由⑤式可得坐标转换公式：

b6)则由⑥式可得：

p_uh＝M_iM_eP_wh ⑦，

其中，P_wh＝[X_w，Y_w，Z_w，1]^T。

本实施例中，由所述采样点在世界坐标系O_WX_WY_WZ_W坐标P_W＝[X_W，Y_W，Z_W]^T，设所述相机坐标系O_CX_CY_CZ_C的坐标P_C＝[X_C，Y_C，Z_C]^T、在所述局部坐标系的坐标P_u＝[x_u,y_u]^T，将P_W＝[X_W，Y_W，Z_W]^T、P_C＝[X_C，Y_C，Z_C]^T、P_u＝[x_u,y_u]^T通过笛卡尔坐标转化为其次坐标，可得到：P_wh＝[X_wh，Y_wh，Z_wh，1]^T，P_ch＝[X_ch，Y_ch，Z_ch，1]^T和P_uh＝[x_uh，y_uh，z_uh]^T；其中，X_w＝X_wh，Y_w＝Y_wh，Z_w＝Z_wh，X_c＝X_ch，Y_c＝Y_ch，Z_c＝Z_ch，则由式⑦可推知：P_c＝M_eP_wh，p_uh＝M_iP_c，

作为一个优选的实施方式，所述步骤S3还包括：根据所述参考点P在所述世界坐标系O_WX_WY_WZ_W中坐标值(u₀，v₀，w₀)与所述参考点P在所述相机坐标系O_CX_CY_CZ_C坐标(x₀,y₀,z₀)，设采样点在所述相机坐标系O_CX_CY_CZ_C坐标(x_c，y_c，z_c)与采样点在所述世界坐标系O_WX_WY_WZ_W的坐标(μ，υ，ω)，根据式：P_c＝M_eP_wh采样点从相机坐标系O_CX_CY_CZ_C坐标转换为世界坐标系O_WX_WY_WZ_W坐标的转换公式为：

。

本发明一种相机采样点的坐标转换方法，通过建立相机坐标系与设置参考点，获取由所述局部坐标系中x坐标、y坐标到所述相机坐标系X_C坐标、Y_C坐标的内参矩阵、由所述相机坐标系O_CX_CY_CZ_C中坐标到所述世界坐标系O_WX_WY_WZ_W中坐标的外参矩阵，根据内参矩阵和外参矩阵建立所述局部坐标系任一点坐标转换到所述世界坐标系O_WX_WY_WZ_W坐标的转换公式，将现有技术中直接在三维空间内进行旋转、平移和计算方法优化为在三维空间内进行投影，并在投影面所在的二维空间内进行旋转、平移和计算，使得坐标转换的计算过程化繁为简，不仅提高了运算速度，而且也保证了转换结果的准确度。

本发明所述相机坐标系O_CX_CY_CZ_C中X_C轴、Y_C轴与Z_C轴只表述一种相对位置关系，只要满足相对位置关系X_C轴、Y_C轴与Z_C轴的表述可以互换。对本发明坐标系中各个坐标轴与其它位置点采用字母名称表述并不构成对本发明的限定。

应当理解的是，本发明的上述具体实施方式仅仅用于示例性说明或解释本发明的原理，而不构成对本发明的限制。因此，在不偏离本发明的精神和范围的情况下所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。此外，本发明所附权利要求旨在涵盖落入所附权利要求范围和边界、或者这种范围和边界的等同形式内的全部变化和修改例。

Claims (10)

1.一种相机采样点的坐标转换方法，其特征在于，包括：

S1，建立相机坐标系O_CX_CY_CZ_C；其中，所述相机坐标系X_C轴与相机局部坐标系x轴平行，且所述相机坐标系Y_C轴与所述局部坐标系y轴平行；

S2，根据参考点，建立由所述局部坐标系中x坐标、y坐标到所述相机坐标系X_C坐标、Y_C坐标的内参矩阵；

S3，根据参考点，建立由所述相机坐标系O_CX_CY_CZ_C中坐标到所述世界坐标系O_WX_WY_WZ_W中坐标的外参矩阵；

S4，根据所述内参矩阵和所述外参矩阵，建立所述局部坐标系任一点坐标转换到所述世界坐标系O_WX_WY_WZ_W坐标的转换公式。

2.根据权利要求1所述的坐标转换方法，其中，所述内参矩阵为其中，焦距f为O_C到所述局部坐标系平面距离，o_ux、o_uy为Z_C轴与所述局部坐标系的二维坐标平面的交点o在局部坐标系下坐标，h_x和h_y为像素在相机靶片上的两个物理尺寸，O_C为所述相机坐标系O_CX_CY_CZ_C的原点。

3.根据权利要求2所述的坐标转换方法，其中，所述步骤S2包括：测量获得所述参考点P在所述局部坐标系中坐标(P_ux，P_uy)、所述参考点P在所述相机坐标系中坐标(P_cx，P_cy)；

由所述参考点P在所述局部坐标系中坐标进行平移和比例运算得到所述参考点P在所述相机坐标系中坐标，则所述参考点P在所述局部坐标系中坐标(P_ux，P_uy)与所述参考点P在所述相机坐标系中坐标(P_cx，P_cy)对应关系：P_cx＝(P_ux-o_ux)h_x；P_cy＝(P_uy-o_uy)h_y；

根据所述对应关系，通过数学运算得到：

4.根据权利要求1所述的坐标转换方法，其中，所述外参矩阵为其中，T为平移向量，R为三维旋转矩阵：

5.根据权利要求4所述的坐标转换方法，其中，所述步骤S3包括：

将所述相机坐标系O_CX_CY_CZ_C的X_CO_CY_C平面投影到所述世界坐标系O_WX_WY_WZ_W的X_WO_WY_W平面获得X₁0₁Y₁平面；

所述参考点P向所述世界坐标系的X_WO_WY_W平面、所述相机坐标系的X_CO_CY_C平面分别投影获得投影点P₁、P₃，直线PP₁与所述相机坐标系的X_CO_CY_C平面相交于P₂点；

在所述相机坐标系内P(x₀,y₀,z₀)、P₃(x₃，y₃，0)，则所述相机坐标系内P与所述投影点P₃的关系：

通过几何关系运算，由所述参考点P和所述投影点P₃得到所述投影点P₂的坐标(x₂，y₂，0)：

根据所述投影点P₂点的坐标，通过几何运算得到所述投影点P₁点在所述X₁0₁Y₁平面中的坐标(x₁₁，y₁₁)：

以所述二维坐标系X₁0₁Y₁原点O₁为中心，在X₁0₁Y₁平面内以一定角度旋转二维坐标系X₁0₁Y₁得到新的二维坐标系X₂0₂Y₂，并通过所述投影点P₁点在X₁0₁Y₁平面中的坐标(x₁₁，y₁₁)求取所述投影点P₁在所述二维坐标系X₂0₂Y₂中的坐标(x₂₁，y₂₁)；其中，在所述二维坐标系X₂0₂Y₂中，X₂轴与所述世界坐标系O_WX_WY_WZ_W中的Y_w轴平行，Y₂轴与所述世界坐标系O_WX_WY_WZ_W中的X_w轴平行，所述旋转角度与所述夹角α相同；

将所述二维坐标系X₂0₂Y₂进行平移得到二维坐标系X₃0₃Y₃，使所述二维坐标系X₃0₃Y₃与所述世界标系O_WX_WY_WZ_W中二维坐标系X_WO_WY_W重合，设所述投影点P₁在所述二维坐标系X₃0₃Y₃中的坐标(x₃₁，y₃₁)，通过测量得到所述参考点P在所述世界坐标系O_WX_WY_WZ_W中坐标值(u₀，v₀，w₀)，则通过几何关系可得到：

设所述二维坐标系X₂0₂Y₂进行平移得到二维坐标系X₃0₃Y₃的平移量(Δx，Δy)，通过几何关系可得到：

将式(5)代入式(6)得到：

通过式(3)、(4)、(5)、(6)、(7)进行代入运算得到：

通过旋转矩阵R与所述参考点P在所述世界坐标系O_WX_WY_WZ_W中的坐标，进行几何运算得到所述投影点P₃在所述世界坐标系O_WX_WY_WZ_W中Z_W轴坐标值：

ω₃＝ω₀-z₀cosβsinθ (9)

根据所述相机坐标系内P₃(x₃，y₃，0)及所述投影点P₃在所述世界坐标系O_WX_WY_WZ_W中Z_W轴坐标值，设所述相机坐标系O_CX_CY_CZ_C经过绕三个坐标轴旋转后平移到所述世界坐标系O_WX_WY_WZ_W中的z轴平移量Δz，经过几何运算得到：

Δz＝ω₀-z₀cosβsinθ (10)

由式(8)、(10)可知平移向量：

6.根据权利要求4所述的坐标转换方法，其中，所述步骤S3包括：

通过测量获得相机坐标系O_CX_CY_CZ_C中X_C轴与世界坐标系O_WX_WY_WZ_W的O_WX_WY_W平面的夹角α、Y_C轴与世界坐标系O_WX_WY_WZ_W的O_WX_WY_W平面的夹角β、X_C轴投影到世界坐标系O_WX_WY_WZ_W的O_WX_WY_W平面的X_c’轴与Z_W轴夹角θ；

通过所述夹角α、β和θ确定三维旋转矩阵R。

7.根据权利要求6所述的坐标转换方法，其中，所述通过所述夹角α、β和θ确定三维旋转矩阵R包括：

根据所述夹角α确定绕X_C轴旋转的旋转矩阵:

根据所述夹角β确定绕Y_C轴旋转的旋转矩阵:

根据所述夹角θ确定绕Z_C轴旋转的旋转矩阵：

通过所述旋转矩阵R_x(α)、R_y(β)、R_z(θ)进行运算得到所述三维旋转矩阵：

8.根据权利要求2所述的坐标转换方法，其中，所述步骤S4包括：

设参考点P在所述相机坐标系中坐标(X_c,Y_c,Z_c)，通过所述参考点P在所述相机坐标系中坐标进行几何运算，得到直线O_CP与所述相机局部坐标系的二维坐标平面的交点p在所述相机坐标系下坐标： p_z＝f；

根据所述相机坐标系O_CX_CY_CZ_C到所述世界坐标系O_WX_WY_WZ_W的坐标转换公式：

与所述交点p在所述相机坐标系下坐标，得到所述相机坐标系O_CX_CY_CZ_C与所述世界坐标系O_WX_WY_WZ_W的坐标关系式：

其中，P_W＝[X_W，Y_W，Z_W]^T为所述参考点P在所述世界坐标系O_WX_WY_WZ_W坐标，旋转矩阵O_w为所述世界坐标系O_WX_WY_WZ_W原点，O_c为所述相机坐标系原点，式②中R_i为旋转矩阵R第i行，即R＝[R_i1,R_i2,R_i3]_3×1；

设所述交点p在所述局部坐标系中坐标(x_u，y_u)，由所述交点p在所述局部坐标系中坐标进行平移和比例运算得到所述交点p在所述相机坐标系中坐标：

p_x＝(x_u-o_ux)h_x；p_y＝(y_u-o_uy)h_y ③；

其中，所述交点p在所述局部坐标系中坐标与所述参考点P在局部坐标系中坐标相同；

将②代入③得：

将④式扩展为关于齐次方程(x_uh,y_uh,z_uh)三个方程组成的方程组：

其中，x_u＝x_uh/z_uh；y_u＝y_uh/z_uh；

由⑤式可得坐标转换公式：

则由⑥式可得：

p_uh＝M_iM_eP_wh ⑦，

其中，P_wh＝[X_w，Y_w，Z_w，1]^T。

9.根据权利要求8所述的坐标转换方法，其中，还包括：

由所述参考点P在世界坐标系O_WX_WY_WZ_W坐标P_W＝[X_W，Y_W，Z_W]^T、所述参考点P在所述相机坐标系O_CX_CY_CZ_C的坐标P_C＝[X_C，Y_C，Z_C]^T、在所述局部坐标系的坐标P_u＝[x_u,y_u]^T，将P_W＝[X_W，Y_W，Z_W]^T、P_C＝[X_C，Y_C，Z_C]^T、P_u＝[x_u,y_u]^T通过笛卡尔坐标转化为其次坐标，可得到：P_wh＝[X_wh，Y_wh，Z_wh，1]^T，P_ch＝[X_ch，Y_ch，Z_ch，1]^T和P_uh＝[x_uh，y_uh，z_uh]^T；其中，X_w＝X_wh，Y_w＝Y_wh，Z_w＝Z_wh，X_c＝X_ch，Y_c＝Y_ch，Z_c＝Z_ch；

则由式⑦可推知：P_c＝M_eP_wh，P_wh，p_uh＝M_iP_c，

10.根据权利要求1所述的坐标转换方法，其中，还包括：

将采样点在所述局部坐标系坐标代入所述转换公式，获得所述采样点在所述世界坐标系中坐标。