双目相机标定模型详解

根据相机投影成像原理，相机模型包含内参模型及外参模型，可以将三维世界坐标转换为二维像素坐标，具体模型及转换关系如下所示。

其中[u, v]^T为矫正后的图像中的点在像素坐标系中的坐标，[xw, yw, zw]^T为点在世界坐标系中的坐标。
接下来要讨论的是如何将二维像素坐标转换为三维世界坐标，主要分为两种计算模型：光轴汇聚模型和光轴平行模型。

一、光轴汇聚模型

对于两相机分别有：


将上述两式左边分别乘以Zc1和Zc2，

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

代码如下：

//opencv2.4.9 vs2012
#include 
#include 
#include 
 
using namespace std;
using namespace cv;
 
Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3]);
Point3f uv2xyz(Point2f uvLeft,Point2f uvRight);
 
//图片对数量
int PicNum = 14;
 
//左相机内参数矩阵
float leftIntrinsic[3][3] = {4037.82450,			 0,		947.65449,
									  0,	3969.79038,		455.48718,
									  0,			 0,				1};
//左相机畸变系数
float leftDistortion[1][5] = {0.18962, -4.05566, -0.00510, 0.02895, 0};
//左相机旋转矩阵
float leftRotation[3][3] = {0.912333,		-0.211508,		 0.350590, 
							0.023249,		-0.828105,		-0.560091, 
							0.408789,		 0.519140,		-0.750590};
//左相机平移向量
float leftTranslation[1][3] = {-127.199992, 28.190639, 1471.356768};
 
//右相机内参数矩阵
float rightIntrinsic[3][3] = {3765.83307,			 0,		339.31958,
										0,	3808.08469,		660.05543,
										0,			 0,				1};
//右相机畸变系数
float rightDistortion[1][5] = {-0.24195, 5.97763, -0.02057, -0.01429, 0};
//右相机旋转矩阵
float rightRotation[3][3] = {-0.134947,		 0.989568,		-0.050442, 
							  0.752355,		 0.069205,		-0.655113, 
							 -0.644788,		-0.126356,		-0.753845};
//右相机平移向量
float rightTranslation[1][3] = {50.877397, -99.796492, 1507.312197};
 
 
int main()
{
	//已知空间坐标求成像坐标
	Point3f point(700,220,530);
	cout<<"左相机中坐标："<
 
  
1、双目立体标定
 
  
双目摄像机需要标定的参数：摄像机内参数矩阵M，畸变系数矩阵，本征矩阵E，基础矩阵F，旋转矩阵R以及平移矩阵T（其中摄像机内参数矩阵和畸变系数矩阵可以通过单目标定的方法标定出来）。
 
  
双目摄像机标定和单目摄像机标定最主要的区别就是双目摄像机需要标定出左右摄像机坐标系之间的相对关系。
 
  
我们用旋转矩阵R和平移矩阵T来描述左右两个摄像机坐标系的相对关系，具体为：在左相机上建立世界坐标系。
 
  
假设空间中有一点P，其在世界坐标系下的坐标为Pw,其在左右摄像机坐标系下的坐标可以表示为：
 
  
Pl = RlPw + Tl
 
  
Pr = RrPw + Tr
 
  
其中Pl和Pr又有如下的关系：
 
  
Pr = RrRl−1(Pl − Tl) + Tr = RrRl−1Pl + Tr − RrRl−1Tl
 
  
综合上式，可以推得：
 
  
R = RrRl−1 = RrRlT
 
  
T = Tr − RrRl−1Tl
 
  
其中Rl, Tl为左摄像头经过单目标定得到的相对标定物的旋转矩阵和平移向量，Rr, Tr为右摄像头经过单目标定得到的相对标定物的旋转矩阵和平移向量。
 
  
左右相机分别进行单目标定，就可以分别Rl,Tl，Rr,Tr。带入上式就可以求出左右相机之间的旋转矩阵R和平移T。
 
  
注意：因为旋转矩阵R、Rl、Rr均是单位正交矩阵。正交矩阵的逆（inverse）等于正交矩阵的转置（transpose）。
 
  
单目摄像机需要标定的参数，双目都需要标定，双目摄像机比单目摄像机多标定的参数：R和T，主要是描述两个摄像机相对位置关系的参数，这些参数在立体校正和对极几何中用处很大,那么得到了立体标定的结果，下一步我们就该进行立体校正。
 
  
2、立体校正
 
  
在介绍立体校正的具体方法之前，让我们来看一下，为什么要进行立体校正？
 
  
双目摄像机系统主要的任务就是测距，而视差求距离公式是在双目系统处于理想情况下推导的，但是在现实的双目立体视觉系统中，是不存在完全的共面行对准的两个摄像机图像平面的。所以我们要进行立体校正。立体校正的目的就是，把实际中消除畸变后的非共面行对准的两幅图像，校正成共面行对准。（共面行对准：两摄像机图像平面在同一平面上，且同一点投影到两个摄像机图像平面时，两个像素坐标系的同一行，这样图片匹配时速度就会有很大的提升）,将实际的双目系统校正为理想的双目系统。
 
  
理想双目系统：两摄像机图像平面平行，光轴和图像平面垂直，极点处于无穷远处，此时点(x0,y0)对应的级线就是y=y0。
 
  
立体校正前：
 
 立体校正后：
 
 
  
3、Bouguet校正原理
 
  
上面两张图中可以看到，立体校正前的左右相机的光心并不是平行的，两个光心的连线就叫基线，像平面与基线的交点就是极点，像点与极点所在的直线就是极线，左右极线与基线构成的平面就是空间点对应的极平面。
 
  
校正后，极点在无穷远处，两个相机的光轴平行。像点在左右图像上的高度一致。这也就是极线校正的目标。校正后做后续的立体匹配时，只需在同一行上搜索左右像平面的匹配点即可，能使效率大大提高。
 
  
Bouguet的方法，是将旋转矩阵R和平移矩阵T分解成左右相机各旋转一半的旋转和平移矩阵Rl, Tl, Rr, Tr。分解的原则是使得，左右图像重投影造成的畸变最小，左右视图的共同面积最大。
 
  
(1) 将右图像平面相对于左图像平面的旋转矩阵分解成两个矩阵Rl和Rr，叫做左右相机的合成旋转矩阵。
 
  
Rl = R1/2
 
  
Rr = R−1/2
 
  
其中R1/2R1/2 = R,R−1/2是R1/2的逆矩阵。
 
  
(2) 将左右相机各旋转一半，使得左右相机的光轴平行。此时左右相机的成像面达到平行，但是基线与成像平面不平行。
 
  
(3)构造变换矩矩阵Rrect使得基线与成像平面平行。构造的方法是通过右相机相对于左相机的偏移矩阵T完成的。
 
  
(4) 构造e1。变换矩阵将左视图的极点变换到无穷远处，则使极线达到水平，可见，左右相机的投影中心之间的平移向量就是左极点方向：
 
 (5) e2方向与主光轴方向正交，沿图像方向，与e1垂直，则知e2方向可通过e1与主光轴方向的叉积并归一化获得：
 
 (6) 获取了e1与e2后，e3与e1和e2正交，e1自然就是他们两个的叉积：
 
 (7) 则可将左相机的极点转换到无穷远处的矩阵Rrect,如下：
 
 (8) 通过合成旋转矩阵与变换矩阵相乘获得左右相机的整体旋转矩阵。左右相机坐标系乘以各自的整体旋转矩阵就可使得左右相机的主光轴平行，且像平面与基线平行。
 
 (9) 通过上述的两个整体旋转矩阵，就能够得到理想的平行配置的双目立体系图像。校正后根据需要对图像进行裁剪，需重新选择一个图像中心，和图像边缘从而让左、右叠加部分最大。
 
  
4、stereoCalibrate 函数
 
  
OpenCV提供了函数cvStereoCalibrate，用来进行双目标定，直接通过stereoCalibrate来实现双目定标，很容易产生比较大的图像畸变，边角处的变形较厉害。最好先通过calibrateCamera() 对每个摄像头单独进行定标，再利用stereoCalibrate进行双目定标。这样定标所得参数才比较准确，随后的校正也不会有明显的畸变。(注意：opencv提供的立体标定函数稳定性不太好，而且精度相对来言也不准，推荐使用matlab的立体标定)
 
  
/*
    进行立体双目标定
    由于左右摄像机分别都经过了单目标定
    所以在此处选择flag = CALIB_USE_INTRINSIC_GUESS
    */
    double rms = stereoCalibrate(objRealPoint,   //vector> 型的数据结构，存储标定角点在世界坐标系中的位置
        imagePointL,                             //vector> 型的数据结构，存储标定角点在第一个摄像机下的投影后的亚像素坐标
        imagePointR,                             //vector> 型的数据结构，存储标定角点在第二个摄像机下的投影后的亚像素坐标
        cameraMatrixL,                           //输入/输出型的第一个摄像机的相机矩阵。如果CV_CALIB_USE_INTRINSIC_GUESS , CV_CALIB_FIX_ASPECT_RATIO ,CV_CALIB_FIX_INTRINSIC , or CV_CALIB_FIX_FOCAL_LENGTH其中的一个或多个标志被设置，该摄像机矩阵的一些或全部参数需要被初始化
        distCoeffL,                              //第一个摄像机的输入/输出型畸变向量。根据矫正模型的不同，输出向量长度由标志决定
        cameraMatrixR,                           //输入/输出型的第二个摄像机的相机矩阵。参数意义同第一个相机矩阵相似
        distCoeffR,                              //第一个摄像机的输入/输出型畸变向量。根据矫正模型的不同，输出向量长度由标志决定
        Size(imageWidth, imageHeight),           //图像的大小
        R,                                       //输出型，第一和第二个摄像机之间的旋转矩阵
        T,                                       //输出型，第一和第二个摄像机之间的平移矩阵
        E,                                       //输出本征矩阵
        F,                                       //输出基础矩阵
        CALIB_USE_INTRINSIC_GUESS,
        TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, 1e-5));
 
  
上面的cameraMatrixL（R），distCoeffL（R） 分别是单目定标后获得的左（右）摄像头的内参矩阵(33）和畸变参数向量（15或者51）；R, T 分别是双目立体标定获取的右摄像头相对于左摄像头的旋转矩阵（33）和平移向量（31）, E是包含了两个摄像头相对位置关系的本征矩阵Essential Matrix（33），F 则是既包含两个摄像头相对位置关系、也包含摄像头各自内参信息的基础矩阵（3*3）。
 
  
 
  
对极几何在双目问题中非常的重要，可以简化立体匹配等问题，而要应用对极几何去解决问题，比如求极线，需要知道本征矩阵或者基础矩阵，因此双目标定过程中也会把本征矩阵和基础矩阵算出来。
 
  
stereoCalibrate 是怎样计算 Essential Matrix 和 Fundamental Matrix 的？
 
  

 
 
 （2）Fundamental Matrix
 
  
由于矩阵E并不包含摄像头内参信息，且E是面向摄像头坐标系的。实际上我们更感兴趣的是在像素坐标系上去研究一个像素点在另一视图上的对极线，这就需要用到摄像机的内参信息将摄像机坐标系和像素坐标系联系起来。我们定义基础矩阵F，描述同一物理点在左右摄像机像素坐标系下的关系，单位为pix。
 
 
  
5、 stereoRectify函数
 介绍完了stereoCalibrate 函数我们再来看看stereoRectify函数，这个函数是用来双目校正的，主要包括畸变矫正、立体校正两部分。
 
 
  
如上图所示：双目校正是根据摄像头定标后获得的单目内参数据（焦距、成像原点、畸变系数）和双目相对位置关系（旋转矩阵和平移向量），分别对左右视图进行消除畸变和行对准，使得左右视图的成像原点坐标一致（CV_CALIB_ZERO_DISPARITY 标志位设置时发生作用）、两摄像头光轴平行、左右成像平面共面、对极线行对齐。在OpenCV2.1版之前，cvStereoRectify 的主要工作就是完成上述操作，校正后的显示效果如上图© 所示。可以看到校正后左右视图的边角区域是不规则的，而且对后续的双目匹配求取视差会产生影响，因为这些边角区域也参与到匹配操作中，其对应的视差值是无用的、而且一般数值比较大，在三维重建和机器人避障导航等应用中会产生不利影响。
 
  
因此，OpenCV2.1 版之后stereoRectify新增了4个参数用于调整双目校正后图像的显示效果，分别是 double alpha, CvSize newImgSize, CvRect* roi1, CvRect* roi2。
 
  
CV_EXPORTS_W void stereoRectify( InputArray cameraMatrix1, InputArray distCoeffs1,
                                 InputArray cameraMatrix2, InputArray distCoeffs2,
                                 Size imageSize, InputArray R, InputArray T,
                                 OutputArray R1, OutputArray R2,
                                 OutputArray P1, OutputArray P2,
                                 OutputArray Q, int flags = CALIB_ZERO_DISPARITY,
                                 double alpha = -1, Size newImageSize = Size(),
                                 CV_OUT Rect* validPixROI1 = 0, CV_OUT Rect* validPixROI2 = 0 );
 
  
下面简要介绍这4个参数的作用：
 
  
（1）newImgSize：校正后remap图像的分辨率。如果输入为(0,0)，则是与原图像大小一致。对于图像畸变系数比较大的，可以把newImgSize 设得大一些，以保留图像细节(你可以把畸变矫正的过程想象成和图片旋转类似，在图片旋转中可能会出现边角超出当前图像，因此为了保留所有的图片信息，我们可以利用公式计算出旋转之后的面积，然后再进行旋转变换，这里畸变矫正也可能存在同样的问题，但是这个大小并不好计算，因此我们可以人为的设置大一些)。
 
  
（2）alpha：图像剪裁系数，取值范围是-1、0~1。当取值为 0 时，OpenCV会对校正后的图像进行缩放和平移，使得remap图像只显示有效像素（即去除不规则的边角区域），如下图，适用于机器人避障导航等应用；
 
 当alpha取值为1时，remap图像将显示所有原图像中包含的像素，该取值适用于畸变系数极少的高端摄像头；alpha取值在0-1之间时，OpenCV按对应比例保留原图像的边角区域像素。Alpha取值为-1时，OpenCV自动进行缩放和平移，其显示效果如图所示。
 
 
  
（3）roi1, roi2：用于标记remap图像中包含有效像素的矩形区域。
 
  
上面还有几个参数需要介绍一下，R1和R2分别为左右相机消除畸变后的像平面投影到公共像平面的旋转矩阵,也就是我们之前介绍的Bouguet校正原理中求解得到的R′l和R′r矩阵。 左相机经过R1旋转，右相机经过R2旋转之后，两幅图像就已经共面并且行对准了。
 
  
P1,P2分别为左右相机的投影矩阵，其作用是将世界坐标系的点转换到像素坐标系下(左相机光心为世界坐标系原点):
 
 
  
Q矩阵为重投影矩阵，即矩阵Q可以把像素坐标系下的点转换到世界坐标系下:
 
 
  
其中d为左右两幅图像的视差,三维坐标为(Xw/W,Yw/W,Zw/W)。下面我们来研究一下如何求解Q矩阵：
 
  
经过双目相机标定和校准后，双目相机的主光轴到达平行，如图所示是双目相机模型，世界坐标系中的任意一点都满足，该点与它在左右相机的成像点在同一个极平面上。OL和OR是左右相机的的光心，长为L的两条线段（端点为蓝色星星的线段）表示的是左右相机的像面。则光心到像面的最短距离就是焦距长度f(物理单位),两个相机的焦距f要求设置为一样的。若P(Xw,Yw,Zw)是世界坐标系中的一点，它在左右像面上的成像点是PL和PR。PL和PR距各自像面的左边缘的物理距离是XL和XR。视差就是XR−XL或者是XL−XR。标定和匹配后f，b，XR，XL都能够得到，那么物体的景深Z是多少呢？它和物体的视差有什么关系呢？
 
 
  
三角行PL-PR-P相似于三角形OL-OR-P，则有比例关系：
 
 
  
由于上面的XR,XL的单位均是物理单位，而我们想使用像素单位去表示，我们在上一节中介绍从图像坐标系转到像素坐标系时说到dx,dy,分别表示每个像素在横轴x和纵轴y上的物理尺寸，则上式变为：
 
 
  
uR,uL分别表示PL、PR距离各自像平面左边缘的像素距离。fx是我们通过相机标定的内参值。
 
  
我们定义视差d=uL−uR(d和fx的单位都是像素，两个正好消去，Zw的单位和b一样，都是物理单位)，所以有：
 
 
  
从上式我们知道距离摄像机越远的物体，视差值越小，越近的物体，视差值越大，这也符合我们人眼的规律。
 
  
现在我们已经求出来Zw的值，再来看看Xw,Yw如何求解：
 
 
  
同样利用三角形相似定理，我们可以得到：
 
 
  
当fx=fy时，我们得到Xw,Yw的坐标为：
 
 
  
其中u,v表示像素坐标系下的点的坐标，u0、v0为左相机图像平面的原点在像素坐标系下的坐标值。
 
  
我们Xw,Yw,Zw坐标联立在一块得到：
 
 
  
空间中某点的三维坐标就是Xw=X/W, Yw=Y/W, Zw=Z/W。
 
  
但是OpenCV中转换公式如下：
 
 
  
这里Tx就是我们上面的b，即两个相机光心的中心距(通过立体标定得出的T的向量的第一个分量Tx的绝对值就是左右摄像头的中心距，你回到上面推导一翻，你会发现Tx一般为负数，因此b=−Tx)，u0、u′0分别为左右相机图像平面的原点水平分量在像素坐标系下的坐标值，实际上两个数值的差值很小，近似为0。如果我们忽略Q的最后一项，我们可以看到这个和我们自己求得近似一样。
 
  
6、initUndistortRectifyMap函数
 根据相机单目标定得到的内参以及stereoRectify 计算出来的R 和 P 来计算畸变矫正和立体校正的映射变换矩阵 mapx,mapy。
 
  
    /*    
    根据stereoRectify 计算出来的R 和 P 来计算畸变矫正和立体校正的映射变换矩阵 mapx,mapy
    mapx,mapy这两个映射表接下来可以给remap()函数调用，来校正图像，使得两幅图像共面并且行对准
    */
    initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_32FC1, mapLx, mapLy);
    initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);
 
  
参考：
 1. 第七节、双目视觉之空间坐标计算
 2.【opencv】双目视觉下空间坐标计算/双目测距 6/13更新