【计算机视觉】双目测距（二）--双目标定与矫正
原文：
http://blog.csdn.NET/chenyusiyuan/article/details/5963256
双目摄像头定标不仅要得出每个摄像头的内部参数，还需要通过标定来测量两个摄像头之间的相对位置（即右摄像头相对于左摄像头的三维平移 t 和旋转 R 参数）。
图6
要计算目标点在左右两个视图上形成的视差，首先要把该点在左右视图上两个对应的像点匹配起来。然而，在二维空间上匹配对应点是非常耗时的，为了减少匹配搜索范围，我们可以利用极线约束使得对应点的匹配由二维搜索降为一维搜索。
图7
而双目校正的作用就是要把消除畸变后的两幅图像严格地行对应，使得两幅图像的对极线恰好在同一水平线上，这样一幅图像上任意一点与其在另一幅图像上的对应点就必然具有相同的行号，只需在该行进行一维搜索即可匹配到对应点。
图8
1． 关于cvStereoCalibrate的使用
如果按照 Learning OpenCV 的例程，直接通过cvStereoCalibrate来实现双目定标，很容易产生比较大的图像畸变，边角处的变形较厉害。最好先通过cvCalibrateCamera2() 对每个摄像头单独进行定标，再利用cvStereoCalibrate进行双目定标。这样定标所得参数才比较准确，随后的校正也不会有明显的畸变。我使用的程序主要基于Learning OpenCV 的例程ch12_ex12_3.cpp，其中主要部分如下：
////////////////////////////////////////////////////////////////////////// // 是否首先进行单目定标？ cvCalibrateCamera2(&_objectPoints, &_imagePoints1, &_npoints, imageSize, &t_M1, &t_D1, NULL, NULL, CV_CALIB_FIX_K3); cvCalibrateCamera2(&_objectPoints, &_imagePoints2, &_npoints, imageSize, &t_M2, &t_D2, NULL, NULL, CV_CALIB_FIX_K3); ////////////////////////////////////////////////////////////////////////// // 进行双目定标 cvStereoCalibrate( &_objectPoints, &_imagePoints1, &_imagePoints2, &_npoints, &t_M1, &t_D1, &t_M2, &t_D2, imageSize, &t_R, &t_T, &t_E, &t_F, cvTermCriteria(CV_TERMCRIT_ITER+ CV_TERMCRIT_EPS, 100, 1e-5)); // flags为默认的CV_CALIB_FIX_INTRINSIC
上面的t_M1(2), t_D1(2) 分别是单目定标后获得的左（右）摄像头的内参矩阵（3*3）和畸变参数向量（1*5）；t_R, t_T 分别是右摄像头相对于左摄像头的旋转矩阵（3*3）和平移向量（3*1）, t_E是包含了两个摄像头相对位置关系的Essential Matrix（3*3），t_F 则是既包含两个摄像头相对位置关系、也包含摄像头各自内参信息的Fundamental Matrix（3*3）。
图9
2． cvStereoCalibrate 是怎样计算 Essential Matrix 和 Fundamental Matrix 的？
首先我们以Learning OpenCV第422页为基础，讨论下 Essential Matrix 和 Fundamental Matrix 的构造过程，再看看 OpenCV 是如何进行计算的。
图10
注：原文中对pl、pr 和ql、qr 物理意义和计算公式的表述有误，已修正。（2011-04-12）
（1）Essential Matrix
如上图所示，给定一个目标点P，以左摄像头光心Ol为原点。点P相对于光心Ol的观察位置为Pl，相对于光心Or的观察位置为Pr。点P在左摄像头成像平面上的位置为pl，在右摄像头成像平面上的位置为pr。注意Pl、Pr、pl、pr 都处于摄像机坐标系，其量纲是与平移向量T相同的（pl、pr 在图像坐标系中对应的像素坐标为 ql、qr ）。
假设右摄像头相对于左摄像头的相对位置关系由旋转矩阵R和平移向量T表示，则可得：Pr = R(Pl-T)。
现在我们要寻找由点P、Ol和Or确定的对极平面的表达式。注意到平面上任意一点x与点a的连线垂直于平面法向量n，即向量 (x-a) 与向量 n 的点积为0：(x-a)·n = 0。在Ol坐标系中，光心Or的位置为T，则P、Ol和Or确定的对极平面可由下式表示：(Pl-T)T·(Pl×T) = 0。
由Pr = R(Pl-T) 和 RT=R-1 可得：(RTPr)T·(Pl×T) = 0。
另一方面，向量的叉积又可表示为矩阵与向量的乘积，记向量T的矩阵表示为S，得：Pl×T = SPl。
图11
那么就得到：(Pr)TRSPl = 0。这样，我们就得到Essential Matrix：E = RS。
通过矩阵E我们知道Pl和Pr的关系满足：(Pr)TEPl = 0。进一步地，由 pl = fl*Pl/Zl 和 pr = fr*Pr/Zr 我们可以得到点P在左右两个摄像机坐标系中的观察点 pl 和 pr 应满足的极线约束关系为：(pr)TEpl = 0。
注意到 E 是不满秩的，它的秩为2，那么 (pr)TEpl = 0 表示的实际上是一条直线，也就是对极线。
（2）Fundamental Matrix
由于矩阵E并不包含摄像头内参信息，且E是面向摄像头坐标系的。实际上我们更感兴趣的是在图像像素坐标系上去研究一个像素点在另一视图上的对极线，这就需要用到摄像机的内参信息将摄像头坐标系和图像像素坐标系联系起来。在（1）中，pl和pr是物理坐标值，对应的像素坐标值为ql和qr，摄像头内参矩阵为M，则有：p=M-1q。从而：(pr)TEpl = 0 à qrT(Mr-1)TE Ml-1ql = 0。这里，我们就得到Fundamental Matrix：F = (Mr-1)TE Ml-1。并有 qrTFql = 0。
（3）OpenCV的相关计算
由上面的分析可见，求取矩阵E和F关键在于旋转矩阵R和平移向量T的计算，而cvStereoCalibrate的代码中大部分（cvcalibration.cpp的第1886-2180行）也是计算和优化R和T的。在cvcalibration.cpp的第1913-1925行给出了计算R和T初始估计值的基本方法：
/* Compute initial estimate of pose For each image, compute: R(om) is the rotation matrix of om om(R) is the rotation vector of R R_ref = R(om_right) * R(om_left)' T_ref_list = [T_ref_list; T_right - R_ref * T_left] om_ref_list = {om_ref_list; om(R_ref)] om = median(om_ref_list) T = median(T_ref_list) */
具体的计算过程比较繁杂，不好理解，这里就不讨论了，下面是计算矩阵E和F的代码：
if( matE || matF ) { double* t = T_LR.data.db; double tx[] = { 0, -t[2], t[1], t[2], 0, -t[0], -t[1], t[0], 0 }; CvMat Tx = cvMat(3, 3, CV_64F, tx); double e[9], f[9]; CvMat E = cvMat(3, 3, CV_64F, e); CvMat F = cvMat(3, 3, CV_64F, f); cvMatMul( &Tx, &R_LR, &E ); if( matE ) cvConvert( &E, matE ); if( matF ) { double ik[9]; CvMat iK = cvMat(3, 3, CV_64F, ik); cvInvert(&K[1], &iK); cvGEMM( &iK, &E, 1, 0, 0, &E, CV_GEMM_A_T ); cvInvert(&K[0], &iK); cvMatMul(&E, &iK, &F); cvConvertScale( &F, matF, fabs(f[8]) > 0 ? 1./f[8] : 1 ); } }
3． 通过双目定标得出的向量T中，Tx符号为什么是负的？
“@scyscyao：这个其实我也不是很清楚。个人的解释是，双目定标得出的T向量指向是从右摄像头指向左摄像头(也就是Tx为负)，而在OpenCV坐标系中，坐标的原点是在左摄像头的。因此，用作校准的时候，要把这个向量的三个分量符号都要换一下，最后求出的距离才会是正的。
图12
但是这里还有一个问题，就是Learning OpenCV中Q的表达式，第四行第三列元素是-1/Tx，而在具体实践中，求出来的实际值是1/Tx。这里我和maxwellsdemon讨论下来的结果是，估计书上Q表达式里的这个负号就是为了抵消T向量的反方向所设的，但在实际写OpenCV代码的过程中，那位朋友却没有把这个负号加进去。”
图13
scyscyao 的分析有一定道理，不过我觉得还有另外一种解释：如上图所示，摄像机C1（C2）与世界坐标系相对位置的外部参数为旋转矩阵R1（R2）和平移向量 t1（t2），如果下标1代表左摄像机，2代表右摄像机，显然在平移向量的水平分量上有 t1x > t2x；若以左摄像机C1为坐标原点，则可得到如上图所示的旋转矩阵R和平移向量t，由于t1x > t2x，则有 tx < 0。
为了抵消Tx为负，在矩阵Q中元素(4,3)应该加上负号，但在cvStereoRectify代码中并没有添加上，这就使得我们通过 cvReprojectImageTo3D 计算得到的三维数据都与实际值反号了。
if( matQ ) { double q[] = { 1, 0, 0, -cc_new[0].x, 0, 1, 0, -cc_new[0].y, 0, 0, 0, fc_new, 0, 0, 1./_t[idx], (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/_t[idx] }; CvMat Q = cvMat(4, 4, CV_64F, q); cvConvert( &Q, matQ ); }
为了避免上述反号的情况，可以在计算得到Q矩阵后，添加以下代码更改 Q[3][2] 的值。
// Q 是 Mat 类型矩阵，OpenCV2.1 C++ 模式 Q.at<double>(3, 2) = -Q.at<double>(3, 2); // Q 是 double 数组定义时 double Q[4][4]; CvMat t_Q = cvMat(4, 4, CV_64F, Q ); cvStereoRectify(…); Q[3][2]=-Q[3][2];
4． 双目校正原理及cvStereoRectify 的应用。
图14
如图14所示，双目校正是根据摄像头定标后获得的单目内参数据（焦距、成像原点、畸变系数）和双目相对位置关系（旋转矩阵和平移向量），分别对左右视图进行消除畸变和行对准，使得左右视图的成像原点坐标一致（CV_CALIB_ZERO_DISPARITY 标志位设置时发生作用）、两摄像头光轴平行、左右成像平面共面、对极线行对齐。在OpenCV2.1版之前，cvStereoRectify 的主要工作就是完成上述操作，校正后的显示效果如图14(c) 所示。可以看到校正后左右视图的边角区域是不规则的，而且对后续的双目匹配求取视差会产生影响，因为这些边角区域也参与到匹配操作中，其对应的视差值是无用的、而且一般数值比较大，在三维重建和机器人避障导航等应用中会产生不利影响。
因此，OpenCV2.1 版中cvStereoRectify新增了4个参数用于调整双目校正后图像的显示效果，分别是 double alpha, CvSize newImgSize, CvRect* roi1, CvRect* roi2。下面结合图15-17简要介绍这4个参数的作用：
（1）newImgSize：校正后remap图像的分辨率。如果输入为(0,0)，则是与原图像大小一致。对于图像畸变系数比较大的，可以把newImgSize 设得大一些，以保留图像细节。
（2）alpha：图像剪裁系数，取值范围是-1、0~1。当取值为 0 时，OpenCV会对校正后的图像进行缩放和平移，使得remap图像只显示有效像素（即去除不规则的边角区域），如图17所示，适用于机器人避障导航等应用；当alpha取值为1时，remap图像将显示所有原图像中包含的像素，该取值适用于畸变系数极少的高端摄像头；alpha取值在0-1之间时，OpenCV按对应比例保留原图像的边角区域像素。Alpha取值为-1时，OpenCV自动进行缩放和平移，其显示效果如图16所示。
（3）roi1, roi2：用于标记remap图像中包含有效像素的矩形区域。对应代码如下：
02433 if(roi1) 02434 { 02435 *roi1 = cv::Rect(cvCeil((inner1.x - cx1_0)*s + cx1), 02436 cvCeil((inner1.y - cy1_0)*s + cy1), 02437 cvFloor(inner1.width*s), cvFloor(inner1.height*s)) 02438 & cv::Rect(0, 0, newImgSize.width, newImgSize.height); 02439 } 02440 02441 if(roi2) 02442 { 02443 *roi2 = cv::Rect(cvCeil((inner2.x - cx2_0)*s + cx2), 02444 cvCeil((inner2.y - cy2_0)*s + cy2), 02445 cvFloor(inner2.width*s), cvFloor(inner2.height*s)) 02446 & cv::Rect(0, 0, newImgSize.width, newImgSize.height); 02447 }
图15
图16
图17
在cvStereoRectify 之后，一般紧接着使用 cvInitUndistortRectifyMap 来产生校正图像所需的变换参数（mapx, mapy）。
////////////////////////////////////////////////////////////////////////// // 执行双目校正 // 利用BOUGUET方法或HARTLEY方法来校正图像 mx1 = cvCreateMat( imageSize.height, imageSize.width, CV_32F ); my1 = cvCreateMat( imageSize.height, imageSize.width, CV_32F ); mx2 = cvCreateMat( imageSize.height, imageSize.width, CV_32F ); my2 = cvCreateMat( imageSize.height, imageSize.width, CV_32F ); double R1[3][3], R2[3][3], P1[3][4], P2[3][4], Q[4][4]; CvMat t_R1 = cvMat(3, 3, CV_64F, R1); CvMat t_R2 = cvMat(3, 3, CV_64F, R2); CvMat t_Q = cvMat(4, 4, CV_64F, Q ); CvRect roi1, roi2; // IF BY CALIBRATED (BOUGUET'S METHOD) CvMat t_P1 = cvMat(3, 4, CV_64F, P1); CvMat t_P2 = cvMat(3, 4, CV_64F, P2); cvStereoRectify( &t_M1, &t_M2, &t_D1, &t_D2, imageSize, &t_R, &t_T, &t_R1, &t_R2, &t_P1, &t_P2, &t_Q, CV_CALIB_ZERO_DISPARITY, 0, imageSize, &roi1, &roi2); // Precompute maps for cvRemap() cvInitUndistortRectifyMap(&t_M1,&t_D1,&t_R1,&t_P1, mx1, my1); cvInitUndistortRectifyMap(&t_M2,&t_D2,&t_R2,&t_P2, mx2, my2);
5． 为什么cvStereoRectify求出的Q矩阵cx, cy, f都与原来的不同？
“@scyscyao：在实际测量中，由于摄像头摆放的关系，左右摄像头的f, cx, cy都是不相同的。而为了使左右视图达到完全平行对准的理想形式从而达到数学上运算的方便，立体校准所做的工作事实上就是在左右像重合区域最大的情况下，让两个摄像头光轴的前向平行，并且让左右摄像头的f, cx, cy相同。因此，Q矩阵中的值与两个instrinsic矩阵的值不一样就可以理解了。”
注：校正后得到的变换矩阵Q，Q[0][3]、Q[1][3]存储的是校正后左摄像头的原点坐标（principal point）cx和cy，Q[2][3]是焦距f。
