[双目视差] 立体校正源码分析(opencv)
[双目视差] 立体校正源码分析(opencv)
一、源码解析
立体校正：把实际中非共面行对准的两幅图像，校正成共面行对准
stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,0, imageSize, &validROIL, &validROIR);
为每个摄像头计算立体校正的映射矩阵。所以其运行结果并不是直接将图片进行立体矫正，而是得出进行立体矫正所需要的映射矩阵。
立体矫正前：
立体矫正后：
过程：
(1)共面：先把旋转矩阵变为旋转向量，对旋转向量的模长平分，这样使两个图像平面共面，此时行未对齐。
cvConvert(matR, &om);
cvConvertScale(&om, &om, -0.5);
(2)行对准：建立行对准换行矩阵Rrect使极点转换到无穷远处。
首先创建平移向量T方向的旋转矩阵Rrec=[e1,e2,e3]，其中e1为与平移向量T同方向的极点，e2为图像与平移向量同一方向的向量，e3为垂直于e1与e2所在平面的向量，通过叉乘方式获得，RL=Rrect rl ，RRrect rr，最后转为旋转矩阵，在通过转置就得到最终的RL和RR，这里求得的RL和RR是用来校正左右图像到第三平面，进行行对齐。
cvRodrigues2(&om, &r_r); // 旋转向量转换为旋转矩阵
cvMatMul(&r_r, matT, &t); //两个数组对应元素的乘法
int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1;
_uu[2] = 1;
cvCrossProduct(&uu, &t, &ww); //对两个三维向量做叉乘
nt = cvNorm(&t, 0, CV_L2); //计算t的绝对范数
CV_Assert(fabs(nt) > 0); //捕获异常而不是程序崩溃
nw = cvNorm(&ww, 0, CV_L2);
CV_Assert(fabs(nw) > 0);
cvConvertScale(&ww, &ww, 1 / nw);
cvCrossProduct(&t, &ww, &w3);
nw = cvNorm(&w3, 0, CV_L2);
CV_Assert(fabs(nw) > 0);
cvConvertScale(&w3, &w3, 1 / nw);
_uu[2] = 0;
for (i = 0; i < 3; ++i)
{
_wr[idx][i] = -_t[i] / nt;
_wr[idx ^ 1][i] = -_ww[i];
_wr[2][i] = _w3[i] * (1 - 2 * idx);
}
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 );
左右两个摄像机共面、行对齐后，分别计算两个相机的内参矩阵，即投影矩阵，过程为：
通过原始两相机的内参矩阵，与当前共面对齐后的图像进行比例计算，得到新的内参信息（fx，fy，这里的fx=fy）
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));
_pts[i].y = (float)(j*(ny));
}
//利用undistortPoints()函数将拍摄的图像矫正为正常的视角，便于检测。
cvUndistortPoints( &pts, &pts, A, Dk, 0, 0 );
//将变换后的点先变化为齐次坐标系
cvConvertPointsHomogeneous( &pts, &pts_3 );
//Change camera matrix to have cc=[0,0] and fc = fc_new
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 );
CvScalar avg = cvAvg(&pts);
cc_new[k].x = (nx)/2 - avg.val[0];
cc_new[k].y = (ny)/2 - avg.val[1];
}
设置CALIB_ZERO_DISPARITY，让两幅校正后的图像的主点有相同的像素坐标
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;
}
获取左右相机的投影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; // baseline * focal length
cvConvert(&pp, _P2);
校正映射：立体校正之后求得左右相机旋转矩阵R、投影矩阵P、重投影矩阵Q后，使用initUndistortRectifyMap()函数，调⽤两次：⼀次为左侧图像，⼀次为右侧图像，求映射变换矩阵
initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_32FC1, mapLx, mapLy);
initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);
输入单相机内参，畸变参数，旋转矩阵R，投影参数矩阵P（R和P是通过立体矫正stereoRectify（）得到），原图像size大小，类型CV_32FC1，输出映射矩阵mapx，mapy
校正映射过程：
获取相机内参cameraMatrix、畸变矩阵distCoeffs、旋转矩阵matR、摄像机投影参数矩阵newCameraMatrix
//相机内参、畸变矩阵
Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
//旋转矩阵、摄像机参数矩阵
Mat matR = _matR.getMat(), newCameraMatrix = _newCameraMatrix.getMat();
创建相机内参矩阵A、投影参数矩阵Ar，旋转矩阵R、畸变矩阵distCoeffs:
Mat_<double> R = Mat_<double>::eye(3, 3);
Mat_<double> A = Mat_<double>(cameraMatrix), Ar;
//A为相机内参
//Ar为摄像机坐标参数,
if( !newCameraMatrix.empty() )
Ar = Mat_<double>(newCameraMatrix);
else
Ar = getDefaultNewCameraMatrix( A, size, true );
//R为旋转矩阵
if( !matR.empty() )
R = Mat_<double>(matR);
//distCoeffs为畸变矩阵
if( !distCoeffs.empty() )
distCoeffs = Mat_<double>(distCoeffs);
else
{
distCoeffs.create(14, 1, CV_64F);
distCoeffs = 0.;
}
通过LU分解求新的内参矩阵Ar与旋转矩阵R乘积的逆矩阵iR
Mat_<double> iR = (Ar.colRange(0,3)*R).inv(DECOMP_LU);
const double* ir = &iR(0,0);
从旧的内参矩阵中取出光心位置u0,v0作为主坐标点，和归一化焦距fx,fy
double u0 = A(0, 2), v0 = A(1, 2);
double fx = A(0, 0), fy = A(1, 1);
畸变参数计算，14个畸变系数，不过大多用到的只有(k1,k2,p1,p2)，k1,k2为径向畸变系数，p1,p2为切向畸变系数，用不到的置为0，tauX,tauY是梯形畸变
const double* const distPtr = distCoeffs.ptr<double>();
double k1 = distPtr[0];
double k2 = distPtr[1];
double p1 = distPtr[2];
double p2 = distPtr[3];
double k3 = distCoeffs.cols + distCoeffs.rows - 1 >= 5 ? distPtr[4] : 0.;
double k4 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? distPtr[5] : 0.;
double k5 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? distPtr[6] : 0.;
double k6 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? distPtr[7] : 0.;
double s1 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? distPtr[8] : 0.;
double s2 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? distPtr[9] : 0.;
double s3 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? distPtr[10] : 0.;
double s4 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? distPtr[11] : 0.;
double tauX = distCoeffs.cols + distCoeffs.rows - 1 >= 14 ? distPtr[12] : 0.;
double tauY = distCoeffs.cols + distCoeffs.rows - 1 >= 14 ? distPtr[13] : 0.;
通过tauX,tauY计算倾斜图像传感器的梯形畸变矩阵matTilt，其中tauX,tauY用不到的话matTilt为单位矩阵
cv::Matx33d matTilt = cv::Matx33d::eye();
cv::detail::computeTiltProjectionMatrix(tauX, tauY, &matTilt);
求得上述提到的逆矩阵ir、梯形矩阵、主坐标点（u0，v0）、焦距（fx，fy）及畸变参数后，反向映射，遍历目标图像所有像素位置，找到畸变图像中对应位置坐标(u,v)，并分别保存坐标(u,v)到mapx和mapy中。
过程如下：
parallel_for_(Range(0, size.height),
initUndistortRectifyMapComputer(size, map1, map2, m1type, ir, matTilt, u0, v0,fx, fy, k1, k2, p1, p2, k3, k4, k5, k6, s1, s2, s3, s4));
const int begin = range.start;
const int end = range.end;
for( int i = begin; i < end; i++ )
{ //定义映射表mapx和mapy行元素指针
float* m1f = map1.ptr<float>(i);//指向第i+1行第一个元素指针
float* m2f = map2.empty() ? 0 : map2.ptr<float>(i);
short* m1 = (short*)m1f;
ushort* m2 = (ushort*)m2f;
//利用逆矩阵iR将二维图像坐标(j,i)转换到摄像机坐标系(_x,_y,_w)
double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8];
int j = 0;
//遍历每个像机坐标位置
for( ; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] )
{
//摄像机坐标系归一化，令Z=1
double w = 1./_w, x = _x*w, y = _y*w;
//根据畸变模型进行变换
double x2 = x*x, y2 = y*y;
double r2 = x2 + y2, _2xy = 2*x*y;
double kr = (1 + ((k3*r2 + k2)*r2 + k1)*r2)/(1 + ((k6*r2 + k5)*r2 + k4)*r2);
double xd = (x*kr + p1*_2xy + p2*(r2 + 2*x2) + s1*r2+s2*r2*r2);
double yd = (y*kr + p1*(r2 + 2*y2) + p2*_2xy + s3*r2+s4*r2*r2);
//根据求取的xd,yd将三维坐标重投影到二维畸变图像坐标(u,v)
cv::Vec3d vecTilt = matTilt*cv::Vec3d(xd, yd, 1);
double invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
double u = fx*invProj*vecTilt(0) + u0;
double v = fy*invProj*vecTilt(1) + v0;
//保存u,v的值到Mapx,Mapy中
if( m1type == CV_16SC2 )
{
int iu = cv::saturate_cast<int>(u*cv::INTER_TAB_SIZE);
int iv = cv::saturate_cast<int>(v*cv::INTER_TAB_SIZE);
m1[j*2] = (short)(iu >> cv::INTER_BITS);
m1[j*2+1] = (short)(iv >> cv::INTER_BITS);
m2[j] = (ushort)((iv & (cv::INTER_TAB_SIZE-1))*cv::INTER_TAB_SIZE + (iu & (cv::INTER_TAB_SIZE-1)));
}
else if( m1type == CV_32FC1 )
{
m1f[j] = (float)u;
m2f[j] = (float)v;
// map_x实际上记录的是对应原图像中（i,j）位置的横坐标，map_y实际上记录的是（i,j）位置的纵坐标
}
else
{
m1f[j*2] = (float)u;
m1f[j*2+1] = (float)v;
}
}
}
initUndistortRectifyMap
-
校正映射参数：
输入单相机内参，畸变参数，旋转矩阵R，投影参数矩阵P（R和P是通过立体矫正stereoRectify（）得到），原图像size大小，类型CV_32FC1，输出映射矩阵mapx，mapy -
校正映射过程：
1、获取相机内参cameraMatrix、畸变矩阵distCoeffs、旋转矩阵matR、摄像机投影参数矩阵newCameraMatrix
2、创建相机内参矩阵A、投影参数矩阵Ar，旋转矩阵R、畸变矩阵distCoeffs
3、通过LU分解求新的内参矩阵Ar与旋转矩阵R乘积的逆矩阵iR
4、从旧的内参矩阵中取出光心位置u0,v0作为主坐标点，和归一化焦距fx,fy
5、畸变参数计算，14个畸变系数，不过大多用到的只有(k1,k2,p1,p2)，k1,k2为径向畸变系数，p1,p2为切向畸变系数，用不到的置为0，tauX,tauY是梯形畸变
6、通过tauX,tauY计算倾斜图像传感器的梯形畸变矩阵matTilt，其中tauX,tauY用不到的话matTilt为单位矩阵
7、求得上述提到的逆矩阵ir、梯形矩阵、主坐标点（u0，v0）、焦距（fx，fy）及畸变参数后，反向映射，遍历目标图像所有像素位置，找到畸变图像中对应位置坐标(u,v)，并分别保存坐标(u,v)到mapx和mapy中。
①定义映射表1、2行元素指针
②利用逆矩阵iR将二维图像坐标(j,i)转换到摄像机坐标系(_x,_y,_w)
③遍历每个相机坐标位置，将相机坐标系归一化，令Z=1平面上。
④畸变模型的转换，求得xd，yd
⑤根据求取的xd,yd将三维坐标重投影到二维畸变图像坐标(u,v)
⑥保存u,v的值到Mapx,Mapy中
map_x实际上记录的是对应原图像中（i,j）位置的横坐标，map_y实际上记录的是（i,j）位置的纵坐标,而我们在这里把像素操作的i当做了横坐标,j当做了纵坐标
remap
remap( InputArray _src, OutputArray _dst,
InputArray _map1, InputArray _map2,
int interpolation, int borderType, const Scalar& borderValue )
-
像素重映射:
重映射，就是把一幅图像中某位置的像素放置到另一个图片指定位置的过程。为了完成映射过程, 我们需要获得一些插值为 非整数像素的坐标,因为源图像与目标图像的像素坐标不是一一对应的
g(x,y) = f ( h(x,y) )
g( ) 是目标图像, f() 是源图像, 而h(x,y) 是作用于 (x,y) 的映射方法函数。简单的说就是改变图片的位置（左，右，上，下，颠倒翻转） -
像素重映射参数:
输入源图像src，目标图像dst，输入记录源图像位置的横坐标Mapx，输入记录源图像位置的纵坐标Mapy，使用双线性插值方式INTER_LINEAR，边界模式使用默认值BORDER_CONSTANT，表示目标图像中“离群点（outliers）”的像素值不会被此函数修改，边界颜色，默认Scalar()黑色 -
对左右摄像头采集到的数据分别进行remap，使源图像中像素位置通过映射表mapx，mapy位置，映射到新的图像中。最后得到的图像就是共面、行对齐的图像。
二、源码中的方法
OpenCV双目视觉：Bouguet立体校正https://jingyan.baidu.com/article/a681b0de74312a3b1843460d.html
-
将旋转矩阵转换为旋转向量:
cvConvert函数用于图像和矩阵之间的相互转换 为什么要用cvConvert 把IplImage转为矩阵？ 因为IplImage里的数据，你只能用uchar的形式存放，当你需要这些图像数据看作数据矩阵来运算时，0~255的精度显然满足不了要求； 然而CvMat里却可以存放任意通道数、任意格式的数据，这个机制方便了研究中的这种需求，转化为矩阵就可以进行更自由的计算。 -
获得平均旋转向量，等比缩放一半：
函数 cvConvertScale 有多个不同的目的因此就有多个同义函数（如上面的#define所示）。该函数首先对输入数组的元素进行比例缩放，然后将shift加到比例缩放后得到的各元素上，即： dst(I)=src(I)*scale + (shift,shift,…) -
旋转向量转换为旋转矩阵：
cvRodrigues2(） -
旋转矩阵r_r 与平移矩阵相乘得到t(3,1)
cvMatMul() -
cvCrossProduct(&uu, &t, &ww); //对两个三维向量做叉乘
-
cvNorm(&t, 0, CV_L2); //计算t的绝对范数
-
CV_Assert(fabs(nt) > 0); //捕获异常而不是程序崩溃
-
对WR矩阵值做变换，与旋转矩阵进行广义矩阵乘法分别得到左旋转矩阵和右旋转矩阵
-
void cvGEMM( const CvArr* src1, const CvArr* src2, double alpha,const CvArr* src3, double beta, CvArr* dst, int tABC=0 ); 广义矩阵的乘法
src1：第一输入数组
src2：第二输入数组
alpha：系数
src3“第三输入数组（偏移量），如果没有偏移量，可以为空（NULL）
beta：表示偏移量的系数
dst：输出数组
tABC：
转置操作标志，可以是0。当为0时，没有转置。或者还有下面的值的组合：
CV_GEMM_A_T：表示src1转置
CV_GEMM_B_T：表示src2转置
CV_GEMM_C_T：表示src3转置
例如，CV_GEMM_A_T+CV_GEMM_C_T对应
alpha*（src1转置）src2+beta（src3转置） -
opencv实现中要先把旋转矩阵变为旋转向量，对旋转向量的模长平分，就得到可以把光轴 摆平的左右矩阵，然后用这个矩阵乘以T，归一化得到e1，然后根据上面的公式构建e2,e3就可以通过叉乘获得，最后转为旋转矩阵，在通过转置就可以得到最终的RL和RR，RL和RR是用来校正左右图像到第三平面，行对齐
-
获取左右摄像头的内参及畸变系数利用undistortPoints()函数将拍摄的图像矫正为正常的视角，便于检测
cvUndistortPoints：https://yongqi.blog.csdn.net/article/details/52946821 -
cvConvertPointsHomogeneous//将变换后的点先变化为齐次坐标系
cvmGet直接存取矩阵元素
-
Opencv,计算三维点在平面中的坐标.
void cvProjectPoints2
(
const CvMat* objectPoints, //是需要投影的点的序列，是一个点位置的N3的矩阵。
const CvMat rvec,
const CvMat* tvec, //建立两个坐标系的联系
const CvMat* cameraMatrix,
const CvMat* distCoeffs,//内参数矩阵和形变系数
CvMat* imagePoints,//N2的矩阵将被写入计算结果
CvMat dpdrot=NULL,
CvMat* dpdt=NULL,
CvMat* dpdf=NULL,
CvMatdpdc=NULL,
CvMat dpddist=NULL //偏导数的雅克比矩阵
) -
如果设置为CALIB_ZERO_DISPARITY的话，该函数会让两幅校正后的图像的主点有相同的像素坐标。否则该函数会水平或垂直的移动图像，以使得其有用的范围最大。
-
// 获取左右相机的投影P1，P2矩阵
-
icvGetRectangles功能是获取标准图像的有效像素(有效像素指在畸变图像中有对应像素的像素)所构成的区域的最大内接矩阵和最小外接矩阵，其实现方式是：取畸变图像中的一些特殊点(四条边上的点和中间区域的一些点)
-
当alpha为0时，取inner即内矩阵，用内矩阵大小作为新的图像大小，重新得到fx,fy,cx,cy，因此新的内参矩阵诞生了. 当alpha为1时，取outer即外矩阵。当alpha介于0~1时，则按照比例重新计算fx,fy,cx,cy。
//事实上，内矩阵等同于不含任何黑色边框的图幅大小，而外矩阵等同于原图大小。
