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 newCameraMatrix.ptr<double>()[2] = (imgsize.width-1)*0.5; 57 newCameraMatrix.ptr<double>()[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.empty() ) 86 Ar = Mat_<double>(newCameraMatrix); 87 else 88 Ar = getDefaultNewCameraMatrix( A, size, true ); 89 90 if( !matR.empty() ) 91 R = Mat_<double>(matR); 92 93 if( !distCoeffs.empty() ) 94 distCoeffs = Mat_<double>(distCoeffs); 95 else 96 { 97 distCoeffs.create(12, 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 distCoeffs.size() == Size(1, 12) || distCoeffs.size() == Size(12, 1)); 113 114 if( distCoeffs.rows != 1 && !distCoeffs.isContinuous() ) 115 distCoeffs = distCoeffs.t(); 116 117 const double* const distPtr = distCoeffs.ptr<double>(); 118 double k1 = distPtr[0]; 119 double k2 = distPtr[1]; 120 double p1 = distPtr[2]; 121 double p2 = distPtr[3]; 122 double k3 = distCoeffs.cols + distCoeffs.rows - 1 >= 5 ? distPtr[4] : 0.; 123 double k4 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? distPtr[5] : 0.; 124 double k5 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? distPtr[6] : 0.; 125 double k6 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? distPtr[7] : 0.; 126 double s1 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? distPtr[8] : 0.; 127 double s2 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? distPtr[9] : 0.; 128 double s3 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? distPtr[10] : 0.; 129 double s4 = distCoeffs.cols + distCoeffs.rows - 1 >= 12 ? distPtr[11] : 0.; 130 131 for( int i = 0; i < size.height; i++ ) 132 { 133 float* m1f = map1.ptr<float>(i); 134 float* m2f = map2.empty() ? 0 : map2.ptr<float>(i); 135 short* m1 = (short*)m1f; 136 ushort* m2 = (ushort*)m2f; 137 double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8]; 138 139 for( int j = 0; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] ) 140 { 141 double w = 1./_w, x = _x*w, y = _y*w; 142 double x2 = x*x, y2 = y*y; 143 double r2 = x2 + y2, _2xy = 2*x*y; 144 double kr = (1 + ((k3*r2 + k2)*r2 + k1)*r2)/(1 + ((k6*r2 + k5)*r2 + k4)*r2); 145 double u = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2) + s1*r2+s2*r2*r2) + u0; 146 double v = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy + s3*r2+s4*r2*r2) + v0; 147 if( m1type == CV_16SC2 ) 148 { 149 int iu = saturate_cast<int>(u*INTER_TAB_SIZE); 150 int iv = saturate_cast<int>(v*INTER_TAB_SIZE); 151 m1[j*2] = (short)(iu >> INTER_BITS); 152 m1[j*2+1] = (short)(iv >> INTER_BITS); 153 m2[j] = (ushort)((iv & (INTER_TAB_SIZE-1))*INTER_TAB_SIZE + (iu & (INTER_TAB_SIZE-1))); 154 } 155 else if( m1type == CV_32FC1 ) 156 { 157 m1f[j] = (float)u; 158 m2f[j] = (float)v; 159 } 160 else 161 { 162 m1f[j*2] = (float)u; 163 m1f[j*2+1] = (float)v; 164 } 165 } 166 } 167 } 168 169 170 void cv::undistort( InputArray _src, OutputArray _dst, InputArray _cameraMatrix, 171 InputArray _distCoeffs, InputArray _newCameraMatrix ) 172 { 173 Mat src = _src.getMat(), cameraMatrix = _cameraMatrix.getMat(); 174 Mat distCoeffs = _distCoeffs.getMat(), newCameraMatrix = _newCameraMatrix.getMat(); 175 176 _dst.create( src.size(), src.type() ); 177 Mat dst = _dst.getMat(); 178 179 CV_Assert( dst.data != src.data ); 180 181 int stripe_size0 = std::min(std::max(1, (1 << 12) / std::max(src.cols, 1)), src.rows); 182 Mat map1(stripe_size0, src.cols, CV_16SC2), map2(stripe_size0, src.cols, CV_16UC1); 183 184 Mat_<double> A, Ar, I = Mat_<double>::eye(3,3); 185 186 cameraMatrix.convertTo(A, CV_64F); 187 if( !distCoeffs.empty() ) 188 distCoeffs = Mat_<double>(distCoeffs); 189 else 190 { 191 distCoeffs.create(5, 1, CV_64F); 192 distCoeffs = 0.; 193 } 194 195 if( !newCameraMatrix.empty() ) 196 newCameraMatrix.convertTo(Ar, CV_64F); 197 else 198 A.copyTo(Ar); 199 200 double v0 = Ar(1, 2); 201 for( int y = 0; y < src.rows; y += stripe_size0 ) 202 { 203 int stripe_size = std::min( stripe_size0, src.rows - y ); 204 Ar(1, 2) = v0 - y; 205 Mat map1_part = map1.rowRange(0, stripe_size), 206 map2_part = map2.rowRange(0, stripe_size), 207 dst_part = dst.rowRange(y, y + stripe_size); 208 209 initUndistortRectifyMap( A, distCoeffs, I, Ar, Size(src.cols, stripe_size), 210 map1_part.type(), map1_part, map2_part ); 211 remap( src, dst_part, map1_part, map2_part, INTER_LINEAR, BORDER_CONSTANT ); 212 } 213 } 214 215 216 CV_IMPL void 217 cvUndistort2( const CvArr* srcarr, CvArr* dstarr, const CvMat* Aarr, const CvMat* dist_coeffs, const CvMat* newAarr ) 218 { 219 cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr), dst0 = dst; 220 cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs), newA; 221 if( newAarr ) 222 newA = cv::cvarrToMat(newAarr); 223 224 CV_Assert( src.size() == dst.size() && src.type() == dst.type() ); 225 cv::undistort( src, dst, A, distCoeffs, newA ); 226 } 227 228 229 CV_IMPL void cvInitUndistortMap( const CvMat* Aarr, const CvMat* dist_coeffs, 230 CvArr* mapxarr, CvArr* mapyarr ) 231 { 232 cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs); 233 cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0; 234 235 if( mapyarr ) 236 mapy0 = mapy = cv::cvarrToMat(mapyarr); 237 238 cv::initUndistortRectifyMap( A, distCoeffs, cv::Mat(), A, 239 mapx.size(), mapx.type(), mapx, mapy ); 240 CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data ); 241 } 242 243 void 244 cvInitUndistortRectifyMap( const CvMat* Aarr, const CvMat* dist_coeffs, 245 const CvMat *Rarr, const CvMat* ArArr, CvArr* mapxarr, CvArr* mapyarr ) 246 { 247 cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs, R, Ar; 248 cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0; 249 250 if( mapyarr ) 251 mapy0 = mapy = cv::cvarrToMat(mapyarr); 252 253 if( dist_coeffs ) 254 distCoeffs = cv::cvarrToMat(dist_coeffs); 255 if( Rarr ) 256 R = cv::cvarrToMat(Rarr); 257 if( ArArr ) 258 Ar = cv::cvarrToMat(ArArr); 259 260 cv::initUndistortRectifyMap( A, distCoeffs, R, Ar, mapx.size(), mapx.type(), mapx, mapy ); 261 CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data ); 262 } 263 264 265 void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix, 266 const CvMat* _distCoeffs, 267 const CvMat* matR, const CvMat* matP ) 268 { 269 double A[3][3], RR[3][3], k[12]={0,0,0,0,0,0,0,0,0,0,0}, fx, fy, ifx, ify, cx, cy; 270 CvMat matA=cvMat(3, 3, CV_64F, A), _Dk; 271 CvMat _RR=cvMat(3, 3, CV_64F, RR); 272 const CvPoint2D32f* srcf; 273 const CvPoint2D64f* srcd; 274 CvPoint2D32f* dstf; 275 CvPoint2D64f* dstd; 276 int stype, dtype; 277 int sstep, dstep; 278 int i, j, n, iters = 1; 279 280 CV_Assert( CV_IS_MAT(_src) && CV_IS_MAT(_dst) && 281 (_src->rows == 1 || _src->cols == 1) && 282 (_dst->rows == 1 || _dst->cols == 1) && 283 _src->cols + _src->rows - 1 == _dst->rows + _dst->cols - 1 && 284 (CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) && 285 (CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2)); 286 287 CV_Assert( CV_IS_MAT(_cameraMatrix) && 288 _cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 ); 289 290 cvConvert( _cameraMatrix, &matA ); 291 292 if( _distCoeffs ) 293 { 294 CV_Assert( CV_IS_MAT(_distCoeffs) && 295 (_distCoeffs->rows == 1 || _distCoeffs->cols == 1) && 296 (_distCoeffs->rows*_distCoeffs->cols == 4 || 297 _distCoeffs->rows*_distCoeffs->cols == 5 || 298 _distCoeffs->rows*_distCoeffs->cols == 8 || 299 _distCoeffs->rows*_distCoeffs->cols == 12)); 300 301 _Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols, 302 CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k); 303 304 cvConvert( _distCoeffs, &_Dk ); 305 iters = 5; 306 } 307 308 if( matR ) 309 { 310 CV_Assert( CV_IS_MAT(matR) && matR->rows == 3 && matR->cols == 3 ); 311 cvConvert( matR, &_RR ); 312 } 313 else 314 cvSetIdentity(&_RR); 315 316 if( matP ) 317 { 318 double PP[3][3]; 319 CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP); 320 CV_Assert( CV_IS_MAT(matP) && matP->rows == 3 && (matP->cols == 3 || matP->cols == 4)); 321 cvConvert( cvGetCols(matP, &_P3x3, 0, 3), &_PP ); 322 cvMatMul( &_PP, &_RR, &_RR ); 323 } 324 325 srcf = (const CvPoint2D32f*)_src->data.ptr; 326 srcd = (const CvPoint2D64f*)_src->data.ptr; 327 dstf = (CvPoint2D32f*)_dst->data.ptr; 328 dstd = (CvPoint2D64f*)_dst->data.ptr; 329 stype = CV_MAT_TYPE(_src->type); 330 dtype = CV_MAT_TYPE(_dst->type); 331 sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype); 332 dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype); 333 334 n = _src->rows + _src->cols - 1; 335 336 fx = A[0][0]; 337 fy = A[1][1]; 338 ifx = 1./fx; 339 ify = 1./fy; 340 cx = A[0][2]; 341 cy = A[1][2]; 342 343 for( i = 0; i < n; i++ ) 344 { 345 double x, y, x0, y0; 346 if( stype == CV_32FC2 ) 347 { 348 x = srcf[i*sstep].x; 349 y = srcf[i*sstep].y; 350 } 351 else 352 { 353 x = srcd[i*sstep].x; 354 y = srcd[i*sstep].y; 355 } 356 357 x0 = x = (x - cx)*ifx; 358 y0 = y = (y - cy)*ify; 359 360 // compensate distortion iteratively 361 for( j = 0; j < iters; j++ ) 362 { 363 double r2 = x*x + y*y; 364 double icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2); 365 double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x)+ k[8]*r2+k[9]*r2*r2; 366 double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y+ k[10]*r2+k[11]*r2*r2; 367 x = (x0 - deltaX)*icdist; 368 y = (y0 - deltaY)*icdist; 369 } 370 371 double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2]; 372 double yy = RR[1][0]*x + RR[1][1]*y + RR[1][2]; 373 double ww = 1./(RR[2][0]*x + RR[2][1]*y + RR[2][2]); 374 x = xx*ww; 375 y = yy*ww; 376 377 if( dtype == CV_32FC2 ) 378 { 379 dstf[i*dstep].x = (float)x; 380 dstf[i*dstep].y = (float)y; 381 } 382 else 383 { 384 dstd[i*dstep].x = x; 385 dstd[i*dstep].y = y; 386 } 387 } 388 } 389 390 391 void cv::undistortPoints( InputArray _src, OutputArray _dst, 392 InputArray _cameraMatrix, 393 InputArray _distCoeffs, 394 InputArray _Rmat, 395 InputArray _Pmat ) 396 { 397 Mat src = _src.getMat(), cameraMatrix = _cameraMatrix.getMat(); 398 Mat distCoeffs = _distCoeffs.getMat(), R = _Rmat.getMat(), P = _Pmat.getMat(); 399 400 CV_Assert( src.isContinuous() && (src.depth() == CV_32F || src.depth() == CV_64F) && 401 ((src.rows == 1 && src.channels() == 2) || src.cols*src.channels() == 2)); 402 403 _dst.create(src.size(), src.type(), -1, true); 404 Mat dst = _dst.getMat(); 405 406 CvMat _csrc = src, _cdst = dst, _ccameraMatrix = cameraMatrix; 407 CvMat matR, matP, _cdistCoeffs, *pR=0, *pP=0, *pD=0; 408 if( !R.empty() ) 409 pR = &(matR = R); 410 if( !P.empty() ) 411 pP = &(matP = P); 412 if( !distCoeffs.empty() ) 413 pD = &(_cdistCoeffs = distCoeffs); 414 cvUndistortPoints(&_csrc, &_cdst, &_ccameraMatrix, pD, pR, pP); 415 } 416 417 namespace cv 418 { 419 420 static Point2f mapPointSpherical(const Point2f& p, float alpha, Vec4d* J, int projType) 421 { 422 double x = p.x, y = p.y; 423 double beta = 1 + 2*alpha; 424 double v = x*x + y*y + 1, iv = 1/v; 425 double u = std::sqrt(beta*v + alpha*alpha); 426 427 double k = (u - alpha)*iv; 428 double kv = (v*beta/u - (u - alpha)*2)*iv*iv; 429 double kx = kv*x, ky = kv*y; 430 431 if( projType == PROJ_SPHERICAL_ORTHO ) 432 { 433 if(J) 434 *J = Vec4d(kx*x + k, kx*y, ky*x, ky*y + k); 435 return Point2f((float)(x*k), (float)(y*k)); 436 } 437 if( projType == PROJ_SPHERICAL_EQRECT ) 438 { 439 // equirectangular 440 double iR = 1/(alpha + 1); 441 double x1 = std::max(std::min(x*k*iR, 1.), -1.); 442 double y1 = std::max(std::min(y*k*iR, 1.), -1.); 443 444 if(J) 445 { 446 double fx1 = iR/std::sqrt(1 - x1*x1); 447 double fy1 = iR/std::sqrt(1 - y1*y1); 448 *J = Vec4d(fx1*(kx*x + k), fx1*ky*x, fy1*kx*y, fy1*(ky*y + k)); 449 } 450 return Point2f((float)asin(x1), (float)asin(y1)); 451 } 452 CV_Error(CV_StsBadArg, "Unknown projection type"); 453 return Point2f(); 454 } 455 456 457 static Point2f invMapPointSpherical(Point2f _p, float alpha, int projType) 458 { 459 static int avgiter = 0, avgn = 0; 460 461 double eps = 1e-12; 462 Vec2d p(_p.x, _p.y), q(_p.x, _p.y), err; 463 Vec4d J; 464 int i, maxiter = 5; 465 466 for( i = 0; i < maxiter; i++ ) 467 { 468 Point2f p1 = mapPointSpherical(Point2f((float)q[0], (float)q[1]), alpha, &J, projType); 469 err = Vec2d(p1.x, p1.y) - p; 470 if( err[0]*err[0] + err[1]*err[1] < eps ) 471 break; 472 473 Vec4d JtJ(J[0]*J[0] + J[2]*J[2], J[0]*J[1] + J[2]*J[3], 474 J[0]*J[1] + J[2]*J[3], J[1]*J[1] + J[3]*J[3]); 475 double d = JtJ[0]*JtJ[3] - JtJ[1]*JtJ[2]; 476 d = d ? 1./d : 0; 477 Vec4d iJtJ(JtJ[3]*d, -JtJ[1]*d, -JtJ[2]*d, JtJ[0]*d); 478 Vec2d JtErr(J[0]*err[0] + J[2]*err[1], J[1]*err[0] + J[3]*err[1]); 479 480 q -= Vec2d(iJtJ[0]*JtErr[0] + iJtJ[1]*JtErr[1], iJtJ[2]*JtErr[0] + iJtJ[3]*JtErr[1]); 481 //Matx22d J(kx*x + k, kx*y, ky*x, ky*y + k); 482 //q -= Vec2d((J.t()*J).inv()*(J.t()*err)); 483 } 484 485 if( i < maxiter ) 486 { 487 avgiter += i; 488 avgn++; 489 if( avgn == 1500 ) 490 printf("avg iters = %g\n", (double)avgiter/avgn); 491 } 492 493 return i < maxiter ? Point2f((float)q[0], (float)q[1]) : Point2f(-FLT_MAX, -FLT_MAX); 494 } 495 496 } 497 498 float cv::initWideAngleProjMap( InputArray _cameraMatrix0, InputArray _distCoeffs0, 499 Size imageSize, int destImageWidth, int m1type, 500 OutputArray _map1, OutputArray _map2, int projType, double _alpha ) 501 { 502 Mat cameraMatrix0 = _cameraMatrix0.getMat(), distCoeffs0 = _distCoeffs0.getMat(); 503 double k[12] = {0,0,0,0,0,0,0,0,0,0,0}, M[9]={0,0,0,0,0,0,0,0,0}; 504 Mat distCoeffs(distCoeffs0.rows, distCoeffs0.cols, CV_MAKETYPE(CV_64F,distCoeffs0.channels()), k); 505 Mat cameraMatrix(3,3,CV_64F,M); 506 Point2f scenter((float)cameraMatrix.at<double>(0,2), (float)cameraMatrix.at<double>(1,2)); 507 Point2f dcenter((destImageWidth-1)*0.5f, 0.f); 508 float xmin = FLT_MAX, xmax = -FLT_MAX, ymin = FLT_MAX, ymax = -FLT_MAX; 509 int N = 9; 510 std::vector<Point2f> uvec(1), vvec(1); 511 Mat I = Mat::eye(3,3,CV_64F); 512 float alpha = (float)_alpha; 513 514 int ndcoeffs = distCoeffs0.cols*distCoeffs0.rows*distCoeffs0.channels(); 515 CV_Assert((distCoeffs0.cols == 1 || distCoeffs0.rows == 1) && 516 (ndcoeffs == 4 || ndcoeffs == 5 || ndcoeffs == 8)); 517 CV_Assert(cameraMatrix0.size() == Size(3,3)); 518 distCoeffs0.convertTo(distCoeffs,CV_64F); 519 cameraMatrix0.convertTo(cameraMatrix,CV_64F); 520 521 alpha = std::min(alpha, 0.999f); 522 523 for( int i = 0; i < N; i++ ) 524 for( int j = 0; j < N; j++ ) 525 { 526 Point2f p((float)j*imageSize.width/(N-1), (float)i*imageSize.height/(N-1)); 527 uvec[0] = p; 528 undistortPoints(uvec, vvec, cameraMatrix, distCoeffs, I, I); 529 Point2f q = mapPointSpherical(vvec[0], alpha, 0, projType); 530 if( xmin > q.x ) xmin = q.x; 531 if( xmax < q.x ) xmax = q.x; 532 if( ymin > q.y ) ymin = q.y; 533 if( ymax < q.y ) ymax = q.y; 534 } 535 536 float scale = (float)std::min(dcenter.x/fabs(xmax), dcenter.x/fabs(xmin)); 537 Size dsize(destImageWidth, cvCeil(std::max(scale*fabs(ymin)*2, scale*fabs(ymax)*2))); 538 dcenter.y = (dsize.height - 1)*0.5f; 539 540 Mat mapxy(dsize, CV_32FC2); 541 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], s1 = k[8], s2 = k[9], s3 = k[10], s4 = k[11]; 542 double fx = cameraMatrix.at<double>(0,0), fy = cameraMatrix.at<double>(1,1), cx = scenter.x, cy = scenter.y; 543 544 for( int y = 0; y < dsize.height; y++ ) 545 { 546 Point2f* mxy = mapxy.ptr<Point2f>(y); 547 for( int x = 0; x < dsize.width; x++ ) 548 { 549 Point2f p = (Point2f((float)x, (float)y) - dcenter)*(1.f/scale); 550 Point2f q = invMapPointSpherical(p, alpha, projType); 551 if( q.x <= -FLT_MAX && q.y <= -FLT_MAX ) 552 { 553 mxy[x] = Point2f(-1.f, -1.f); 554 continue; 555 } 556 double x2 = q.x*q.x, y2 = q.y*q.y; 557 double r2 = x2 + y2, _2xy = 2*q.x*q.y; 558 double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2/(1 + ((k6*r2 + k5)*r2 + k4)*r2); 559 double u = fx*(q.x*kr + p1*_2xy + p2*(r2 + 2*x2) + s1*r2+ s2*r2*r2) + cx; 560 double v = fy*(q.y*kr + p1*(r2 + 2*y2) + p2*_2xy + s3*r2+ s4*r2*r2) + cy; 561 562 mxy[x] = Point2f((float)u, (float)v); 563 } 564 } 565 566 if(m1type == CV_32FC2) 567 { 568 _map1.create(mapxy.size(), mapxy.type()); 569 Mat map1 = _map1.getMat(); 570 mapxy.copyTo(map1); 571 _map2.release(); 572 } 573 else 574 convertMaps(mapxy, Mat(), _map1, _map2, m1type, false); 575 576 return scale; 577 } 578 579 /* End of file */ 580
