modules/imgproc/src/undistort.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                           License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
  14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
  15 // Third party copyrights are property of their respective owners.
  16 //
  17 // Redistribution and use in source and binary forms, with or without modification,
  18 // are permitted provided that the following conditions are met:
  19 //
  20 //   * Redistribution's of source code must retain the above copyright notice,
  21 //     this list of conditions and the following disclaimer.
  22 //
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,
  24 //     this list of conditions and the following disclaimer in the documentation
  25 //     and/or other materials provided with the distribution.
  26 //
  27 //   * The name of the copyright holders may not be used to endorse or promote products
  28 //     derived from this software without specific prior written permission.
  29 //
  30 // This software is provided by the copyright holders and contributors "as is" and
  31 // any express or implied warranties, including, but not limited to, the implied
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,
  34 // indirect, incidental, special, exemplary, or consequential damages
  35 // (including, but not limited to, procurement of substitute goods or services;
  36 // loss of use, data, or profits; or business interruption) however caused
  37 // and on any theory of liability, whether in contract, strict liability,
  38 // or tort (including negligence or otherwise) arising in any way out of
  39 // the use of this software, even if advised of the possibility of such damage.
  40 //
  41 //M*/
  42
  43 #include "precomp.hpp"
  44
  45 cv::Mat cv::getDefaultNewCameraMatrix( InputArray _cameraMatrix, Size imgsize,
  46                                bool centerPrincipalPoint )
  47 {
  48     Mat cameraMatrix = _cameraMatrix.getMat();
  49     if( !centerPrincipalPoint && cameraMatrix.type() == CV_64F )
  50         return cameraMatrix;
  51
  52     Mat newCameraMatrix;
  53     cameraMatrix.convertTo(newCameraMatrix, CV_64F);
  54     if( centerPrincipalPoint )
  55     {
  56         ((double*)newCameraMatrix.data)[2] = (imgsize.width-1)*0.5;
  57         ((double*)newCameraMatrix.data)[5] = (imgsize.height-1)*0.5;
  58     }
  59     return newCameraMatrix;
  60 }
  61
  62 void cv::initUndistortRectifyMap( InputArray _cameraMatrix, InputArray _distCoeffs,
  63                               InputArray _matR, InputArray _newCameraMatrix,
  64                               Size size, int m1type, OutputArray _map1, OutputArray _map2 )
  65 {
  66     Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
  67     Mat matR = _matR.getMat(), newCameraMatrix = _newCameraMatrix.getMat();
  68
  69     if( m1type <= 0 )
  70         m1type = CV_16SC2;
  71     CV_Assert( m1type == CV_16SC2 || m1type == CV_32FC1 || m1type == CV_32FC2 );
  72     _map1.create( size, m1type );
  73     Mat map1 = _map1.getMat(), map2;
  74     if( m1type != CV_32FC2 )
  75     {
  76         _map2.create( size, m1type == CV_16SC2 ? CV_16UC1 : CV_32FC1 );
  77         map2 = _map2.getMat();
  78     }
  79     else
  80         _map2.release();
  81
  82     Mat_<double> R = Mat_<double>::eye(3, 3);
  83     Mat_<double> A = Mat_<double>(cameraMatrix), Ar;
  84
  85     if( newCameraMatrix.data )
  86         Ar = Mat_<double>(newCameraMatrix);
  87     else
  88         Ar = getDefaultNewCameraMatrix( A, size, true );
  89
  90     if( matR.data )
  91         R = Mat_<double>(matR);
  92
  93     if( distCoeffs.data )
  94         distCoeffs = Mat_<double>(distCoeffs);
  95     else
  96     {
  97         distCoeffs.create(8, 1, CV_64F);
  98         distCoeffs = 0.;
  99     }
 100
 101     CV_Assert( A.size() == Size(3,3) && A.size() == R.size() );
 102     CV_Assert( Ar.size() == Size(3,3) || Ar.size() == Size(4, 3));
 103     Mat_<double> iR = (Ar.colRange(0,3)*R).inv(DECOMP_LU);
 104     const double* ir = &iR(0,0);
 105
 106     double u0 = A(0, 2),  v0 = A(1, 2);
 107     double fx = A(0, 0),  fy = A(1, 1);
 108
 109     CV_Assert( distCoeffs.size() == Size(1, 4) || distCoeffs.size() == Size(4, 1) ||
 110                distCoeffs.size() == Size(1, 5) || distCoeffs.size() == Size(5, 1) ||
 111                distCoeffs.size() == Size(1, 8) || distCoeffs.size() == Size(8, 1));
 112
 113     if( distCoeffs.rows != 1 && !distCoeffs.isContinuous() )
 114         distCoeffs = distCoeffs.t();
 115
 116     double k1 = ((double*)distCoeffs.data)[0];
 117     double k2 = ((double*)distCoeffs.data)[1];
 118     double p1 = ((double*)distCoeffs.data)[2];
 119     double p2 = ((double*)distCoeffs.data)[3];
 120     double k3 = distCoeffs.cols + distCoeffs.rows - 1 >= 5 ? ((double*)distCoeffs.data)[4] : 0.;
 121     double k4 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? ((double*)distCoeffs.data)[5] : 0.;
 122     double k5 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? ((double*)distCoeffs.data)[6] : 0.;
 123     double k6 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? ((double*)distCoeffs.data)[7] : 0.;
 124
 125     for( int i = 0; i < size.height; i++ )
 126     {
 127         float* m1f = (float*)(map1.data + map1.step*i);
 128         float* m2f = (float*)(map2.data + map2.step*i);
 129         short* m1 = (short*)m1f;
 130         ushort* m2 = (ushort*)m2f;
 131         double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8];
 132
 133         for( int j = 0; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] )
 134         {
 135             double w = 1./_w, x = _x*w, y = _y*w;
 136             double x2 = x*x, y2 = y*y;
 137             double r2 = x2 + y2, _2xy = 2*x*y;
 138             double kr = (1 + ((k3*r2 + k2)*r2 + k1)*r2)/(1 + ((k6*r2 + k5)*r2 + k4)*r2);
 139             double u = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + u0;
 140             double v = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + v0;
 141             if( m1type == CV_16SC2 )
 142             {
 143                 int iu = saturate_cast<int>(u*INTER_TAB_SIZE);
 144                 int iv = saturate_cast<int>(v*INTER_TAB_SIZE);
 145                 m1[j*2] = (short)(iu >> INTER_BITS);
 146                 m1[j*2+1] = (short)(iv >> INTER_BITS);
 147                 m2[j] = (ushort)((iv & (INTER_TAB_SIZE-1))*INTER_TAB_SIZE + (iu & (INTER_TAB_SIZE-1)));
 148             }
 149             else if( m1type == CV_32FC1 )
 150             {
 151                 m1f[j] = (float)u;
 152                 m2f[j] = (float)v;
 153             }
 154             else
 155             {
 156                 m1f[j*2] = (float)u;
 157                 m1f[j*2+1] = (float)v;
 158             }
 159         }
 160     }
 161 }
 162
 163
 164 void cv::undistort( InputArray _src, OutputArray _dst, InputArray _cameraMatrix,
 165                     InputArray _distCoeffs, InputArray _newCameraMatrix )
 166 {
 167     Mat src = _src.getMat(), cameraMatrix = _cameraMatrix.getMat();
 168     Mat distCoeffs = _distCoeffs.getMat(), newCameraMatrix = _newCameraMatrix.getMat();
 169
 170     _dst.create( src.size(), src.type() );
 171     Mat dst = _dst.getMat();
 172
 173     CV_Assert( dst.data != src.data );
 174
 175     int stripe_size0 = std::min(std::max(1, (1 << 12) / std::max(src.cols, 1)), src.rows);
 176     Mat map1(stripe_size0, src.cols, CV_16SC2), map2(stripe_size0, src.cols, CV_16UC1);
 177
 178     Mat_<double> A, Ar, I = Mat_<double>::eye(3,3);
 179
 180     cameraMatrix.convertTo(A, CV_64F);
 181     if( distCoeffs.data )
 182         distCoeffs = Mat_<double>(distCoeffs);
 183     else
 184     {
 185         distCoeffs.create(5, 1, CV_64F);
 186         distCoeffs = 0.;
 187     }
 188
 189     if( newCameraMatrix.data )
 190         newCameraMatrix.convertTo(Ar, CV_64F);
 191     else
 192         A.copyTo(Ar);
 193
 194     double v0 = Ar(1, 2);
 195     for( int y = 0; y < src.rows; y += stripe_size0 )
 196     {
 197         int stripe_size = std::min( stripe_size0, src.rows - y );
 198         Ar(1, 2) = v0 - y;
 199         Mat map1_part = map1.rowRange(0, stripe_size),
 200             map2_part = map2.rowRange(0, stripe_size),
 201             dst_part = dst.rowRange(y, y + stripe_size);
 202
 203         initUndistortRectifyMap( A, distCoeffs, I, Ar, Size(src.cols, stripe_size),
 204                                  map1_part.type(), map1_part, map2_part );
 205         remap( src, dst_part, map1_part, map2_part, INTER_LINEAR, BORDER_CONSTANT );
 206     }
 207 }
 208
 209
 210 CV_IMPL void
 211 cvUndistort2( const CvArr* srcarr, CvArr* dstarr, const CvMat* Aarr, const CvMat* dist_coeffs, const CvMat* newAarr )
 212 {
 213     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr), dst0 = dst;
 214     cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs), newA;
 215     if( newAarr )
 216         newA = cv::cvarrToMat(newAarr);
 217
 218     CV_Assert( src.size() == dst.size() && src.type() == dst.type() );
 219     cv::undistort( src, dst, A, distCoeffs, newA );
 220 }
 221
 222
 223 CV_IMPL void cvInitUndistortMap( const CvMat* Aarr, const CvMat* dist_coeffs,
 224                                  CvArr* mapxarr, CvArr* mapyarr )
 225 {
 226     cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs);
 227     cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0;
 228
 229     if( mapyarr )
 230         mapy0 = mapy = cv::cvarrToMat(mapyarr);
 231
 232     cv::initUndistortRectifyMap( A, distCoeffs, cv::Mat(), A,
 233                                  mapx.size(), mapx.type(), mapx, mapy );
 234     CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data );
 235 }
 236
 237 void
 238 cvInitUndistortRectifyMap( const CvMat* Aarr, const CvMat* dist_coeffs,
 239     const CvMat *Rarr, const CvMat* ArArr, CvArr* mapxarr, CvArr* mapyarr )
 240 {
 241     cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs, R, Ar;
 242     cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0;
 243
 244     if( mapyarr )
 245         mapy0 = mapy = cv::cvarrToMat(mapyarr);
 246
 247     if( dist_coeffs )
 248         distCoeffs = cv::cvarrToMat(dist_coeffs);
 249     if( Rarr )
 250         R = cv::cvarrToMat(Rarr);
 251     if( ArArr )
 252         Ar = cv::cvarrToMat(ArArr);
 253
 254     cv::initUndistortRectifyMap( A, distCoeffs, R, Ar, mapx.size(), mapx.type(), mapx, mapy );
 255     CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data );
 256 }
 257
 258
 259 void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
 260                    const CvMat* _distCoeffs,
 261                    const CvMat* matR, const CvMat* matP )
 262 {
 263     double A[3][3], RR[3][3], k[8]={0,0,0,0,0,0,0,0}, fx, fy, ifx, ify, cx, cy;
 264     CvMat matA=cvMat(3, 3, CV_64F, A), _Dk;
 265     CvMat _RR=cvMat(3, 3, CV_64F, RR);
 266     const CvPoint2D32f* srcf;
 267     const CvPoint2D64f* srcd;
 268     CvPoint2D32f* dstf;
 269     CvPoint2D64f* dstd;
 270     int stype, dtype;
 271     int sstep, dstep;
 272     int i, j, n, iters = 1;
 273
 274     CV_Assert( CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&
 275         (_src->rows == 1 || _src->cols == 1) &&
 276         (_dst->rows == 1 || _dst->cols == 1) &&
 277         _src->cols + _src->rows - 1 == _dst->rows + _dst->cols - 1 &&
 278         (CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) &&
 279         (CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2));
 280
 281     CV_Assert( CV_IS_MAT(_cameraMatrix) &&
 282         _cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 );
 283
 284     cvConvert( _cameraMatrix, &matA );
 285
 286     if( _distCoeffs )
 287     {
 288         CV_Assert( CV_IS_MAT(_distCoeffs) &&
 289             (_distCoeffs->rows == 1 || _distCoeffs->cols == 1) &&
 290             (_distCoeffs->rows*_distCoeffs->cols == 4 ||
 291              _distCoeffs->rows*_distCoeffs->cols == 5 ||
 292              _distCoeffs->rows*_distCoeffs->cols == 8));
 293
 294         _Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols,
 295             CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k);
 296
 297         cvConvert( _distCoeffs, &_Dk );
 298         iters = 5;
 299     }
 300
 301     if( matR )
 302     {
 303         CV_Assert( CV_IS_MAT(matR) && matR->rows == 3 && matR->cols == 3 );
 304         cvConvert( matR, &_RR );
 305     }
 306     else
 307         cvSetIdentity(&_RR);
 308
 309     if( matP )
 310     {
 311         double PP[3][3];
 312         CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP);
 313         CV_Assert( CV_IS_MAT(matP) && matP->rows == 3 && (matP->cols == 3 || matP->cols == 4));
 314         cvConvert( cvGetCols(matP, &_P3x3, 0, 3), &_PP );
 315         cvMatMul( &_PP, &_RR, &_RR );
 316     }
 317
 318     srcf = (const CvPoint2D32f*)_src->data.ptr;
 319     srcd = (const CvPoint2D64f*)_src->data.ptr;
 320     dstf = (CvPoint2D32f*)_dst->data.ptr;
 321     dstd = (CvPoint2D64f*)_dst->data.ptr;
 322     stype = CV_MAT_TYPE(_src->type);
 323     dtype = CV_MAT_TYPE(_dst->type);
 324     sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);
 325     dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);
 326
 327     n = _src->rows + _src->cols - 1;
 328
 329     fx = A[0][0];
 330     fy = A[1][1];
 331     ifx = 1./fx;
 332     ify = 1./fy;
 333     cx = A[0][2];
 334     cy = A[1][2];
 335
 336     for( i = 0; i < n; i++ )
 337     {
 338         double x, y, x0, y0;
 339         if( stype == CV_32FC2 )
 340         {
 341             x = srcf[i*sstep].x;
 342             y = srcf[i*sstep].y;
 343         }
 344         else
 345         {
 346             x = srcd[i*sstep].x;
 347             y = srcd[i*sstep].y;
 348         }
 349
 350         x0 = x = (x - cx)*ifx;
 351         y0 = y = (y - cy)*ify;
 352
 353         // compensate distortion iteratively
 354         for( j = 0; j < iters; j++ )
 355         {
 356             double r2 = x*x + y*y;
 357             double icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
 358             double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
 359             double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
 360             x = (x0 - deltaX)*icdist;
 361             y = (y0 - deltaY)*icdist;
 362         }
 363
 364         double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2];
 365         double yy = RR[1][0]*x + RR[1][1]*y + RR[1][2];
 366         double ww = 1./(RR[2][0]*x + RR[2][1]*y + RR[2][2]);
 367         x = xx*ww;
 368         y = yy*ww;
 369
 370         if( dtype == CV_32FC2 )
 371         {
 372             dstf[i*dstep].x = (float)x;
 373             dstf[i*dstep].y = (float)y;
 374         }
 375         else
 376         {
 377             dstd[i*dstep].x = x;
 378             dstd[i*dstep].y = y;
 379         }
 380     }
 381 }
 382
 383
 384 void cv::undistortPoints( InputArray _src, OutputArray _dst,
 385                           InputArray _cameraMatrix,
 386                           InputArray _distCoeffs,
 387                           InputArray _Rmat,
 388                           InputArray _Pmat )
 389 {
 390     Mat src = _src.getMat(), cameraMatrix = _cameraMatrix.getMat();
 391     Mat distCoeffs = _distCoeffs.getMat(), R = _Rmat.getMat(), P = _Pmat.getMat();
 392
 393     CV_Assert( src.isContinuous() && (src.depth() == CV_32F || src.depth() == CV_64F) &&
 394               ((src.rows == 1 && src.channels() == 2) || src.cols*src.channels() == 2));
 395
 396     _dst.create(src.size(), src.type(), -1, true);
 397     Mat dst = _dst.getMat();
 398
 399     CvMat _csrc = src, _cdst = dst, _ccameraMatrix = cameraMatrix;
 400     CvMat matR, matP, _cdistCoeffs, *pR=0, *pP=0, *pD=0;
 401     if( R.data )
 402         pR = &(matR = R);
 403     if( P.data )
 404         pP = &(matP = P);
 405     if( distCoeffs.data )
 406         pD = &(_cdistCoeffs = distCoeffs);
 407     cvUndistortPoints(&_csrc, &_cdst, &_ccameraMatrix, pD, pR, pP);
 408 }
 409
 410 namespace cv
 411 {
 412
 413 static Point2f mapPointSpherical(const Point2f& p, float alpha, Vec4d* J, int projType)
 414 {
 415     double x = p.x, y = p.y;
 416     double beta = 1 + 2*alpha;
 417     double v = x*x + y*y + 1, iv = 1/v;
 418     double u = sqrt(beta*v + alpha*alpha);
 419
 420     double k = (u - alpha)*iv;
 421     double kv = (v*beta/u - (u - alpha)*2)*iv*iv;
 422     double kx = kv*x, ky = kv*y;
 423
 424     if( projType == PROJ_SPHERICAL_ORTHO )
 425     {
 426         if(J)
 427             *J = Vec4d(kx*x + k, kx*y, ky*x, ky*y + k);
 428         return Point2f((float)(x*k), (float)(y*k));
 429     }
 430     if( projType == PROJ_SPHERICAL_EQRECT )
 431     {
 432         // equirectangular
 433         double iR = 1/(alpha + 1);
 434         double x1 = std::max(std::min(x*k*iR, 1.), -1.);
 435         double y1 = std::max(std::min(y*k*iR, 1.), -1.);
 436
 437         if(J)
 438         {
 439             double fx1 = iR/sqrt(1 - x1*x1);
 440             double fy1 = iR/sqrt(1 - y1*y1);
 441             *J = Vec4d(fx1*(kx*x + k), fx1*ky*x, fy1*kx*y, fy1*(ky*y + k));
 442         }
 443         return Point2f((float)asin(x1), (float)asin(y1));
 444     }
 445     CV_Error(CV_StsBadArg, "Unknown projection type");
 446     return Point2f();
 447 }
 448
 449
 450 static Point2f invMapPointSpherical(Point2f _p, float alpha, int projType)
 451 {
 452     static int avgiter = 0, avgn = 0;
 453
 454     double eps = 1e-12;
 455     Vec2d p(_p.x, _p.y), q(_p.x, _p.y), err;
 456     Vec4d J;
 457     int i, maxiter = 5;
 458
 459     for( i = 0; i < maxiter; i++ )
 460     {
 461         Point2f p1 = mapPointSpherical(Point2f((float)q[0], (float)q[1]), alpha, &J, projType);
 462         err = Vec2d(p1.x, p1.y) - p;
 463         if( err[0]*err[0] + err[1]*err[1] < eps )
 464             break;
 465
 466         Vec4d JtJ(J[0]*J[0] + J[2]*J[2], J[0]*J[1] + J[2]*J[3],
 467                   J[0]*J[1] + J[2]*J[3], J[1]*J[1] + J[3]*J[3]);
 468         double d = JtJ[0]*JtJ[3] - JtJ[1]*JtJ[2];
 469         d = d ? 1./d : 0;
 470         Vec4d iJtJ(JtJ[3]*d, -JtJ[1]*d, -JtJ[2]*d, JtJ[0]*d);
 471         Vec2d JtErr(J[0]*err[0] + J[2]*err[1], J[1]*err[0] + J[3]*err[1]);
 472
 473         q -= Vec2d(iJtJ[0]*JtErr[0] + iJtJ[1]*JtErr[1], iJtJ[2]*JtErr[0] + iJtJ[3]*JtErr[1]);
 474         //Matx22d J(kx*x + k, kx*y, ky*x, ky*y + k);
 475         //q -= Vec2d((J.t()*J).inv()*(J.t()*err));
 476     }
 477
 478     if( i < maxiter )
 479     {
 480         avgiter += i;
 481         avgn++;
 482         if( avgn == 1500 )
 483             printf("avg iters = %g\n", (double)avgiter/avgn);
 484     }
 485
 486     return i < maxiter ? Point2f((float)q[0], (float)q[1]) : Point2f(-FLT_MAX, -FLT_MAX);
 487 }
 488
 489 }
 490
 491 float cv::initWideAngleProjMap( InputArray _cameraMatrix0, InputArray _distCoeffs0,
 492                             Size imageSize, int destImageWidth, int m1type,
 493                             OutputArray _map1, OutputArray _map2, int projType, double _alpha )
 494 {
 495     Mat cameraMatrix0 = _cameraMatrix0.getMat(), distCoeffs0 = _distCoeffs0.getMat();
 496     double k[8] = {0,0,0,0,0,0,0,0}, M[9]={0,0,0,0,0,0,0,0,0};
 497     Mat distCoeffs(distCoeffs0.rows, distCoeffs0.cols, CV_MAKETYPE(CV_64F,distCoeffs0.channels()), k);
 498     Mat cameraMatrix(3,3,CV_64F,M);
 499     Point2f scenter((float)cameraMatrix.at<double>(0,2), (float)cameraMatrix.at<double>(1,2));
 500     Point2f dcenter((destImageWidth-1)*0.5f, 0.f);
 501     float xmin = FLT_MAX, xmax = -FLT_MAX, ymin = FLT_MAX, ymax = -FLT_MAX;
 502     int N = 9;
 503     std::vector<Point2f> uvec(1), vvec(1);
 504     Mat I = Mat::eye(3,3,CV_64F);
 505     float alpha = (float)_alpha;
 506
 507     int ndcoeffs = distCoeffs0.cols*distCoeffs0.rows*distCoeffs0.channels();
 508     CV_Assert((distCoeffs0.cols == 1 || distCoeffs0.rows == 1) &&
 509               (ndcoeffs == 4 || ndcoeffs == 5 || ndcoeffs == 8));
 510     CV_Assert(cameraMatrix0.size() == Size(3,3));
 511     distCoeffs0.convertTo(distCoeffs,CV_64F);
 512     cameraMatrix0.convertTo(cameraMatrix,CV_64F);
 513
 514     alpha = std::min(alpha, 0.999f);
 515
 516     for( int i = 0; i < N; i++ )
 517         for( int j = 0; j < N; j++ )
 518         {
 519             Point2f p((float)j*imageSize.width/(N-1), (float)i*imageSize.height/(N-1));
 520             uvec[0] = p;
 521             undistortPoints(uvec, vvec, cameraMatrix, distCoeffs, I, I);
 522             Point2f q = mapPointSpherical(vvec[0], alpha, 0, projType);
 523             if( xmin > q.x ) xmin = q.x;
 524             if( xmax < q.x ) xmax = q.x;
 525             if( ymin > q.y ) ymin = q.y;
 526             if( ymax < q.y ) ymax = q.y;
 527         }
 528
 529     float scale = (float)std::min(dcenter.x/fabs(xmax), dcenter.x/fabs(xmin));
 530     Size dsize(destImageWidth, cvCeil(std::max(scale*fabs(ymin)*2, scale*fabs(ymax)*2)));
 531     dcenter.y = (dsize.height - 1)*0.5f;
 532
 533     Mat mapxy(dsize, CV_32FC2);
 534     double k1 = k[0], k2 = k[1], k3 = k[2], p1 = k[3], p2 = k[4], k4 = k[5], k5 = k[6], k6 = k[7];
 535     double fx = cameraMatrix.at<double>(0,0), fy = cameraMatrix.at<double>(1,1), cx = scenter.x, cy = scenter.y;
 536
 537     for( int y = 0; y < dsize.height; y++ )
 538     {
 539         Point2f* mxy = mapxy.ptr<Point2f>(y);
 540         for( int x = 0; x < dsize.width; x++ )
 541         {
 542             Point2f p = (Point2f((float)x, (float)y) - dcenter)*(1.f/scale);
 543             Point2f q = invMapPointSpherical(p, alpha, projType);
 544             if( q.x <= -FLT_MAX && q.y <= -FLT_MAX )
 545             {
 546                 mxy[x] = Point2f(-1.f, -1.f);
 547                 continue;
 548             }
 549             double x2 = q.x*q.x, y2 = q.y*q.y;
 550             double r2 = x2 + y2, _2xy = 2*q.x*q.y;
 551             double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2/(1 + ((k6*r2 + k5)*r2 + k4)*r2);
 552             double u = fx*(q.x*kr + p1*_2xy + p2*(r2 + 2*x2)) + cx;
 553             double v = fy*(q.y*kr + p1*(r2 + 2*y2) + p2*_2xy) + cy;
 554
 555             mxy[x] = Point2f((float)u, (float)v);
 556         }
 557     }
 558
 559     if(m1type == CV_32FC2)
 560     {
 561         _map1.create(mapxy.size(), mapxy.type());
 562         Mat map1 = _map1.getMat();
 563         mapxy.copyTo(map1);
 564         _map2.release();
 565     }
 566     else
 567         convertMaps(mapxy, Mat(), _map1, _map2, m1type, false);
 568
 569     return scale;
 570 }
 571
 572 /*  End of file  */