三维点云获取——空间点的计算

我们的目标是通过有效区域内的一组对应点m，m´来求得空间点M的坐标，设m，m’和M的坐标分别为(u,v,1)，(u’,v’,1)和(X,Y,Z,1)。
首先我们需要知道相机的投影矩阵，设左目相机的投影矩阵为P，右目相机的投影矩阵为P’，那么有：

K是左目相机的内参，I是单位矩阵，K’是右目相机的内参，R和T是右目相机的外参，即右目相机相对于左目相机位姿的旋转和平移，不难得知，这些参数都可以通过标定来获得，也就是说投影矩阵P和P’可以通过计算得到。
然后，我们就可以利用投影矩阵，通过对应点m和m’来求空间点M的坐标。对于左目相机来说，有：

由于m和M的坐标最后一项为1，所以设左右两边相差一个常量λ，因此式(1.2)可以表示如下：

按行展开可得：

将式(1.4)与式(1.6)相除可以消去常量λ，得：

即：

再将式(1.5)与式(1.6)相除可以消去常量λ，得：

即：

同理对于右目相机来说，也有：

由于m’和M的坐标最后一项为1，所以可以设左右两边相差一个常量λ’，因此式(1.11)可以表示如下：

按行展开：

将式(1.13)与式(1.15)相除可以消去常量λ^’，得：

即：

再将式(1.14)与式(1.15)相除可以消去常量λ^’，得：

即：

联合式(1.8)、(1.10)、(1.17)和(1.19)，可得：

结合实际意义，相机的标定过程不可能完全精确没有一点误差，所以式(1.20)无解，也就是说该方程组是不相容的，只能退一步求该方程的最小二乘解。令

接下来我们求解方程组Ax=b的最小二乘解。
设A=BC是A的一个满秩分解，则有

是A的伪逆矩阵，H是Hermite矩阵。根据最佳最小二乘解定理可知，

是方程Ax=b的最佳最小二乘解，那么空间点M的坐标也就求了出来。