数码相机定位（数学建模）
我们组的答案，文章中的有些公式复制不上，大家就凑和这看吧，呵呵
数码相机定位
【摘要】 双目测试法的数码相机定位，精度准确的关键在于对相机内外参数的标定[1]。在不考虑切向畸和变径向畸变的前提下建立理想的针孔成像模型，通过基于Sobel算子图像的边缘检测法确定靶标上偏移以后的象上成像边缘在平面中的坐标，然后根据基于随机Hough变换的图形检测法确定成像上的靶标的圆心的坐标，采用传统的线性法求出相机参数初值，再用最小二乘法进行线性拟合优化初值，利用投影成像的几何关系容易得出要特征点在相机坐标系的坐标，进而求出相机的相对位置。
【关键词】双目测试 相机标定 线性法 靶标定位最小二乘法 Sobel算子边缘检测 随机Hough变换
一、 模型的基本假设与符号说明
1.模型的基本假设：
（1）假设拍摄时光线正常。
（2）假设 , 位于图像中心[2]
（3）假设成像过程严格遵守针孔成像模型无几何畸变
（4）假设实体靶标平面平行与世界坐标系的Ow-XwYw即 =0
2.符号说明：
（1） 以实物的固定点为原点建立世界坐标系Ow-XwYwZw。
（2） Pw表示实物上的观测点。
（3） 以O为原点建立以像素为单位的像平面的坐标系O-UV。
（4） 以主光轴和像平面的交点为原点建立以mm为单位的像平面物理坐标系O1-XY。
（5） Pd 为实际在像平面上的影像。
（6） 以相机的固定点为原点建立相机的坐标系Oc-XcYcZc.
二、 模型的建立
1.相机成像模型
相机的成像过程可以用传统的针孔成像模型来模拟，那么建立模型如图1所示包含世界坐标系Ow-XwYwZw、相机坐标系Oc-XcYcZc、像平面坐标系O-UV(像素单位)、像平面物理坐标系O1-XY(毫米单位)。Pw(Xw,Yw,Zw) 为世界坐标系中点Pw的坐标，Pc (Xc,Yc,Zc)为同一点在相机坐标系下的坐标, Pu(u,v)为理想针孔模型下Pw的像点坐标, Pu(x,y)则为理想情况下的像平面物理坐标。
图1-1
1.1针孔成像模型
根据针孔成像原理，世界坐标点Pw(Xw,Yw,Zw)到理想坐标点Pu(u,v)的齐次变换如下：
= (1-1)
其中A为内参矩阵，[R T]为相机外参矩阵，和T分别为平板相对相机的旋转矩阵和平移矩阵。定义如下[3]：
, ，
其中 , 分别分别表示在x方向和y方向上像点的物理坐标到图像坐标的比例系数， 、 表示主光轴与像平面交点的图像坐标。设世界坐标系的0-XY面平行于平板，则 =0，代入公式(1-1)式可得：
(1-2)
进一步消去s即可得到理想针孔成像的数学模型：
(1-3)
而实际像点物理坐标 到实际像点 的关系如下：
(1-4)
那么由公式(1-3)、(1-4)可得理想像点的物理坐标与世界坐标之间的关系：
(1-5)
2.相机标定模型
从实际成像模型可以看出需要标定的有6个外部参数，即旋转矩阵中反映的绕3个坐标轴的旋转角以及平移矩阵中沿3个坐标轴方向的位移，4个内部参数，即 、 , 。这里采用逐步推导求解的方法。
2.1投影矩阵模型
根据题意可知 在图像中心，所以选择其为初值，由公式(1-2)可得
(1-6)
那么可以设投影矩阵
(1-7)
则(1-9)可以重写为
(1-8)
将(1-8)消去 ，移项整理得
(1-9)
其中 T
每组像点与世界坐标点可以确定一个方程组，8组对应点则可求出未知参数 ，而图像坐标的取值存在误差，实际标定点应该多于8个，然后统计结果用正态分布来获取最接近实际值的结果。
2.2转换矩阵模型(外部参数)
先利用旋转矩阵R的正交性，通过变换可得
然后便可根据(1-7)求出旋转矩阵R和平移矩阵T。
2.3内部参数模型
根据转换矩阵模型可计算出 ，则由消去S的理想针孔成像模型(1-3)解得 。
3.求取成像坐标的模型的建立
依据图像处理算法得到的图象二值化、高通滤波、Sobel边缘算子的处理结果，再用Sobel算子进行处理后，又对它进行了二值化处理，最终得到了成像边缘上的一个点在成像平面上的坐标，把坐标值D[0][0]存放到数组D[m][n]中。如此重复有限次，获得图象边缘上的足够多的点，并把点的坐标值D[i][j]保存到数组D[m][n]中。在数组D[m][n]中任意取三点D[X1][Y1]、D[X2][Y2]、D[X3][Y3]根据基于随机Hough变换为基础直接提取椭圆的技术获取椭圆的重心这个重心就是实际图像对应的靶标Pd
3.1图像边缘检测
采用边缘检测来在实际的成像中取出任意多个份额有效的点常用的边缘检测算子有五种（梯度算法、Prewitt，Sobel，Roberts，Robins．Mar-Hildret等）在这里主要采用梯度算子，其优点是简单有效。
3.1.1边缘检测算法的基本步骤：
1)滤波：改善与噪声有关的边缘检测器的性能；一般滤波器降导
致了边缘的损失；增强边缘和降低噪声之间需要折衷。
2)增强：将邻域强度值有显著变化的点突显出来，边缘增强一般
是通过计算梯度幅值来完成的。
3)检测：最简单的边缘检测判据是梯度幅值阀值。
4)定位：边缘的位置和方位在子像素分辨率上估计，在平面坐标系中确定实际成像点的坐标。
3.2定理公式的整理
Sobel算子是一种一阶微分算子，它利用像素邻近区域的梯度值来计算1个像素的梯度，然后根据一定的阈值来取舍。它由下式给出：
M= (2)
其中的偏导数用下式计算
Sx=(a2+ca3+a4)-(a0+cd7+a6)
Sy=(a0+ca1+a2)+(a6+ca5+a4)
其中常熟c=2。Sx、Sy+可用卷积模板来实现，如下图：
|
-1 |
0 |
1 |
|
|
Sx= |
-2 |
0 |
2 |
|
|
-1 |
0 |
1 |
(图3-1 卷积模板x)
|
1 |
2 |
1 |
|
|
Sy= |
0 |
0 |
0 |
|
-1 |
-2 |
-1 |
(图3-2 卷积模板y)
图像中的每个点都用这两个模板做卷积。一个模板对通常的
垂直边缘响应最大．而另一个对水平边缘响应最大。两个卷积的最大值作为该点的输出值。运算结果是1幅边缘幅度图像。此算子对灰度渐变噪声较多的图像处理得较好
|
a0 |
A1 |
A2 |
|
A7 |
[i.j] |
A3 |
|
A6 |
A5 |
A4 |
(图3-3 际对应的坐标)
对于平面中不规则图形可以用以及差分代替一阶微分
求梯度时对于平方和运算及开方运算，可以用两个分量的
绝对值之和表示，即：
Sobel梯度算子是先做成加权平均，再微分，然后求梯度，即：
G[f(x,y)]=|Δxf(x,y) |+|Δyf(x,y) |
上述各式中像素之间的关系见下图：
|
f(x-1,y-1) |
f(x,y-1) |
F(x+1,y_1) |
|
f(x-1,y) |
f(x,y) |
f(x+1,y) |
|
f(x-1,y+1) |
f(x,y+1) |
f(x+1,y+1) |
(图3-4 像素关系表)
3.3.基于随机Hough变换的椭圆型检测法求椭圆中心模型
算法简要描述如下:设D为图像空间的边缘点集;P为参数空间的参数单元集，是动态链表结构。从D中随机采样5点，计算这5点所确定的二次曲线参数P，看是否存在一参数PC和P的误差在容许范围之内。若有，则将参数单元PC的计数值score加1并将参数单元PC更新;若没有，则在P中插入新的参数单元P。当参数单元PC的计数值score达到指定阂值Nt(它是一个很小的数，例如2, 3)时，该参数对应的椭圆成为候选椭圆，判断该候选椭圆是否为真实椭圆，若是，则说明已检测到了一个椭圆，将落在该椭圆上的点从D中去掉，并释放P中所有参数单元占用的内存，然后继续检测下一个椭圆;否则，说明该侯选的椭圆为虚假椭圆，从P中去掉该参数单元，继续检测。
4.1具体标定的过程
二次曲线在空间坐标系中的表示：
Ax2+Bxy+Cy2+2Dx+2Ey+F=0 （4-1）
其中(x,y)为图像控件的坐标，A、B、C、D、E、F是二次曲线的参数，其中F是自由参数。
B2-AC<0 (4-2)
B2-AC<0且A=C (4-3)
若A、B、C满足(1-2)二次曲线为椭圆，若A, B, C满足(1-3)则二次曲线为圆。
将随机Hough变换(RHT)直接用于椭圆检测时，需随机采样5个点，由方程(1-1)得5个方程，由它们构成关于参数的线形方程组，对其进行求解，若有解且该解满足式（1-1），则对该解（也就是求得的参数)进行累积。
4.2椭圆中心提取方法
本算法是利用由椭圆的极点一极线性质开发的三点组到椭圆参数的收敛映射，是一个以随机Hough变换为基础直接提取椭圆的技术。
椭圆的基本性质
定义:若椭圆上的任意两点P和Q的切线交于点N，则称这两点间的连线PN为点A所对应的极线，点A为弦PN所对应的极点，三角形APN为椭圆的极三角形。如下图所示：
（图 3-5 椭圆中心的确定示意图）
性质1:如图3-5所示，极点A、极线P口的中点E与椭圆中心。共线。
性质2:如图3-5所示，若点D为射线OA与弧PN的交点，则过点D的切线平行
性质3：如图3-5所示，若三点组K, P和N在椭圆上，则此三点组所对应的3个极三角形APN, BPK, CNK的极线边PN, KP,NK上的中线延长线必共点于椭圆的中心Oo
利用椭圆方程和椭圆的极点一极线性质可推导出椭圆点的方向和中心位置所应满足的约束条件。这样，就可以确定椭圆中心位置。即靶标象Pu的坐标。
在图像上（如下图3-6）根据上述的“求取成像坐标的模型的建立”利用边缘取点法，求取有限组点坐标数据记录到无穷大的一个二维数组D[i][j]记录到下表中
（图3-6 成像照片上的一个变形后的形状）
|
A' |
B' |
C' |
D' |
E' |
|||||
|
X坐标 |
Y坐标 |
X坐标 |
Y坐标 |
X坐标 |
Y坐标 |
X坐标 |
Y坐标 |
X坐标 |
Y坐标 |
|
102.8 |
56.9 |
137.4 |
61.3 |
212.8 |
77.1 |
87.0 |
178.8 |
192.8 |
180.1 |
|
103.6 |
56.6 |
139.3 |
59.4 |
215.2 |
68.8 |
87.4 |
173.4 |
198.3 |
170.2 |
|
107.2 |
54.9 |
148.7 |
55.6 |
220.4 |
63.8 |
94.7 |
106.7 |
202.5 |
167.4 |
|
112.7 |
52.5 |
155.1 |
57.4 |
229.2 |
69.7 |
99.4 |
164.9 |
215.8 |
169.6 |
|
118.0 |
52.9 |
162.1 |
64.3 |
233.6 |
64.5 |
105.7 |
165.7 |
216.9 |
177.3 |
|
120.5 |
53.9 |
160.8 |
76.3 |
238.4 |
72.7 |
109.6 |
167.5 |
213.2 |
184.3 |
|
126.1 |
72.6 |
155.2 |
81.4 |
235.2 |
82.1 |
112.4 |
170.0 |
207.8 |
188.6 |
|
126.8 |
71.5 |
151.5 |
83.1 |
228.7 |
87.6 |
114.1 |
173.7 |
202.3 |
189.2 |
|
115.9 |
81.1 |
140.1 |
79.8 |
223.0 |
88.5 |
112.7 |
180.9 |
197.6 |
186.8 |
|
108.1 |
80.1 |
139.6 |
79.6 |
215.3 |
84.9 |
108.9 |
185.1 |
193.6 |
182.3 |
|
102.8 |
76.1 |
136.7 |
75.5 |
213.6 |
82.3 |
93.3 |
188.3 |
192.7 |
177.9 |
|
112.1 |
65.4 |
145.3 |
64.2 |
220.1 |
72.1 |
102.3 |
179.2 |
205.1 |
179.6 |
|
113.4 |
66.8 |
148.6 |
69.3 |
225.4 |
75.2 |
100.1 |
176.9 |
205.5 |
177.3 |
（表3-7 实际测量数据）
然后根据上述的“基于随机Hough变换的椭圆型检测法求椭圆中心模型”确定不规则图形的中心。分别如下：
|
推算的靶标象的坐标 |
靶标象对应的坐标 |
|||
|
X坐标 |
Y坐标 |
X坐标 |
Y坐标 |
|
|
A' |
112.1 |
65.4 |
113.4 |
66.8 |
|
B' |
145.3 |
64.2 |
148.6 |
69.3 |
|
C' |
220.1 |
72.1 |
225.4 |
75.2 |
|
D' |
102.3 |
179.2 |
100.1 |
176.9 |
|
E' |
205.1 |
179.6 |
205.5 |
177.3 |
(表3-8 实际靶标的位置和计算的到的靶标 )
4.求两相机距离d的模型
4.1求平面夹角
（图4-1 两平面夹角）
如上图4-1所示：在以左相机为基准的空间坐标系中原图象所在的平面可以通过世界坐标系和基准坐标系的转换公式(1-3)得到其在基准坐标系中的平面表示公式：
(4-1)
即上图中的CDEF平面在基准坐标系中的表示
又由于平面ABCD和基准平面的xy平面相平行，故平面ABCD的坐标表示也可以求出设其表示公式如下：
（4-2）
应为两平面无限延伸会得到一个交线（假设不是在特设的情况下两平面）通过公式()和公式()可以求出一条交线CD在空间坐标系中的表示，其公式表示：
下面利用空间点到直线的距离即图中直线O’Oc’和直线OO’的距离。在由点Oc点O、点O’构成的三角形平面中直线O’Oc’、 直线OO’和直线OOc的三角形中根据余弦定理知道角∠OO’Oc的角度即两个平面平面ABCD和平面CDEF间的面夹角。
4.2
(图4-2 求取d的示意图))
图中点说明：（此图为从整个空间上方的俯视图）
Oc：左侧相机的光学中心即三维原点
Oc’：右侧相机的三维原点
O: 实际物体所在三位坐标的原点该点在平面ABCD中
Pu: 左面相机理想成像点
Pu’: 右面相机理想点成像
直线OO’：Oc和Oc’在平面ABCD中的投影
平面ABCD: 空间俯视后对原图象的平面
通过点Pu向原图象ABCD做垂线，在直角三角形P’PuO和平面ABCD的三垂线定理计算出O’’O与OcO的夹角∠O’’OOc.同理可以求出∠O’OOc’，则根据平角定理可以求出∠OcOoC’：
∠OcOoC’=Л-∠O’OOc’-∠O’’OOc
在三角形O’OOc’中已经知道两边和两边夹角利用余弦定理可以求得相机的相对位置d：
三、 模型的求解
1. 相机成像模型和相机标定模型的求解
求解针孔成像模型必然要先进行相机内外参数的标定即相机标定模型。在引入先前的假设 在图像中心后求的投影矩阵，通过投影矩阵D分离其参数可以简化计算。然后根据多组对应点利用公式(9)即可求得D的参数。这里假设世界坐标的原点为靶标的中心(即正方形中心),则可得对应点的结果：
|
|
A |
C |
D |
E |
||||
|
|
X坐标 |
Y坐标 |
X坐标 |
Y坐标 |
X坐标 |
Y坐标 |
X坐标 |
Y坐标 |
|
世界坐标系的坐标 |
-50 |
50 |
50 |
50 |
50 |
-50 |
-50 |
-50 |
|
像平面的坐标 |
113.4 |
66.8 |
225.4 |
75.2 |
100.1 |
176.9 |
205.5 |
177.3 |
用MATLAB求得
在利用投影矩阵的变换求得 后，便可通过(7)来求得转换矩阵 ，进而由(1-3)求出 。
2.成像坐标模型的求解
3.两相机距离d模型的求解
四、 模型检验方法和评价
五、 参考文献
[1]马颂德，张正友．《计算机视觉——计算理论与算法基础[M]》．北京：科学出版社，1998
[2] 毛剑飞 邹细勇 诸静. 改进的平面模板两步法标定摄像机 中国图象图形学报：A辑.2004,9(7).-846-852
[3]Heikkila J．Geometric camera calibration using Circular control
points l-J]．IEEE Transactions on Pattern Analysis and Machine Intelligence，2000，22(10)：1066～ 1077．
