数码相机定位理工学院2队A题

数码相机的定位问题

摘要

本文讨论了关于数码相机定位的问题，即通过分析提取数码相机摄制物体相片的信息来确定物体表面的信息。传统解决此问题的方法是双目定位法，而在本文中我们建立了如何用一部相机来获得物体表面信息的模型——单目法。

所谓单目法，就是通过分析一部相机所摄得的像来确定物体实际坐标的方法。在本文中我们利用靶标的物体坐标及靶标的像坐标建立了求物体坐标系中任一点的像坐标的算法，其模型的建立过程大致步骤如下：

1. 首先分别建立相机坐标系、像平面坐标系与物体坐标系。然后我们建立各坐标系之间的旋转关系，从而得到了一个旋转矩阵。

2. 我们通过“网格标定法”分析靶标上的像，从而得到其“中心点”在像平面坐标系下的坐标。所谓“网格标定法”是指：将像坐标按照像素分格加以格栅，得到“中心点”的像素坐标，再按比例变换成像平面上的像坐标。

3. 建立像平面坐标，物体坐标与旋转矩阵之间的函数关系，从而求得旋转矩阵中的各旋转角。

这样，我们就建立了单目法模型。

运用单目法本文解决了下述问题：

1. 利用网格标定法，我们得到了靶标上各圆的圆心的像坐标：

A(-49.7857，51.65559)，B(-24.5423，49.31824)，C(33.89168，45.57846)， D(18.23139，-30.6194)，E(-60.3038，-30.3856) (单位：mm) 。

2. 运用单目法模型与A, B, C, D, E各点在不同坐标系下的坐标关系，得到了物体坐标系坐标轴绕着Z轴逆时针旋转的旋转角约为?4.36?，绕着X轴逆时针旋转的旋转角约为7.60?，绕着Y轴逆时针旋转的旋转角约为8.49?。

3. 利用“像点往复循环对应”法，我们对单目法模型进行了精度和稳定性的检验，即在物体坐标系下的任一点M经过往复对应后得到物体坐标系下的点M?，我们用MM?来度量模型的精度；用往复次数来衡量模型的稳定性。

4. 我们建立了两部相机之间的相机坐标系及物体坐标系，得到了物体坐标系与两部相机的相机坐标系之间的旋转矩阵与平移矩阵，从而得到了两部固定相机的相对位置关系。

关键词 单目法 坐标旋转关系 平移矩阵 双目标定

- 1 -

一 问题重述

数码相机定位是指用数码相机摄得的物体的相片来确定物体表面某些特征点的位置。最常用的定位方法是双目定位，即用两部相机来定位。对于双目定位，精确地确定两部相机的相对位置就是关键，这一过程称为系统标定。

标定的一种做法是：在一块平板上画若干个点， 同时用这两部相机照相，分别得到这些点在它们像平面上的像点，利用这两组像点的几何关系就可以得到这两部相机的相对位置。实际的做法是在物平面上画若干个圆（称为靶标），它们的圆心就是几何的点了。而它们的像一般会变形，所以必须从靶标上的这些圆的像中把圆心的像精确地找到，标定就可实现。

有人设计靶标如下，取1个边长为100mm的正方形，分别以四个顶点（对应为A、C、D、E）为圆心，12mm为半径作圆。以AC边上距离A点30mm处的B为圆心，12mm为半径作圆，用一位置固定的数码相机摄得其像，请做以下工作：

（1） 建立数学模型和算法以确定靶标上圆的圆心在该相机像平面的像坐标, 这里坐标系原点取在该相机的光学中心，x-y平面平行于像平面；

（2）对由图2、图3分别给出的靶标及其像，计算靶标上圆的圆心在像平面上的像坐标, 该相机的像距（即光学中心到像平面的距离）是1577个像素单位(1毫米约为3.78个像素单位)，相机分辨率为1024×768；

（3）设计一种方法检验你们的模型，并对方法的精度和稳定性进行讨论； （4）建立用此靶标给出两部固定相机相对位置的数学模型和方法。

二 模型假设

1、 2、 3、 4、

两部数码相机的规格一样，使用时的调试状态一样。 数码相机的透镜两侧介质相同。

数码相机的透镜是规则的，其光学中心在几何中心。 在计算时，像距近似等于焦距。

三 符号说明

符号 ? ? ? L Ri 说明 物体坐标系绕着Z轴逆时针旋转的角度. 物体坐标系绕着X轴逆时针旋转的角度. 物体坐标系绕着Y轴逆时针旋转的角度. 空间矩形ACDE的边长. 绕着i轴旋转后旋转矩阵（i?X,Y,Z）.

四 模型建立与求解

1．问题（1）的解决：

为了表示靶标的位置与靶标的像的位置关系,需要建立3个坐标系:相机坐标系

- 2 -

像平面坐标系o?uv和物体坐标系O0?X0Y0Z0,利用相机的成像原理建立各C0?XcYcZc、

坐标系如图1所示。

L

AC

L Z0 Y0 E X0D O0 v ZC o u Yc f C0 Xc

图1 相机、像平面和物体坐标系

相机坐标系C0?XcYcZc,原点C0位于相机镜头的中心,Xc轴平行于成像平面的u轴,Zc轴平行于成像平面的v轴,Yc轴与相机光轴重合。

像平面坐标系o?uv的原点在成像平面的中心,像平面垂直于Yc轴并且到原点C0的距离为f(相机焦距)。

空间矩形ACDE的尺寸大小(长和宽)已知,将物体坐标系O0?X0Y0Z0建立在矩形顶点E处。物体坐标系原点O0与点E重合,Z0轴垂直于矩形平面,X0轴沿着ED方向,Y0轴沿着EA方向。

建立了坐标系之后,需要定义各坐标系之间的旋转关系和位置关系，从而得到物平面与像平面之间的旋转关系。

1.1 坐标系旋转下的坐标变换.

在这里首先定义物体坐标系和相机坐标系之间的旋转关系。设物体坐标系坐标轴的旋转顺序为:首先,绕着Z轴逆时针旋转?角;接着,绕着X轴逆时针旋转?角;最后,绕着Y轴逆时针旋转?角。如图2所示。

- 3 -

z

y

? ? o ?

图2 坐标系的旋转

x

?C??S?0???绕着Z轴逆时针旋转?角的旋转矩阵为: Rz??S?C?0?;

?001???0??10??绕着X轴逆时针旋转?角的旋转矩阵为:Rx??0C??S??;

?0S?C?????C?0S????11?; 绕着Y轴逆时针旋转?角的旋转矩阵为:Ry???0??S?0C????由以上定义的旋转矩阵为:

?C?S??S?S?C?S?C???C?C?+S?S?S???R?Ry×Rx×Rz??C?S?C?C?-S?? ; (1)

??S?C??C?S?S?S?S??C?S?C???C?C????

式中:R为正交矩阵,其中S≡sin,C≡cos。

物体坐标系和相机之间的位置关系为平移矩阵T。

由以上定义可以得到物体坐标系和相机坐标系之间的关系: C?R?O?T 1.2 像坐标的确定.

设点P在物体坐标系O0?X0Y0Z0中的坐标为P?x,y,z?,点P在相机坐标系

C0?XcYcZc中的坐标为P?X,Y,Z?,点P所成的像点p在相机坐标系C0?XcYcZc中的坐标为p?u*,f,v*?。由式(1)可得P?X,Y,Z?与P?x,y,z?之间的旋转关系为：

?X,Y,Z?T?R??x,y,z? (2)

T相机的成像原理为[3]:

- 4 -

X?*u?f???Y ? (3)

Z?v*?f???Y由式(1)、(2)和(3),可得所求像点?u*,v*?的坐标为: ?u*??C??*????v???S?S???u????? (4) C????v??式中:u??f?x?C??y?S?

x?C??S??y?C??C??z?S?x?S??S??y?S?C??zC? (5) v??f?x?C??S??y?C??C??z?S??x,y,z?为P在物体坐标系下的坐标

u?和v?是中间变量,是物体坐标系尚未绕相机坐标系Y轴旋转前的点的坐标。

2 问题（2）的解决

由上述问题（1）的模型中的式(5)可以得到关于P?x,y,z?所在的2个平面分别为:

?u?C?S??fC??x??u?C?C??fS??y??u?S??z?0 (6) ?v?C?S??fS?S??x??v?C?C??fS?C??y??v?S?C??y??v?S??fC??z?0 (7) 因此关于P?x,y,z?所在的直线方程可以由式(6)和式(7)得到,利用向量叉乘原理可以得到P?x,y,z?所在的直线方程的方向数为:

s?n1?n2 (8) 式中:n1和n2为平面的法向量,通过相机可以得到?u*,v*?,因此由式(8)可以得到点

P?x,y,z?所在的直线方程为:

?x???f?u??C??f2?S??C??f?v??S??S???t??2?y??f?u??S??f?C??C??f?v??C??S???t (9) ?2z?f?S??f?v??C???t???*?u???C??S???u?式中:t为参数,???????*?。 ?vS?C??????v?以上推导了物体坐标系中点的坐标和其所成像的像平面坐标之间的关系,通过式(9)可以看出关系中包含了物体坐标系和相机坐标系之间的旋转关系。

因为空间矩形的尺寸已知,且为正方形，假定其边长为L。定义空间矩形各顶点在物体坐标系下的坐标为:

?x1??x1?L??x1??x1?L??，D??y?，A??y?L?,C??y?L?,如图1所示。 E??y1???1??1??1??????z1???z1???z1???z1??矩形4个顶点对应于像平面的坐标为:

- 5 -

****????????uuuu3214****E??*?，D??*?，A??*?，C??*?通过式(9)可以得到如下关系:

?????v2???v1???v4???v3????f?u??C??f2?S??C??f?v??S??S??11?x1????y???f?u??S??f2?C??C??f?v??C??S???t ; (10)

11?1???1???z1???f2?S??f?v1??C?????f?u??C??f2?S??C??f?v??S??S??22?x1?L????y???f?u??S??f2?C??C??f?v??C??S???t; (11)

22?1???2???z1???f2?S??f?v2??C?????f?u??C??f2?S??C??f?v??S??S??33?x1????y?L???f?u??S??f2?C??C??f?v??C??S???t; (12)

33?1???3???z1???f2?S??f?v3??C?????f?u??C??f2?S??C??f?v??S??S??44?x1?L????y?L???f?u??S??f2?C??C??f?v??C??S???t; (13)

44?1???4???z1???f2?S??f?v4??C???*?u???C??S???u?式中: ???????*?. ?vS?C??????v?在这里将利用式(10)、(11)、(12)和(13),求出旋转角度?和?。由以上四式可以看出ti,ui*,vi*?1?i?5?都是未知的,只有各点在物体坐标系下的坐标是已知的。

通过观察式(10)和(12)以及式(11)和(13)有相同的x坐标和z坐标。所以由式(10)和(12)消去t1和t3,通过化简得到如下关系:

C??S??f?u1??u3??S??f?v1??v3??C??C??u1?v3??u3?v1??0 (14) 同理,由式(11)和(13)消去t2和t4通过化简得到如下关系:

??????C??S??f?u2??u4??S??f?v2??v4??C??C??u2?v4??u4?v2??0 (15) 将式( 14 )乘以?v2??v4??,式( 15 )乘以?v1??v3??,两式相减得:

????C??S??f??u1??u3?v2??v4??u2??u4?v1??v3??????? (16)

C?C??u1?v3??u3?v1?v2??v4??u2?v4??u4?v2?v1??v3???0????将式(16)除以C?,得到?的解为:

??????????????????????????v1?v2??v4??u2?v4??u4?v2?v1??v3??uv?u133????arctan?? (17)

???fu1??u3?v2??v4??u2??u4?v1??v3????????根据实际测量的情况,可以定义?的范围为?90?～90?。

????????????????- 6 -

将式( 14 )乘以?u2??u4??,式( 15 )乘以?u1??u3??,两式相减得:

????S?f?u2??u4?v1??v3??u1??u3?v2??v4??????? (18)

C?C??u1?v3??u3?v1?v2??v4??u2?v4??u4?v2?v1??v3???0????通过(18)可以得到?的解为:

??v3??u3?v1?v2??v4??u2?v4??u4?v2?v1??v3??u1?C?? ??arctan??? (19)

f????u1??u3?v2??v4??u2??u4?v1??v3???????只要知道了?,就可以得到?。同样,可以定义?的范围为?90?～90?。

通过式(17)和(19)可以看出,只要知道了?,那么?和?就可以求出来。现在的关键就是首先求出?。通过观察式(10)和(11)以及式(12)和(13)有相同的y坐标和z坐标。利用相同的方法,由式(10)和(11)可得:

????????????????????????????????S??S??f?u1??u2??C??f?v1??v2??S??C??u1?v2??u2?v1??0 (20) 由式(12)和(13)可得:

??????S??S??f?u3??u4??C??f?v3??v4??S??C??u3?v4??u4?v3??0 (21) 将式( 20 )乘以?z3??z4??,式( 21 )乘以?z1??z2??,两式相减可以得到?的另一独立

????的解法:

??????????????uv?uvv?v?uv?uvv?v2134344312??12??arctan?? (22)

?f?u1??u2?v3??v4??u3??u4?v1??v2?????????由式(17)和(22)关于?的两个独立的解法,可以得到:

????????????????421???????u?v??u?v???v??v????u?v??u?v???v??v???f??u??u???v??v????u??u???v??v??????? (23)

?u?v??u?v???v??v????u?v??u?v???v??v??f??u??u???v??v????u??u???v??v???????1331242413132424312213434431212343412*?u???C??S???u?在式(23)中,唯一的未知量是?,将???????*?代入式(23),就可以求?vS?C??????v?出?。

因为：

********u1?v3??u3?v1???u1C??v1S???u3S??v3C????u3C??v3S???u1S??v1C??

****2****2****??u1v3?u3v1?*C??(u1v3?u3v1)S??u1v3?u3v1同理:

- 7 -

****u1?v2??u2?v1??u1v2?u2v1****u2?v4??u4?v2??u2v4?u4v2

****u3?v4??u4?v3??u3v4?u4v3

则式(23)变为:

************???u2?S??v1S?v?v?C?v?vv?uv?v?C?v?v?u1*v3*?u3*v1*???????????2424424313?????********f?u?uv?v?u?uv?v?????????13242413???uv**12????uv???S??v?v??C??v?v????uv?uv??S??v?v??C??v?v??********f?u?uv?v?u?uv?v?????????12343412??**21*3*4*3*4**34**34*1*2*1*2(24)

由式(24)可以得到?的解为:

??arctan**?A?v1?v3??B?v2*?v4*??C?v1*?v2*??D?v3*?v4*?A?u?u??B?u?u??C?u?u??D?u?u*1*3*2*4*1*2*3*4? ， (25)

式中：****************A??u2v4?u4v2?E;B??u1v4?u4v3?F;D??u1v2?u2v1?F; v3?u3v1?E;C??u3********?E?f??u?uv?v?u?uv?v???????13242413??; ?********?F?f?u?uv?v?u?uv?v???????2343412??. ?1这样通过式(25)可以求解出?,然后再通过式(17)和(19)可以求解出?和?。定义?的范围为?90?～90?.

为求出?，我们要找到数据ui*,vi*?1?i?5?，为此从靶标的像图中近似求

把靶标的像经过处理加上网格线(如图3所示)，并算得靶标上各圆ui*,vi*?1?i?5?。

的圆心的像坐标如下：

A(-49.7857，51.65559)，B(-24.5423，49.31824)，C(33.89168，45.57846)， D(18.23139，-30.6194)，E(-60.3038，-30.3856) (单位：mm) 。 把上面A,B,C,D,E五个点中每四个点的坐标代入公式(17),(19),(25),利用Matlab求解[2]，在计算时把焦距近似的等于已知的像距，由此可得五组??,?,??的值，最后求其平均值得到??,?,?????4.36?7.60?8.49??。

[1]- 8 -

图3 像平面网格图

3、问题（3）的解决：

M0

M0?

M1

M1? M2

M2? 像 坐 标

??? 图4像点往复循环原理图

物 坐 标

- 9 -

为了讨论模型的精度和稳定性：我们设计如下一个方法（如图3）：在物体平面内任意选取一点M0?x0,y0,z0?，利用问题（1），（2）中所建立的模型，算得M0在像平面上对应的点M0?x0?,y0?,z0?，再利用模型找到M0?点在物平面上对应的点M1?x1,y1,z1?， 依次下去。

循环到第N次，得到点MN?xN,yN,zN?，然后用MN与M0两点间的距离来测量模型的精度，用往复循环的次数来衡量模型的稳定性。

显然，当N越大、MNM0越小时，则精度越高，稳定性越好；反之，N越小，MNM0越大时，则精度越低，稳定性越差。

从点Mn?xn,yn,zn? ?0?n?N?1?对应到像平面上的坐标Mn?xn?,yn?时。 由式（4）和（5）有： ?x???cos?sin???u???n??????v?? ?sin?cos??????yn????xncos??ynsin?式中：u??f?，

xncos?sin??yncos?cos??znsin?xsin?sin??ynsin?cos??zncos?， v??f?nxncos?sin??yncos?cos??znsin?从像平面上的点Mn?xn?,yn?，对应到物平面上的点Mn?1?xn?1,yn?1,zn?1?时，根据物体坐标的建立方式可知：物平面上的点在物体坐标系下的坐标中zn?1?0，由公式（4）和(5)可得：

f??xn?1cos??yn?1sin?????x???cos?sin???xcos?sin??ycos?cos??n?1n?1? ， (26) ?n???????sin?cos???f??xn?1sin?cos??yn?1sin?cos?????yn????????xn?1cos?sin??yn?1cos?cos???对于上述公式中xn?,yn?,?,?,?已知，所以对于上式隐式方程中xn?1与yn?1是可解的，只须

??????用Matlab实现循环计算。

不妨取物平面上的点(25,50)进行计算，其结果如下： 1 2 3 4 M?x,y,z? (25,50,O) (24.41,48.74,0) (23.76,47.16,0) (20.82,44.58,0) M?x?,y? (-37.36,14.01) (-38.50,13.64) (-39.41,13.28) (-40.87,12.14) ??循环次数 单位：mm

4. 问题（4）的解决

如前面问题（2）中建立两部相机的相机坐标系：C0?XcYcZc，Cc??Xc?Yc?Zc?，同样建立物体坐标系O0?X0Y0Z0（如图4）。

- 10 -

Z0 Y0 X0 T R

物体坐标系

O0

R?

T?

Zc

Xc

Yc C0

Zc'

Yc'

XC'

' C0相机1

图4 双目相机定位图

相机2

设物体坐标系和相机1，2之间的位置关系分别为平移矩阵T,T?由问题（2）可得，物体坐标系和相机1坐标系之间的关系为：

C?R?O?T

?C?S??S?S?C?S?C???C?C?+S?S?S???Rx×Rz??C?S?C?C?-S?? 其中R?Ry×??S?C??C?S?S?S?S??C?S?C???C?C????式中:R为正交矩阵,其中S≡sin，C≡cos。

?，?，?分别为物体坐标系绕着Z轴，X轴，Y轴逆时针旋转的角度。 同理，得到物体坐标系和相机2坐标系之间的关系为： C??R??O??T?

?C?R?O?T 所以?

???C?R?O?T?解得：

R?1?C?T??O ，R?1为R的逆矩阵；

R'?1?C??T???O， R'?1为R?的逆矩阵；

?C?T??RR'?1?C??T??

C?T?RR'?1C??RR'?1T? C?RR'?1C??(T?RR'?1T?);

即相机2与相机1之间的旋转矩阵为RR'?1,位置关系为平移矩阵(T?RR'?1T?),其中R与R?可利用与问题(2)相同的算法求出.T,T?可由实际中测得，至此,两部相机的相对位置可以确定。.

- 11 -

五 模型评价与改进

模型的评价：

1. 模型具有一般性，适用于任何位置关系的旋转变换。

2. 考虑坐标系沿三个坐标系旋转，省去了空间复杂的平面夹角之间的描述，使模型容易理解。

模型的改进：

由于在使用“网格标定法”求靶标上圆的圆心时精度不是很高，可考虑如下方法进行改进：用Matlab进行编程求靶标的像边界上最短距离与最长距离的交点作为圆心的像坐标。

六 参考文献

[1] 高学海，徐科军等，基于单目视觉和激光测距仪的位姿测量算法，仪器仪表学报，第28卷第8期：1479-1485，2007年。

[2] 石辛民，郝整清等，基于Matlab的实用数值计算，北京：清华大学出版社，2006年。

[3] 吴锡真，李祝霞等，物理手册，北京：北京大学出版社，2004年。

- 12 -

七 附录

部分程序如下： 第二问程序

u=[-49.7857 -24.5423 33.89168 -60.3038]; v=[51.65559 49.31824 45.57846 30.3856]; f=1577/3.78;

E=f*(-(u(1)-u(3))*(v(2)-v(4))+(u(2)-u(4))*(v(1)-v(3))); F=f*((u(1)-u(2))*(v(3)-v(4))-(u(3)-u(4))*(v(1)-v(2))); D=(u(1)*v(2)-u(2)*v(1))/F; C=(u(3)*v(4)-u(4)*v(3))/F; B=(u(1)*v(3)-u(3)*v(1))/E; A=(u(2)*v(4)-u(4)*v(2))/E; b=[u;v];

kes=atan((-A*(v(1)-v(3))+B*(v(2)-v(4))-C*(v(1)-v(2))+D*(v(3)-v(4)))/((A*(u(1)-u(3))-B*(u(2)-u(4))+C*(u(1)-u(2))-D*(u(3)-u(4))))) c=[cos(kes) -sin(kes);sin(kes) cos(kes)]; h=c*b;

fai=atan(((h(1,1)*h(2,3)-h(1,3)*h(2,1))*(h(2,2)-h(2,4))-((h(1,2)*h(2,4))-h(1,4)*h(2,2))*(h(2,1)-h(2,3)))/(f*(h(1,1)-h(1,3))*(h(2,2)-h(2,4))-(h(1,2)-h(1,4))*(h(2,1)-h(2,3))))

the=atan((cos(fai)*((h(1,1)*h(2,3)-h(1,3)*h(2,1))*(h(2,2)-h(2,4))-(h(1,2)*h(2,4)-h(1,4)*h(2,2))*(h(2,1)-h(2,3))))/(f*((h(1,2)-h(1,4))*(h(2,1)-h(2,3))-(h(1,1)-h(1,3))*(h(2,2)-h(2,4)))))

第三问程序 x=25;y=50;z=0; f=1577/3.78;

kes=8.49332;fai=9.6009;the=-4.36228

m=f*(x*cos(the)-y*sin(the))/(x*cos(fai)*sin(the)+y*cos(fai)*cos(the)-z*sin(fai));

n=f*(x*sin(fai)*sin(the)+y*sin(fai)*cos(the)+z*cos(fai))/(x*cos(fai)*sin(the)+y*cos(fai)*cos(the)-z*sin(fai)); e=[m;n];

j=[cos(-kes) sin(-kes);-sin(-kes) cos(-kes)]; g=j*e

注：程序中the、kes、fai 分别表示?，?，?。

- 13 -

本文来源：https://www.bwwdw.com/article/qci3.html