目录

0.极限约束，对极校正

1.摄像机成像原理简述

2.成像畸变

2.1. 畸变数学模型

2.2. 公式推导

3.畸变校正

3.1. 理论推导

4. 图像去畸变**

5. 图像尺度缩放与内参的关系**

5.1 undistortPoints()

5.2 initUndistortRectifyMap()

5.3 undistort()

6.UndistortPoints源码

---

 

0.极限约束，对极校正

---

 

 

1.摄像机成像原理简述

成像的过程实质上是几个坐标系的转换。首先空间中的一点由 世界坐标系 转换到 摄像机坐标系 ，然后再将其投影到成像平面 ( 图像物理坐标系 ) ，最后再将成像平面上的数据转换到图像平面 ( 图像像素坐标系 ) 。

图像像素坐标系 (uOv坐标系) 下的无畸变坐标 (U, V)，经过 经向畸变 和 切向畸变 后落在了uOv坐标系 的 (Ud, Vd) 上。即就是说，真实图像 imgR 与 畸变图像 imgD 之间的关系为: imgR(U, V) = imgD(Ud, Vd) 。

2.成像畸变

2.1. 畸变数学模型

摄像头成像畸变的数学模型 (符合的对应关系有问题，可能会造成一些干扰，公式主要看后面推导的过程)

 

2.2. 公式推导

公式推导:

3.畸变校正

3.1. 理论推导

我们已知的是畸变后的图像，要得到没有畸变的图像就要通过畸变模型推导其映射关系。 真实图像 imgR 与 畸变图像 imgD 之间的关系为: imgR(U, V) = imgD(Ud, Vd) 。通过这个关系，找出所有的 imgR(U, V) 。(U, V) 映射到 (Ud, Vd) 中的 (Ud, Vd) 往往不是整数 (U和V是整数，因为它是我们要组成图像的像素坐标位置，以这正常图像的坐标位置去求在畸变图像中的坐标位置，取出对应的像素值，这也是正常图像的像素值)。 但是畸变的像素往往不是整数，所以需要通过插值来进行求解，详细见我之前的博客 [图像]图像缩放算法-双线性内插法 。

 

 

 

4. 图像去畸变**

图像去畸变的思路是：对于目标图像(无畸变)上的每个像素点，转换到normalize平面，再进行畸变并投影到源图像(带畸变), 获取原图对应位置的像素值作为目标图像该点的像素值。

这里容易有一个误解，以为去畸变是对畸变图像进行畸变逆变换得到无畸变图像，实际不是的，畸变模型太复杂了，很难求逆变换，所以是将无畸变图像进行畸变变换到原图像去获得对应像素值。

 

图像去畸变流程如下:

注意：源相机和目标相机使用的内参矩阵不一定是一样的。如果是调用opencv的undistort()函数，cameraMatrix是源相机的内参矩阵，newCameraMatrix是目标相机的内参矩阵，如果不设置newCameraMatrix，则默认与源相机内参一样，即去畸变后，相机的内参矩阵不变。

5. 图像尺度缩放与内参的关系**

结论：图像分辨率缩放比例k， 相机焦距光心等比例缩放k， 畸变系数不变。

证明：图像缩放k倍后，图像平面所有的像素点坐标变为：

而图像畸变是发生在normalize平面，不管图像分辨率如何改变，normalize平面(只取决于焦距光心)是不变的，所以畸变系数不变。

 

5.1 undistortPoints()

1.1功能： 从观测点坐标计算理想点坐标。

void cv::undistortPoints	(	InputArray	src,
		OutputArray	dst,
		InputArray	cameraMatrix,
		InputArray	distCoeffs,
		InputArray	R = noArray(),
		InputArray	P = noArray()
	)

 

5.2 initUndistortRectifyMap()

2.1功能 Computes the undistortion and rectification transformation map. 计算去畸变和校正变换映射。

void cv::initUndistortRectifyMap	(	InputArray	cameraMatrix,
		InputArray	distCoeffs,
		InputArray	R,
		InputArray	newCameraMatrix,
		Size	size,
		int	m1type,
		OutputArray	map1,
		OutputArray	map2
	)

模型见4去畸变

5.3 undistort()

void cv::undistort	(	InputArray	src,
		OutputArray	dst,
		InputArray	cameraMatrix,
		InputArray	distCoeffs,
		InputArray	newCameraMatrix = noArray()
	)

3.1 功能 Transforms an image to compensate for lens distortion. ， 对图像进行变换以补偿镜头失真。

The function transforms an image to compensate radial and tangential lens distortion.

The function is simply a combination of cv::initUndistortRectifyMap (with unity R ) and cv::remap (with bilinear interpolation). See the former function for details of the transformation being performed.

6.UndistortPoints源码

 

void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
                   const CvMat* _distCoeffs,
                   const CvMat* matR, const CvMat* matP, cv::TermCriteria criteria)
{
    // 判断迭代条件是否有效
    CV_Assert(criteria.isValid());
    // 定义中间变量--A相机内参数组，和matA共享内存；RR-矫正变换数组，和_RR共享内存
    // k-畸变系数数组
    double A[3][3], RR[3][3], k[14]={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
    CvMat matA=cvMat(3, 3, CV_64F, A), _Dk;
    CvMat _RR=cvMat(3, 3, CV_64F, RR);
    cv::Matx33d invMatTilt = cv::Matx33d::eye();
    cv::Matx33d matTilt = cv::Matx33d::eye();
    
    // 检查输入变量是否有效
    CV_Assert( CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&
        (_src->rows == 1 || _src->cols == 1) &&
        (_dst->rows == 1 || _dst->cols == 1) &&
        _src->cols + _src->rows - 1 == _dst->rows + _dst->cols - 1 &&
        (CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) &&
        (CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2));
 
    CV_Assert( CV_IS_MAT(_cameraMatrix) &&
        _cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 );
 
    cvConvert( _cameraMatrix, &matA );// _cameraMatrix <--> matA / A
 
    // 判断输入的畸变系数是否有效
    if( _distCoeffs )
    {
        CV_Assert( CV_IS_MAT(_distCoeffs) &&
            (_distCoeffs->rows == 1 || _distCoeffs->cols == 1) &&
            (_distCoeffs->rows*_distCoeffs->cols == 4 ||
             _distCoeffs->rows*_distCoeffs->cols == 5 ||
             _distCoeffs->rows*_distCoeffs->cols == 8 ||
             _distCoeffs->rows*_distCoeffs->cols == 12 ||
             _distCoeffs->rows*_distCoeffs->cols == 14));
 
        _Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols,
            CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k);// _Dk和数组k共享内存指针
 
        cvConvert( _distCoeffs, &_Dk );
        if (k[12] != 0 || k[13] != 0)
        {
            cv::detail::computeTiltProjectionMatrix<double>(k[12], k[13], NULL, NULL, NULL, &invMatTilt);
            cv::detail::computeTiltProjectionMatrix<double>(k[12], k[13], &matTilt, NULL, NULL);
        }
    }
 
    if( matR )
    {
        CV_Assert( CV_IS_MAT(matR) && matR->rows == 3 && matR->cols == 3 );
        cvConvert( matR, &_RR );// matR和_RR共享内存指针
    }
    else
        cvSetIdentity(&_RR);
 
    if( matP )
    {
        double PP[3][3];
        CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP);
        CV_Assert( CV_IS_MAT(matP) && matP->rows == 3 && (matP->cols == 3 || matP->cols == 4));
        cvConvert( cvGetCols(matP, &_P3x3, 0, 3), &_PP );// _PP和数组PP共享内存指针
        cvMatMul( &_PP, &_RR, &_RR );// _RR=_PP*_RR 放在一起计算比较高效
    }
 
    const CvPoint2D32f* srcf = (const CvPoint2D32f*)_src->data.ptr;
    const CvPoint2D64f* srcd = (const CvPoint2D64f*)_src->data.ptr;
    CvPoint2D32f* dstf = (CvPoint2D32f*)_dst->data.ptr;
    CvPoint2D64f* dstd = (CvPoint2D64f*)_dst->data.ptr;
    int stype = CV_MAT_TYPE(_src->type);
    int dtype = CV_MAT_TYPE(_dst->type);
    int sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);
    int dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);
 
    double fx = A[0][0];
    double fy = A[1][1];
    double ifx = 1./fx;
    double ify = 1./fy;
    double cx = A[0][2];
    double cy = A[1][2];
 
    int n = _src->rows + _src->cols - 1;
    // 开始对所有点开始遍历
    for( int i = 0; i < n; i++ )
    {
        double x, y, x0 = 0, y0 = 0, u, v;
        if( stype == CV_32FC2 )
        {
            x = srcf[i*sstep].x;
            y = srcf[i*sstep].y;
        }
        else
        {
            x = srcd[i*sstep].x;
            y = srcd[i*sstep].y;
        }
        u = x; v = y;
        x = (x - cx)*ifx;//转换到归一化图像坐标系（含有畸变）
        y = (y - cy)*ify;
 
        //进行畸变矫正
        if( _distCoeffs ) {
            // compensate tilt distortion--该部分系数用来弥补沙氏镜头畸变？？
            // 如果不懂也没管，因为普通镜头中没有这些畸变系数
            cv::Vec3d vecUntilt = invMatTilt * cv::Vec3d(x, y, 1);
            double invProj = vecUntilt(2) ? 1./vecUntilt(2) : 1;
            x0 = x = invProj * vecUntilt(0);
            y0 = y = invProj * vecUntilt(1);
 
            double error = std::numeric_limits<double>::max();// error设定为系统最大值
            // compensate distortion iteratively
            // 迭代去除镜头畸变
            // 迭代公式    x′= (x−2p1 xy−p2 (r^2 + 2x^2))∕( 1 + k1*r^2 + k2*r^4 + k3*r^6)
            //             y′= (y−2p2 xy−p1 (r^2 + 2y^2))∕( 1 + k1*r^2 + k2*r^4 + k3*r^6)
 
            for( int j = 0; ; j++ )
            {
                if ((criteria.type & cv::TermCriteria::COUNT) && j >= criteria.maxCount)// 迭代最大次数为5次
                    break;
                if ((criteria.type & cv::TermCriteria::EPS) && error < criteria.epsilon)// 迭代误差阈值为0.01
                    break;
                double r2 = x*x + y*y;
                double icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
                double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x)+ k[8]*r2+k[9]*r2*r2;
                double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y+ k[10]*r2+k[11]*r2*r2;
                x = (x0 - deltaX)*icdist;
                y = (y0 - deltaY)*icdist;
 
                // 对当前迭代的坐标加畸变，计算误差error用于判断迭代条件
                if(criteria.type & cv::TermCriteria::EPS)
                {
                    double r4, r6, a1, a2, a3, cdist, icdist2;
                    double xd, yd, xd0, yd0;
                    cv::Vec3d vecTilt;
 
                    r2 = x*x + y*y;
                    r4 = r2*r2;
                    r6 = r4*r2;
                    a1 = 2*x*y;
                    a2 = r2 + 2*x*x;
                    a3 = r2 + 2*y*y;
                    cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
                    icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
                    xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
                    yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
 
                    vecTilt = matTilt*cv::Vec3d(xd0, yd0, 1);
                    invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
                    xd = invProj * vecTilt(0);
                    yd = invProj * vecTilt(1);
 
                    double x_proj = xd*fx + cx;
                    double y_proj = yd*fy + cy;
 
                    error = sqrt( pow(x_proj - u, 2) + pow(y_proj - v, 2) );
                }
            }
        }
        // 将坐标从归一化图像坐标系转换到成像平面坐标系
        double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2];
        double yy = RR[1][0]*x + RR[1][1]*y + RR[1][2];
        double ww = 1./(RR[2][0]*x + RR[2][1]*y + RR[2][2]);
        x = xx*ww;
        y = yy*ww;
 
        if( dtype == CV_32FC2 )
        {
            dstf[i*dstep].x = (float)x;
            dstf[i*dstep].y = (float)y;
        }
        else
        {
            dstd[i*dstep].x = x;
            dstd[i*dstep].y = y;
        }
    }
}

简化版ubdistortpoint

//for (size_t u = 0; u < ir_image_height; u++)
      //{
      //    for (size_t v = 0; v < ir_image_width; v++)
      //    {//(u,v) undistort
      //        float x = (u - cx) * fx_inv;
      //        float y = (v - cy) * fy_inv;
​
      //        float r2 = (x*x + y*y);
      //        float r = std::sqrt(r2);
      //        float r4 = r2 * r2;
      //        float x_distort = x*(1 + k1*r2 + k2 * r4) + 2 * p1*x*y + p2*(r2 + 2 * x*x);
      //        float y_distort = y*(1 + k1*r2 + k2 * r4) + p1*(r2 + 2 * y*y) + 2 * p2*x*y;
​
      //        float X = ir_depth_rx.at<float>(0, 0) * x_distort + ir_depth_rx.at<float>(0, 1)*y_distort +ir_depth_rx.at<float>(0, 2) * 1;
      //        float Y = ir_depth_rx.at<float>(1, 0) * x_distort + ir_depth_rx.at<float>(1, 1)*y_distort +ir_depth_rx.at<float>(1, 2) * 1;
      //        float W = ir_depth_rx.at<float>(2, 0) * x_distort + ir_depth_rx.at<float>(2, 1)*y_distort +ir_depth_rx.at<float>(2, 2) * 1;
      //        
      //        float x_camera = X / W;
      //        float y_camera = Y / W;
      //        
      //        float u_distort = fx*x_camera + cx;
      //        float v_distort = fy*y_camera + cy;
​
      //        calib_params->updated_ir_depth_forward_map_x->operator()(v, u) = u_distort;
      //        calib_params->updated_ir_depth_forward_map_y->operator()(v, u) = v_distort;
​
      //    }
      //}