git.maemo.org Git - opencv/blob - cv/src/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 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "_cv.h"
  43
  44 static CvStatus
  45 icvUnDistort_8u_CnR( const uchar* src, int srcstep,
  46                      uchar* dst, int dststep, CvSize size,
  47                      const float* intrinsic_matrix,
  48                      const float* dist_coeffs, int cn )
  49 {
  50     int u, v, i;
  51     float u0 = intrinsic_matrix[2], v0 = intrinsic_matrix[5];
  52     float fx = intrinsic_matrix[0], fy = intrinsic_matrix[4];
  53     float _fx = 1.f/fx, _fy = 1.f/fy;
  54     float k1 = dist_coeffs[0], k2 = dist_coeffs[1];
  55     float p1 = dist_coeffs[2], p2 = dist_coeffs[3];
  56
  57     srcstep /= sizeof(src[0]);
  58     dststep /= sizeof(dst[0]);
  59
  60     for( v = 0; v < size.height; v++, dst += dststep )
  61     {
  62         float y = (v - v0)*_fy;
  63         float y2 = y*y;
  64         float ky = 1 + (k1 + k2*y2)*y2;
  65         float k2y = 2*k2*y2;
  66         float _2p1y = 2*p1*y;
  67         float _3p1y2 = 3*p1*y2;
  68         float p2y2 = p2*y2;
  69
  70         for( u = 0; u < size.width; u++ )
  71         {
  72             float x = (u - u0)*_fx;
  73             float x2 = x*x;
  74             float kx = (k1 + k2*x2)*x2;
  75             float d = kx + ky + k2y*x2;
  76             float _u = fx*(x*(d + _2p1y) + p2y2 + (3*p2)*x2) + u0;
  77             float _v = fy*(y*(d + (2*p2)*x) + _3p1y2 + p1*x2) + v0;
  78             int iu = cvRound(_u*(1 << ICV_WARP_SHIFT));
  79             int iv = cvRound(_v*(1 << ICV_WARP_SHIFT));
  80             int ifx = iu & ICV_WARP_MASK;
  81             int ify = iv & ICV_WARP_MASK;
  82             iu >>= ICV_WARP_SHIFT;
  83             iv >>= ICV_WARP_SHIFT;
  84
  85             float a0 = icvLinearCoeffs[ifx*2];
  86             float a1 = icvLinearCoeffs[ifx*2 + 1];
  87             float b0 = icvLinearCoeffs[ify*2];
  88             float b1 = icvLinearCoeffs[ify*2 + 1];
  89
  90             if( (unsigned)iv < (unsigned)(size.height - 1) &&
  91                 (unsigned)iu < (unsigned)(size.width - 1) )
  92             {
  93                 const uchar* ptr = src + iv*srcstep + iu*cn;
  94                 for( i = 0; i < cn; i++ )
  95                 {
  96                     float t0 = a1*CV_8TO32F(ptr[i]) + a0*CV_8TO32F(ptr[i+cn]);
  97                     float t1 = a1*CV_8TO32F(ptr[i+srcstep]) + a0*CV_8TO32F(ptr[i + srcstep + cn]);
  98                     dst[u*cn + i] = (uchar)cvRound(b1*t0 + b0*t1);
  99                 }
 100             }
 101             else
 102             {
 103                 for( i = 0; i < cn; i++ )
 104                     dst[u*cn + i] = 0;
 105             }
 106         }
 107     }
 108
 109     return CV_OK;
 110 }
 111
 112
 113 icvUndistortGetSize_t icvUndistortGetSize_p = 0;
 114 icvCreateMapCameraUndistort_32f_C1R_t icvCreateMapCameraUndistort_32f_C1R_p = 0;
 115 icvUndistortRadial_8u_C1R_t icvUndistortRadial_8u_C1R_p = 0;
 116 icvUndistortRadial_8u_C3R_t icvUndistortRadial_8u_C3R_p = 0;
 117
 118 typedef CvStatus (CV_STDCALL * CvUndistortRadialIPPFunc)
 119     ( const void* pSrc, int srcStep, void* pDst, int dstStep, CvSize roiSize,
 120       float fx, float fy, float cx, float cy, float k1, float k2, uchar *pBuffer );
 121
 122 CV_IMPL void
 123 cvUndistort2( const CvArr* _src, CvArr* _dst, const CvMat* A, const CvMat* dist_coeffs )
 124 {
 125     static int inittab = 0;
 126     uchar* buffer = 0;
 127
 128     CV_FUNCNAME( "cvUndistort2" );
 129
 130     __BEGIN__;
 131
 132     float a[9], k[4];
 133     int coi1 = 0, coi2 = 0;
 134     CvMat srcstub, *src = (CvMat*)_src;
 135     CvMat dststub, *dst = (CvMat*)_dst;
 136     CvMat _a = cvMat( 3, 3, CV_32F, a ), _k;
 137     int cn, src_step, dst_step;
 138     CvSize size;
 139
 140     if( !inittab )
 141     {
 142         icvInitLinearCoeffTab();
 143         icvInitCubicCoeffTab();
 144         inittab = 1;
 145     }
 146
 147     CV_CALL( src = cvGetMat( src, &srcstub, &coi1 ));
 148     CV_CALL( dst = cvGetMat( dst, &dststub, &coi2 ));
 149
 150     if( coi1 != 0 || coi2 != 0 )
 151         CV_ERROR( CV_BadCOI, "The function does not support COI" );
 152
 153     if( CV_MAT_DEPTH(src->type) != CV_8U )
 154         CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit images are supported" );
 155
 156     if( src->data.ptr == dst->data.ptr )
 157         CV_ERROR( CV_StsNotImplemented, "In-place undistortion is not implemented" );
 158
 159     if( !CV_ARE_TYPES_EQ( src, dst ))
 160         CV_ERROR( CV_StsUnmatchedFormats, "" );
 161
 162     if( !CV_ARE_SIZES_EQ( src, dst ))
 163         CV_ERROR( CV_StsUnmatchedSizes, "" );
 164
 165     if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3  ||
 166         CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1 )
 167         CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" );
 168
 169     if( !CV_IS_MAT(dist_coeffs) || dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
 170         dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 ||
 171         CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
 172         CV_MAT_DEPTH(dist_coeffs->type) != CV_32F )
 173         CV_ERROR( CV_StsBadArg,
 174             "Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
 175
 176     cvConvert( A, &_a );
 177     _k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
 178                 CV_MAKETYPE(CV_32F, CV_MAT_CN(dist_coeffs->type)), k );
 179     cvConvert( dist_coeffs, &_k );
 180
 181     cn = CV_MAT_CN(src->type);
 182     size = cvGetMatSize(src);
 183     src_step = src->step ? src->step : CV_STUB_STEP;
 184     dst_step = dst->step ? dst->step : CV_STUB_STEP;
 185
 186     if( fabs((double)k[2]) < 1e-5 && fabs((double)k[3]) < 1e-5 && icvUndistortGetSize_p )
 187     {
 188         int buf_size = 0;
 189         CvUndistortRadialIPPFunc func =
 190             cn == 1 ? (CvUndistortRadialIPPFunc)icvUndistortRadial_8u_C1R_p :
 191                       (CvUndistortRadialIPPFunc)icvUndistortRadial_8u_C3R_p;
 192
 193         if( func && icvUndistortGetSize_p( size, &buf_size ) >= 0 && buf_size > 0 )
 194         {
 195             CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
 196             if( func( src->data.ptr, src_step, dst->data.ptr,
 197                       dst_step, size, a[0], a[4],
 198                       a[2], a[5], k[0], k[1], buffer ) >= 0 )
 199                 EXIT;
 200         }
 201     }
 202
 203     icvUnDistort_8u_CnR( src->data.ptr, src_step,
 204         dst->data.ptr, dst_step, size, a, k, cn );
 205
 206     __END__;
 207
 208     cvFree( &buffer );
 209 }
 210
 211
 212 CV_IMPL void
 213 cvInitUndistortMap( const CvMat* A, const CvMat* dist_coeffs,
 214                     CvArr* mapxarr, CvArr* mapyarr )
 215 {
 216     uchar* buffer = 0;
 217
 218     CV_FUNCNAME( "cvInitUndistortMap" );
 219
 220     __BEGIN__;
 221
 222     float a[9], k[4];
 223     int coi1 = 0, coi2 = 0;
 224     CvMat mapxstub, *_mapx = (CvMat*)mapxarr;
 225     CvMat mapystub, *_mapy = (CvMat*)mapyarr;
 226     float *mapx, *mapy;
 227     CvMat _a = cvMat( 3, 3, CV_32F, a ), _k;
 228     int mapxstep, mapystep;
 229     int u, v;
 230     float u0, v0, fx, fy, _fx, _fy, k1, k2, p1, p2;
 231     CvSize size;
 232
 233     CV_CALL( _mapx = cvGetMat( _mapx, &mapxstub, &coi1 ));
 234     CV_CALL( _mapy = cvGetMat( _mapy, &mapystub, &coi2 ));
 235
 236     if( coi1 != 0 || coi2 != 0 )
 237         CV_ERROR( CV_BadCOI, "The function does not support COI" );
 238
 239     if( CV_MAT_TYPE(_mapx->type) != CV_32FC1 )
 240         CV_ERROR( CV_StsUnsupportedFormat, "Both maps must have 32fC1 type" );
 241
 242     if( !CV_ARE_TYPES_EQ( _mapx, _mapy ))
 243         CV_ERROR( CV_StsUnmatchedFormats, "" );
 244
 245     if( !CV_ARE_SIZES_EQ( _mapx, _mapy ))
 246         CV_ERROR( CV_StsUnmatchedSizes, "" );
 247
 248     if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3  ||
 249         CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1 )
 250         CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" );
 251
 252     if( !CV_IS_MAT(dist_coeffs) || dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
 253         dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 ||
 254         CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
 255         CV_MAT_DEPTH(dist_coeffs->type) != CV_32F )
 256         CV_ERROR( CV_StsBadArg,
 257             "Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
 258
 259     cvConvert( A, &_a );
 260     _k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
 261                 CV_MAKETYPE(CV_32F, CV_MAT_CN(dist_coeffs->type)), k );
 262     cvConvert( dist_coeffs, &_k );
 263
 264     u0 = a[2]; v0 = a[5];
 265     fx = a[0]; fy = a[4];
 266     _fx = 1.f/fx; _fy = 1.f/fy;
 267     k1 = k[0]; k2 = k[1];
 268     p1 = k[2]; p2 = k[3];
 269
 270     mapxstep = _mapx->step ? _mapx->step : CV_STUB_STEP;
 271     mapystep = _mapy->step ? _mapy->step : CV_STUB_STEP;
 272     mapx = _mapx->data.fl;
 273     mapy = _mapy->data.fl;
 274
 275     size = cvGetMatSize(_mapx);
 276
 277     /*if( icvUndistortGetSize_p && icvCreateMapCameraUndistort_32f_C1R_p )
 278     {
 279         int buf_size = 0;
 280         if( icvUndistortGetSize_p( size, &buf_size ) && buf_size > 0 )
 281         {
 282             CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
 283             if( icvCreateMapCameraUndistort_32f_C1R_p(
 284                 mapx, mapxstep, mapy, mapystep, size,
 285                 a[0], a[4], a[2], a[5], k[0], k[1], k[2], k[3], buffer ) >= 0 )
 286                 EXIT;
 287         }
 288     }*/
 289
 290     mapxstep /= sizeof(mapx[0]);
 291     mapystep /= sizeof(mapy[0]);
 292
 293     for( v = 0; v < size.height; v++, mapx += mapxstep, mapy += mapystep )
 294     {
 295         float y = (v - v0)*_fy;
 296         float y2 = y*y;
 297         float _2p1y = 2*p1*y;
 298         float _3p1y2 = 3*p1*y2;
 299         float p2y2 = p2*y2;
 300
 301         for( u = 0; u < size.width; u++ )
 302         {
 303             float x = (u - u0)*_fx;
 304             float x2 = x*x;
 305             float r2 = x2 + y2;
 306             float d = 1 + (k1 + k2*r2)*r2;
 307             float _u = fx*(x*(d + _2p1y) + p2y2 + (3*p2)*x2) + u0;
 308             float _v = fy*(y*(d + (2*p2)*x) + _3p1y2 + p1*x2) + v0;
 309             mapx[u] = _u;
 310             mapy[u] = _v;
 311         }
 312     }
 313
 314     __END__;
 315
 316     cvFree( &buffer );
 317 }
 318
 319 /*  End of file  */