由左右相机像素点坐标反求世界坐标点

最小二乘法求解三维坐标值
       下面我们来具体介绍求解三维点坐标的算法。在进行讨论之前，我们要明确一下几点基本假设：空间任意点P在两个摄像机C1与C2上的图像点平p1与p2已经从两个图像中分别检测出来。即已知p1与p2为空间同一点P的对应点。至于对应点是如何找到的，这属于图像配准问题。还有一个假设就是，摄像机C1与C2已标定，他们的投影矩阵分别为M1与M2，于是有：
                   
        其中，(u1,v1,1)与(u2,v2,1)分别为P1与P2点在各自图像中的图像齐次坐标；(X,y,Z,1)为P点在世界坐标系下的齐次坐标；mijk为Mk的第i行第j列元素。式(4.1)与式(4.2)消去Zc1与Zc2得到关于X,Y,Z的四个线性方程：
              
       由于空间点P是O1p1与O2p2的交点，它必然同时满足式(4.3)，式(4.4)，我们可以联立这四个式子，从而求出P点的坐标(X,Y,Z)。四个方程，三个未知数，也就是说这个方程组只有三个独立的方程，这是因为我们已经假设P1与P2点是空间同一点P的对应点，因此已经假设了直线O1p1与O2p2一定相交，或者说，这四个方程必定有解，而且解是唯一的。而实际应用中，由于数据总是有噪声的，我们可以用最小二乘法求出P的三维点坐标(X,Y,Z)。