张氏标定法原理及其改进1

张正友算法原理及其改进由于世界坐标系的位置可以任意选取，我们可以假定世界坐标系和摄像机坐标系重合，故定义模板平面落在世界坐标系的0W Z =平面上。

用i r 表示R 的每一列向量，那么对平面上的每一点，有：[][]123120111W W W W X u X Y s v A r r r t A r r t Y ⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥==⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦这样，在模板平面上的点和它的像点之间建立了一个单应性映射H ，又称单应性矩阵或投影矩阵。

如果已知模板点的空间坐标和图像坐标，那么就已知m 和M ，可以求解单应性矩阵H 。

)1,,(w w Y X )1,,(v u因为11W W u X s v H Y ⎡⎤⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦,其中11121321222331321h h h H h h h h h ⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦,可推出： 11121321222331321W W W W W W su h X h Y h sv h X h Y h s h X h Y =++⎧⎪=++⎨⎪=++⎩故，1112133132212223313211W W W W WW W W h X h Y h u h X h Y h X h Y h v h X h Y ++⎧=⎪++⎪⎨++⎪=⎪++⎩将分母乘到等式左边，即有31321112133132212223W W W W W W W W uX h uY h u h X h Y h vX h vY h v h X h Y h ++=++⎧⎨++=++⎩又令[]Th h h h h h h h h 3231232221131211='，则10000001WW W W WWWW X Y uX uY u h X Y vX vY v --⎡⎤⎡⎤'=⎢⎥⎢⎥--⎣⎦⎣⎦多个对应点的方程叠加起来可以看成Sh d '=。

利用最小二乘法求解该方程，即1()T T h S S S d -'=，进而得到H 。

摄像机内部参数求解在求取单应性矩阵后，我们进一步要求得摄像机的内参数。

首先令i h 表示H 的每一列向量，需要注意到上述方法求得的H 和真正的单应性矩阵之间可能相差一个比例因子，则H 可写成：[][]12312h h h A r r t λ=又因为1r 和2r 是单位正交向量，所以有1210T T h A A h --=111122T T T T h A A h h A A h ----=这样就为内参数的求解提供了两个约束方程。

下面，令()()()1112131212223313233002222000222222222000000002222222111T B B B B A A B B B B B B v u v u v v u v u v u v v γβγααβαβγγβγγαβαββαββγγβγβγβαβαββαββ--⎡⎤⎢⎥==⎢⎥⎢⎥⎣⎦⎡⎤--⎢⎥⎢⎥⎢⎥--⎢⎥=+--⎢⎥⎢⎥---⎢⎥--++⎢⎥⎣⎦注意到B 是一个对称矩阵，所以它可以由一个6维向量来定义，即[]111222132333Tb B B B B B B =设H 的第i 列向量为123[,,]T i i i i h h h h =，因此有 T T i j ij h Bh V b =其中111221223113322333,,,,,Tij i j i j i j i j i j i j i j i j i j V h h h h h h h h h h h h h h h h h h ⎡⎤=+++⎣⎦，那么，就可以将内参数的两个约束写成关于b 的两个方程为：1211220TT T V b V V ⎡⎤=⎢⎥-⎢⎥⎣⎦如果有n 幅图像的话，把它们的方程式叠加起来，得到0Vb =其中，V 是一个26n ⨯的矩阵。

当3n ≥时，一般情况下，b 可以在相差一个尺度因子的意义下唯一确定；当2n =时，此时的方程的个数少于未知数的个数，我们可以加上一个附加约束0γ=，即120B =，因此可用[]0100000b =作为式的一个附加方程。

方程的最小二乘解即是T V V 的最小特征值对应的特征向量，将该向量归一化即得到要求的b ，进而得到B ；当1n =时，两个方程只能解两个未知数，我们可以假定光心投影在图像的中心，从而求出摄像机在水平和垂直方向上的最大倍数。

一旦b 被求出，就能根据下面两种方法计算出摄像机的内参数矩阵A ： (1)由b 构造出B ，再利用Cholesky 矩阵分解算法求解出1A -，再求逆得到A 。

(2) 由b 构造出B ，在相差一个尺度因子的意义下(1T B A A λ--=,其中为λ尺度因子) ,由绝对二次曲线的性质，很容易求出摄像机的内部参数：2012131123112212()/()v B B B B B B B =-- 2221301213112311[()]/B B v B B B B B λ=-+-x fy f =212/x y B f f αλ=-20013//x x u v f B f αλ=-摄像机外部参数求解由每幅图像的单应性矩阵H 和上一节的计算结果就可以求得每幅图像的外部参数。

一旦A 求得后，根据式，每幅图像的外部参数很容易求出：111r A h λ-= 122r A h λ-= 312r r r =⨯ 13t A h λ-=这里的尺度因子11121/1/A h A h λ--==。

当然，由于图像必然有噪声，因此这样解得的123[,,]R r r r =并不能完全满足旋转矩阵的性质，所以要从一个给定的矩阵求解一个最佳的旋转矩阵。

非线性优化（优化内参）以上我们所得到的摄像机的内参数矩阵和每幅图像对应的外参数矩阵都只是一个粗糙解，没有具体的物理意义，可以通过最大似然估计对所有参数进行非线性优化，进一步求精。

在这里可以假定有n 幅关于模板平面的图像，模板平面上有m 个标定点，那么可建立评价函数：()211,,,nmij i j i j C m m A R t M ===-∑∑其中ij m 是第i 幅图像中的第j 个像点，i R 是第i 幅图坐标系的旋转矩阵，i t 是第i 幅图坐标系的平移向量，j M 是第j 个点的空间坐标，(),,,i j m A R t M 是通过这些已知量求得的像点坐标。

由于旋转矩阵有9个参量但是只有三个自由度，因此可用三个参量的矢量来表示，即一个旋转可由一个三维向量即旋转向量来表示，他的方向就是旋转轴的方向，他的模等于旋转角。

由三个欧拉角参数确定，123(,,)T r r r r 是旋转矩阵的罗德里克(Rodrigues)表示，R 与r 之间的关系由公式给出：22sin /[](1cos )/[]x x R I r r θθθθ=++-其中，旋转向量T z y x r ),,(=，定义由它构成的反对称矩阵为⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡---=000][x y x z y z r x ， 其中||||222r z y x =++=θ是旋转角。

使评价函数最小的,,,i j A R t M 就是这个问题的最优解。

这是一个经典的非线性最小二乘问题，对式求极小值仍采用Levenberg-Marquardt 算法来求解，其初始估计可利用上面线性求解的结果。

很明显可以看出，计算顺序依次是投影矩阵、内部参数、外部参数，最后进行优化。

对径向畸变处理(u,v)理想像素坐标，),(v u 为实际的像素坐标，同样(x,y)和),(y x 为理想和实际的图像坐标。

⎪⎩⎪⎨⎧++++=++++=])()([])()([22222212222221y x k y x k y y y y x k y x k x x x 其中，21,k k 为径向畸变，对于中心点畸变同样适用：⎪⎩⎪⎨⎧+++-+=+++-+=])()()[(])()()[(2222221022222210y x k y x k v v v v y x k y x k u u u u 可以通过下面的方法求解畸变系数：⎥⎥⎦⎤⎢⎢⎣⎡--=⎥⎦⎤⎢⎣⎡⎥⎥⎦⎤⎢⎢⎣⎡+-+-+-+-v v u u k k y x v v y x v v y x u u y x u u 2122202202220220))(())(())(()()( 高斯一牛顿方法优化：可以利用极大似然估计来得到畸变系数：21121||),,,,,(||∑∑==-n i mj j i i ijM t R k k A m m非线性优化的过程：采用高斯一牛顿方法，而没有用Levenberg 一Marquardt(LM)法是因为在实际应用中发现，如果没有先验知识，LM 法阻尼因子u 不知道去什么值合适，而随意的取值使得方程在求解时收烟速度慢。

而高斯一牛顿法在求解的附近一般收敛速度快，虽然对初始值要求严格，当初始值与实际值偏离的太远，其系数阵J T J 容易出现病态解，但是，所以参数的初始值必须要做优化。

这里),,,,(j i i c M T R K K m 是点j M 按照投影公式在第i 幅图片上的投影，K 是摄像机标定内参数矩阵，c K 是畸变系数矩阵，i R 、i T 是第i 副图像的外参数矩阵，其中R 通过Rodrigues 公式用三个参数的旋转向量r 来表达。

优化的步骤：(1)、首先确定目标函数。

这里的优化的目的是使优化后的外参数，按照本文建立的摄像机模型将空间点投影到图像平面上时，投影点与实际获取的图像控制点之间的残差平方和最小。

不妨设投影函数为其中K 是摄像机内参数矩阵，是畸变系数矩阵，是旋转向量，T 是平移矩阵，是模板平面上控制点i M 的坐标。

是投影点图像像素坐标，该点实际的图像坐标是，共n 个控制点。

要优化外参数，则最后需要的偏导数系数Jacobian 矩阵r m i ∂∂/和T m i ∂∂/得组合，而函数)显然是个复合函数，因此这实际上是个复合函数求偏导的问题。

下面针对第一次迭代即r=r 0、T=T 0为初值时的一个点来分步求解第一步，是r 与R 的转换。

由Rodrigues 公式可以求出.第二步，是世界坐标系到摄像机坐标系的转换。

由坐标转换公式求出摄像机坐标系下的坐标及相应偏导数系数阵：这里偏导数矩阵中的下标i 表示求出偏导数矩阵后，再将当前点(即第i 点)各相应坐标代入以求得偏导数系数矩阵。

同时可以看出，若直接以R 为参量，因为复合函数求偏导是偏导矩阵相乘的关系，则由于是3*9矩阵而。

第三步，将厉ci M 转化为归一化图像坐标。

设ci M 的归一化图像坐标为ni M ， 可得ni M 的计算公式及其偏导数系数阵：注意：表示qxp 系数矩阵B 将与矩阵A 中的所有分块矩阵ij A (i=1,2,,n,j=1,2,,m)均相乘，其中分块矩阵ij A (i=1,2,,n,j=1,2,,m)的行数等于系数矩阵B 的列数p 。