张正友标定算法理论及算法实现
1999年，微软研究院的张正友提出了基于移动平面模板的相机标定方法。此方法是介于传统标定方法和自标定方法之间的一种方法，传统标定方法虽然精度高设备有较高的要求，其操作过程也比较繁琐，自标定方法的精度不高，张正友标定算法克服了这两者的缺点同时又兼备二者的优点，因此对办公、家庭的场合使用的桌面视觉系统(DVS)很适合。
设三维世界中坐标的点为：
为方便运算，模板被定义在世界坐标系中与X-y平面平行(即Z=0)的平面上，
根据旋转矩阵的性质
根据矩阵知识，当所取图像数目大于等于3时含有5个未知的内参数的矩阵A可以被唯一求解。
B矩阵是个对称矩阵，所以可以写成一个六维向量形式：
我们把H矩阵的列向量形式为：
式(b)改写成：
最后根据内参数的限制条件式(a)：
矩阵V是2x6矩阵，即每张照片可以建立两个方程组，包含六个未知数。根据线性代数知识可知，要求解所有未知数至少需要六个方程组，因此这样至少需要三张照片才能解出所有未知数。b解出后就可以解出相机内参矩阵A，然后解得每张图像的外部参数R、t：
其中
(1)极大似然估计
当初始参数已经解出后，每张图像的控制点根据求解的参数再还原回出三维世界坐标，再通过建立非线性最小化模型优化解得值与真实值的差异，也即：
利用这个模型结合LM优化算法，就可以得到最优化的参数。
(2)径向畸变的处理
这个模型建立到此还为考虑过镜头的畸变问题，但一般情况下的相机都会存在一定程度的径向畸。这里我们只考虑二次畸变。
设(u , v)为理想的像素坐标，
其中k1与k2为径向畸变系数。对于中心点畸变同样适用。然后由 
已知，n副图的m个点，通过最小二乘法求解这个线性方程组的解。一旦k1、k2得到后，就可以利用上式替代通过处理式极大似然公式中的
除此之外，求解出的畸变系数还可以通过极大似然估计来优化，其公式为：
首先，对于张正友算法的标定点选取，一般是选择能均匀分布于整个图像的一些点。对于相机的标定精度而言，选取的点越多，反应图像的信息越完整，因而得到的结果越好。但是随着点数的增加，其运算量增加，同时增加点数带来精度提高的效果随着点数增加而逐渐减弱。因此，通常情况下张正友算法选取均匀分布于图像上的十个点。本次试验中共取用了同一摄像机从不同角度拍摄的六幅图像，从每幅图像中取十个特征点来进行摄像机标定(这十个点的世界坐标相同)。图中显示了这六幅图，每幅图的大小为384×512。
具体流程是：
1)把被取的十个点的世界坐标(齐次坐标)进行转置。
2)对单应性矩阵求解并优化。
3)把六幅图的单应矩阵求解出来后求解出6向量(B矩阵)。因为每个单应矩阵可以得到两个方程，通过循环对矩阵y赋值后，再对y进行正交分解即可得到6向量。进而得到相机的内参矩阵。
4)先求解出相机的外参，然后对畸变系数进行求解，得到相机坐标（Xc, Yc, Zc）。
5)调用函数对内参和畸变系数进行优化，并显示优化后的结果。然后根据优化后的结果求解外参矩阵。
6)从旋转矩阵中分解出独立变量(三个坐标的转角)，再得到平移矩阵，最后把它们和内参、畸变系数一起优化进行最终优化。
******************
作者：hao_09
时间：2015/8/13
文章地址：http://blog.csdn.net/lsh_2013/article/details/47617909
******************
版权声明：本文为博主原创文章，未经博主允许不得转载。
原文地址：http://blog.csdn.net/lsh_2013/article/details/47617909
