转OpenCV学习笔记17双目测距与三维重建

转OpenCV学习笔记17 双目测距
与三维重建
三、双目定标和双目校正
双目摄像头定标不仅要得出每个摄像头的内部参数，还需要通过标定来测量两个摄像头之间的相对位置(即右摄像头相对于左摄像头的三维平移t和旋转R参数)。

图6
要计算目标点在左右两个视图上形成的视差，首先要把该点在左右视图上两个对应的像点匹配起来。

然而，在二维空间上匹配对应点是非常耗时的，为了减少匹配搜索范围，我们可以利用极线约束使得对应点的匹配由二维搜索降为一维搜索。

图7
而双目校正的作用就是要把消除畸变后的两幅图像严格地行对应，使得两幅图像的对极线恰好在同一水平线上，这样一幅图像上任意一点与其在另一幅图像上的对应点就必然具有相同的行号，只需在该行进行一维搜索即可匹配到对应点。

图8 1.关于cvStereoCalibrate的使用
如果按照Learning OpenCV的例程，直接通过cvStereoCalibrate来实现双目定标，很容易产生比较大的图像畸变，边角处的变形较厉害。

最好先通过cvCalibrateCamera2()对每个摄像头单独进行定标，再利用cvStereoCalibrate进行双目定标。

这样定标所得参数才比较准确，随后的校正也不会有明显的畸变。

我使用的程序主要基于Learning OpenCV的例程
ch12_ex12_3.cpp，其中主要部分如下：
///////////////////////////////////////////////////////////////// ///////////是否首先进行单目定
标?cvCalibrateCamera2(&_objectPoints,&_imagePoints1,&_npoints,imageSi ze,&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,&_npoi nts,&t_M1,&t_D1,&t_M2,&t_D2,imageSize,&t_R,&t_T,&t_E,&t_F,cvTermCrite ria(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
(1)Essential Matrix
如上图所示，给定一个目标点P，以左摄像头光心Ol为原点。

点P相对于
光心Ol的观察位置为Pl，相对于光心Or的观察位置为Pr。

点P在左摄像头成像平面上的位置为pl，在右摄像头成像平面上的位置为pr。

注意Pl、Pr是处
于摄像机坐标系，其量纲是与平移向量T相同的，而pl、pr是处于图像坐标系，其量纲则是像素。

假设右摄像头相对于左摄像头的相对位置关系由旋转矩阵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*Zl/Pl和pr=fr*Zr/Pr我们可以得到点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初始估计值的基本方法：
具体的计算过程比较繁杂，不好理解，这里就不讨论了，下面是计算矩阵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图像中包含有效像素的矩形区域。

对应代
码如下：
02433if(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}024*******
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。

(待续…)。