opencv_stereoRectify源码解析_cv::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，对应源码和注释如下。左右相机具有一定角度的像面，经过这一步后，像面平行，再后续进一步统一两个相机的焦距后，像面平移到在同一个面内。

 // 计算旋转向量om if( matR->rows == 3 && matR->cols == 3 ) cvRodrigues2(matR, &om); // 如果是旋转矩阵，转为旋转向量 else cvConvert(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的向量点乘uu⃗⋅t⃗=∣uu∣⋅∣t∣⋅cos⁡θ=tx\\vec{uu}\\cdot \\vec{t}=|uu|\\cdot|t|\\cdot\\cos\\theta=txuu⋅t=∣uu∣⋅∣t∣⋅cosθ=tx其中，uu为单位向量，模为1，那么夹角为$$\theta =acos(tx/norm(t))$$
为了构造旋转轴为Z轴，旋转角度为$\theta$的角轴，首先求解Z轴的向量，由向量uu=[1,0,0]和t=[tx,ty,tz]叉乘得到ww⃗=t⃗×uu⃗\\vec{ww}=\\vec{t}\\times \\vec{uu}ww=t×uu
进一步的，对ww⃗\\vec{ww}ww的模缩放为$\theta$（角轴的定义中，向量表示旋转轴，模表示旋转角度），scale=θnorm(ww)=acos(tx/norm(t))norm(ww)scale=\\frac{\\theta}{norm(ww)}=\\frac{acos(tx/norm(t))}{norm(ww)}scale=norm(ww)θ​=norm(ww)acos(tx/norm(t))​这样，$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; // 计算旋转向量om if( matR->rows == 3 && matR->cols == 3 ) cvRodrigues2(matR, &om); // 如果是旋转矩阵，转为旋转向量 else cvConvert(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和P2 cvZero( &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) ); } } // 计算视差到三维重建的变换矩阵Q 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 ); }}