initUndistortRectifyMap源码解析

void cv::initUndistortRectifyMap( InputArray _cameraMatrix, InputArray _distCoeffs,                              InputArray _matR, InputArray _newCameraMatrix,                              Size size, int m1type, OutputArray _map1, OutputArray _map2 ){    //相机内参、畸变矩阵    Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();    //旋转矩阵、摄像机参数矩阵    Mat matR = _matR.getMat(), newCameraMatrix = _newCameraMatrix.getMat();    if( m1type <= 0 )        m1type = CV_16SC2;    CV_Assert( m1type == CV_16SC2 || m1type == CV_32FC1 || m1type == CV_32FC2 );    _map1.create( size, m1type );    Mat map1 = _map1.getMat(), map2;    if( m1type != CV_32FC2 )    {        _map2.create( size, m1type == CV_16SC2 ? CV_16UC1 : CV_32FC1 );        map2 = _map2.getMat();    }    else        _map2.release();    Mat_<double> R = Mat_<double>::eye(3, 3);    //A为相机内参    Mat_<double> A = Mat_<double>(cameraMatrix), Ar;    //Ar 为摄像机坐标参数    if( newCameraMatrix.data )        Ar = Mat_<double>(newCameraMatrix);    else        Ar = getDefaultNewCameraMatrix( A, size, true );    //R  为旋转矩阵    if( matR.data )        R = Mat_<double>(matR);    //distCoeffs为畸变矩阵    if( distCoeffs.data )        distCoeffs = Mat_<double>(distCoeffs);    else    {        distCoeffs.create(8, 1, CV_64F);        distCoeffs = 0.;    }    CV_Assert( A.size() == Size(3,3) && A.size() == R.size() );    CV_Assert( Ar.size() == Size(3,3) || Ar.size() == Size(4, 3));    //摄像机坐标系第四列参数  旋转向量转为旋转矩阵    Mat_<double> iR = (Ar.colRange(0,3)*R).inv(DECOMP_LU);    //ir IR矩阵的指针    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);    CV_Assert( distCoeffs.size() == Size(1, 4) || distCoeffs.size() == Size(4, 1) ||               distCoeffs.size() == Size(1, 5) || distCoeffs.size() == Size(5, 1) ||               distCoeffs.size() == Size(1, 8) || distCoeffs.size() == Size(8, 1));    if( distCoeffs.rows != 1 && !distCoeffs.isContinuous() )        distCoeffs = distCoeffs.t();        //畸变参数计算    double k1 = ((double*)distCoeffs.data)[0];    double k2 = ((double*)distCoeffs.data)[1];    double p1 = ((double*)distCoeffs.data)[2];    double p2 = ((double*)distCoeffs.data)[3];    double k3 = distCoeffs.cols + distCoeffs.rows - 1 >= 5 ? ((double*)distCoeffs.data)[4] : 0.;    double k4 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? ((double*)distCoeffs.data)[5] : 0.;    double k5 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? ((double*)distCoeffs.data)[6] : 0.;    double k6 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? ((double*)distCoeffs.data)[7] : 0.;    //图像高度    for( int i = 0; i < size.height; i++ )    {        //映射矩阵map1         float* m1f = (float*)(map1.data + map1.step*i);        //映射矩阵map2        float* m2f = (float*)(map2.data + map2.step*i);        short* m1 = (short*)m1f;        ushort* m2 = (ushort*)m2f;        //摄像机参数矩阵最后一列向量转换成的3*3矩阵参数        double _x = i*ir[1] + ir[2];        double _y = i*ir[4] + ir[5];        double _w = i*ir[7] + ir[8];        //图像宽度        for( int j = 0; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] )        {            //获取摄像机坐标系第四列参数            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 u = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + u0;            double v = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + v0;            if( m1type == CV_16SC2 )            {                int iu = saturate_cast<int>(u*INTER_TAB_SIZE);                int iv = saturate_cast<int>(v*INTER_TAB_SIZE);                m1[j*2] = (short)(iu >> INTER_BITS);                m1[j*2+1] = (short)(iv >> INTER_BITS);                m2[j] = (ushort)((iv & (INTER_TAB_SIZE-1))*INTER_TAB_SIZE + (iu & (INTER_TAB_SIZE-1)));            }            else if( m1type == CV_32FC1 )            {                m1f[j] = (float)u;                m2f[j] = (float)v;            }            else            {                m1f[j*2] = (float)u;                m1f[j*2+1] = (float)v;            }        }    }}