list

双目测距与三维重建的OpenCV实现问题集锦（一）图像获取与单目定标

双目测距的基本原理

如上图所示，双目测距主要是利用了目标点在左右两幅视图上成像的横向坐标直接存在的差异（即视差）与目标点到成像平面的距离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 同时读取两个摄像头，不过后来一位研友来信讨论说只要把摄像头指针的创建代码按照摄像头序号降序执行，就可以顺利打开多个摄像头 ，例如：


   
   
  
  
    
    
   
    
     
     [c-sharp] 
     view plain 
      copy 
      
       
     
   
    
    
   
   
CvCapture* capture2 = cvCreateCameraCapture( 1 );   
    CvCapture* capture1 = cvCreateCameraCapture( 0 );   
   

采用DirectShow的方式读入时：

   
   
  
  
    
    
   
    
     
     [c-sharp] 
     view plain 
      copy 
      
       
     
   
    
    
   
   
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把帧数据取出来。例如：


   
   
  
  
    
    
   
    
     
     [c-sharp] 
     view plain 
      copy 
      
       
     
   
    
    
   
   
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．图像坐标系、摄像头坐标系和世界坐标系的关系

摄像头成像几何关系，其中O_c 点称为摄像头（透镜）的光心，X_c 轴和Y_c 轴与图像的x轴和Y轴平行，Z_c轴为摄像头的光轴，它与图像平面垂直。光轴与图像平面的交点O₁ ，即为图像坐标系的原点。由点O_c 与X_c 、Y_c 、Z_c 轴组成的坐标系称为摄像头坐标系，O_c O₁ 的距离为摄像头焦距，用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标定工具箱来进行定标，再将定标结果取回来用于立体匹配和视差计算。工具箱的使用官方主页有图文并茂的详细说明，此外，有两篇博文也进行了不错的总结，推荐阅读：

（1）分享一些OpenCV实现立体视觉的经验

（2）Matlab标定工具箱使用的一些注意事项

双目测距与三维重建的OpenCV实现问题集锦（二）双目定标与双目校正

三、双目定标和双目校正

双目摄像头定标不仅要得出每个摄像头的内部参数，还需要通过标定来测量两个摄像头之间的相对位置（即右摄像头相对于左摄像头的三维平移 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

注：原文中对p_l、p_r和q_l、q_r物理意义和计算公式的表述有误，已修正。（2011-04-12）

（1）Essential Matrix

如上图所示，给定一个目标点P，以左摄像头光心O_l为原点。点P相对于光心O_l的观察位置为P_l，相对于光心O_r的观察位置为P_r。点P在左摄像头成像平面上的位置为p_l，在右摄像头成像平面上的位置为p_r。注意P_l、P_r、p_l、p_r都处于摄像机坐标系，其量纲是与平移向量T相同的（p_l、p_r在图像坐标系中对应的像素坐标为 q_l、q_r）。

假设右摄像头相对于左摄像头的相对位置关系由旋转矩阵R和平移向量T表示，则可得：P_r = R(P_l-T)。

现在我们要寻找由点P、O_l和O_r确定的对极平面的表达式。注意到平面上任意一点x与点a的连线垂直于平面法向量n，即向量 (x-a) 与向量 n 的点积为0：(x-a)·n = 0。在Ol坐标系中，光心Or的位置为T，则P、O_l和O_r确定的对极平面可由下式表示：(P_l-T)^T·(P_l×T) = 0。

由 P_r = R(P_l-T) 和 R^T=R^-1 可得：(R^TP_r)^T·(P_l×T) = 0。

另一方面，向量的叉积又可表示为矩阵与向量的乘积，记向量T的矩阵表示为S，得：P_l×T = SP_l。

图11

那么就得到：(P_r)^TRSP_l = 0。这样，我们就得到Essential Matrix：E = RS。

通过矩阵E我们知道P_l和P_r的关系满足：(P_r)^TEP_l = 0。进一步地，由 p_l = f_l*P_l/Z_l 和 p_r = f_r*P_r/Z_r 我们可以得到点P在左右两个摄像机坐标系中的观察点 p_l和 p_r应满足的极线约束关系为：(p_r)^TEp_l = 0。

注意到 E 是不满秩的，它的秩为2，那么 (p_r)^TEp_l = 0 表示的实际上是一条直线，也就是对极线。

（2）Fundamental Matrix

由于矩阵E并不包含摄像头内参信息，且E是面向摄像头坐标系的。实际上我们更感兴趣的是在图像像素坐标系上去研究一个像素点在另一视图上的对极线，这就需要用到摄像机的内参信息将摄像头坐标系和图像像素坐标系联系起来。在（1）中，p_l和p_r是物理坐标值，对应的像素坐标值为q_l和q_r，摄像头内参矩阵为M，则有：p=M^-1q。从而：(p_r)^TEp_l = 0 à q_r^T(M_r^-1)^TE M_l^-1q_l = 0。这里，我们就得到Fundamental Matrix：F = (M_r^-1)^TE M_l^-1。并有 q_r^TFq_l = 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 的分析有一定道理，不过我觉得还有另外一种解释：如上图所示，摄像机C₁（C₂）与世界坐标系相对位置的外部参数为旋转矩阵R₁（R₂）和平移向量 t₁（t₂），如果下标1代表左摄像机，2代表右摄像机，显然在平移向量的水平分量上有 t_1x> t₂_x；若以左摄像机C₁为坐标原点，则可得到如上图所示的旋转矩阵R和平移向量t，由于t_1x> t_2x，则有 t_x < 0。

为了抵消T_x为负，在矩阵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。

（待续……）

【评论】前辈，我这两天用MATLAB和OPENCV做了好10组左右标定实验发现改变两个摄像头之间的六个*度（微调，变动不大）基于MATLAB标定的结果中内参数都基本比较稳定都是围绕在某个值波动但是幅值不大然后看3D效果，可以说标定结果是稳定的但是如果我用OPENCV标定的话如果两个摄像头的光轴不共面存在高度差的话，则标定的结果中某一个摄像头的内参数是稳定的但是另外一个摄像头的内参数的变化随着高度差的变大而越来越大，相当不稳定。基于平行双目视觉，两种标定方法得出的结果都差不多。那是不是说明，如果要基于OPENCV来标定，必须要保持两个光轴共面而且处于同一水平面？

【评论】你好，急切想知道一些问题，我在利用cvReprojectImageTo3D函数输出的矩阵数据读出三个坐标时是较小的数值，scal.val(2)Z=2.09323等，物体实际距离约为35cm，想请问这是什么原因吗？博主的三维坐标输出后是否经过什么处理（乘以16外）才能正确得到景深的距离？再请问一下坐标输出是的世界坐标的尺度单位是什么？

双目测距与三维重建的OpenCV实现问题集锦（三）立体匹配与视差计算

四、双目匹配与视差计算

立体匹配主要是通过找出每对图像间的对应关系，根据三角测量原理，得到视差图；在获得了视差信息后，根据投影模型很容易地可以得到原始图像的深度信息和三维信息。立体匹配技术被普遍认为是立体视觉中最困难也是最关键的问题，主要是以下因素的影响：

（1）光学失真和噪声（亮度、色调、饱和度等失衡）

【视觉-立体视觉】双目测距与三维重建的OpenCV实现问题集锦

（2）平滑表面的镜面反射

（3）投影缩减（Foreshortening）

（4）透视失真（Perspective distortions）

（5）低纹理（Low texture）

（6）重复纹理（Repetitive/ambiguous patterns）

（7）透明物体

（8）重叠和非连续

目前立体匹配算法是计算机视觉中的一个难点和热点，算法很多，但是一般的步骤是：

A、匹配代价计算

匹配代价计算是整个立体匹配算法的基础，实际是对不同视差下进行灰度相似性测量。常见的方法有灰度差的平方SD（squared intensity differences），灰度差的绝对值AD（absolute intensity differences）等。另外，在求原始匹配代价时可以设定一个上限值，来减弱叠加过程中的误匹配的影响。以AD法求匹配代价为例，可用下式进行计算，其中T为设定的阈值。

【视觉-立体视觉】双目测距与三维重建的OpenCV实现问题集锦

图18

B、匹配代价叠加

一般来说，全局算法基于原始匹配代价进行后续算法计算。而区域算法则需要通过窗口叠加来增强匹配代价的可靠性，根据原始匹配代价不同，可分为：

【视觉-立体视觉】双目测距与三维重建的OpenCV实现问题集锦

图19

C、视差获取

对于区域算法来说，在完成匹配代价的叠加以后，视差的获取就很容易了，只需在一定范围内选取叠加匹配代价最优的点（SAD和SSD取最小值，NCC取最大值）作为对应匹配点，如胜者为王算法WTA（Winner-take-all）。而全局算法则直接对原始匹配代价进行处理，一般会先给出一个能量评价函数，然后通过不同的优化算法来求得能量的最小值，同时每个点的视差值也就计算出来了。

D、视差细化（亚像素级）

大多数立体匹配算法计算出来的视差都是一些离散的特定整数值，可满足一般应用的精度要求。但在一些精度要求比较高的场合，如精确的三维重构中，就需要在初始视差获取后采用一些措施对视差进行细化，如匹配代价的曲线拟合、图像滤波、图像分割等。

有关立体匹配的介绍和常见匹配算法的比较，推荐大家看看Stefano Mattoccia 的讲义 Stereo Vision: algorithms and applications，190页的ppt，讲解得非常形象详尽。

1． opencv2.1和opencv2.0在做stereo vision方面有什么区别了？

2.1版增强了Stereo Vision方面的功能：

(1) 新增了 SGBM 立体匹配算法（源自Heiko Hirschmuller的《Stereo Processing by Semi-global Matching and Mutual Information》），可以获得比 BM 算法物体轮廓更清晰的视差图（但低纹理区域容易出现横/斜纹路，在 GCstate->fullDP 选项使能时可消减这种异常纹路，但对应区域视差变为0，且运行速度会有所下降），速度比 BM 稍慢， 352*288的帧处理速度大约是 5 帧/秒；

(2) 视差效果：BM < SGBM < GC；处理速度：BM > SGBM > GC ；

(3) BM 算法比2.0版性能有所提升，其状态参数新增了对左右视图感兴趣区域 ROI 的支持（roi1 和 roi2，由stereoRectify函数产生）；

(4) BM 算法和 GC 算法的核心代码改动不大，主要是面向多线程运算方面的（由 OpenMP 转向 Intel TBB）；

(5) cvFindStereoCorrespondenceBM 函数的disparity参数的数据格式新增了 CV_32F 的支持，这种格式的数据给出实际视差，而 2.0 版只支持 CV_16S，需要除以 16.0 才能得到实际的视差数值。

2．用于立体匹配的图像可以是彩色的吗？

在OpenCV2.1中，BM和GC算法只能对8位灰度图像计算视差，SGBM算法则可以处理24位（8bits*3）彩色图像。所以在读入图像时，应该根据采用的算法来处理图像：

int color_mode = alg == STEREO_SGBM ? 1 : 0;
//////////////////////////////////////////////////////////////////////////
// 载入图像
cvGrabFrame( lfCam );
cvGrabFrame( riCam );
frame1 = cvRetrieveFrame( lfCam );
frame2 = cvRetrieveFrame( riCam );
if(frame1.empty()) break;
resize(frame1, img1, img_size, 0, 0);
resize(frame2, img2, img_size, 0, 0);
// 选择彩色或灰度格式作为双目匹配的处理图像
if (!color_mode && cn>1)
{
cvtColor(img1, img1gray, CV_BGR2GRAY);
cvtColor(img2, img2gray, CV_BGR2GRAY);
img1p = img1gray;
img2p = img2gray;
}
else
{
img1p = img1;
img2p = img2;
}

3．怎样获取与原图像有效像素区域相同的视差图？

在OpenCV2.0及以前的版本中，所获取的视差图总是在左侧和右侧有明显的黑色区域，这些区域没有有效的视差数据。视差图有效像素区域与视差窗口（ndisp，一般取正值且能被16整除）和最小视差值（mindisp，一般取0或负值）相关，视差窗口越大，视差图左侧的黑色区域越大，最小视差值越小，视差图右侧的黑色区域越大。其原因是为了保证参考图像（一般是左视图）的像素点能在目标图像（右视图）中按照设定的视差匹配窗口匹配对应点，OpenCV 只从参考图像的第 (ndisp - 1 + mindisp) 列开始向右计算视差，第 0 列到第 (ndisp - 1 + mindisp) 列的区域视差统一设置为 (mindisp - 1) *16；视差计算到第 width + mindisp 列时停止，余下的右侧区域视差值也统一设置为 (mindisp - 1) *16。

00177 static const int DISPARITY_SHIFT = 4;
…
00411     int ndisp = state->numberOfDisparities;
00412     int mindisp = state->minDisparity;
00413     int lofs = MAX(ndisp - 1 + mindisp, 0);
00414     int rofs = -MIN(ndisp - 1 + mindisp, 0);
00415     int width = left->cols, height = left->rows;
00416     int width1 = width - rofs - ndisp + 1;
…
00420     short FILTERED = (short)((mindisp - 1) << DISPARITY_SHIFT);
…
00466     // initialize the left and right borders of the disparity map
00467     for( y = 0; y < height; y++ )
00468     {
00469         for( x = 0; x < lofs; x++ )
00470             dptr[y*dstep + x] = FILTERED;
00471         for( x = lofs + width1; x < width; x++ )
00472             dptr[y*dstep + x] = FILTERED;
00473     }
00474     dptr += lofs;
00475
00476     for( x = 0; x < width1; x++, dptr++ )

…

这样的设置很明显是不符合实际应用的需求的，它相当于把摄像头的视场范围缩窄了。因此，OpenCV2.1做了明显的改进，不再要求左右视图和视差图的大小（size）一致，允许对视差图进行左右边界延拓，这样，虽然计算视差时还是按上面的代码思路来处理左右边界，但是视差图的边界得到延拓后，有效视差的范围就能够与对应视图完全对应。具体的实现代码范例如下：

//////////////////////////////////////////////////////////////////////////
// 对左右视图的左边进行边界延拓，以获取与原始视图相同大小的有效视差区域
copyMakeBorder(img1r, img1b, 0, 0, m_nMaxDisp, 0, IPL_BORDER_REPLICATE);
copyMakeBorder(img2r, img2b, 0, 0, m_nMaxDisp, 0, IPL_BORDER_REPLICATE);

//////////////////////////////////////////////////////////////////////////
// 计算视差
if( alg == STEREO_BM )
{
	bm(img1b, img2b, dispb);
	// 截取与原始画面对应的视差区域（舍去加宽的部分）
	displf = dispb.colRange(m_nMaxDisp, img1b.cols);	
}
else if(alg == STEREO_SGBM)
{
	sgbm(img1b, img2b, dispb);
	displf = dispb.colRange(m_nMaxDisp, img1b.cols);
}

4． cvFindStereoCorrespondenceBM的输出结果好像不是以像素点为单位的视差？

“@scyscyao：在OpenCV2.0中，BM函数得出的结果是以16位符号数的形式的存储的，出于精度需要，所有的视差在输出时都扩大了16倍(2^4)。其具体代码表示如下：

dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4);

可以看到，原始视差在左移8位(256)并且加上一个修正值之后又右移了4位，最终的结果就是左移4位。

因此，在实际求距离时，cvReprojectTo3D出来的X/W,Y/W,Z/W都要乘以16 (也就是W除以16)，才能得到正确的三维坐标信息。”

在OpenCV2.1中，BM算法可以用 CV_16S 或者 CV_32F 的方式输出视差数据，使用32位float格式可以得到真实的视差值，而CV_16S 格式得到的视差矩阵则需要除以16 才能得到正确的视差。另外，OpenCV2.1另外两种立体匹配算法 SGBM 和 GC 只支持 CV_16S 格式的 disparity 矩阵。

5．如何设置BM、SGBM和GC算法的状态参数？

（1）StereoBMState

// 预处理滤波参数

preFilterType：预处理滤波器的类型，主要是用于降低亮度失真（photometric distortions）、消除噪声和增强纹理等, 有两种可选类型：CV_STEREO_BM_NORMALIZED_RESPONSE（归一化响应）或者 CV_STEREO_BM_XSOBEL（水平方向Sobel算子，默认类型）, 该参数为 int 型；
preFilterSize：预处理滤波器窗口大小，容许范围是[5,255]，一般应该在 5x5..21x21 之间，参数必须为奇数值, int 型
preFilterCap：预处理滤波器的截断值，预处理的输出值仅保留[-preFilterCap, preFilterCap]范围内的值，参数范围：1 - 31（文档中是31，但代码中是 63）, int

// SAD 参数

SADWindowSize：SAD窗口大小，容许范围是[5,255]，一般应该在 5x5 至 21x21 之间，参数必须是奇数，int 型
minDisparity：最小视差，默认值为 0, 可以是负值，int 型
numberOfDisparities：视差窗口，即最大视差值与最小视差值之差, 窗口大小必须是 16 的整数倍，int 型

// 后处理参数

textureThreshold：低纹理区域的判断阈值。如果当前SAD窗口内所有邻居像素点的x导数绝对值之和小于指定阈值，则该窗口对应的像素点的视差值为 0（That is, if the sum of absolute values of x-derivatives computed over SADWindowSize by SADWindowSize pixel neighborhood is smaller than the parameter, no disparity is computed at the pixel），该参数不能为负值，int 型
uniquenessRatio：视差唯一性百分比，视差窗口范围内最低代价是次低代价的(1 + uniquenessRatio/100)倍时，最低代价对应的视差值才是该像素点的视差，否则该像素点的视差为 0 （the minimum margin in percents between the best (minimum) cost function value and the second best value to accept the computed disparity, that is, accept the computed disparity d^ only if SAD(d) >= SAD(d^) x (1 + uniquenessRatio/100.) for any d != d*+/-1 within the search range ），该参数不能为负值，一般5-15左右的值比较合适，int 型
speckleWindowSize：检查视差连通区域变化度的窗口大小, 值为 0 时取消 speckle 检查，int 型
speckleRange：视差变化阈值，当窗口内视差变化大于阈值时，该窗口内的视差清零，int 型

// OpenCV2.1 新增的状态参数

roi1, roi2：左右视图的有效像素区域，一般由双目校正阶段的 cvStereoRectify 函数传递，也可以自行设定。一旦在状态参数中设定了 roi1 和 roi2，OpenCV 会通过cvGetValidDisparityROI 函数计算出视差图的有效区域，在有效区域外的视差值将被清零。
disp12MaxDiff：左视差图（直接计算得出）和右视差图（通过cvValidateDisparity计算得出）之间的最大容许差异。超过该阈值的视差值将被清零。该参数默认为 -1，即不执行左右视差检查。int 型。注意在程序调试阶段最好保持该值为 -1，以便查看不同视差窗口生成的视差效果。具体请参见《使用OpenGL动态显示双目视觉三维重构效果示例》一文中的讨论。

在上述参数中，对视差生成效果影响较大的主要参数是 SADWindowSize、numberOfDisparities 和 uniquenessRatio 三个，一般只需对这三个参数进行调整，其余参数按默认设置即可。

在OpenCV2.1中，BM算法有C和C++ 两种实现模块。

（2）StereoSGBMState

SGBM算法的状态参数大部分与BM算法的一致，下面只解释不同的部分：

SADWindowSize：SAD窗口大小，容许范围是[1,11]，一般应该在 3x3 至 11x11 之间，参数必须是奇数，int 型
P1, P2：控制视差变化平滑性的参数。P1、P2的值越大，视差越平滑。P1是相邻像素点视差增/减 1 时的惩罚系数；P2是相邻像素点视差变化值大于1时的惩罚系数。P2必须大于P1。OpenCV2.1提供的例程 stereo_match.cpp 给出了 P1 和 P2 比较合适的数值。
fullDP：布尔值，当设置为 TRUE 时，运行双通道动态编程算法（full-scale 2-pass dynamic programming algorithm），会占用O(W*H*numDisparities)个字节，对于高分辨率图像将占用较大的内存空间。一般设置为 FALSE。

注意OpenCV2.1的SGBM算法是用C++ 语言编写的，没有C实现模块。与H. Hirschmuller提出的原算法相比，主要有如下变化：

算法默认运行单通道DP算法，只用了5个方向，而fullDP使能时则使用8个方向（可能需要占用大量内存）。
算法在计算匹配代价函数时，采用块匹配方法而非像素匹配（不过SADWindowSize=1时就等于像素匹配了）。
匹配代价的计算采用BT算法（"Depth Discontinuities by Pixel-to-Pixel Stereo" by S. Birchfield and C. Tomasi），并没有实现基于互熵信息的匹配代价计算。
增加了一些BM算法中的预处理和后处理程序。

（3）StereoGCState

GC算法的状态参数只有两个：numberOfDisparities 和 maxIters ，并且只能通过cvCreateStereoGCState 在创建算法状态结构体时一次性确定，不能在循环中更新状态信息。GC算法并不是一种实时算法，但可以得到物体轮廓清晰准确的视差图，适用于静态环境物体的深度重构。

注意GC算法只能在C语言模式下运行，并且不能对视差图进行预先的边界延拓，左右视图和左右视差矩阵的大小必须一致。

6．如何实现视差图的伪彩色显示？

首先要将16位符号整形的视差矩阵转换为8位无符号整形矩阵，然后按照一定的变换关系进行伪彩色处理。我的实现代码如下：

// 转换为 CV_8U 格式，彩色显示
dispLfcv = displf, dispRicv = dispri, disp8cv = disp8;
if (alg == STEREO_GC)
{
	cvNormalize( &dispLfcv, &disp8cv, 0, 256, CV_MINMAX );
} 
else
{
	displf.convertTo(disp8, CV_8U, 255/(m_nMaxDisp*16.));
}
F_Gray2Color(&disp8cv, vdispRGB);

灰度图转伪彩色图的代码，主要功能是使灰度图中亮度越高的像素点，在伪彩色图中对应的点越趋向于红色；亮度越低，则对应的伪彩色越趋向于蓝色；总体上按照灰度值高低，由红渐变至蓝，中间色为绿色。其对应关系如下图所示：

【视觉-立体视觉】双目测距与三维重建的OpenCV实现问题集锦

图20

void F_Gray2Color(CvMat* gray_mat, CvMat* color_mat)
{
	if(color_mat)
		cvZero(color_mat);
		
	int stype = CV_MAT_TYPE(gray_mat->type), dtype = CV_MAT_TYPE(color_mat->type);
	int rows = gray_mat->rows, cols = gray_mat->cols;

	// 判断输入的灰度图和输出的伪彩色图是否大小相同、格式是否符合要求
	if (CV_ARE_SIZES_EQ(gray_mat, color_mat) && stype == CV_8UC1 && dtype == CV_8UC3)
	{
		CvMat* red = cvCreateMat(gray_mat->rows, gray_mat->cols, CV_8U);
		CvMat* green = cvCreateMat(gray_mat->rows, gray_mat->cols, CV_8U);
		CvMat* blue = cvCreateMat(gray_mat->rows, gray_mat->cols, CV_8U);
		CvMat* mask = cvCreateMat(gray_mat->rows, gray_mat->cols, CV_8U);

		// 计算各彩色通道的像素值
		cvSubRS(gray_mat, cvScalar(255), blue);	// blue(I) = 255 - gray(I)
		cvCopy(gray_mat, red);			// red(I) = gray(I)
		cvCopy(gray_mat, green);			// green(I) = gray(I),if gray(I) < 128
		cvCmpS(green, 128, mask, CV_CMP_GE );	// green(I) = 255 - gray(I), if gray(I) >= 128
		cvSubRS(green, cvScalar(255), green, mask);
		cvConvertScale(green, green, 2.0, 0.0);

		// 合成伪彩色图
		cvMerge(blue, green, red, NULL, color_mat);

		cvReleaseMat( &red );
		cvReleaseMat( &green );
		cvReleaseMat( &blue );
		cvReleaseMat( &mask );
	}
}

7．如何将视差数据保存为 txt 数据文件以便在 Matlab 中读取分析？

由于OpenCV本身只支持 xml、yml 的数据文件读写功能，并且其xml文件与构建网页数据所用的xml文件格式不一致，在Matlab中无法读取。我们可以通过以下方式将视差数据保存为txt文件，再导入到Matlab中。

void saveDisp(const char* filename, const Mat& mat)		
{
	FILE* fp = fopen(filename, "wt");
	fprintf(fp, "%02d/n", mat.rows);
	fprintf(fp, "%02d/n", mat.cols);
	for(int y = 0; y < mat.rows; y++)
	{
		for(int x = 0; x < mat.cols; x++)
		{
			short disp = mat.at<short>(y, x); // 这里视差矩阵是CV_16S 格式的，故用 short 类型读取
			fprintf(fp, "%d/n", disp); // 若视差矩阵是 CV_32F 格式，则用 float 类型读取
		}
	}
	fclose(fp);
}

相应的Matlab代码为：

function img = txt2img(filename)
data = importdata(filename);
r = data(1);    % 行数
c = data(2);    % 列数
disp = data(3:end); % 视差
vmin = min(disp);
vmax = max(disp);
disp = reshape(disp, [c,r])'; % 将列向量形式的 disp 重构为 矩阵形式
%  OpenCV 是行扫描存储图像，Matlab 是列扫描存储图像
%  故对 disp 的重新排列是首先变成 c 行 r 列的矩阵，然后再转置回 r 行 c 列
img = uint8( 255 * ( disp - vmin ) / ( vmax - vmin ) );
mesh(disp);
set(gca,'YDir','reverse');  % 通过 mesh 方式绘图时，需倒置 Y 轴方向
axis tight; % 使坐标轴显示范围与数据范围相贴合，去除空白显示区

显示效果如下：

【视觉-立体视觉】双目测距与三维重建的OpenCV实现问题集锦

图21

【评论】

你好，我打不开 “Stereo Vision: algorithms and applications，190页的ppt，讲解得非常形象详尽。”的链接，能不能给我发一份
回复md104104：StereoVision.pdf
http://pan.baidu.com/s/1bnGYKnt
请问楼主有没有做过Heiko Hirschmuller的《Stereo Processing by Semi-global Matching and Mutual Information》的基于互信息的半全局匹配。
“获取视差图后要通过 reProjectImageTo3D 函数才能得到真实的三维坐标数据。”这里的真实的三维坐标数据是以什么为参考坐标？还有就是得出的是那些点的三维坐标，我用过自己拍的图像试验了一下导出reProjectImageTo3D的数据，在tet中有256个点，搞不懂这些数据与真实的物体有什么对应关系。

双目测距与三维重建的OpenCV实现问题集锦（四）三维重建与OpenGL显示

五、三维重建与 OpenGL 显示

.

在获取到视差数据后，利用 OpenCV 的 reProjectImageTo3D 函数结合 Bouquet 校正方法得到的 Q 矩阵就可以得到环境的三维坐标数据，然后利用 OpenGL 来实现三维重构。 OpenCV 与 OpenGL 的编程范例，我在学习笔记（ 15 ）中有详细的讨论，这里就不重复了，下面补充一些细节问题：.

1． reProjectImageTo3D 是怎样计算出三维坐标数据的？

图 22

.相信看过 OpenCV 第 12 章的朋友对上图中的 Q 矩阵不会陌生，根据以上变换公式，按理说 OpenCV 应该也是通过矩阵运算的方式来计算出三维坐标数据的，但实际上仔细查看源代码，会发现cvReprojectImageTo3D 用了比较奇怪的方法来实现，主要代码如下：
[cpp] view plain copy
1. 02737 for( y = 0; y < rows; y++ )
2. 02738 {
3. 02739 const float* sptr = (const float*)(src->data.ptr + src->step*y); // 视差矩阵指针
4. 02740 float* dptr0 = (float*)(dst->data.ptr + dst->step*y), *dptr = dptr0; // 三维坐标矩阵指针
5. // 每一行运算开始时，用当前行号y 乘以Q阵第2列、再加上Q阵第4列，作为初始值
6. // 记 qq=[qx, qy, qz, qw]’
7. 02741 double qx = q[0][1]*y + q[0][3], qy = q[1][1]*y + q[1][3];
8. 02742 double qz = q[2][1]*y + q[2][3], qw = q[3][1]*y + q[3][3];
9. …
10. // 每算完一个像素的三维坐标，向量qq 累加一次q阵第1列
11. // 即：qq = qq + q(:,1)
12. 02769 for( x = 0; x < cols; x++, qx += q[0][0], qy += q[1][0], qz += q[2][0], qw += q[3][0] )
13. 02770 {
14. 02771 double d = sptr[x];
15. // 计算当前像素三维坐标
16. // 将向量qq 加上 Q阵第3列与当前像素视差d的乘积，用所得结果的第4元素除前三位元素即可
17. // [X,Y,Z,W]’ = qq + q(:,3) * d; iW = 1/W; X=X*iW; Y=Y*iW; Z=Z*iW;
18. 02772 double iW = 1./(qw + q[3][2]*d);
19. 02773 double X = (qx + q[0][2]*d)*iW;
20. 02774 double Y = (qy + q[1][2]*d)*iW;
21. 02775 double Z = (qz + q[2][2]*d)*iW;
22. 02776 if( fabs(d-minDisparity) <= FLT_EPSILON )
23. 02777 Z = bigZ; // 02713 const double bigZ = 10000.;
24. 02778
25. 02779 dptr[x*3] = (float)X;
26. 02780 dptr[x*3+1] = (float)Y;
27. 02781 dptr[x*3+2] = (float)Z;
28. 02782 }
OpenCV 的这种计算方式比较令人费解，我的理解是可能这种方式的计算速度比较快。理论上，直接通过矩阵 Q 与向量 [x,y,d,1]’ 的乘积就可以得到相同的结果，下面用 Matlab 来验证一下两种方式是异曲同工的，用 Matlab 按照 OpenCV 计算方式得到的结果称为“ OpenCV method ”，直接按公式计算得到的结果称为“ Equation method ”，用 OpenCV 本身算出的三维坐标作为参考，程序代码如下：
[c-sharp] view plain copy
1. close all;clear all;clc
2. im = imread('C:/Stereo IO Data/lfFrame_01.jpg');
3. data = importdata('C:/Stereo IO Data/disparity_01.txt');
4. r = data(1); % 行数
5. c = data(2); % 列数
6. disp = data(3:end); % 视差
7. vmin = min(disp);
8. vmax = max(disp);
9. disp = reshape(disp, [c,r])'; % 将列向量形式的 disp 重构为矩阵形式
10. % OpenCV 是行扫描存储图像，Matlab 是列扫描存储图像
11. % 故对 disp 的重新排列是首先变成 c 行 r 列的矩阵，然后再转置回 r 行 c 列
12. img = uint8( 255 * ( disp - vmin ) / ( vmax - vmin ) );
13. q = [1. 0. 0. -1.5690376663208008e+002;...
14. 0. 1. 0. -1.4282237243652344e+002;...
15. 0. 0. 0. 5.2004731331639300e+002;...
16. 0. 0. 1.0945105843175637e-002 0.]; % q(4,3) 原为负值，现修正为正值
17. big_z = 1e5;
18. pos1 = zeros(r,c,3);
19. pos2 = zeros(r,c,3);
20. for i = 1:r
21. qq = q*[0 i 0 1]';
22. for j = 1:c
23. if disp(i,j)>0
24. % OpenCV method
25. vec = qq + q(:,3)*disp(i,j);
26. vec = vec/vec(4);
27. pos1(i,j,:) = vec(1:3);
28. % Textbook method
29. tmp = q*[j,i,disp(i,j),1]'; % j 是列数，i 是行数，分别对应公式中的 x 和 y
30. pos2(i,j,:) = tmp(1:3)/tmp(4);
31. else
32. pos1(i,j,3) = big_z;
33. pos2(i,j,3) = big_z;
34. end
35. qq = qq + q(:,1);
36. end
37. end
38. subplot(221);
39. imshow(im); title('Left Frame');
40. subplot(222);
41. imshow(img); title('Disparity map');
42. % Matlab按OpenCV计算方式得到的三维坐标
43. x = pos1(:,:,1);
44. y = -pos1(:,:,2); % 图像坐标系Y轴是向下为正方向，因此需添加负号来修正
45. z = pos1(:,:,3);
46. ind = find(z>10000); % 以毫米为量纲
47. x(ind)=NaN; y(ind)=NaN; z(ind)=NaN;
48. subplot(234);
49. mesh(x,z,y,double(im),'FaceColor','texturemap'); % Matlab 的 mesh、surf 函数支持纹理映射
50. colormap(gray);
51. axis equal;
52. axis([-1000 1000 0 9000 -500 2000]);
53. xlabel('Horizonal');ylabel('Depth');zlabel('Vertical'); title('OpenCV method');
54. view([0 0]); % 正视图
55. % view([0 90]); % 俯视图
56. % view([90 0]); % 侧视图
57. % Matlab 按公式直接计算得到的三维坐标
58. x = pos2(:,:,1);
59. y = -pos2(:,:,2);
60. z = pos2(:,:,3);
61. ind = find(z>10000); % 以毫米为量纲
62. x(ind)=NaN; y(ind)=NaN; z(ind)=NaN;
63. subplot(235);
64. mesh(x,z,y,double(im),'FaceColor','texturemap');
65. colormap(gray);
66. axis equal;
67. axis([-1000 1000 0 9000 -500 2000]);
68. xlabel('Horizonal');ylabel('Depth');zlabel('Vertical'); title('Equation method');
69. view([0 0]);
70. % 读入OpenCV计算保存到本地的三维坐标作为参考
71. data=importdata('C:/Stereo IO Data/xyz.txt');
72. x=data(:,1); y=data(:,2); z=data(:,3);
73. ind=find(z>1000); % 以厘米为量纲
74. x(ind)=NaN; y(ind)=NaN; z(ind)=NaN;
75. x=reshape(x,[352 288])'; % 数据写入时是逐行进行的，而Matlab是逐列读取
76. y=-reshape(y,[352 288])';
77. z=reshape(z,[352 288])';
78. subplot(236)
79. mesh(x,z, y,double(im),'FaceColor','texturemap');
80. colormap(gray);
81. axis equal;axis([-100 100 0 900 -50 200]);
82. xlabel('Horizonal');ylabel('Depth');zlabel('Vertical'); title('OpenCV result');
83. view([0 0]);
图 23

.

2．             为什么利用修正了的 Q 矩阵所计算得到的三维数据中， Y 坐标数据是正负颠倒的？

图 24

这个问题我觉得可以从图像坐标系与摄像机坐标系的关系这一角度来解释。如上图所示，一般图像坐标系和摄像机坐标系都是以从左至右为 X 轴正方向，从上至下为 Y 轴正方向 ，摄像机坐标系的 Z 轴正方向则是从光心到成像平面的垂线方向。因此，我们得到的三维坐标数据中 Y 轴数据的正负与实际是相反的，在应用时要添加负号来修正。

.

.

3．             如何画出三维重建图像和景深图像？

利用 cvReprojectImageTo3D 计算出的三维坐标数据矩阵一般是三通道浮点型的，需要注意的是这个矩阵存储的是三维坐标数据，而不是 RGB 颜色值，所以是不能调用 cvShowImage() 或者 OpenCV2.1 版的imshow() 等函数来显示这个矩阵，否则就会看到这种图像：

图 25

.这里出现的明显的四个色块，其实应该是由三维坐标数据中的 X 轴和 Y 轴数据造成，不同象限的数据形成相应的色块。

要画出正确的三维重建图像，可以结合 OpenGL （可参考我的学习笔记（ 15 ））或者 Matlab （例如保存三维数据到本地然后用 Matlab 的 mesh 函数画出，例程见本文问题 1 ；也可以考虑在 OpenCV 中调用Matlab 混合编程）来实现。

深度图像的显示相对比较简单，只要从三维坐标数据中分离出来（可用 cvSplit() 函数），经过适当的格式转换（例如转换为 CV_8U 格式），就可用 cvShowImage() 或者 OpenCV2.1 版的 imshow() 等函数来显示了，伪彩色的深度图 也可以参考我的学习笔记（ 18 ） 问题 6 给出的例程 稍作修改即可实现。

.

4．             怎样把 OpenGL 窗口的图像复制到 OpenCV 中用 IplImage 格式显示和保存？

在学习笔记（ 15 ）中详细给出了将 OpenCV 生成的 IplImage 图像和三维坐标数据复制到 OpenGL 中显示的例程，而在应用中，我们有时候也需要把 OpenGL 实时显示的三维图像复制到 OpenCV 中，用IplImage 格式保存，以便和其它图像组合起来显示或保存为视频文件。这里给出相应的例程以供参考：

首先在创建 OpenGL 窗口时，显示模式要如下设置：
[c-sharp] view plain copy
1. //***OpenGL Window
2. glutInit(&argc, argv);
3. glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);
4. glutInitWindowPosition(10,420);
5. glutInitWindowSize(glWinWidth, glWinHeight);
6. glutCreateWindow("3D disparity image");
在循环中的调用：
[c-sharp] view plain copy
1. //////////////////////////////////////////////////////////////////////////
2. // OpenGL显示
3. img3dIpl = img3d;
4. load3dDataToGL(&img3dIpl); // 载入需要显示的图像（视差数据）
5. loadTextureToGL(&img1roi); // 显示纹理
6. glutReshapeFunc (reshape); // 窗口变化时重绘图像
7. glutDisplayFunc(renderScene); // 显示三维图像
8. glutPostRedisplay(); // 刷新画面（不用此语句则不能动态更新图像）
9. loadPixel2IplImage(imgGL); // 将 OpenGL 生成的像素值存储到 IplImage 中
loadGLPixelToIplImage 函数定义：

c-sharp] view plain copy
1. //////////////////////////////////////////////////////////////////////////
2. // 将OpenGL窗口像素存储到 IplImage 中
3. void loadGLPixelToIplImage(IplImage* img)
4. {
5. const int n = 3*glWinWidth*glWinHeight;
6. float *pixels = (float *)malloc(n * sizeof(GL_FLOAT));
7. IplImage *tmp = cvCreateImage(cvSize(glWinWidth, glWinHeight), 8, 3);
8. tmp->origin = CV_ORIGIN_BL;
9. /* 后台缓存的图像数据才是我们需要复制的，若复制前台缓存会把可能的叠加在OpenGL窗口上的对象（其它窗口或者鼠标指针）也复制进去*/
10. glReadBuffer(GL_BACK);
11. glReadPixels(0, 0, glWinWidth, glWinHeight, GL_RGB, GL_FLOAT, pixels);
12. int k = 0;
13. for(int i = 0 ; i < glWinHeight; i++)
14. {
15. for(int j = 0 ; j < glWinWidth; j++,k+=3)
16. {
17. CvPoint pt = {j, glWinHeight - i - 1};
18. uchar* temp_ptr = &((uchar*)(tmp->imageData + tmp->widthStep*pt.y))[pt.x*3];
19. //OpenGL采用的是BGR格式，所以,读出来以后,还要换一下R<->B,才能得到RGB
20. temp_ptr[0] = pixels[k+2] * 255; //blue
21. temp_ptr[1] = pixels[k+1] * 255; //green
22. temp_ptr[2] = pixels[k] * 255; //red
23. }
24. }
25. cvResize(tmp, img);
26. // 释放内存
27. free(pixels);
28. cvReleaseImage(&tmp);
29. }
显示效果如下：

图26

（待续）

.

.