vcs.maemo.org Git - opencv/blob - src/cv/cvundistort.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 "_cv.h"
  44
  45 namespace cv
  46 {
  47
  48 Mat getDefaultNewCameraMatrix( const Mat& cameraMatrix, Size imgsize,
  49                                bool centerPrincipalPoint )
  50 {
  51     if( !centerPrincipalPoint && cameraMatrix.type() == CV_64F )
  52         return cameraMatrix;
  53
  54     Mat newCameraMatrix;
  55     cameraMatrix.convertTo(newCameraMatrix, CV_64F);
  56     if( centerPrincipalPoint )
  57     {
  58         ((double*)newCameraMatrix.data)[2] = (imgsize.width-1)*0.5;
  59         ((double*)newCameraMatrix.data)[5] = (imgsize.height-1)*0.5;
  60     }
  61     return newCameraMatrix;
  62 }
  63
  64 void initUndistortRectifyMap( const Mat& _cameraMatrix, const Mat& _distCoeffs,
  65                               const Mat& _R, const Mat& _newCameraMatrix,
  66                               Size size, int m1type, Mat& map1, Mat& map2 )
  67 {
  68     if( m1type <= 0 )
  69         m1type = CV_16SC2;
  70     CV_Assert( m1type == CV_16SC2 || m1type == CV_32FC1 || m1type == CV_32FC2 );
  71     map1.create( size, m1type );
  72     if( m1type != CV_32FC2 )
  73         map2.create( size, m1type == CV_16SC2 ? CV_16UC1 : CV_32FC1 );
  74     else
  75         map2.release();
  76
  77     Mat_<double> R = Mat_<double>::eye(3, 3), distCoeffs;
  78     Mat_<double> A = Mat_<double>(_cameraMatrix), Ar;
  79
  80     if( _newCameraMatrix.data )
  81         Ar = Mat_<double>(_newCameraMatrix);
  82     else
  83         Ar = getDefaultNewCameraMatrix( A, size, true );
  84
  85     if( _R.data )
  86         R = Mat_<double>(_R);
  87
  88     if( _distCoeffs.data )
  89         distCoeffs = Mat_<double>(_distCoeffs);
  90     else
  91     {
  92         distCoeffs.create(5, 1);
  93         distCoeffs = 0.;
  94     }
  95
  96     CV_Assert( A.size() == Size(3,3) && A.size() == R.size() );
  97     CV_Assert( Ar.size() == Size(3,3) || Ar.size() == Size(4, 3));
  98     Mat_<double> iR = (Ar.colRange(0,3)*R).inv(DECOMP_LU);
  99     const double* ir = &iR(0,0);
 100
 101     double u0 = A(0, 2),  v0 = A(1, 2);
 102     double fx = A(0, 0),  fy = A(1, 1);
 103
 104     CV_Assert( distCoeffs.size() == Size(1, 4) || distCoeffs.size() == Size(1, 5) ||
 105                distCoeffs.size() == Size(4, 1) || distCoeffs.size() == Size(5, 1));
 106
 107     if( distCoeffs.rows != 1 && !distCoeffs.isContinuous() )
 108         distCoeffs = distCoeffs.t();
 109
 110     double k1 = ((double*)distCoeffs.data)[0];
 111     double k2 = ((double*)distCoeffs.data)[1];
 112     double p1 = ((double*)distCoeffs.data)[2];
 113     double p2 = ((double*)distCoeffs.data)[3];
 114     double k3 = distCoeffs.cols + distCoeffs.rows - 1 == 5 ? ((double*)distCoeffs.data)[4] : 0.;
 115
 116     for( int i = 0; i < size.height; i++ )
 117     {
 118         float* m1f = (float*)(map1.data + map1.step*i);
 119         float* m2f = (float*)(map2.data + map2.step*i);
 120         short* m1 = (short*)m1f;
 121         ushort* m2 = (ushort*)m2f;
 122         double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8];
 123
 124         for( int j = 0; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] )
 125         {
 126             double w = 1./_w, x = _x*w, y = _y*w;
 127             double x2 = x*x, y2 = y*y;
 128             double r2 = x2 + y2, _2xy = 2*x*y;
 129             double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2;
 130             double u = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + u0;
 131             double v = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + v0;
 132             if( m1type == CV_16SC2 )
 133             {
 134                 int iu = saturate_cast<int>(u*INTER_TAB_SIZE);
 135                 int iv = saturate_cast<int>(v*INTER_TAB_SIZE);
 136                 m1[j*2] = (short)(iu >> INTER_BITS);
 137                 m1[j*2+1] = (short)(iv >> INTER_BITS);
 138                 m2[j] = (ushort)((iv & (INTER_TAB_SIZE-1))*INTER_TAB_SIZE + (iu & (INTER_TAB_SIZE-1)));
 139             }
 140             else if( m1type == CV_32FC1 )
 141             {
 142                 m1f[j] = (float)u;
 143                 m2f[j] = (float)v;
 144             }
 145             else
 146             {
 147                 m1f[j*2] = (float)u;
 148                 m1f[j*2+1] = (float)v;
 149             }
 150         }
 151     }
 152 }
 153
 154
 155 void undistort( const Mat& src, Mat& dst, const Mat& _cameraMatrix,
 156                 const Mat& _distCoeffs, const Mat& _newCameraMatrix )
 157 {
 158     dst.create( src.size(), src.type() );
 159     int stripe_size0 = std::min(std::max(1, (1 << 12) / std::max(src.cols, 1)), src.rows);
 160     Mat map1(stripe_size0, src.cols, CV_16SC2), map2(stripe_size0, src.cols, CV_16UC1);
 161
 162     Mat_<double> A, distCoeffs, Ar, I = Mat_<double>::eye(3,3);
 163
 164     _cameraMatrix.convertTo(A, CV_64F);
 165     if( _distCoeffs.data )
 166         distCoeffs = Mat_<double>(_distCoeffs);
 167     else
 168     {
 169         distCoeffs.create(5, 1);
 170         distCoeffs = 0.;
 171     }
 172
 173     if( _newCameraMatrix.data )
 174         _newCameraMatrix.convertTo(Ar, CV_64F);
 175     else
 176         A.copyTo(Ar);
 177
 178     double v0 = Ar(1, 2);
 179     for( int y = 0; y < src.rows; y += stripe_size0 )
 180     {
 181         int stripe_size = std::min( stripe_size0, src.rows - y );
 182         Ar(1, 2) = v0 - y;
 183         Mat map1_part = map1.rowRange(0, stripe_size),
 184             map2_part = map2.rowRange(0, stripe_size),
 185             dst_part = dst.rowRange(y, y + stripe_size);
 186
 187         initUndistortRectifyMap( A, distCoeffs, I, Ar, Size(src.cols, stripe_size),
 188                                  map1_part.type(), map1_part, map2_part );
 189         remap( src, dst_part, map1_part, map2_part, INTER_LINEAR, BORDER_REPLICATE );
 190     }
 191 }
 192
 193 }
 194
 195
 196 CV_IMPL void
 197 cvUndistort2( const CvArr* srcarr, CvArr* dstarr, const CvMat* Aarr, const CvMat* dist_coeffs )
 198 {
 199     cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr), dst0 = dst;
 200     cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs);
 201
 202     CV_Assert( src.size() == dst.size() && src.type() == dst.type() );
 203     cv::undistort( src, dst, A, distCoeffs, cv::Mat() );
 204 }
 205
 206
 207 CV_IMPL void cvInitUndistortMap( const CvMat* Aarr, const CvMat* dist_coeffs,
 208                                  CvArr* mapxarr, CvArr* mapyarr )
 209 {
 210     cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs);
 211     cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0;
 212
 213     if( mapyarr )
 214         mapy0 = mapy = cv::cvarrToMat(mapyarr);
 215
 216     cv::initUndistortRectifyMap( A, distCoeffs, cv::Mat(), A,
 217                                  mapx.size(), mapx.type(), mapx, mapy );
 218     CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data );
 219 }
 220
 221 void
 222 cvInitUndistortRectifyMap( const CvMat* Aarr, const CvMat* dist_coeffs,
 223     const CvMat *Rarr, const CvMat* ArArr, CvArr* mapxarr, CvArr* mapyarr )
 224 {
 225     cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs, R, Ar;
 226     cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0;
 227
 228     if( mapyarr )
 229         mapy0 = mapy = cv::cvarrToMat(mapyarr);
 230
 231     if( dist_coeffs )
 232         distCoeffs = cv::cvarrToMat(dist_coeffs);
 233     if( Rarr )
 234         R = cv::cvarrToMat(Rarr);
 235     if( ArArr )
 236         Ar = cv::cvarrToMat(ArArr);
 237
 238     cv::initUndistortRectifyMap( A, distCoeffs, R, Ar, mapx.size(), mapx.type(), mapx, mapy );
 239     CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data );
 240 }
 241
 242
 243 void
 244 cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
 245                    const CvMat* _distCoeffs,
 246                    const CvMat* _R, const CvMat* _P )
 247 {
 248     CV_FUNCNAME( "cvUndistortPoints" );
 249
 250     __BEGIN__;
 251
 252     double A[3][3], RR[3][3], k[5]={0,0,0,0,0}, fx, fy, ifx, ify, cx, cy;
 253     CvMat _A=cvMat(3, 3, CV_64F, A), _Dk;
 254     CvMat _RR=cvMat(3, 3, CV_64F, RR);
 255     const CvPoint2D32f* srcf;
 256     const CvPoint2D64f* srcd;
 257     CvPoint2D32f* dstf;
 258     CvPoint2D64f* dstd;
 259     int stype, dtype;
 260     int sstep, dstep;
 261     int i, j, n, iters = 1;
 262
 263     CV_ASSERT( CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&
 264         (_src->rows == 1 || _src->cols == 1) &&
 265         (_dst->rows == 1 || _dst->cols == 1) &&
 266         CV_ARE_SIZES_EQ(_src, _dst) &&
 267         (CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) &&
 268         (CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2));
 269
 270     CV_ASSERT( CV_IS_MAT(_cameraMatrix) &&
 271         _cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 );
 272
 273     cvConvert( _cameraMatrix, &_A );
 274
 275     if( _distCoeffs )
 276     {
 277         CV_ASSERT( CV_IS_MAT(_distCoeffs) &&
 278             (_distCoeffs->rows == 1 || _distCoeffs->cols == 1) &&
 279             (_distCoeffs->rows*_distCoeffs->cols == 4 ||
 280             _distCoeffs->rows*_distCoeffs->cols == 5) );
 281
 282         _Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols,
 283             CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k);
 284
 285         cvConvert( _distCoeffs, &_Dk );
 286         iters = 5;
 287     }
 288
 289     if( _R )
 290     {
 291         CV_ASSERT( CV_IS_MAT(_R) && _R->rows == 3 && _R->cols == 3 );
 292         cvConvert( _R, &_RR );
 293     }
 294     else
 295         cvSetIdentity(&_RR);
 296
 297     if( _P )
 298     {
 299         double PP[3][3];
 300         CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP);
 301         CV_ASSERT( CV_IS_MAT(_P) && _P->rows == 3 && (_P->cols == 3 || _P->cols == 4));
 302         cvConvert( cvGetCols(_P, &_P3x3, 0, 3), &_PP );
 303         cvMatMul( &_PP, &_RR, &_RR );
 304     }
 305
 306     srcf = (const CvPoint2D32f*)_src->data.ptr;
 307     srcd = (const CvPoint2D64f*)_src->data.ptr;
 308     dstf = (CvPoint2D32f*)_dst->data.ptr;
 309     dstd = (CvPoint2D64f*)_dst->data.ptr;
 310     stype = CV_MAT_TYPE(_src->type);
 311     dtype = CV_MAT_TYPE(_dst->type);
 312     sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);
 313     dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);
 314
 315     n = _src->rows + _src->cols - 1;
 316
 317     fx = A[0][0];
 318     fy = A[1][1];
 319     ifx = 1./fx;
 320     ify = 1./fy;
 321     cx = A[0][2];
 322     cy = A[1][2];
 323
 324     for( i = 0; i < n; i++ )
 325     {
 326         double x, y, x0, y0;
 327         if( stype == CV_32FC2 )
 328         {
 329             x = srcf[i*sstep].x;
 330             y = srcf[i*sstep].y;
 331         }
 332         else
 333         {
 334             x = srcd[i*sstep].x;
 335             y = srcd[i*sstep].y;
 336         }
 337
 338         x0 = x = (x - cx)*ifx;
 339         y0 = y = (y - cy)*ify;
 340
 341         // compensate distortion iteratively
 342         for( j = 0; j < iters; j++ )
 343         {
 344             double r2 = x*x + y*y;
 345             double icdist = 1./(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
 346             double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
 347             double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
 348             x = (x0 - deltaX)*icdist;
 349             y = (y0 - deltaY)*icdist;
 350         }
 351
 352         double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2];
 353         double yy = RR[1][0]*x + RR[1][1]*y + RR[1][2];
 354         double ww = 1./(RR[2][0]*x + RR[2][1]*y + RR[2][2]);
 355         x = xx*ww;
 356         y = yy*ww;
 357
 358         if( dtype == CV_32FC2 )
 359         {
 360             dstf[i*dstep].x = (float)x;
 361             dstf[i*dstep].y = (float)y;
 362         }
 363         else
 364         {
 365             dstd[i*dstep].x = x;
 366             dstd[i*dstep].y = y;
 367         }
 368     }
 369
 370     __END__;
 371 }
 372
 373
 374 void cv::undistortPoints( const Mat& src, Mat& dst,
 375                           const Mat& cameraMatrix, const Mat& distCoeffs,
 376                           const Mat& R, const Mat& P )
 377 {
 378     CV_Assert( src.isContinuous() && src.depth() == CV_32F &&
 379               ((src.rows == 1 && src.channels() == 2) || src.cols*src.channels() == 2));
 380
 381     dst.create(src.size(), src.type());
 382     CvMat _src = src, _dst = dst, _cameraMatrix = cameraMatrix;
 383     CvMat _R, _P, _distCoeffs, *pR=0, *pP=0, *pD=0;
 384     if( R.data )
 385         pR = &(_R = R);
 386     if( P.data )
 387         pP = &(_P = P);
 388     if( distCoeffs.data )
 389         pD = &(_distCoeffs = distCoeffs);
 390     cvUndistortPoints(&_src, &_dst, &_cameraMatrix, pD, pR, pP);
 391 }
 392
 393 void cv::undistortPoints( const Mat& src, std::vector<Point2f>& dst,
 394                           const Mat& cameraMatrix, const Mat& distCoeffs,
 395                           const Mat& R, const Mat& P )
 396 {
 397     size_t sz = src.cols*src.rows*src.channels()/2;
 398     CV_Assert( src.isContinuous() && src.depth() == CV_32F &&
 399                ((src.rows == 1 && src.channels() == 2) || src.cols*src.channels() == 2));
 400
 401     dst.resize(sz);
 402     CvMat _src = src, _dst = Mat(dst), _cameraMatrix = cameraMatrix;
 403     CvMat _R, _P, _distCoeffs, *pR=0, *pP=0, *pD=0;
 404     if( R.data )
 405         pR = &(_R = R);
 406     if( P.data )
 407         pP = &(_P = P);
 408     if( distCoeffs.data )
 409         pD = &(_distCoeffs = distCoeffs);
 410     cvUndistortPoints(&_src, &_dst, &_cameraMatrix, pD, pR, pP);
 411 }
 412
 413 /*  End of file  */