张正友算法的原理：这里假定模板平面在世界坐标系Z=0的平面上：如下图，其中，K为摄像机的内参数矩阵，[X Y 1]T为模板平面上点的齐次坐标，[u v 1]T为模板平面上点投影到图象平面上对应点的齐次坐标，[r1r2r3]和t 分别是摄像机