opencv/modules/imgproc/src/undistort.cpp

/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"

namespace cv
{

Mat getDefaultNewCameraMatrix( const Mat& cameraMatrix, Size imgsize,
                               bool centerPrincipalPoint )
{
    if( !centerPrincipalPoint && cameraMatrix.type() == CV_64F )
        return cameraMatrix;
    
    Mat newCameraMatrix;
    cameraMatrix.convertTo(newCameraMatrix, CV_64F);
    if( centerPrincipalPoint )
    {
        ((double*)newCameraMatrix.data)[2] = (imgsize.width-1)*0.5;
        ((double*)newCameraMatrix.data)[5] = (imgsize.height-1)*0.5;
    }
    return newCameraMatrix;
}

void initUndistortRectifyMap( const Mat& _cameraMatrix, const Mat& _distCoeffs,
                              const Mat& matR, const Mat& _newCameraMatrix,
                              Size size, int m1type, Mat& map1, Mat& map2 )
{
    if( m1type <= 0 )
        m1type = CV_16SC2;
    CV_Assert( m1type == CV_16SC2 || m1type == CV_32FC1 || m1type == CV_32FC2 );
    map1.create( size, m1type );
    if( m1type != CV_32FC2 )
        map2.create( size, m1type == CV_16SC2 ? CV_16UC1 : CV_32FC1 );
    else
        map2.release();

    Mat_<double> R = Mat_<double>::eye(3, 3), distCoeffs;
    Mat_<double> A = Mat_<double>(_cameraMatrix), Ar;

    if( _newCameraMatrix.data )
        Ar = Mat_<double>(_newCameraMatrix);
    else
        Ar = getDefaultNewCameraMatrix( A, size, true );

    if( matR.data )
        R = Mat_<double>(matR);

    if( _distCoeffs.data )
        distCoeffs = Mat_<double>(_distCoeffs);
    else
    {
        distCoeffs.create(5, 1);
        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);
    const double* ir = &iR(0,0);

    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(1, 5) ||
               distCoeffs.size() == Size(4, 1) || distCoeffs.size() == Size(5, 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.;

    for( int i = 0; i < size.height; i++ )
    {
        float* m1f = (float*)(map1.data + map1.step*i);
        float* m2f = (float*)(map2.data + map2.step*i);
        short* m1 = (short*)m1f;
        ushort* m2 = (ushort*)m2f;
        double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _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;
            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;
            }
        }
    }
}


void undistort( const Mat& src, Mat& dst, const Mat& _cameraMatrix,
                const Mat& _distCoeffs, const Mat& _newCameraMatrix )
{
    dst.create( src.size(), src.type() );
    CV_Assert( dst.data != src.data );

    int stripe_size0 = std::min(std::max(1, (1 << 12) / std::max(src.cols, 1)), src.rows);
    Mat map1(stripe_size0, src.cols, CV_16SC2), map2(stripe_size0, src.cols, CV_16UC1);

    Mat_<double> A, distCoeffs, Ar, I = Mat_<double>::eye(3,3);

    _cameraMatrix.convertTo(A, CV_64F);
    if( _distCoeffs.data )
        distCoeffs = Mat_<double>(_distCoeffs);
    else
    {
        distCoeffs.create(5, 1);
        distCoeffs = 0.;
    }

    if( _newCameraMatrix.data )
        _newCameraMatrix.convertTo(Ar, CV_64F);
    else
        A.copyTo(Ar);

    double v0 = Ar(1, 2);
    for( int y = 0; y < src.rows; y += stripe_size0 )
    {
        int stripe_size = std::min( stripe_size0, src.rows - y );
        Ar(1, 2) = v0 - y;
        Mat map1_part = map1.rowRange(0, stripe_size),
            map2_part = map2.rowRange(0, stripe_size),
            dst_part = dst.rowRange(y, y + stripe_size);

        initUndistortRectifyMap( A, distCoeffs, I, Ar, Size(src.cols, stripe_size),
                                 map1_part.type(), map1_part, map2_part );
        remap( src, dst_part, map1_part, map2_part, INTER_LINEAR, BORDER_CONSTANT );
    }
}

}


CV_IMPL void
cvUndistort2( const CvArr* srcarr, CvArr* dstarr, const CvMat* Aarr, const CvMat* dist_coeffs, const CvMat* newAarr )
{
    cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr), dst0 = dst;
    cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs), newA;
    if( newAarr )
        newA = cv::cvarrToMat(newAarr);

    CV_Assert( src.size() == dst.size() && src.type() == dst.type() );
    cv::undistort( src, dst, A, distCoeffs, newA );
}


CV_IMPL void cvInitUndistortMap( const CvMat* Aarr, const CvMat* dist_coeffs,
                                 CvArr* mapxarr, CvArr* mapyarr )
{
    cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs);
    cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0;

    if( mapyarr )
        mapy0 = mapy = cv::cvarrToMat(mapyarr);

    cv::initUndistortRectifyMap( A, distCoeffs, cv::Mat(), A,
                                 mapx.size(), mapx.type(), mapx, mapy );
    CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data );
}

void
cvInitUndistortRectifyMap( const CvMat* Aarr, const CvMat* dist_coeffs,
    const CvMat *Rarr, const CvMat* ArArr, CvArr* mapxarr, CvArr* mapyarr )
{
    cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs, R, Ar;
    cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0;

    if( mapyarr )
        mapy0 = mapy = cv::cvarrToMat(mapyarr);

    if( dist_coeffs )
        distCoeffs = cv::cvarrToMat(dist_coeffs);
    if( Rarr )
        R = cv::cvarrToMat(Rarr);
    if( ArArr )
        Ar = cv::cvarrToMat(ArArr);

    cv::initUndistortRectifyMap( A, distCoeffs, R, Ar, mapx.size(), mapx.type(), mapx, mapy );
    CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data );
}


void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
                   const CvMat* _distCoeffs,
                   const CvMat* matR, const CvMat* matP )
{
    double A[3][3], RR[3][3], k[5]={0,0,0,0,0}, fx, fy, ifx, ify, cx, cy;
    CvMat matA=cvMat(3, 3, CV_64F, A), _Dk;
    CvMat _RR=cvMat(3, 3, CV_64F, RR);
    const CvPoint2D32f* srcf;
    const CvPoint2D64f* srcd;
    CvPoint2D32f* dstf;
    CvPoint2D64f* dstd;
    int stype, dtype;
    int sstep, dstep;
    int i, j, n, iters = 1;

    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 );

    if( _distCoeffs )
    {
        CV_Assert( CV_IS_MAT(_distCoeffs) &&
            (_distCoeffs->rows == 1 || _distCoeffs->cols == 1) &&
            (_distCoeffs->rows*_distCoeffs->cols == 4 ||
            _distCoeffs->rows*_distCoeffs->cols == 5) );

        _Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols,
            CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k);
        
        cvConvert( _distCoeffs, &_Dk );
        iters = 5;
    }

    if( matR )
    {
        CV_Assert( CV_IS_MAT(matR) && matR->rows == 3 && matR->cols == 3 );
        cvConvert( 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 );
        cvMatMul( &_PP, &_RR, &_RR );
    }

    srcf = (const CvPoint2D32f*)_src->data.ptr;
    srcd = (const CvPoint2D64f*)_src->data.ptr;
    dstf = (CvPoint2D32f*)_dst->data.ptr;
    dstd = (CvPoint2D64f*)_dst->data.ptr;
    stype = CV_MAT_TYPE(_src->type);
    dtype = CV_MAT_TYPE(_dst->type);
    sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);
    dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);

    n = _src->rows + _src->cols - 1;

    fx = A[0][0];
    fy = A[1][1];
    ifx = 1./fx;
    ify = 1./fy;
    cx = A[0][2];
    cy = A[1][2];

    for( i = 0; i < n; i++ )
    {
        double x, y, x0, y0;
        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;
        }

        x0 = x = (x - cx)*ifx;
        y0 = y = (y - cy)*ify;

        // compensate distortion iteratively
        for( j = 0; j < iters; j++ )
        {
            double r2 = x*x + y*y;
            double icdist = 1./(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
            double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
            double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
            x = (x0 - deltaX)*icdist;
            y = (y0 - deltaY)*icdist;
        }

        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;
        }
    }
}


namespace cv
{
    
void undistortPoints( const Mat& src, Mat& dst,
                          const Mat& cameraMatrix, const Mat& distCoeffs,
                          const Mat& R, const Mat& P )
{
    CV_Assert( src.isContinuous() && src.depth() == CV_32F &&
              ((src.rows == 1 && src.channels() == 2) || src.cols*src.channels() == 2));
    
    dst.create(src.size(), src.type());
    CvMat _src = src, _dst = dst, _cameraMatrix = cameraMatrix;
    CvMat matR, matP, _distCoeffs, *pR=0, *pP=0, *pD=0;
    if( R.data )
        pR = &(matR = R);
    if( P.data )
        pP = &(matP = P);
    if( distCoeffs.data )
        pD = &(_distCoeffs = distCoeffs);
    cvUndistortPoints(&_src, &_dst, &_cameraMatrix, pD, pR, pP);
}

void undistortPoints( const Mat& src, std::vector<Point2f>& dst,
                      const Mat& cameraMatrix, const Mat& distCoeffs,
                      const Mat& R, const Mat& P )
{
    size_t sz = src.cols*src.rows*src.channels()/2;
    CV_Assert( src.isContinuous() && src.depth() == CV_32F &&
               ((src.rows == 1 && src.channels() == 2) || src.cols*src.channels() == 2));
    
    dst.resize(sz);
    CvMat _src = src, _dst = Mat(dst), _cameraMatrix = cameraMatrix;
    CvMat matR, matP, _distCoeffs, *pR=0, *pP=0, *pD=0;
    if( R.data )
        pR = &(matR = R);
    if( P.data )
        pP = &(matP = P);
    if( distCoeffs.data )
        pD = &(_distCoeffs = distCoeffs);
    cvUndistortPoints(&_src, &_dst, &_cameraMatrix, pD, pR, pP);
}


static Point2f mapPointSpherical(const Point2f& p, float alpha, Vec4d* J, int projType)
{
    double x = p.x, y = p.y;
    double beta = 1 + 2*alpha;
    double v = x*x + y*y + 1, iv = 1/v;
    double u = sqrt(beta*v + alpha*alpha);
    
    double k = (u - alpha)*iv;
    double kv = (v*beta/u - (u - alpha)*2)*iv*iv;
    double kx = kv*x, ky = kv*y;
    
    if( projType == PROJ_SPHERICAL_ORTHO )
    {
        if(J)
            *J = Vec4d(kx*x + k, kx*y, ky*x, ky*y + k);
        return Point2f((float)(x*k), (float)(y*k));
    }
    if( projType == PROJ_SPHERICAL_EQRECT )
    {
        // equirectangular
        double iR = 1/(alpha + 1);
        double x1 = std::max(std::min(x*k*iR, 1.), -1.);
        double y1 = std::max(std::min(y*k*iR, 1.), -1.);
        
        if(J)
        {
            double fx1 = iR/sqrt(1 - x1*x1);
            double fy1 = iR/sqrt(1 - y1*y1);
            *J = Vec4d(fx1*(kx*x + k), fx1*ky*x, fy1*kx*y, fy1*(ky*y + k));
        }
        return Point2f((float)asin(x1), (float)asin(y1));
    }
    CV_Error(CV_StsBadArg, "Unknown projection type");
    return Point2f();
}

    
static Point2f invMapPointSpherical(Point2f _p, float alpha, int projType)
{
    static int avgiter = 0, avgn = 0;
    
    double eps = 1e-12;
    Vec2d p(_p.x, _p.y), q(_p.x, _p.y), err;
    Vec4d J;
    int i, maxiter = 5;
    
    for( i = 0; i < maxiter; i++ )
    {
        Point2f p1 = mapPointSpherical(Point2f((float)q[0], (float)q[1]), alpha, &J, projType);
        err = Vec2d(p1.x, p1.y) - p;
        if( err[0]*err[0] + err[1]*err[1] < eps )
            break;
        
        Vec4d JtJ(J[0]*J[0] + J[2]*J[2], J[0]*J[1] + J[2]*J[3],
                  J[0]*J[1] + J[2]*J[3], J[1]*J[1] + J[3]*J[3]);
        double d = JtJ[0]*JtJ[3] - JtJ[1]*JtJ[2];
        d = d ? 1./d : 0;
        Vec4d iJtJ(JtJ[3]*d, -JtJ[1]*d, -JtJ[2]*d, JtJ[0]*d);
        Vec2d JtErr(J[0]*err[0] + J[2]*err[1], J[1]*err[0] + J[3]*err[1]);
        
        q -= Vec2d(iJtJ[0]*JtErr[0] + iJtJ[1]*JtErr[1], iJtJ[2]*JtErr[0] + iJtJ[3]*JtErr[1]);
        //Matx22d J(kx*x + k, kx*y, ky*x, ky*y + k);
        //q -= Vec2d((J.t()*J).inv()*(J.t()*err));
    }
    
    if( i < maxiter )
    {
        avgiter += i;
        avgn++;
        if( avgn == 1500 )
            printf("avg iters = %g\n", (double)avgiter/avgn);
    }
    
    return i < maxiter ? Point2f((float)q[0], (float)q[1]) : Point2f(-FLT_MAX, -FLT_MAX);
}

    
float initWideAngleProjMap( const Mat& cameraMatrix, const Mat& distCoeffs,
                            Size imageSize, int destImageWidth, int m1type,
                            Mat& map1, Mat& map2, int projType, double _alpha )
{
    Point2f scenter((float)cameraMatrix.at<double>(0,2), (float)cameraMatrix.at<double>(1,2));
    Point2f dcenter((destImageWidth-1)*0.5f, 0.f);
    float xmin = FLT_MAX, xmax = -FLT_MAX, ymin = FLT_MAX, ymax = -FLT_MAX;
    int N = 9;
    std::vector<Point2f> u(1), v(1);
    Mat _u(u), I = Mat::eye(3,3,CV_64F);
    float alpha = (float)_alpha;
    
    alpha = std::min(alpha, 0.999f);
    
    for( int i = 0; i < N; i++ )
        for( int j = 0; j < N; j++ )
        {
            Point2f p((float)j*imageSize.width/(N-1), (float)i*imageSize.height/(N-1));
            u[0] = p;
            undistortPoints(_u, v, cameraMatrix, distCoeffs, I, I);
            Point2f q = mapPointSpherical(v[0], alpha, 0, projType);
            if( xmin > q.x ) xmin = q.x;
            if( xmax < q.x ) xmax = q.x;
            if( ymin > q.y ) ymin = q.y;
            if( ymax < q.y ) ymax = q.y;
        }
    
    float scale = (float)std::min(dcenter.x/fabs(xmax), dcenter.x/fabs(xmin));
    Size dsize(destImageWidth, cvCeil(std::max(scale*fabs(ymin)*2, scale*fabs(ymax)*2)));
    dcenter.y = (dsize.height - 1)*0.5f;
    
    Mat mapxy(dsize, CV_32FC2);
    double k1 = distCoeffs.at<double>(0,0),
    k2 = distCoeffs.at<double>(1,0),
    k3 = distCoeffs.at<double>(4,0),
    p1 = distCoeffs.at<double>(2,0),
    p2 = distCoeffs.at<double>(3,0);
    double fx = cameraMatrix.at<double>(0,0),
    fy = cameraMatrix.at<double>(1,1),
    cx = scenter.x, cy = scenter.y;
    
    for( int y = 0; y < dsize.height; y++ )
    {
        Point2f* mxy = mapxy.ptr<Point2f>(y);
        for( int x = 0; x < dsize.width; x++ )
        {
            Point2f p = (Point2f((float)x, (float)y) - dcenter)*(1.f/scale);
            Point2f q = invMapPointSpherical(p, alpha, projType);
            if( q.x <= -FLT_MAX && q.y <= -FLT_MAX )
            {
                mxy[x] = Point2f(-1.f, -1.f);
                continue;
            }
            double x2 = q.x*q.x, y2 = q.y*q.y;
            double r2 = x2 + y2, _2xy = 2*q.x*q.y;
            double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2;
            double u = fx*(q.x*kr + p1*_2xy + p2*(r2 + 2*x2)) + cx;
            double v = fy*(q.y*kr + p1*(r2 + 2*y2) + p2*_2xy) + cy;
            
            mxy[x] = Point2f((float)u, (float)v);
        }
    }
    
    if(m1type == CV_32FC2)
    {
        mapxy.copyTo(map1);
        map2.release();
    }
    else
        convertMaps(mapxy, Mat(), map1, map2, m1type, false);
    
    return scale;
}

}

/*  End of file  */