双目视觉下空间坐标计算 opencv+ 个人理解

简单的理解思路：（世界坐标系固定到左目）

空间中一点P，在左目像素坐标（u1，v1），转成mm为单位的坐标（x1，y1），在左目坐标系下建立过（x，y）的直线lineL；

同样的思路，空间中同一点P，在右目坐标系下建立，过（x2，y2）的直线lineR，LineR上的点要经过（R，T）变换到左目坐标系下；

求直线lineL与lineR的交点 就是P的坐标。

摄像机矩阵由内参矩阵和外参矩阵组成，对摄像机矩阵进行QR分解可以得到内参矩阵和外参矩阵。

内参包括焦距、主点、倾斜系数、畸变系数

（1）

其中，fx，fy为焦距，一般情况下，二者相等，x0、y0为主点坐标（相对于成像平面），s为坐标轴倾斜参数，理想情况下为0

 

外参包括旋转矩阵R3×3、平移向量T3×1，它们共同描述了如何把点从世界坐标系转换到摄像机坐标系，旋转矩阵描述了世界坐标系的坐标轴相对于摄像机坐标轴的方向，平移向量描述了在摄像机坐标系下空间原点的位置。

 

标定双目后，首先要根据其畸变系数来校正原图，可以参考

http://blog.csdn.net/qq_15947787/article/details/51471535

有矫正的代码：：：：参见后面的链接

校正完成后就可以进行坐标计算了，分两种

（1）世界坐标系——>像面坐标系

首先将世界坐标系——>摄像机坐标系

已知某点在世界坐标系中的坐标为(Xw, Yw, Zw)，由旋转和平移矩阵可得摄像机坐标系和世界坐标系的关系为

（2）

然后将摄像机坐标系——>像面坐标系

（3）

其中[u v 1]^T为点在图像坐标系中的坐标，[Xc Yc  Zc  1]^T为点在摄像机坐标系中的坐标，K为摄像机内参数矩阵。

这样最终可以得到：

（4）

（2）像面坐标系——>世界坐标系

光轴会聚模型：

对于两相机分别有：

（5）          （6）

公式56，左边Z应分别为Zc1，Zc2

其中，

（7）

这样可以把（5）（6）写成

（8）  

公式8左边Z应为Zc1

（9）

公式9左边Z应为Zc2

将（8）（9）整理可以得到

注意Zc1不等于Zc2.。 通过（8）消去Zc1得到（10）中的前两个等式；通过（9）消去Zc2得到（10）中的后两个等式；

（10）

采用最小二乘法求解X,Y,Z，在opencv中可以用solve(A,B,XYZ,DECOMP_SVD)求解(10)

A为超定方程的等式左边的系数矩阵，B为等式右边的矩阵。因为是超定方程只能求出最小二乘解。

matlab或者opencv标定完都是在左相机上建立世界坐标系，于是上面代码对应的改为：

//左相机旋转矩阵   

float leftRotation[3][3] = {1,0,0,  0,1,0,  0,0,1 }; 

//左相机平移向量

float leftTranslation[1][3] = {0,0,0};

有计算的代码：：：：参见后面的链接

代码链接：https://blog.csdn.net/qq_15947787/article/details/53366592