通过双目标定计算空间三维点坐标

代码可以参考这篇博客：https://blog.youkuaiyun.com/qq_15947787/article/details/53366592

首先要明确的是，对于两台相机，空间中的物点必须是同一个，也就是说，两台相机位于同一个空间坐标系中。

 

我们规定左相机为主相机，通过标定获得左相机的相对于标记点的旋转矩阵和平移向量，接下来我们通过立体标定得到右相机相对于左相机的旋转矩阵和平移向量，这样就可以求出右相机相对于物点的旋转矩阵和平移向量。物点所在世界坐标系和左相机的相机坐标系的转换过程为：

通过立体标定可以得到右相机相对于左相机位置关系，如下表示：

结合上面两个变换，就可以得到右相机相对于物点的变换关系：

令

得到右相机相对于物点的旋转矩阵和平移向量。

 

我们已经知道，从相机坐标系到像素坐标系有如下变换：

那么从世界坐标系到像素坐标系的转换只需要在乘上从世界坐标系到相机坐标系的变换矩阵即可，有如下变换：

下面来探究从像素坐标系到世界坐标系的转换，由上式知对左相机有：

通过双目标定，我们可以求出右相机相对于物点的旋转矩阵和平移向量，故对于空间中同一个物点，对于右相机我们有类似的结论：

将上两式展开整理有：

将上式转变成矩阵形式有：

最终可以通过最小二乘法求解出空间物点的坐标。