[双目视差] 立体校正源码分析(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。
//事实上，内矩阵等同于不含任何黑色边框的图幅大小，而外矩阵等同于原图大小。