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 x0 = (size.width-1)*0.5f, y0 = (size.height-1)*0.5f; 53 float fx = intrinsic_matrix[0], fy = intrinsic_matrix[4]; 54 float ifx = 1.f/fx, ify = 1.f/fy; 55 float k1 = dist_coeffs[0], k2 = dist_coeffs[1], k3 = dist_coeffs[4]; 56 float p1 = dist_coeffs[2], p2 = dist_coeffs[3]; 57 58 srcstep /= sizeof(src[0]); 59 dststep /= sizeof(dst[0]); 60 61 for( v = 0; v < size.height; v++, dst += dststep ) 62 { 63 float y = (v - v0)*ify, y2 = y*y; 64 65 for( u = 0; u < size.width; u++ ) 66 { 67 float x = (u - u0)*ifx, x2 = x*x, r2 = x2 + y2, _2xy = 2*x*y; 68 float kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2; 69 float _x = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + x0; 70 float _y = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + y0; 71 int ix = cvFloor(_x), iy = cvFloor(_y); 72 73 if( (unsigned)iy < (unsigned)(size.height - 1) && 74 (unsigned)ix < (unsigned)(size.width - 1) ) 75 { 76 const uchar* ptr = src + iy*srcstep + ix*cn; 77 _x -= ix; _y -= iy; 78 for( i = 0; i < cn; i++ ) 79 { 80 float t0 = CV_8TO32F(ptr[i]), t1 = CV_8TO32F(ptr[i+srcstep]); 81 t0 += _x*(CV_8TO32F(ptr[i+cn]) - t0); 82 t1 += _x*(CV_8TO32F(ptr[i + srcstep + cn]) - t1); 83 dst[u*cn + i] = (uchar)cvRound(t0 + _y*(t1 - t0)); 84 } 85 } 86 else 87 { 88 for( i = 0; i < cn; i++ ) 89 dst[u*cn + i] = 0; 90 } 91 92 } 93 } 94 95 return CV_OK; 96 } 97 98 99 icvUndistortGetSize_t icvUndistortGetSize_p = 0; 100 icvCreateMapCameraUndistort_32f_C1R_t icvCreateMapCameraUndistort_32f_C1R_p = 0; 101 icvUndistortRadial_8u_C1R_t icvUndistortRadial_8u_C1R_p = 0; 102 icvUndistortRadial_8u_C3R_t icvUndistortRadial_8u_C3R_p = 0; 103 104 typedef CvStatus (CV_STDCALL * CvUndistortRadialIPPFunc) 105 ( const void* pSrc, int srcStep, void* pDst, int dstStep, CvSize roiSize, 106 float fx, float fy, float cx, float cy, float k1, float k2, uchar *pBuffer ); 107 108 CV_IMPL void 109 cvUndistort2( const CvArr* _src, CvArr* _dst, const CvMat* A, const CvMat* dist_coeffs ) 110 { 111 static int inittab = 0; 112 113 CV_FUNCNAME( "cvUndistort2" ); 114 115 __BEGIN__; 116 117 float a[9], k[5]={0,0,0,0,0}; 118 int coi1 = 0, coi2 = 0; 119 CvMat srcstub, *src = (CvMat*)_src; 120 CvMat dststub, *dst = (CvMat*)_dst; 121 CvMat _a = cvMat( 3, 3, CV_32F, a ), _k; 122 int cn, src_step, dst_step; 123 CvSize size; 124 125 if( !inittab ) 126 { 127 icvInitLinearCoeffTab(); 128 icvInitCubicCoeffTab(); 129 inittab = 1; 130 } 131 132 CV_CALL( src = cvGetMat( src, &srcstub, &coi1 )); 133 CV_CALL( dst = cvGetMat( dst, &dststub, &coi2 )); 134 135 if( coi1 != 0 || coi2 != 0 ) 136 CV_ERROR( CV_BadCOI, "The function does not support COI" ); 137 138 if( CV_MAT_DEPTH(src->type) != CV_8U ) 139 CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit images are supported" ); 140 141 if( src->data.ptr == dst->data.ptr ) 142 CV_ERROR( CV_StsNotImplemented, "In-place undistortion is not implemented" ); 143 144 if( !CV_ARE_TYPES_EQ( src, dst )) 145 CV_ERROR( CV_StsUnmatchedFormats, "" ); 146 147 if( !CV_ARE_SIZES_EQ( src, dst )) 148 CV_ERROR( CV_StsUnmatchedSizes, "" ); 149 150 if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 || 151 (CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1) ) 152 CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" ); 153 154 if( !CV_IS_MAT(dist_coeffs) || (dist_coeffs->rows != 1 && dist_coeffs->cols != 1) || 155 (dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 && 156 dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 5) || 157 (CV_MAT_DEPTH(dist_coeffs->type) != CV_64F && 158 CV_MAT_DEPTH(dist_coeffs->type) != CV_32F) ) 159 CV_ERROR( CV_StsBadArg, 160 "Distortion coefficients must be 1x4, 4x1, 1x5 or 5x1 floating-point vector" ); 161 162 cvConvert( A, &_a ); 163 _k = cvMat( dist_coeffs->rows, dist_coeffs->cols, 164 CV_MAKETYPE(CV_32F, CV_MAT_CN(dist_coeffs->type)), k ); 165 cvConvert( dist_coeffs, &_k ); 166 167 cn = CV_MAT_CN(src->type); 168 size = cvGetMatSize(src); 169 src_step = src->step ? src->step : CV_STUB_STEP; 170 dst_step = dst->step ? dst->step : CV_STUB_STEP; 171 172 icvUnDistort_8u_CnR( src->data.ptr, src_step, 173 dst->data.ptr, dst_step, size, a, k, cn ); 174 175 __END__; 176 } 177 178 179 CV_IMPL void 180 cvInitUndistortMap( const CvMat* A, const CvMat* dist_coeffs, 181 CvArr* mapxarr, CvArr* mapyarr ) 182 { 183 CV_FUNCNAME( "cvInitUndistortMap" ); 184 185 __BEGIN__; 186 187 float a[9], k[5]={0,0,0,0,0}; 188 int coi1 = 0, coi2 = 0; 189 CvMat mapxstub, *_mapx = (CvMat*)mapxarr; 190 CvMat mapystub, *_mapy = (CvMat*)mapyarr; 191 CvMat _a = cvMat( 3, 3, CV_32F, a ), _k; 192 int u, v; 193 float u0, v0, fx, fy, ifx, ify, x0, y0, k1, k2, k3, p1, p2; 194 CvSize size; 195 196 CV_CALL( _mapx = cvGetMat( _mapx, &mapxstub, &coi1 )); 197 CV_CALL( _mapy = cvGetMat( _mapy, &mapystub, &coi2 )); 198 199 if( coi1 != 0 || coi2 != 0 ) 200 CV_ERROR( CV_BadCOI, "The function does not support COI" ); 201 202 if( CV_MAT_TYPE(_mapx->type) != CV_32FC1 ) 203 CV_ERROR( CV_StsUnsupportedFormat, "Both maps must have 32fC1 type" ); 204 205 if( !CV_ARE_TYPES_EQ( _mapx, _mapy )) 206 CV_ERROR( CV_StsUnmatchedFormats, "" ); 207 208 if( !CV_ARE_SIZES_EQ( _mapx, _mapy )) 209 CV_ERROR( CV_StsUnmatchedSizes, "" ); 210 211 size = cvGetMatSize(_mapx); 212 213 if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 || 214 (CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1) ) 215 CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" ); 216 217 if( !CV_IS_MAT(dist_coeffs) || (dist_coeffs->rows != 1 && dist_coeffs->cols != 1) || 218 (dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 && 219 dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 5) || 220 (CV_MAT_DEPTH(dist_coeffs->type) != CV_64F && 221 CV_MAT_DEPTH(dist_coeffs->type) != CV_32F) ) 222 CV_ERROR( CV_StsBadArg, 223 "Distortion coefficients must be 1x4, 4x1, 1x5 or 5x1 floating-point vector" ); 224 225 cvConvert( A, &_a ); 226 _k = cvMat( dist_coeffs->rows, dist_coeffs->cols, 227 CV_MAKETYPE(CV_32F, CV_MAT_CN(dist_coeffs->type)), k ); 228 cvConvert( dist_coeffs, &_k ); 229 230 u0 = a[2]; v0 = a[5]; 231 fx = a[0]; fy = a[4]; 232 ifx = 1.f/fx; ify = 1.f/fy; 233 k1 = k[0]; k2 = k[1]; k3 = k[4]; 234 p1 = k[2]; p2 = k[3]; 235 x0 = (size.width-1)*0.5f; 236 y0 = (size.height-1)*0.5f; 237 238 for( v = 0; v < size.height; v++ ) 239 { 240 float* mapx = (float*)(_mapx->data.ptr + _mapx->step*v); 241 float* mapy = (float*)(_mapy->data.ptr + _mapy->step*v); 242 float y = (v - v0)*ify, y2 = y*y; 243 244 for( u = 0; u < size.width; u++ ) 245 { 246 float x = (u - u0)*ifx, x2 = x*x, r2 = x2 + y2, _2xy = 2*x*y; 247 double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2; 248 double _x = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + x0; 249 double _y = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + y0; 250 mapx[u] = (float)_x; 251 mapy[u] = (float)_y; 252 } 253 } 254 255 __END__; 256 } 257 258 259 void 260 cvInitUndistortRectifyMap( const CvMat* A, const CvMat* distCoeffs, 261 const CvMat *R, const CvMat* Ar, CvArr* mapxarr, CvArr* mapyarr ) 262 { 263 CV_FUNCNAME( "cvInitUndistortMap" ); 264 265 __BEGIN__; 266 267 double a[9], ar[9], r[9], ir[9], k[5]={0,0,0,0,0}; 268 int coi1 = 0, coi2 = 0; 269 CvMat mapxstub, *_mapx = (CvMat*)mapxarr; 270 CvMat mapystub, *_mapy = (CvMat*)mapyarr; 271 CvMat _a = cvMat( 3, 3, CV_64F, a ); 272 CvMat _k = cvMat( 4, 1, CV_64F, k ); 273 CvMat _ar = cvMat( 3, 3, CV_64F, ar ); 274 CvMat _r = cvMat( 3, 3, CV_64F, r ); 275 CvMat _ir = cvMat( 3, 3, CV_64F, ir ); 276 int i, j; 277 double fx, fy, u0, v0, k1, k2, k3, p1, p2; 278 CvSize size; 279 280 CV_CALL( _mapx = cvGetMat( _mapx, &mapxstub, &coi1 )); 281 CV_CALL( _mapy = cvGetMat( _mapy, &mapystub, &coi2 )); 282 283 if( coi1 != 0 || coi2 != 0 ) 284 CV_ERROR( CV_BadCOI, "The function does not support COI" ); 285 286 if( CV_MAT_TYPE(_mapx->type) != CV_32FC1 ) 287 CV_ERROR( CV_StsUnsupportedFormat, "Both maps must have 32fC1 type" ); 288 289 if( !CV_ARE_TYPES_EQ( _mapx, _mapy )) 290 CV_ERROR( CV_StsUnmatchedFormats, "" ); 291 292 if( !CV_ARE_SIZES_EQ( _mapx, _mapy )) 293 CV_ERROR( CV_StsUnmatchedSizes, "" ); 294 295 if( A ) 296 { 297 if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 || 298 (CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1) ) 299 CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" ); 300 cvConvert( A, &_a ); 301 } 302 else 303 cvSetIdentity( &_a ); 304 305 if( Ar ) 306 { 307 CvMat Ar33; 308 if( !CV_IS_MAT(Ar) || Ar->rows != 3 || (Ar->cols != 3 && Ar->cols != 4) || 309 (CV_MAT_TYPE(Ar->type) != CV_32FC1 && CV_MAT_TYPE(Ar->type) != CV_64FC1) ) 310 CV_ERROR( CV_StsBadArg, "The new intrinsic matrix must be a valid 3x3 floating-point matrix" ); 311 cvGetCols( Ar, &Ar33, 0, 3 ); 312 cvConvert( &Ar33, &_ar ); 313 } 314 else 315 cvSetIdentity( &_ar ); 316 317 if( !CV_IS_MAT(R) || R->rows != 3 || R->cols != 3 || 318 (CV_MAT_TYPE(R->type) != CV_32FC1 && CV_MAT_TYPE(R->type) != CV_64FC1) ) 319 CV_ERROR( CV_StsBadArg, "Rotaion/homography matrix must be a valid 3x3 floating-point matrix" ); 320 321 if( distCoeffs ) 322 { 323 CV_ASSERT( CV_IS_MAT(distCoeffs) && 324 (distCoeffs->rows == 1 || distCoeffs->cols == 1) && 325 (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 4 || 326 distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 5) && 327 (CV_MAT_DEPTH(distCoeffs->type) == CV_64F || 328 CV_MAT_DEPTH(distCoeffs->type) == CV_32F) ); 329 _k = cvMat( distCoeffs->rows, distCoeffs->cols, 330 CV_MAKETYPE(CV_64F, CV_MAT_CN(distCoeffs->type)), k ); 331 cvConvert( distCoeffs, &_k ); 332 } 333 else 334 cvZero( &_k ); 335 336 cvConvert( R, &_r ); // rectification matrix 337 cvMatMul( &_ar, &_r, &_r ); // Ar*R 338 cvInvert( &_r, &_ir ); // inverse: R^-1*Ar^-1 339 340 u0 = a[2]; v0 = a[5]; 341 fx = a[0]; fy = a[4]; 342 k1 = k[0]; k2 = k[1]; k3 = k[4]; 343 p1 = k[2]; p2 = k[3]; 344 345 size = cvGetMatSize(_mapx); 346 347 for( i = 0; i < size.height; i++ ) 348 { 349 float* mapx = (float*)(_mapx->data.ptr + _mapx->step*i); 350 float* mapy = (float*)(_mapy->data.ptr + _mapy->step*i); 351 double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8]; 352 353 for( j = 0; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] ) 354 { 355 double w = 1./_w, x = _x*w, y = _y*w; 356 double x2 = x*x, y2 = y*y; 357 double r2 = x2 + y2, _2xy = 2*x*y; 358 double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2; 359 double u = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + u0; 360 double v = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + v0; 361 mapx[j] = (float)u; 362 mapy[j] = (float)v; 363 } 364 } 365 366 __END__; 367 } 368 369 370 void 371 cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix, 372 const CvMat* _distCoeffs, 373 const CvMat* _R, const CvMat* _P ) 374 { 375 CV_FUNCNAME( "cvUndistortPoints" ); 376 377 __BEGIN__; 378 379 double A[3][3], RR[3][3], k[5]={0,0,0,0,0}, fx, fy, ifx, ify, cx, cy; 380 CvMat _A=cvMat(3, 3, CV_64F, A), _Dk; 381 CvMat _RR=cvMat(3, 3, CV_64F, RR); 382 const CvPoint2D32f* srcf; 383 const CvPoint2D64f* srcd; 384 CvPoint2D32f* dstf; 385 CvPoint2D64f* dstd; 386 int stype, dtype; 387 int sstep, dstep; 388 int i, j, n; 389 390 CV_ASSERT( CV_IS_MAT(_src) && CV_IS_MAT(_dst) && 391 (_src->rows == 1 || _src->cols == 1) && 392 (_dst->rows == 1 || _dst->cols == 1) && 393 CV_ARE_SIZES_EQ(_src, _dst) && 394 (CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) && 395 (CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2)); 396 397 CV_ASSERT( CV_IS_MAT(_cameraMatrix) && CV_IS_MAT(_distCoeffs) && 398 _cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 && 399 (_distCoeffs->rows == 1 || _distCoeffs->cols == 1) && 400 (_distCoeffs->rows*_distCoeffs->cols == 4 || 401 _distCoeffs->rows*_distCoeffs->cols == 5) ); 402 _Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols, 403 CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k); 404 cvConvert( _cameraMatrix, &_A ); 405 cvConvert( _distCoeffs, &_Dk ); 406 407 if( _R ) 408 { 409 CV_ASSERT( CV_IS_MAT(_R) && _R->rows == 3 && _R->cols == 3 ); 410 cvConvert( _R, &_RR ); 411 } 412 else 413 cvSetIdentity(&_RR); 414 415 if( _P ) 416 { 417 double PP[3][3]; 418 CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP); 419 CV_ASSERT( CV_IS_MAT(_P) && _P->rows == 3 && (_P->cols == 3 || _P->cols == 4)); 420 cvConvert( cvGetCols(_P, &_P3x3, 0, 3), &_PP ); 421 cvMatMul( &_PP, &_RR, &_RR ); 422 } 423 424 srcf = (const CvPoint2D32f*)_src->data.ptr; 425 srcd = (const CvPoint2D64f*)_src->data.ptr; 426 dstf = (CvPoint2D32f*)_dst->data.ptr; 427 dstd = (CvPoint2D64f*)_dst->data.ptr; 428 stype = CV_MAT_TYPE(_src->type); 429 dtype = CV_MAT_TYPE(_dst->type); 430 sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype); 431 dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype); 432 433 n = _src->rows + _src->cols - 1; 434 435 fx = A[0][0]; 436 fy = A[1][1]; 437 ifx = 1./fx; 438 ify = 1./fy; 439 cx = A[0][2]; 440 cy = A[1][2]; 441 442 for( i = 0; i < n; i++ ) 443 { 444 double x, y, x0, y0; 445 if( stype == CV_32FC2 ) 446 { 447 x = srcf[i*sstep].x; 448 y = srcf[i*sstep].y; 449 } 450 else 451 { 452 x = srcd[i*sstep].x; 453 y = srcd[i*sstep].y; 454 } 455 456 x0 = x = (x - cx)*ifx; 457 y0 = y = (y - cy)*ify; 458 459 // compensate distortion iteratively 460 for( j = 0; j < 5; j++ ) 461 { 462 double r2 = x*x + y*y; 463 double icdist = 1./(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2); 464 double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x); 465 double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y; 466 x = (x0 - deltaX)*icdist; 467 y = (y0 - deltaY)*icdist; 468 } 469 470 double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2]; 471 double yy = RR[1][0]*x + RR[1][1]*y + RR[1][2]; 472 double ww = 1./(RR[2][0]*x + RR[2][1]*y + RR[2][2]); 473 x = xx*ww; 474 y = yy*ww; 475 476 if( dtype == CV_32FC2 ) 477 { 478 dstf[i*dstep].x = (float)x; 479 dstf[i*dstep].y = (float)y; 480 } 481 else 482 { 483 dstd[i*dstep].x = x; 484 dstd[i*dstep].y = y; 485 } 486 } 487 488 __END__; 489 } 490 491 /* End of file */ 492
