opencv_stereoRectify源码解析

背景

双目视觉中计算视差等需要用到立体矫正，目前对于源码解析的较少，本期对原理和opencv stereoRectify函数进行介绍和对应源码进行解析

原理

stereoRectify包含5个输出，即
R1 - 立体矫正时左相机需要变换的旋转矩阵
R2 - 立体矫正时右相机需要变换的旋转矩阵
P1 - 立体矫正后左相机在新像平面下的投影矩阵
P2 - 立体矫正后右相机在新像平面下的投影矩阵
Q - 立体矫正后利用视差进行计算深度的矩阵

void cv::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(),Rect*       validPixROI1 = 0,Rect*       validPixROI2 = 0
)

整个stereoRectify包含着4个步骤：
① 左右相机根据相对的旋转矩阵，各自转一半的角度，使得两个坐标系的3个轴都平行，像面平行
② 将两个坐标系绕Z轴旋转，使得X轴平行于相机光心的连线，达到极点无穷远，两个图像极线对齐的目的
③ 构造新的投影矩阵
④ 调整焦距来缩放矫正后图像的视野范围

1. 像面平行

将左右相机之间的外参数中的旋转矩阵matR，罗德里格斯转换为角轴om表示，方便求解空间旋转角度的一半, 一半旋转矩阵为r_r，对应源码和注释如下。左右相机具有一定角度的像面，经过这一步后，像面平行，再后续进一步统一两个相机的焦距后，像面平移到在同一个面内。

    // 计算旋转向量omif( matR->rows == 3 && matR->cols == 3 )cvRodrigues2(matR, &om);          // 如果是旋转矩阵，转为旋转向量elsecvConvert(matR, &om); // 已经是旋转向量cvConvertScale(&om, &om, -0.5); // 角轴层面做平均cvRodrigues2(&om, &r_r);        // 罗德里格斯转换为旋转矩阵

2. 极线对齐

对极几何中，两个相机光心连线与像面的交点为极点，当连线与像面平行，交点位于无穷远的地方，基础矩阵具有特殊形式，使得左右图像上行的极线是对齐的。

如图所示，经过步骤1后，两个相机坐标轴的位姿可能如示意图所示，为了让X轴平行于两个光心0102的连线，需要将坐标系绕着Z轴旋转$\theta$角度。opencv中是这样求解的该旋转矩阵：
定义沿着X轴的单位向量uu=[1,0,0], 求解步骤1后两个光心连线的向量t=[tx,ty,tz], uu和t的向量点乘$$\overrightarrow{uu}\cdot \vec{t}=|uu|\cdot |t|\cdot \cos \theta =tx$$其中，uu为单位向量，模为1，那么夹角为$$\theta =acos(tx/norm(t))$$
为了构造旋转轴为Z轴，旋转角度为$\theta$的角轴，首先求解Z轴的向量，由向量uu=[1,0,0]和t=[tx,ty,tz]叉乘得到$$\overrightarrow{ww}=\vec{t}\times \overrightarrow{uu}$$
进一步的，对$\overrightarrow{ww}$的模缩放为$\theta$（角轴的定义中，向量表示旋转轴，模表示旋转角度），$$scale=\frac{\theta }{norm(ww)}=\frac{acos(tx/norm(t))}{norm(ww)}$$这样，$ww$乘上scale之后，向量的模就是$\theta$, 然后，将sacle转换后的向量经过罗德里格斯转为旋转矩阵。

    cvMatMul(&r_r, matT, &t); // 步骤1完成后，右相机光心(Tx,Ty,Tz)转换到新的左相机坐标系下的位置int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1;   // 判断主平移方向（水平/垂直）// 水平方向是Tx为主量double c = _t[idx], nt = cvNorm(&t, 0, CV_L2); // c = Tx，并得到单位向量nt_uu[idx] = c > 0 ? 1 : -1; // 若水平方向为主分量，uu[3]={1,0,0}CV_Assert(nt > 0.0);// t为步骤1完成后，两个光心的连线的向量，uu为{1,0,0}，即X轴的单位向量，两者叉乘为Z轴的方向cvCrossProduct(&t,&uu,&ww); // 求解叉乘后向量的模double nw = cvNorm(&ww, 0, CV_L2);if (nw > 0.0)cvConvertScale(&ww, &ww, acos(fabs(c)/nt)/nw);  // 将叉乘的轴的模缩放为旋转角度theta，cvRodrigues2(&ww, &wR); // 向量转为旋转矩阵，此向量模为转角theta，方向为Z轴

3. 构造新的投影矩阵

步骤1和步骤2完成后，左右相机的像面已经平行，并且极线也对齐，通过两个相机焦距取平均来使得两个像面在同一个面上。如图所示，此时两个相机的各个轴平行，相对旋转矩阵为单位阵，T为光心的距离，那么构造投影矩阵为

其中f焦距为两个相机焦距取平均

fc_new = (cvmGet(_cameraMatrix1, idx ^ 1, idx ^ 1) + cvmGet(_cameraMatrix2, idx ^ 1, idx ^ 1)) * ratio;

左相机投影矩阵为内参矩阵乘以RT，其中R为单位阵，T为0；
右相机投影矩阵为内参矩阵乘以RT，其中R为单位阵，T为光心连线向量（tx,0,0),因此，矩阵相乘后，在第一行第四列是tx*f。
并且，其中cx,cy也不再是原内参矩阵的cx，cy。而是对原图的四个角点(0,0),(0,width-1),(height-1,0),(height-1,width-1), 先去除畸变，然后转为三维点，并通过步骤1和2的变换投影到新的像面上，求解这四个点的重心与图像重心的偏移为cx,cy

4. 缩放视野

自由缩放参数。如果该参数为 - 1 或不存在，则函数执行默认缩放。否则，该参数的值应介于 0 和 1 之间。
alpha=0：表示对校正后的图像进行缩放和平移，使得仅显示有效像素（校正后无黑色区域）。
alpha=1：表示对校正后的图像进行抽取和平移，使得原始相机图像中的所有像素均保留在校正后的图像中（不丢失源图像像素）。
显然，任何中间值都会产生介于这两种极端情况之间的中间结果。

// 对输入的两个相机的内参、畸变参数、旋转、平移等进行立体校正，计算校正变换矩阵和投影矩阵
void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2,const CvMat* _distCoeffs1, const CvMat* _distCoeffs2,CvSize imageSize, const CvMat* matR, const CvMat* matT,CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2,CvMat* matQ, int flags, double alpha, CvSize newImgSize,CvRect* roi1, CvRect* roi2 )
{// 定义中间变量double _om[3], _t[3] = {0}, _uu[3]={0,0,0}, _r_r[3][3], _pp[3][4];double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3];cv::Rect_<float> inner1, inner2, outer1, outer2;// 构造CvMat类型的中间变量CvMat om  = cvMat(3, 1, CV_64F, _om);CvMat t   = cvMat(3, 1, CV_64F, _t);CvMat uu  = cvMat(3, 1, CV_64F, _uu);CvMat r_r = cvMat(3, 3, CV_64F, _r_r);CvMat pp  = cvMat(3, 4, CV_64F, _pp);CvMat ww  = cvMat(3, 1, CV_64F, _ww); // 临时变量CvMat wR  = cvMat(3, 3, CV_64F, _wr);CvMat Z   = cvMat(3, 1, CV_64F, _z);CvMat Ri  = cvMat(3, 3, CV_64F, _ri);double nx = imageSize.width, ny = imageSize.height;int i, k;// 计算旋转向量omif( matR->rows == 3 && matR->cols == 3 )cvRodrigues2(matR, &om);          // 如果是旋转矩阵，转为旋转向量elsecvConvert(matR, &om); // 已经是旋转向量cvConvertScale(&om, &om, -0.5); // 角轴层面做平均cvRodrigues2(&om, &r_r);        // 罗德里格斯转换为旋转矩阵cvMatMul(&r_r, matT, &t);       // 右相机光心(Tx,Ty,Tz)转换到新的左相机坐标系下// 判断主平移方向（水平/垂直）int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1;   // 水平方向是Tx为主量double c = _t[idx], nt = cvNorm(&t, 0, CV_L2); // c = Tx，并得到单位向量nt_uu[idx] = c > 0 ? 1 : -1;CV_Assert(nt > 0.0);// 计算全局Z轴旋转，使极线平行cvCrossProduct(&t,&uu,&ww);double nw = cvNorm(&ww, 0, CV_L2);if (nw > 0.0)cvConvertScale(&ww, &ww, acos(fabs(c)/nt)/nw);  // 将叉乘的轴的模缩放为旋转角度theta，cvRodrigues2(&ww, &wR); // 向量转为旋转矩阵，此向量模为转角theta，方向为Z轴// 应用旋转到两个视图cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T);cvConvert( &Ri, _R1 );cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, 0);cvConvert( &Ri, _R2 );cvMatMul(&Ri, matT, &t);// 计算投影矩阵参数double fc_new = DBL_MAX;CvPoint2D64f cc_new[2] = {};newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize;const double ratio_x = (double)newImgSize.width / imageSize.width / 2;const double ratio_y = (double)newImgSize.height / imageSize.height / 2;const double ratio = idx == 1 ? ratio_x : ratio_y;// 若矫正前后图像分辨率不变，两个相机的焦距取平均fc_new = (cvmGet(_cameraMatrix1, idx ^ 1, idx ^ 1) + cvmGet(_cameraMatrix2, idx ^ 1, idx ^ 1)) * ratio;// 计算两个相机的主点坐标for( k = 0; k < 2; k++ ){const CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2;const CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2;CvPoint2D32f _pts[4] = {};CvPoint3D32f _pts_3[4] = {};CvMat pts = cvMat(1, 4, CV_32FC2, _pts);CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3);for( i = 0; i < 4; i++ ){int j = (i<2) ? 0 : 1;_pts[i].x = (float)((i % 2)*(nx-1));_pts[i].y = (float)(j*(ny-1));}// 图像的四个角点去畸变cvUndistortPoints( &pts, &pts, A, Dk, 0, 0 );// 四个角点单应性变换到新的平面上的坐标cvConvertPointsHomogeneous( &pts, &pts_3 );double _a_tmp[3][3];CvMat A_tmp  = cvMat(3, 3, CV_64F, _a_tmp);_a_tmp[0][0]=fc_new;_a_tmp[1][1]=fc_new;_a_tmp[0][2]=0.0;_a_tmp[1][2]=0.0;cvProjectPoints2( &pts_3, k == 0 ? _R1 : _R2, &Z, &A_tmp, 0, &pts );// 新的cx，cy指的是无畸变原图和矫正后图像，中心点的偏移CvScalar avg = cvAvg(&pts);cc_new[k].x = (nx-1)/2 - avg.val[0];cc_new[k].y = (ny-1)/2 - avg.val[1];}// 保证两个相机的主点和焦距一致，满足极线约束if( flags & CALIB_ZERO_DISPARITY ){cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;}else if( idx == 0 ) // 水平立体cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;else // 垂直立体cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;// 构造投影矩阵P1和P2cvZero( &pp );_pp[0][0] = _pp[1][1] = fc_new;_pp[0][2] = cc_new[0].x;_pp[1][2] = cc_new[0].y;_pp[2][2] = 1;cvConvert(&pp, _P1);_pp[0][2] = cc_new[1].x;_pp[1][2] = cc_new[1].y;_pp[idx][3] = _t[idx]*fc_new; // 基线*焦距cvConvert(&pp, _P2);alpha = MIN(alpha, 1.);// 计算有效视场区域icvGetRectangles( _cameraMatrix1, _distCoeffs1, _R1, _P1, imageSize, inner1, outer1 );icvGetRectangles( _cameraMatrix2, _distCoeffs2, _R2, _P2, imageSize, inner2, outer2 );{newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imageSize;double cx1_0 = cc_new[0].x;double cy1_0 = cc_new[0].y;double cx2_0 = cc_new[1].x;double cy2_0 = cc_new[1].y;double cx1 = newImgSize.width*cx1_0/imageSize.width;double cy1 = newImgSize.height*cy1_0/imageSize.height;double cx2 = newImgSize.width*cx2_0/imageSize.width;double cy2 = newImgSize.height*cy2_0/imageSize.height;double s = 1.;if( alpha >= 0 ){// 计算缩放比例，保证视场有效double s0 = std::max(std::max(std::max((double)cx1/(cx1_0 - inner1.x), (double)cy1/(cy1_0 - inner1.y)),(double)(newImgSize.width - cx1)/(inner1.x + inner1.width - cx1_0)),(double)(newImgSize.height - cy1)/(inner1.y + inner1.height - cy1_0));s0 = std::max(std::max(std::max(std::max((double)cx2/(cx2_0 - inner2.x), (double)cy2/(cy2_0 - inner2.y)),(double)(newImgSize.width - cx2)/(inner2.x + inner2.width - cx2_0)),(double)(newImgSize.height - cy2)/(inner2.y + inner2.height - cy2_0)),s0);double s1 = std::min(std::min(std::min((double)cx1/(cx1_0 - outer1.x), (double)cy1/(cy1_0 - outer1.y)),(double)(newImgSize.width - cx1)/(outer1.x + outer1.width - cx1_0)),(double)(newImgSize.height - cy1)/(outer1.y + outer1.height - cy1_0));s1 = std::min(std::min(std::min(std::min((double)cx2/(cx2_0 - outer2.x), (double)cy2/(cy2_0 - outer2.y)),(double)(newImgSize.width - cx2)/(outer2.x + outer2.width - cx2_0)),(double)(newImgSize.height - cy2)/(outer2.y + outer2.height - cy2_0)),s1);s = s0*(1 - alpha) + s1*alpha;}fc_new *= s;cc_new[0] = cvPoint2D64f(cx1, cy1);cc_new[1] = cvPoint2D64f(cx2, cy2);// 更新投影矩阵cvmSet(_P1, 0, 0, fc_new);cvmSet(_P1, 1, 1, fc_new);cvmSet(_P1, 0, 2, cx1);cvmSet(_P1, 1, 2, cy1);cvmSet(_P2, 0, 0, fc_new);cvmSet(_P2, 1, 1, fc_new);cvmSet(_P2, 0, 2, cx2);cvmSet(_P2, 1, 2, cy2);cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3));// 计算有效ROI区域if(roi1){*roi1 = cvRect(cv::Rect(cvCeil((inner1.x - cx1_0)*s + cx1),cvCeil((inner1.y - cy1_0)*s + cy1),cvFloor(inner1.width*s), cvFloor(inner1.height*s))& cv::Rect(0, 0, newImgSize.width, newImgSize.height));}if(roi2){*roi2 = cvRect(cv::Rect(cvCeil((inner2.x - cx2_0)*s + cx2),cvCeil((inner2.y - cy2_0)*s + cy2),cvFloor(inner2.width*s), cvFloor(inner2.height*s))& cv::Rect(0, 0, newImgSize.width, newImgSize.height));}}// 计算视差到三维重建的变换矩阵Qif( 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 );}
}