http://blog.csdn.net/chenyusiyuan/article/details/5961769
双目测距的基本原理
如上图所示,双目测距主要是利用了目标点在左右两幅视图上成像的横向坐标直接存在的差异(即视差 )与目标点到成像平面的距离Z存在着反比例的关系:Z=fT/d。“@scyscyao :在OpenCV中,f的量纲是像素点,T的量纲由定标板棋盘格的实际尺寸和用户输入值确定,一般是以毫米为单位(当然为了精度提高也可以设置为0.1毫米量级),d=xl-xr的量纲也是像素点。因此分子分母约去,Z的量纲与T相同。 ”
假设目标点在左视图中的坐标为(x,y),在左右视图上形成的视差为d,目标点在以左摄像头光心为原点的世界坐标系中的坐标为(X,Y,Z),则存在上图所示的变换矩阵Q,使得 Q*[x y d 1]’ = [X Y Z W]’。
“@scyscyao :为了精确地求得某个点在三维空间里的距离Z,我们需要获得的参数有焦距f、视差d、摄像头中心距Tx。如果还需要获得X坐标和Y坐标的话,那么还需要额外知道左右像平面的坐标系与立体坐标系中原点的偏移cx和cy。其中f, Tx, cx和cy可以通过立体标定获得初始值,并通过立体校准优化,使得两个摄像头在数学上完全平行放置,并且左右摄像头的cx, cy和f相同(也就是实现图2中左右视图完全平行对准的理想形式)。而立体匹配所做的工作,就是在之前的基础上,求取最后一个变量:视差d(这个d一般需要达到亚像素精度)。从而最终完成求一个点三维坐标所需要的准备工作。在清楚了上述原理之后,我们也就知道了,所有的这几步:标定、校准和匹配,都是围绕着如何更精确地获得 f, d, Tx, cx 和cy 而设计的 。 ”
一、图像的获取
1. 如何打开两个或多个摄像头?
可以通过OpenCV的capture类函数或者结合DirectShow来实现双摄像头的捕获,具体可见我的读书笔记《OpenCV学习笔记(6)基于 VC+OpenCV+DirectShow 的多个摄像头同步工作 》。文中曾提及不能用cvCreateCameraCapture 同时读取两个摄像头,不过后来一位研友来信讨论说只要把摄像头指针的创建代码按照摄像头序号降序执行,就可以顺利打开多个摄像头 ,例如:
- CvCapture* capture2 = cvCreateCameraCapture( 1 );
- CvCapture* capture1 = cvCreateCameraCapture( 0 );
采用DirectShow的方式读入时:
- camera2.OpenCamera(1, false, 640,480);
- camera1.OpenCamera(0, false, 640,480);
这样就可以同时采集两个摄像头。我也验证过这种方法确实有效,而且还解决了我遇到的cvSetCaptureProperty调整帧画面大小速度过慢的问题。当摄像头的打开或创建代码按照摄像头序号从0开始以升序编写执行时,使用cvSetCaptureProperty就会出现第一个摄像头(序号为0)的显示窗口为灰色(即无图像)、且程序运行速度缓慢的现象。而改为降序编写执行后,则能正常、实时地显示各摄像头的画面。具体原因有待分析讨论。
2. 如何实现多个摄像头帧画面的同步抓取?
在单摄像头情况下用 cvQueryFrame 即可抓取一帧画面,实际上这个函数是由两个routine组成的:cvGrabFrame和cvRetrieveFrame(详见Learning OpenCV第103页)。cvGrabFrame将摄像头帧画面即时复制到内部缓存中,然后通过cvRetrieveFrame把我们预定义的一个IplImage型空指针指向缓存内的帧数据。注意这时我们并没有真正把帧数据取出来,它还保存在OpenCV的内部缓存中,下一次读取操作就会被覆盖掉。所以一般我们要另外定义一个IplImage来复制所抓取的帧数据,然后对这个新IplImage进行操作。
由上面的解释也可以看出,cvGrabFrame的作用就是尽可能快的将摄像头画面数据复制到计算机缓存,这个功能就方便我们实现对多个摄像头的同步抓取,即首先用cvGrabFrame依次抓取各个CvCapture*,然后再用cvRetrieveFrame把帧数据取出来。例如:
- cvGrabFrame( lfCam );
- cvGrabFrame( riCam );
- frame1 = cvRetrieveFrame( lfCam );
- frame2 = cvRetrieveFrame( riCam );
- if( !frame1|| !frame2) break;
- cvCopyImage(frame1, image1);
- cvCopyImage(frame2, image2);
二、摄像头定标
摄像头定标一般都需要一个放在摄像头前的特制的标定参照物(棋盘纸),摄像头获取该物体的图像,并由此计算摄像头的内外参数。标定参照物上的每一个特征点相对于世界坐标系的位置在制作时应精确测定,世界坐标系可选为参照物的物体坐标系。在得到这些已知点在图像上的投影位置后,可计算出摄像头的内外参数。
如上图所示,摄像头由于光学透镜的特性使得成像存在着径向畸变,可由三个参数k1,k2,k3确定;由于装配方面的误差,传感器与光学镜头之间并非完全平行,因此成像存在切向畸变,可由两个参数p1,p2确定。单个摄像头的定标主要是计算出摄像头的内参(焦距f和成像原点cx,cy、五个畸变参数(一般只需要计算出k1,k2,p1,p2,对于鱼眼镜头等径向畸变特别大的才需要计算k3))以及外参(标定物的世界坐标)。 OpenCV 中使用的求解焦距和成像原点的算法是基于张正友的方法( pdf ),而求解畸变参数是基于 Brown 的方法( pdf )。
1. 图像坐标系、摄像头坐标系和世界坐标系的关系
摄像头成像几何关系,其中Oc 点称为摄像头(透镜)的光心,Xc 轴和Yc 轴与图像的x轴和Y轴平行,Zc 轴为摄像头的光轴,它与图像平面垂直。光轴与图像平面的交点O1 ,即为图像坐标系的原点。由点Oc 与Xc 、Yc 、Zc 轴组成的坐标系称为摄像头坐标系,Oc O1 的距离为摄像头焦距,用f表示。
图像坐标系是一个二维平面,又称为像平面,“@scyscyao :实际上就是摄像头的CCD传感器的表面。每个CCD传感器都有一定的尺寸,也有一定的分辨率,这个就确定了毫米与像素点之间的转换关系。举个例子,CCD的尺寸是8mm X 6mm,帧画面的分辨率设置为640X480,那么毫米与像素点之间的转换关系就是80pixel/mm。”设CCD传感器每个像素点的物理大小为dx*dy,相应地,就有 dx=dy=1/80。
2. 进行摄像头定标时,棋盘方格的实际大小 square_size (默认为 1.0f )的设置对定标参数是否有影响?
“@scyscyao :当然有。在标定时,需要指定一个棋盘方格的长度,这个长度(一般以毫米为单位,如果需要更精确可以设为0.1毫米量级)与实际长度相同,标定得出的结果才能用于实际距离测量。一般如果尺寸设定准确的话,通过立体标定得出的Translation向量的第一个分量Tx的绝对值就是左右摄像头的中心距。一般可以用这个来验证立体标定的准确度。比如我设定的棋盘格大小为270 (27mm),最终得出的Tx大小就是602.8 (60.28mm),相当精确。”
3. 定标所得的摄像头内参数,即焦距和原点坐标,其数值单位都是一致的吗?怎么把焦距数值换算为实际的物理量?
“@wobject :是的,都是以像素为单位。假设像素点的大小为k x l,单位为mm,则fx = f / k, fy = f / (l * sinA), A一般假设为 90°,是指摄像头坐标系的偏斜度(就是镜头坐标和CCD是否垂直)。摄像头矩阵(内参)的目的是把图像的点从图像坐标转换成实际物理的三维坐标。因此其中的fx, fy, cx, cy 都是使用类似上面的纲量。同样,Q 中的变量 f,cx, cy 也应该是一样的。”
4. 棋盘图像数目应该取多少对摄像头定标比较适宜?
OpenCV中文论坛上piao的帖子《在OpenCV中用cvCalibrateCamera2进行相机标定(附程序) 》中指出影响摄像头定标结果的准确性和稳定性的因素主要有三个:
(1) 标定板所在平面与成像平面(image plane)之间的夹角;
(2) 标定时拍摄的图片数目(棋盘图像数目);
(3) 图像上角点提取的不准确。
感觉OpenCV1.2以后对图像角点的提取准确度是比较高的,cvFindChessboardCorners 和 cvFindCornerSubPix结合可以获得很好的角点检测效果(hqhuang1在《[HQ]角点检测(Corner Detection) cvFindCornerSubPix 使用范例 》中给出了相关的应用范例)。因此,影响定标结果较大的就是标定板与镜头的夹角和棋盘图像数目,在实际定标过程中,我感觉棋盘图像数目应该大于20张,每成功检测一次完整的棋盘角点就要变换一下标定板的姿态(包括角度、距离) 。
5. 单目定标函数cvCalibrateCamera2采用怎样的 flags 比较合适?
由于一般镜头只需要计算k1,k2,p1,p2四个参数,所以我们首先要设置 CV_CALIB_FIX_K3;其次,如果所用的摄像头不是高端的、切向畸变系数非常少的,则不要设置 CV_CALIB_ZERO_TANGENT_DIST,否则单目校正误差会很大;如果事先知道摄像头内参的大概数值,并且cvCalibrateCamera2函数的第五个参数intrinsic_matrix非空,则也可设置 CV_CALIB_USE_INTRINSIC_GUESS ,以输入的intrinsic_matrix为初始估计值来加快内参的计算;其它的 flag 一般都不需要设置,对单目定标的影响不大。
P.S. 使用OpenCV进行摄像机定标虽然方便,但是定标结果往往不够准确和稳定,最好是使用 Matlab标定工具箱 来进行定标,再将定标结果取回来用于立体匹配和视差计算。工具箱的使用官方主页 有图文并茂的详细说明,此外,有两篇博文也进行了不错的总结,推荐阅读:
三、双目定标和双目校正
双目摄像头定标不仅要得出每个摄像头的内部参数,还需要通过标定来测量两个摄像头之间的相对位置(即右摄像头相对于左摄像头的三维平移 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。
(待续……)