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 /* 45 This is stright-forward port v3 of Matlab calibration engine by Jean-Yves Bouguet 46 that is (in a large extent) based on the paper: 47 Z. Zhang. "A flexible new technique for camera calibration". 48 IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000. 49 50 The 1st initial port was done by Valery Mosyagin. 51 */ 52 53 CvLevMarq::CvLevMarq() 54 { 55 mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = 0; 56 lambdaLg10 = 0; state = DONE; 57 criteria = cvTermCriteria(0,0,0); 58 iters = 0; 59 completeSymmFlag = false; 60 } 61 62 CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag ) 63 { 64 mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = 0; 65 init(nparams, nerrs, criteria0, _completeSymmFlag); 66 } 67 68 void CvLevMarq::clear() 69 { 70 cvReleaseMat(&mask); 71 cvReleaseMat(&prevParam); 72 cvReleaseMat(¶m); 73 cvReleaseMat(&J); 74 cvReleaseMat(&err); 75 cvReleaseMat(&JtJ); 76 cvReleaseMat(&JtJN); 77 cvReleaseMat(&JtErr); 78 cvReleaseMat(&JtJV); 79 cvReleaseMat(&JtJW); 80 } 81 82 CvLevMarq::~CvLevMarq() 83 { 84 clear(); 85 } 86 87 void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag ) 88 { 89 if( !param || param->rows != nparams || nerrs != (err ? err->rows : 0) ) 90 clear(); 91 mask = cvCreateMat( nparams, 1, CV_8U ); 92 cvSet(mask, cvScalarAll(1)); 93 prevParam = cvCreateMat( nparams, 1, CV_64F ); 94 param = cvCreateMat( nparams, 1, CV_64F ); 95 JtJ = cvCreateMat( nparams, nparams, CV_64F ); 96 JtJN = cvCreateMat( nparams, nparams, CV_64F ); 97 JtJV = cvCreateMat( nparams, nparams, CV_64F ); 98 JtJW = cvCreateMat( nparams, 1, CV_64F ); 99 JtErr = cvCreateMat( nparams, 1, CV_64F ); 100 if( nerrs > 0 ) 101 { 102 J = cvCreateMat( nerrs, nparams, CV_64F ); 103 err = cvCreateMat( nerrs, 1, CV_64F ); 104 } 105 prevErrNorm = DBL_MAX; 106 lambdaLg10 = -3; 107 criteria = criteria0; 108 if( criteria.type & CV_TERMCRIT_ITER ) 109 criteria.max_iter = MIN(MAX(criteria.max_iter,1),1000); 110 else 111 criteria.max_iter = 30; 112 if( criteria.type & CV_TERMCRIT_EPS ) 113 criteria.epsilon = MAX(criteria.epsilon, 0); 114 else 115 criteria.epsilon = DBL_EPSILON; 116 state = STARTED; 117 iters = 0; 118 completeSymmFlag = _completeSymmFlag; 119 } 120 121 bool CvLevMarq::update( const CvMat*& _param, CvMat*& _J, CvMat*& _err ) 122 { 123 double change; 124 125 assert( err != 0 ); 126 if( state == DONE ) 127 { 128 _param = param; 129 return false; 130 } 131 132 if( state == STARTED ) 133 { 134 _param = param; 135 cvZero( J ); 136 cvZero( err ); 137 _J = J; 138 _err = err; 139 state = CALC_J; 140 return true; 141 } 142 143 if( state == CALC_J ) 144 { 145 cvMulTransposed( J, JtJ, 1 ); 146 cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T ); 147 cvCopy( param, prevParam ); 148 step(); 149 if( iters == 0 ) 150 prevErrNorm = cvNorm(err, 0, CV_L2); 151 _param = param; 152 cvZero( err ); 153 _err = err; 154 state = CHECK_ERR; 155 return true; 156 } 157 158 assert( state == CHECK_ERR ); 159 errNorm = cvNorm( err, 0, CV_L2 ); 160 if( errNorm > prevErrNorm ) 161 { 162 lambdaLg10++; 163 step(); 164 _param = param; 165 cvZero( err ); 166 _err = err; 167 state = CHECK_ERR; 168 return true; 169 } 170 171 lambdaLg10 = MAX(lambdaLg10-1, -16); 172 if( ++iters >= criteria.max_iter || 173 (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon ) 174 { 175 _param = param; 176 state = DONE; 177 return true; 178 } 179 180 prevErrNorm = errNorm; 181 _param = param; 182 cvZero(J); 183 _J = J; 184 state = CALC_J; 185 return false; 186 } 187 188 189 bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm ) 190 { 191 double change; 192 193 assert( err == 0 ); 194 if( state == DONE ) 195 { 196 _param = param; 197 return false; 198 } 199 200 if( state == STARTED ) 201 { 202 _param = param; 203 cvZero( JtJ ); 204 cvZero( JtErr ); 205 errNorm = 0; 206 _JtJ = JtJ; 207 _JtErr = JtErr; 208 _errNorm = &errNorm; 209 state = CALC_J; 210 return true; 211 } 212 213 if( state == CALC_J ) 214 { 215 cvCopy( param, prevParam ); 216 step(); 217 _param = param; 218 prevErrNorm = errNorm; 219 errNorm = 0; 220 _errNorm = &errNorm; 221 state = CHECK_ERR; 222 return true; 223 } 224 225 assert( state == CHECK_ERR ); 226 if( errNorm > prevErrNorm ) 227 { 228 lambdaLg10++; 229 step(); 230 _param = param; 231 errNorm = 0; 232 _errNorm = &errNorm; 233 state = CHECK_ERR; 234 return true; 235 } 236 237 lambdaLg10 = MAX(lambdaLg10-1, -16); 238 if( ++iters >= criteria.max_iter || 239 (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon ) 240 { 241 _param = param; 242 state = DONE; 243 return false; 244 } 245 246 prevErrNorm = errNorm; 247 cvZero( JtJ ); 248 cvZero( JtErr ); 249 _param = param; 250 _JtJ = JtJ; 251 _JtErr = JtErr; 252 state = CALC_J; 253 return true; 254 } 255 256 void CvLevMarq::step() 257 { 258 const double LOG10 = log(10.); 259 double lambda = exp(lambdaLg10*LOG10); 260 int i, j, nparams = param->rows; 261 262 for( i = 0; i < nparams; i++ ) 263 if( mask->data.ptr[i] == 0 ) 264 { 265 double *row = JtJ->data.db + i*nparams, *col = JtJ->data.db + i; 266 for( j = 0; j < nparams; j++ ) 267 row[j] = col[j*nparams] = 0; 268 JtErr->data.db[i] = 0; 269 } 270 271 if( !err ) 272 cvCompleteSymm( JtJ, completeSymmFlag ); 273 cvSetIdentity( JtJN, cvRealScalar(lambda) ); 274 cvAdd( JtJ, JtJN, JtJN ); 275 cvSVD( JtJN, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T ); 276 cvSVBkSb( JtJW, JtJV, JtJV, JtErr, param, CV_SVD_U_T + CV_SVD_V_T ); 277 for( i = 0; i < nparams; i++ ) 278 param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? param->data.db[i] : 0); 279 } 280 281 // reimplementation of dAB.m 282 CV_IMPL void 283 cvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB ) 284 { 285 CV_FUNCNAME( "cvCalcMatMulDeriv" ); 286 287 __BEGIN__; 288 289 int i, j, M, N, L; 290 int bstep; 291 292 CV_ASSERT( CV_IS_MAT(A) && CV_IS_MAT(B) ); 293 CV_ASSERT( CV_ARE_TYPES_EQ(A, B) && 294 (CV_MAT_TYPE(A->type) == CV_32F || CV_MAT_TYPE(A->type) == CV_64F) ); 295 CV_ASSERT( A->cols == B->rows ); 296 297 M = A->rows; 298 L = A->cols; 299 N = B->cols; 300 bstep = B->step/CV_ELEM_SIZE(B->type); 301 302 if( dABdA ) 303 { 304 CV_ASSERT( CV_ARE_TYPES_EQ(A, dABdA) && 305 dABdA->rows == A->rows*B->cols && dABdA->cols == A->rows*A->cols ); 306 } 307 308 if( dABdB ) 309 { 310 CV_ASSERT( CV_ARE_TYPES_EQ(A, dABdB) && 311 dABdB->rows == A->rows*B->cols && dABdB->cols == B->rows*B->cols ); 312 } 313 314 if( CV_MAT_TYPE(A->type) == CV_32F ) 315 { 316 for( i = 0; i < M*N; i++ ) 317 { 318 int i1 = i / N, i2 = i % N; 319 320 if( dABdA ) 321 { 322 float* dcda = (float*)(dABdA->data.ptr + dABdA->step*i); 323 const float* b = (const float*)B->data.ptr + i2; 324 325 for( j = 0; j < M*L; j++ ) 326 dcda[j] = 0; 327 for( j = 0; j < L; j++ ) 328 dcda[i1*L + j] = b[j*bstep]; 329 } 330 331 if( dABdB ) 332 { 333 float* dcdb = (float*)(dABdB->data.ptr + dABdB->step*i); 334 const float* a = (const float*)(A->data.ptr + A->step*i1); 335 336 for( j = 0; j < L*N; j++ ) 337 dcdb[j] = 0; 338 for( j = 0; j < L; j++ ) 339 dcdb[j*N + i2] = a[j]; 340 } 341 } 342 } 343 else 344 { 345 for( i = 0; i < M*N; i++ ) 346 { 347 int i1 = i / N, i2 = i % N; 348 349 if( dABdA ) 350 { 351 double* dcda = (double*)(dABdA->data.ptr + dABdA->step*i); 352 const double* b = (const double*)B->data.ptr + i2; 353 354 for( j = 0; j < M*L; j++ ) 355 dcda[j] = 0; 356 for( j = 0; j < L; j++ ) 357 dcda[i1*L + j] = b[j*bstep]; 358 } 359 360 if( dABdB ) 361 { 362 double* dcdb = (double*)(dABdB->data.ptr + dABdB->step*i); 363 const double* a = (const double*)(A->data.ptr + A->step*i1); 364 365 for( j = 0; j < L*N; j++ ) 366 dcdb[j] = 0; 367 for( j = 0; j < L; j++ ) 368 dcdb[j*N + i2] = a[j]; 369 } 370 } 371 } 372 373 __END__; 374 } 375 376 // reimplementation of compose_motion.m 377 CV_IMPL void 378 cvComposeRT( const CvMat* _rvec1, const CvMat* _tvec1, 379 const CvMat* _rvec2, const CvMat* _tvec2, 380 CvMat* _rvec3, CvMat* _tvec3, 381 CvMat* dr3dr1, CvMat* dr3dt1, 382 CvMat* dr3dr2, CvMat* dr3dt2, 383 CvMat* dt3dr1, CvMat* dt3dt1, 384 CvMat* dt3dr2, CvMat* dt3dt2 ) 385 { 386 CV_FUNCNAME( "cvComposeRT" ); 387 388 __BEGIN__; 389 390 double _r1[3], _r2[3]; 391 double _R1[9], _d1[9*3], _R2[9], _d2[9*3]; 392 CvMat r1 = cvMat(3,1,CV_64F,_r1), r2 = cvMat(3,1,CV_64F,_r2); 393 CvMat R1 = cvMat(3,3,CV_64F,_R1), R2 = cvMat(3,3,CV_64F,_R2); 394 CvMat dR1dr1 = cvMat(9,3,CV_64F,_d1), dR2dr2 = cvMat(9,3,CV_64F,_d2); 395 396 CV_ASSERT( CV_IS_MAT(_rvec1) && CV_IS_MAT(_rvec2) ); 397 398 CV_ASSERT( CV_MAT_TYPE(_rvec1->type) == CV_32F || 399 CV_MAT_TYPE(_rvec1->type) == CV_64F ); 400 401 CV_ASSERT( _rvec1->rows == 3 && _rvec1->cols == 1 && CV_ARE_SIZES_EQ(_rvec1, _rvec2) ); 402 403 cvConvert( _rvec1, &r1 ); 404 cvConvert( _rvec2, &r2 ); 405 406 cvRodrigues2( &r1, &R1, &dR1dr1 ); 407 cvRodrigues2( &r2, &R2, &dR2dr2 ); 408 409 if( _rvec3 || dr3dr1 || dr3dr1 ) 410 { 411 double _r3[3], _R3[9], _dR3dR1[9*9], _dR3dR2[9*9], _dr3dR3[9*3]; 412 double _W1[9*3], _W2[3*3]; 413 CvMat r3 = cvMat(3,1,CV_64F,_r3), R3 = cvMat(3,3,CV_64F,_R3); 414 CvMat dR3dR1 = cvMat(9,9,CV_64F,_dR3dR1), dR3dR2 = cvMat(9,9,CV_64F,_dR3dR2); 415 CvMat dr3dR3 = cvMat(3,9,CV_64F,_dr3dR3); 416 CvMat W1 = cvMat(3,9,CV_64F,_W1), W2 = cvMat(3,3,CV_64F,_W2); 417 418 cvMatMul( &R2, &R1, &R3 ); 419 cvCalcMatMulDeriv( &R2, &R1, &dR3dR2, &dR3dR1 ); 420 421 cvRodrigues2( &R3, &r3, &dr3dR3 ); 422 423 if( _rvec3 ) 424 cvConvert( &r3, _rvec3 ); 425 426 if( dr3dr1 ) 427 { 428 cvMatMul( &dr3dR3, &dR3dR1, &W1 ); 429 cvMatMul( &W1, &dR1dr1, &W2 ); 430 cvConvert( &W2, dr3dr1 ); 431 } 432 433 if( dr3dr2 ) 434 { 435 cvMatMul( &dr3dR3, &dR3dR2, &W1 ); 436 cvMatMul( &W1, &dR2dr2, &W2 ); 437 cvConvert( &W2, dr3dr2 ); 438 } 439 } 440 441 if( dr3dt1 ) 442 cvZero( dr3dt1 ); 443 if( dr3dt2 ) 444 cvZero( dr3dt2 ); 445 446 if( _tvec3 || dt3dr2 || dt3dt1 ) 447 { 448 double _t1[3], _t2[3], _t3[3], _dxdR2[3*9], _dxdt1[3*3], _W3[3*3]; 449 CvMat t1 = cvMat(3,1,CV_64F,_t1), t2 = cvMat(3,1,CV_64F,_t2); 450 CvMat t3 = cvMat(3,1,CV_64F,_t3); 451 CvMat dxdR2 = cvMat(3, 9, CV_64F, _dxdR2); 452 CvMat dxdt1 = cvMat(3, 3, CV_64F, _dxdt1); 453 CvMat W3 = cvMat(3, 3, CV_64F, _W3); 454 455 CV_ASSERT( CV_IS_MAT(_tvec1) && CV_IS_MAT(_tvec2) ); 456 CV_ASSERT( CV_ARE_SIZES_EQ(_tvec1, _tvec2) && CV_ARE_SIZES_EQ(_tvec1, _rvec1) ); 457 458 cvConvert( _tvec1, &t1 ); 459 cvConvert( _tvec2, &t2 ); 460 cvMatMulAdd( &R2, &t1, &t2, &t3 ); 461 462 if( _tvec3 ) 463 cvConvert( &t3, _tvec3 ); 464 465 if( dt3dr2 || dt3dt1 ) 466 { 467 cvCalcMatMulDeriv( &R2, &t1, &dxdR2, &dxdt1 ); 468 if( dt3dr2 ) 469 { 470 cvMatMul( &dxdR2, &dR2dr2, &W3 ); 471 cvConvert( &W3, dt3dr2 ); 472 } 473 if( dt3dt1 ) 474 cvConvert( &dxdt1, dt3dt1 ); 475 } 476 } 477 478 if( dt3dt2 ) 479 cvSetIdentity( dt3dt2 ); 480 if( dt3dr1 ) 481 cvZero( dt3dr1 ); 482 483 __END__; 484 } 485 486 CV_IMPL int 487 cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) 488 { 489 int result = 0; 490 491 CV_FUNCNAME( "cvRogrigues2" ); 492 493 __BEGIN__; 494 495 int depth, elem_size; 496 int i, k; 497 double J[27]; 498 CvMat _J = cvMat( 3, 9, CV_64F, J ); 499 500 if( !CV_IS_MAT(src) ) 501 CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg, "Input argument is not a valid matrix" ); 502 503 if( !CV_IS_MAT(dst) ) 504 CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg, 505 "The first output argument is not a valid matrix" ); 506 507 depth = CV_MAT_DEPTH(src->type); 508 elem_size = CV_ELEM_SIZE(depth); 509 510 if( depth != CV_32F && depth != CV_64F ) 511 CV_ERROR( CV_StsUnsupportedFormat, "The matrices must have 32f or 64f data type" ); 512 513 if( !CV_ARE_DEPTHS_EQ(src, dst) ) 514 CV_ERROR( CV_StsUnmatchedFormats, "All the matrices must have the same data type" ); 515 516 if( jacobian ) 517 { 518 if( !CV_IS_MAT(jacobian) ) 519 CV_ERROR( CV_StsBadArg, "Jacobian is not a valid matrix" ); 520 521 if( !CV_ARE_DEPTHS_EQ(src, jacobian) || CV_MAT_CN(jacobian->type) != 1 ) 522 CV_ERROR( CV_StsUnmatchedFormats, "Jacobian must have 32fC1 or 64fC1 datatype" ); 523 524 if( (jacobian->rows != 9 || jacobian->cols != 3) && 525 (jacobian->rows != 3 || jacobian->cols != 9)) 526 CV_ERROR( CV_StsBadSize, "Jacobian must be 3x9 or 9x3" ); 527 } 528 529 if( src->cols == 1 || src->rows == 1 ) 530 { 531 double rx, ry, rz, theta; 532 int step = src->rows > 1 ? src->step / elem_size : 1; 533 534 if( src->rows + src->cols*CV_MAT_CN(src->type) - 1 != 3 ) 535 CV_ERROR( CV_StsBadSize, "Input matrix must be 1x3, 3x1 or 3x3" ); 536 537 if( dst->rows != 3 || dst->cols != 3 || CV_MAT_CN(dst->type) != 1 ) 538 CV_ERROR( CV_StsBadSize, "Output matrix must be 3x3, single-channel floating point matrix" ); 539 540 if( depth == CV_32F ) 541 { 542 rx = src->data.fl[0]; 543 ry = src->data.fl[step]; 544 rz = src->data.fl[step*2]; 545 } 546 else 547 { 548 rx = src->data.db[0]; 549 ry = src->data.db[step]; 550 rz = src->data.db[step*2]; 551 } 552 theta = sqrt(rx*rx + ry*ry + rz*rz); 553 554 if( theta < DBL_EPSILON ) 555 { 556 cvSetIdentity( dst ); 557 558 if( jacobian ) 559 { 560 memset( J, 0, sizeof(J) ); 561 J[5] = J[15] = J[19] = -1; 562 J[7] = J[11] = J[21] = 1; 563 } 564 } 565 else 566 { 567 const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; 568 569 double c = cos(theta); 570 double s = sin(theta); 571 double c1 = 1. - c; 572 double itheta = theta ? 1./theta : 0.; 573 574 rx *= itheta; ry *= itheta; rz *= itheta; 575 576 double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz }; 577 double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 }; 578 double R[9]; 579 CvMat _R = cvMat( 3, 3, CV_64F, R ); 580 581 // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] 582 // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0] 583 for( k = 0; k < 9; k++ ) 584 R[k] = c*I[k] + c1*rrt[k] + s*_r_x_[k]; 585 586 cvConvert( &_R, dst ); 587 588 if( jacobian ) 589 { 590 double drrt[] = { rx+rx, ry, rz, ry, 0, 0, rz, 0, 0, 591 0, rx, 0, rx, ry+ry, rz, 0, rz, 0, 592 0, 0, rx, 0, 0, ry, rx, ry, rz+rz }; 593 double d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0, 594 0, 0, 1, 0, 0, 0, -1, 0, 0, 595 0, -1, 0, 1, 0, 0, 0, 0, 0 }; 596 for( i = 0; i < 3; i++ ) 597 { 598 double ri = i == 0 ? rx : i == 1 ? ry : rz; 599 double a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta; 600 double a3 = (c - s*itheta)*ri, a4 = s*itheta; 601 for( k = 0; k < 9; k++ ) 602 J[i*9+k] = a0*I[k] + a1*rrt[k] + a2*drrt[i*9+k] + 603 a3*_r_x_[k] + a4*d_r_x_[i*9+k]; 604 } 605 } 606 } 607 } 608 else if( src->cols == 3 && src->rows == 3 ) 609 { 610 double R[9], U[9], V[9], W[3], rx, ry, rz; 611 CvMat _R = cvMat( 3, 3, CV_64F, R ); 612 CvMat _U = cvMat( 3, 3, CV_64F, U ); 613 CvMat _V = cvMat( 3, 3, CV_64F, V ); 614 CvMat _W = cvMat( 3, 1, CV_64F, W ); 615 double theta, s, c; 616 int step = dst->rows > 1 ? dst->step / elem_size : 1; 617 618 if( (dst->rows != 1 || dst->cols*CV_MAT_CN(dst->type) != 3) && 619 (dst->rows != 3 || dst->cols != 1 || CV_MAT_CN(dst->type) != 1)) 620 CV_ERROR( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" ); 621 622 cvConvert( src, &_R ); 623 if( !cvCheckArr( &_R, CV_CHECK_RANGE+CV_CHECK_QUIET, -100, 100 ) ) 624 { 625 cvZero(dst); 626 if( jacobian ) 627 cvZero(jacobian); 628 EXIT; 629 } 630 631 cvSVD( &_R, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T ); 632 cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T ); 633 634 rx = R[7] - R[5]; 635 ry = R[2] - R[6]; 636 rz = R[3] - R[1]; 637 638 s = sqrt((rx*rx + ry*ry + rz*rz)*0.25); 639 c = (R[0] + R[4] + R[8] - 1)*0.5; 640 c = c > 1. ? 1. : c < -1. ? -1. : c; 641 theta = acos(c); 642 643 if( s < 1e-5 ) 644 { 645 double t; 646 647 if( c > 0 ) 648 rx = ry = rz = 0; 649 else 650 { 651 t = (R[0] + 1)*0.5; 652 rx = theta*sqrt(MAX(t,0.)); 653 t = (R[4] + 1)*0.5; 654 ry = theta*sqrt(MAX(t,0.))*(R[1] < 0 ? -1. : 1.); 655 t = (R[8] + 1)*0.5; 656 rz = theta*sqrt(MAX(t,0.))*(R[2] < 0 ? -1. : 1.); 657 } 658 659 if( jacobian ) 660 { 661 memset( J, 0, sizeof(J) ); 662 if( c > 0 ) 663 { 664 J[5] = J[15] = J[19] = -0.5; 665 J[7] = J[11] = J[21] = 0.5; 666 } 667 } 668 } 669 else 670 { 671 double vth = 1/(2*s); 672 673 if( jacobian ) 674 { 675 double t, dtheta_dtr = -1./s; 676 // var1 = [vth;theta] 677 // var = [om1;var1] = [om1;vth;theta] 678 double dvth_dtheta = -vth*c/s; 679 double d1 = 0.5*dvth_dtheta*dtheta_dtr; 680 double d2 = 0.5*dtheta_dtr; 681 // dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR 682 double dvardR[5*9] = 683 { 684 0, 0, 0, 0, 0, 1, 0, -1, 0, 685 0, 0, -1, 0, 0, 0, 1, 0, 0, 686 0, 1, 0, -1, 0, 0, 0, 0, 0, 687 d1, 0, 0, 0, d1, 0, 0, 0, d1, 688 d2, 0, 0, 0, d2, 0, 0, 0, d2 689 }; 690 // var2 = [om;theta] 691 double dvar2dvar[] = 692 { 693 vth, 0, 0, rx, 0, 694 0, vth, 0, ry, 0, 695 0, 0, vth, rz, 0, 696 0, 0, 0, 0, 1 697 }; 698 double domegadvar2[] = 699 { 700 theta, 0, 0, rx*vth, 701 0, theta, 0, ry*vth, 702 0, 0, theta, rz*vth 703 }; 704 705 CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR ); 706 CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar ); 707 CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 ); 708 double t0[3*5]; 709 CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 ); 710 711 cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 ); 712 cvMatMul( &_t0, &_dvardR, &_J ); 713 714 // transpose every row of _J (treat the rows as 3x3 matrices) 715 CV_SWAP(J[1], J[3], t); CV_SWAP(J[2], J[6], t); CV_SWAP(J[5], J[7], t); 716 CV_SWAP(J[10], J[12], t); CV_SWAP(J[11], J[15], t); CV_SWAP(J[14], J[16], t); 717 CV_SWAP(J[19], J[21], t); CV_SWAP(J[20], J[24], t); CV_SWAP(J[23], J[25], t); 718 } 719 720 vth *= theta; 721 rx *= vth; ry *= vth; rz *= vth; 722 } 723 724 if( depth == CV_32F ) 725 { 726 dst->data.fl[0] = (float)rx; 727 dst->data.fl[step] = (float)ry; 728 dst->data.fl[step*2] = (float)rz; 729 } 730 else 731 { 732 dst->data.db[0] = rx; 733 dst->data.db[step] = ry; 734 dst->data.db[step*2] = rz; 735 } 736 } 737 738 if( jacobian ) 739 { 740 if( depth == CV_32F ) 741 { 742 if( jacobian->rows == _J.rows ) 743 cvConvert( &_J, jacobian ); 744 else 745 { 746 float Jf[3*9]; 747 CvMat _Jf = cvMat( _J.rows, _J.cols, CV_32FC1, Jf ); 748 cvConvert( &_J, &_Jf ); 749 cvTranspose( &_Jf, jacobian ); 750 } 751 } 752 else if( jacobian->rows == _J.rows ) 753 cvCopy( &_J, jacobian ); 754 else 755 cvTranspose( &_J, jacobian ); 756 } 757 758 result = 1; 759 760 __END__; 761 762 return result; 763 } 764 765 766 CV_IMPL void 767 cvProjectPoints2( const CvMat* objectPoints, 768 const CvMat* r_vec, 769 const CvMat* t_vec, 770 const CvMat* A, 771 const CvMat* distCoeffs, 772 CvMat* imagePoints, CvMat* dpdr, 773 CvMat* dpdt, CvMat* dpdf, 774 CvMat* dpdc, CvMat* dpdk, 775 double aspectRatio ) 776 { 777 CvMat *_M = 0, *_m = 0; 778 CvMat *_dpdr = 0, *_dpdt = 0, *_dpdc = 0, *_dpdf = 0, *_dpdk = 0; 779 780 CV_FUNCNAME( "cvProjectPoints2" ); 781 782 __BEGIN__; 783 784 int i, j, count; 785 int calc_derivatives; 786 const CvPoint3D64f* M; 787 CvPoint2D64f* m; 788 double r[3], R[9], dRdr[27], t[3], a[9], k[5] = {0,0,0,0,0}, fx, fy, cx, cy; 789 CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k; 790 CvMat _R = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr ); 791 double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0; 792 int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0; 793 bool fixedAspectRatio = aspectRatio > FLT_EPSILON; 794 795 if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) || 796 !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) || 797 /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) ) 798 CV_ERROR( CV_StsBadArg, "One of required arguments is not a valid matrix" ); 799 800 count = MAX(objectPoints->rows, objectPoints->cols); 801 802 if( CV_IS_CONT_MAT(objectPoints->type) && CV_MAT_DEPTH(objectPoints->type) == CV_64F && 803 ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) || 804 (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3))) 805 _M = (CvMat*)objectPoints; 806 else 807 { 808 CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 )); 809 CV_CALL( cvConvertPointsHomogeneous( objectPoints, _M )); 810 } 811 812 if( CV_IS_CONT_MAT(imagePoints->type) && CV_MAT_DEPTH(imagePoints->type) == CV_64F && 813 ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 2) || 814 (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 2))) 815 _m = imagePoints; 816 else 817 CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 )); 818 819 M = (CvPoint3D64f*)_M->data.db; 820 m = (CvPoint2D64f*)_m->data.db; 821 822 if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) || 823 (((r_vec->rows != 1 && r_vec->cols != 1) || 824 r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) && 825 ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1))) 826 CV_ERROR( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 " 827 "floating-point rotation vector, or 3x3 rotation matrix" ); 828 829 if( r_vec->rows == 3 && r_vec->cols == 3 ) 830 { 831 _r = cvMat( 3, 1, CV_64FC1, r ); 832 CV_CALL( cvRodrigues2( r_vec, &_r )); 833 CV_CALL( cvRodrigues2( &_r, &_R, &_dRdr )); 834 cvCopy( r_vec, &_R ); 835 } 836 else 837 { 838 _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r ); 839 CV_CALL( cvConvert( r_vec, &_r )); 840 CV_CALL( cvRodrigues2( &_r, &_R, &_dRdr ) ); 841 } 842 843 if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) || 844 (t_vec->rows != 1 && t_vec->cols != 1) || 845 t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 ) 846 CV_ERROR( CV_StsBadArg, 847 "Translation vector must be 1x3 or 3x1 floating-point vector" ); 848 849 _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t ); 850 CV_CALL( cvConvert( t_vec, &_t )); 851 852 if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) || 853 A->rows != 3 || A->cols != 3 ) 854 CV_ERROR( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" ); 855 856 CV_CALL( cvConvert( A, &_a )); 857 fx = a[0]; fy = a[4]; 858 cx = a[2]; cy = a[5]; 859 860 if( fixedAspectRatio ) 861 fx = fy*aspectRatio; 862 863 if( distCoeffs ) 864 { 865 if( !CV_IS_MAT(distCoeffs) || 866 (CV_MAT_DEPTH(distCoeffs->type) != CV_64F && 867 CV_MAT_DEPTH(distCoeffs->type) != CV_32F) || 868 (distCoeffs->rows != 1 && distCoeffs->cols != 1) || 869 (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 && 870 distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5) ) 871 CV_ERROR( CV_StsBadArg, 872 "Distortion coefficients must be 1x4, 4x1, 1x5 or 5x1 floating-point vector" ); 873 874 _k = cvMat( distCoeffs->rows, distCoeffs->cols, 875 CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k ); 876 CV_CALL( cvConvert( distCoeffs, &_k )); 877 } 878 879 if( dpdr ) 880 { 881 if( !CV_IS_MAT(dpdr) || 882 (CV_MAT_TYPE(dpdr->type) != CV_32FC1 && 883 CV_MAT_TYPE(dpdr->type) != CV_64FC1) || 884 dpdr->rows != count*2 || dpdr->cols != 3 ) 885 CV_ERROR( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" ); 886 887 if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 ) 888 _dpdr = dpdr; 889 else 890 CV_CALL( _dpdr = cvCreateMat( 2*count, 3, CV_64FC1 )); 891 dpdr_p = _dpdr->data.db; 892 dpdr_step = _dpdr->step/sizeof(dpdr_p[0]); 893 } 894 895 if( dpdt ) 896 { 897 if( !CV_IS_MAT(dpdt) || 898 (CV_MAT_TYPE(dpdt->type) != CV_32FC1 && 899 CV_MAT_TYPE(dpdt->type) != CV_64FC1) || 900 dpdt->rows != count*2 || dpdt->cols != 3 ) 901 CV_ERROR( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" ); 902 903 if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 ) 904 _dpdt = dpdt; 905 else 906 CV_CALL( _dpdt = cvCreateMat( 2*count, 3, CV_64FC1 )); 907 dpdt_p = _dpdt->data.db; 908 dpdt_step = _dpdt->step/sizeof(dpdt_p[0]); 909 } 910 911 if( dpdf ) 912 { 913 if( !CV_IS_MAT(dpdf) || 914 (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) || 915 dpdf->rows != count*2 || dpdf->cols != 2 ) 916 CV_ERROR( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" ); 917 918 if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 ) 919 _dpdf = dpdf; 920 else 921 CV_CALL( _dpdf = cvCreateMat( 2*count, 2, CV_64FC1 )); 922 dpdf_p = _dpdf->data.db; 923 dpdf_step = _dpdf->step/sizeof(dpdf_p[0]); 924 } 925 926 if( dpdc ) 927 { 928 if( !CV_IS_MAT(dpdc) || 929 (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) || 930 dpdc->rows != count*2 || dpdc->cols != 2 ) 931 CV_ERROR( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" ); 932 933 if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 ) 934 _dpdc = dpdc; 935 else 936 CV_CALL( _dpdc = cvCreateMat( 2*count, 2, CV_64FC1 )); 937 dpdc_p = _dpdc->data.db; 938 dpdc_step = _dpdc->step/sizeof(dpdc_p[0]); 939 } 940 941 if( dpdk ) 942 { 943 if( !CV_IS_MAT(dpdk) || 944 (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) || 945 dpdk->rows != count*2 || (dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) ) 946 CV_ERROR( CV_StsBadArg, "dp/df must be 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" ); 947 948 if( !distCoeffs ) 949 CV_ERROR( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" ); 950 951 if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 ) 952 _dpdk = dpdk; 953 else 954 CV_CALL( _dpdk = cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 )); 955 dpdk_p = _dpdk->data.db; 956 dpdk_step = _dpdk->step/sizeof(dpdk_p[0]); 957 } 958 959 calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk; 960 961 for( i = 0; i < count; i++ ) 962 { 963 double X = M[i].x, Y = M[i].y, Z = M[i].z; 964 double x = R[0]*X + R[1]*Y + R[2]*Z + t[0]; 965 double y = R[3]*X + R[4]*Y + R[5]*Z + t[1]; 966 double z = R[6]*X + R[7]*Y + R[8]*Z + t[2]; 967 double r2, r4, r6, a1, a2, a3, cdist; 968 double xd, yd; 969 970 z = z ? 1./z : 1; 971 x *= z; y *= z; 972 973 r2 = x*x + y*y; 974 r4 = r2*r2; 975 r6 = r4*r2; 976 a1 = 2*x*y; 977 a2 = r2 + 2*x*x; 978 a3 = r2 + 2*y*y; 979 cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6; 980 xd = x*cdist + k[2]*a1 + k[3]*a2; 981 yd = y*cdist + k[2]*a3 + k[3]*a1; 982 983 m[i].x = xd*fx + cx; 984 m[i].y = yd*fy + cy; 985 986 if( calc_derivatives ) 987 { 988 if( dpdc_p ) 989 { 990 dpdc_p[0] = 1; dpdc_p[1] = 0; 991 dpdc_p[dpdc_step] = 0; 992 dpdc_p[dpdc_step+1] = 1; 993 dpdc_p += dpdc_step*2; 994 } 995 996 if( dpdf_p ) 997 { 998 if( fixedAspectRatio ) 999 { 1000 dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; 1001 dpdf_p[dpdf_step] = 0; 1002 dpdf_p[dpdf_step+1] = yd; 1003 } 1004 else 1005 { 1006 dpdf_p[0] = xd; dpdf_p[1] = 0; 1007 dpdf_p[dpdf_step] = 0; 1008 dpdf_p[dpdf_step+1] = yd; 1009 } 1010 dpdf_p += dpdf_step*2; 1011 } 1012 1013 if( dpdk_p ) 1014 { 1015 dpdk_p[0] = fx*x*r2; 1016 dpdk_p[1] = fx*x*r4; 1017 dpdk_p[dpdk_step] = fy*y*r2; 1018 dpdk_p[dpdk_step+1] = fy*y*r4; 1019 if( _dpdk->cols > 2 ) 1020 { 1021 dpdk_p[2] = fx*a1; 1022 dpdk_p[3] = fx*a2; 1023 dpdk_p[dpdk_step+2] = fy*a3; 1024 dpdk_p[dpdk_step+3] = fy*a1; 1025 if( _dpdk->cols > 4 ) 1026 { 1027 dpdk_p[4] = fx*x*r6; 1028 dpdk_p[dpdk_step+4] = fy*y*r6; 1029 } 1030 } 1031 dpdk_p += dpdk_step*2; 1032 } 1033 1034 if( dpdt_p ) 1035 { 1036 double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z }; 1037 for( j = 0; j < 3; j++ ) 1038 { 1039 double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j]; 1040 double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt; 1041 double da1dt = 2*(x*dydt[j] + y*dxdt[j]); 1042 double dmxdt = fx*(dxdt[j]*cdist + x*dcdist_dt + 1043 k[2]*da1dt + k[3]*(dr2dt + 2*x*dxdt[j])); 1044 double dmydt = fy*(dydt[j]*cdist + y*dcdist_dt + 1045 k[2]*(dr2dt + 2*y*dydt[j]) + k[3]*da1dt); 1046 dpdt_p[j] = dmxdt; 1047 dpdt_p[dpdt_step+j] = dmydt; 1048 } 1049 dpdt_p += dpdt_step*2; 1050 } 1051 1052 if( dpdr_p ) 1053 { 1054 double dx0dr[] = 1055 { 1056 X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2], 1057 X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11], 1058 X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20] 1059 }; 1060 double dy0dr[] = 1061 { 1062 X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5], 1063 X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14], 1064 X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23] 1065 }; 1066 double dz0dr[] = 1067 { 1068 X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8], 1069 X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17], 1070 X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26] 1071 }; 1072 for( j = 0; j < 3; j++ ) 1073 { 1074 double dxdr = z*(dx0dr[j] - x*dz0dr[j]); 1075 double dydr = z*(dy0dr[j] - y*dz0dr[j]); 1076 double dr2dr = 2*x*dxdr + 2*y*dydr; 1077 double dcdist_dr = k[0]*dr2dr + 2*k[1]*r2*dr2dr + 3*k[4]*r4*dr2dr; 1078 double da1dr = 2*(x*dydr + y*dxdr); 1079 double dmxdr = fx*(dxdr*cdist + x*dcdist_dr + 1080 k[2]*da1dr + k[3]*(dr2dr + 2*x*dxdr)); 1081 double dmydr = fy*(dydr*cdist + y*dcdist_dr + 1082 k[2]*(dr2dr + 2*y*dydr) + k[3]*da1dr); 1083 dpdr_p[j] = dmxdr; 1084 dpdr_p[dpdr_step+j] = dmydr; 1085 } 1086 dpdr_p += dpdr_step*2; 1087 } 1088 } 1089 } 1090 1091 if( _m != imagePoints ) 1092 cvConvertPointsHomogeneous( _m, imagePoints ); 1093 if( _dpdr != dpdr ) 1094 cvConvert( _dpdr, dpdr ); 1095 if( _dpdt != dpdt ) 1096 cvConvert( _dpdt, dpdt ); 1097 if( _dpdf != dpdf ) 1098 cvConvert( _dpdf, dpdf ); 1099 if( _dpdc != dpdc ) 1100 cvConvert( _dpdc, dpdc ); 1101 if( _dpdk != dpdk ) 1102 cvConvert( _dpdk, dpdk ); 1103 1104 __END__; 1105 1106 if( _M != objectPoints ) 1107 cvReleaseMat( &_M ); 1108 if( _m != imagePoints ) 1109 cvReleaseMat( &_m ); 1110 if( _dpdr != dpdr ) 1111 cvReleaseMat( &_dpdr ); 1112 if( _dpdt != dpdt ) 1113 cvReleaseMat( &_dpdt ); 1114 if( _dpdf != dpdf ) 1115 cvReleaseMat( &_dpdf ); 1116 if( _dpdc != dpdc ) 1117 cvReleaseMat( &_dpdc ); 1118 if( _dpdk != dpdk ) 1119 cvReleaseMat( &_dpdk ); 1120 } 1121 1122 1123 CV_IMPL void 1124 cvFindExtrinsicCameraParams2( const CvMat* objectPoints, 1125 const CvMat* imagePoints, const CvMat* A, 1126 const CvMat* distCoeffs, 1127 CvMat* rvec, CvMat* tvec ) 1128 { 1129 const int max_iter = 20; 1130 CvMat *_M = 0, *_Mxy = 0, *_m = 0, *_mn = 0, *_L = 0, *_J = 0; 1131 1132 CV_FUNCNAME( "cvFindExtrinsicCameraParams2" ); 1133 1134 __BEGIN__; 1135 1136 int i, count; 1137 double a[9], ar[9]={1,0,0,0,1,0,0,0,1}, R[9]; 1138 double MM[9], U[9], V[9], W[3]; 1139 CvScalar Mc; 1140 double JtJ[6*6], JtErr[6], JtJW[6], JtJV[6*6], delta[6], param[6]; 1141 CvMat _A = cvMat( 3, 3, CV_64F, a ); 1142 CvMat _Ar = cvMat( 3, 3, CV_64F, ar ); 1143 CvMat _R = cvMat( 3, 3, CV_64F, R ); 1144 CvMat _r = cvMat( 3, 1, CV_64F, param ); 1145 CvMat _t = cvMat( 3, 1, CV_64F, param + 3 ); 1146 CvMat _Mc = cvMat( 1, 3, CV_64F, Mc.val ); 1147 CvMat _MM = cvMat( 3, 3, CV_64F, MM ); 1148 CvMat _U = cvMat( 3, 3, CV_64F, U ); 1149 CvMat _V = cvMat( 3, 3, CV_64F, V ); 1150 CvMat _W = cvMat( 3, 1, CV_64F, W ); 1151 CvMat _JtJ = cvMat( 6, 6, CV_64F, JtJ ); 1152 CvMat _JtErr = cvMat( 6, 1, CV_64F, JtErr ); 1153 CvMat _JtJW = cvMat( 6, 1, CV_64F, JtJW ); 1154 CvMat _JtJV = cvMat( 6, 6, CV_64F, JtJV ); 1155 CvMat _delta = cvMat( 6, 1, CV_64F, delta ); 1156 CvMat _param = cvMat( 6, 1, CV_64F, param ); 1157 CvMat _dpdr, _dpdt; 1158 1159 CV_ASSERT( CV_IS_MAT(objectPoints) && CV_IS_MAT(imagePoints) && 1160 CV_IS_MAT(A) && CV_IS_MAT(rvec) && CV_IS_MAT(tvec) ); 1161 1162 count = MAX(objectPoints->cols, objectPoints->rows); 1163 CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 )); 1164 CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 )); 1165 1166 CV_CALL( cvConvertPointsHomogeneous( objectPoints, _M )); 1167 CV_CALL( cvConvertPointsHomogeneous( imagePoints, _m )); 1168 CV_CALL( cvConvert( A, &_A )); 1169 1170 CV_ASSERT( (CV_MAT_DEPTH(rvec->type) == CV_64F || CV_MAT_DEPTH(rvec->type) == CV_32F) && 1171 (rvec->rows == 1 || rvec->cols == 1) && rvec->rows*rvec->cols*CV_MAT_CN(rvec->type) == 3 ); 1172 1173 CV_ASSERT( (CV_MAT_DEPTH(tvec->type) == CV_64F || CV_MAT_DEPTH(tvec->type) == CV_32F) && 1174 (tvec->rows == 1 || tvec->cols == 1) && tvec->rows*tvec->cols*CV_MAT_CN(tvec->type) == 3 ); 1175 1176 CV_CALL( _mn = cvCreateMat( 1, count, CV_64FC2 )); 1177 CV_CALL( _Mxy = cvCreateMat( 1, count, CV_64FC2 )); 1178 1179 // normalize image points 1180 // (unapply the intrinsic matrix transformation and distortion) 1181 cvUndistortPoints( _m, _mn, &_A, distCoeffs, 0, &_Ar ); 1182 1183 Mc = cvAvg(_M); 1184 cvReshape( _M, _M, 1, count ); 1185 cvMulTransposed( _M, &_MM, 1, &_Mc ); 1186 cvSVD( &_MM, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T ); 1187 1188 // initialize extrinsic parameters 1189 if( W[2]/W[1] < 1e-3 || count < 4 ) 1190 { 1191 // a planar structure case (all M's lie in the same plane) 1192 double tt[3], h[9], h1_norm, h2_norm; 1193 CvMat* R_transform = &_V; 1194 CvMat T_transform = cvMat( 3, 1, CV_64F, tt ); 1195 CvMat _H = cvMat( 3, 3, CV_64F, h ); 1196 CvMat _h1, _h2, _h3; 1197 1198 if( V[2]*V[2] + V[5]*V[5] < 1e-10 ) 1199 cvSetIdentity( R_transform ); 1200 1201 if( cvDet(R_transform) < 0 ) 1202 cvScale( R_transform, R_transform, -1 ); 1203 1204 cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T ); 1205 1206 for( i = 0; i < count; i++ ) 1207 { 1208 const double* Rp = R_transform->data.db; 1209 const double* Tp = T_transform.data.db; 1210 const double* src = _M->data.db + i*3; 1211 double* dst = _Mxy->data.db + i*2; 1212 1213 dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0]; 1214 dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1]; 1215 } 1216 1217 cvFindHomography( _Mxy, _mn, &_H ); 1218 1219 cvGetCol( &_H, &_h1, 0 ); 1220 _h2 = _h1; _h2.data.db++; 1221 _h3 = _h2; _h3.data.db++; 1222 h1_norm = sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]); 1223 h2_norm = sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]); 1224 1225 cvScale( &_h1, &_h1, 1./h1_norm ); 1226 cvScale( &_h2, &_h2, 1./h2_norm ); 1227 cvScale( &_h3, &_t, 2./(h1_norm + h2_norm)); 1228 cvCrossProduct( &_h1, &_h2, &_h3 ); 1229 1230 cvRodrigues2( &_H, &_r ); 1231 cvRodrigues2( &_r, &_H ); 1232 cvMatMulAdd( &_H, &T_transform, &_t, &_t ); 1233 cvMatMul( &_H, R_transform, &_R ); 1234 cvRodrigues2( &_R, &_r ); 1235 } 1236 else 1237 { 1238 // non-planar structure. Use DLT method 1239 double* L; 1240 double LL[12*12], LW[12], LV[12*12], sc; 1241 CvMat _LL = cvMat( 12, 12, CV_64F, LL ); 1242 CvMat _LW = cvMat( 12, 1, CV_64F, LW ); 1243 CvMat _LV = cvMat( 12, 12, CV_64F, LV ); 1244 CvMat _RRt, _RR, _tt; 1245 CvPoint3D64f* M = (CvPoint3D64f*)_M->data.db; 1246 CvPoint2D64f* mn = (CvPoint2D64f*)_mn->data.db; 1247 1248 CV_CALL( _L = cvCreateMat( 2*count, 12, CV_64F )); 1249 L = _L->data.db; 1250 1251 for( i = 0; i < count; i++, L += 24 ) 1252 { 1253 double x = -mn[i].x, y = -mn[i].y; 1254 L[0] = L[16] = M[i].x; 1255 L[1] = L[17] = M[i].y; 1256 L[2] = L[18] = M[i].z; 1257 L[3] = L[19] = 1.; 1258 L[4] = L[5] = L[6] = L[7] = 0.; 1259 L[12] = L[13] = L[14] = L[15] = 0.; 1260 L[8] = x*M[i].x; 1261 L[9] = x*M[i].y; 1262 L[10] = x*M[i].z; 1263 L[11] = x; 1264 L[20] = y*M[i].x; 1265 L[21] = y*M[i].y; 1266 L[22] = y*M[i].z; 1267 L[23] = y; 1268 } 1269 1270 cvMulTransposed( _L, &_LL, 1 ); 1271 cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T ); 1272 _RRt = cvMat( 3, 4, CV_64F, LV + 11*12 ); 1273 cvGetCols( &_RRt, &_RR, 0, 3 ); 1274 cvGetCol( &_RRt, &_tt, 3 ); 1275 if( cvDet(&_RR) < 0 ) 1276 cvScale( &_RRt, &_RRt, -1 ); 1277 sc = cvNorm(&_RR); 1278 cvSVD( &_RR, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T ); 1279 cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T ); 1280 cvScale( &_tt, &_t, cvNorm(&_R)/sc ); 1281 cvRodrigues2( &_R, &_r ); 1282 cvReleaseMat( &_L ); 1283 } 1284 1285 cvReshape( _M, _M, 3, 1 ); 1286 cvReshape( _mn, _mn, 2, 1 ); 1287 1288 CV_CALL( _J = cvCreateMat( 2*count, 6, CV_64FC1 )); 1289 cvGetCols( _J, &_dpdr, 0, 3 ); 1290 cvGetCols( _J, &_dpdt, 3, 6 ); 1291 1292 // refine extrinsic parameters using iterative algorithm 1293 for( i = 0; i < max_iter; i++ ) 1294 { 1295 double n1, n2; 1296 cvReshape( _mn, _mn, 2, 1 ); 1297 cvProjectPoints2( _M, &_r, &_t, &_A, distCoeffs, 1298 _mn, &_dpdr, &_dpdt, 0, 0, 0 ); 1299 cvSub( _m, _mn, _mn ); 1300 cvReshape( _mn, _mn, 1, 2*count ); 1301 1302 cvMulTransposed( _J, &_JtJ, 1 ); 1303 cvGEMM( _J, _mn, 1, 0, 0, &_JtErr, CV_GEMM_A_T ); 1304 cvSVD( &_JtJ, &_JtJW, 0, &_JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T ); 1305 if( JtJW[5]/JtJW[0] < 1e-12 ) 1306 break; 1307 cvSVBkSb( &_JtJW, &_JtJV, &_JtJV, &_JtErr, 1308 &_delta, CV_SVD_U_T + CV_SVD_V_T ); 1309 cvAdd( &_delta, &_param, &_param ); 1310 n1 = cvNorm( &_delta ); 1311 n2 = cvNorm( &_param ); 1312 if( n1/n2 < 1e-10 ) 1313 break; 1314 } 1315 1316 _r = cvMat( rvec->rows, rvec->cols, 1317 CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param ); 1318 _t = cvMat( tvec->rows, tvec->cols, 1319 CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3 ); 1320 1321 cvConvert( &_r, rvec ); 1322 cvConvert( &_t, tvec ); 1323 1324 __END__; 1325 1326 cvReleaseMat( &_M ); 1327 cvReleaseMat( &_Mxy ); 1328 cvReleaseMat( &_m ); 1329 cvReleaseMat( &_mn ); 1330 cvReleaseMat( &_L ); 1331 cvReleaseMat( &_J ); 1332 } 1333 1334 1335 CV_IMPL void 1336 cvInitIntrinsicParams2D( const CvMat* objectPoints, 1337 const CvMat* imagePoints, 1338 const CvMat* npoints, 1339 CvSize imageSize, 1340 CvMat* cameraMatrix, 1341 double aspectRatio ) 1342 { 1343 CvMat *_A = 0, *_b = 0, *_allH = 0, *_allK = 0; 1344 1345 CV_FUNCNAME( "cvInitIntrinsicParams2D" ); 1346 1347 __BEGIN__; 1348 1349 int i, j, pos, nimages, total, ni = 0; 1350 double a[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 }; 1351 double H[9], f[2]; 1352 CvMat _a = cvMat( 3, 3, CV_64F, a ); 1353 CvMat _H = cvMat( 3, 3, CV_64F, H ); 1354 CvMat _f = cvMat( 2, 1, CV_64F, f ); 1355 1356 assert( CV_MAT_TYPE(npoints->type) == CV_32SC1 && 1357 CV_IS_MAT_CONT(npoints->type) ); 1358 nimages = npoints->rows + npoints->cols - 1; 1359 1360 if( (CV_MAT_TYPE(objectPoints->type) != CV_32FC3 && 1361 CV_MAT_TYPE(objectPoints->type) != CV_64FC3) || 1362 (CV_MAT_TYPE(imagePoints->type) != CV_32FC2 && 1363 CV_MAT_TYPE(imagePoints->type) != CV_64FC2) ) 1364 CV_ERROR( CV_StsUnsupportedFormat, "Both object points and image points must be 2D" ); 1365 1366 if( objectPoints->rows != 1 || imagePoints->rows != 1 ) 1367 CV_ERROR( CV_StsBadSize, "object points and image points must be a single-row matrices" ); 1368 1369 _A = cvCreateMat( 2*nimages, 2, CV_64F ); 1370 _b = cvCreateMat( 2*nimages, 1, CV_64F ); 1371 a[2] = (imageSize.width - 1)*0.5; 1372 a[5] = (imageSize.height - 1)*0.5; 1373 _allH = cvCreateMat( nimages, 9, CV_64F ); 1374 1375 total = cvRound(cvSum(npoints).val[0]); 1376 1377 // extract vanishing points in order to obtain initial value for the focal length 1378 for( i = 0, pos = 0; i < nimages; i++, pos += ni ) 1379 { 1380 double* Ap = _A->data.db + i*4; 1381 double* bp = _b->data.db + i*2; 1382 ni = npoints->data.i[i]; 1383 double h[3], v[3], d1[3], d2[3]; 1384 double n[4] = {0,0,0,0}; 1385 CvMat _m, _M; 1386 cvGetCols( objectPoints, &_M, pos, pos + ni ); 1387 cvGetCols( imagePoints, &_m, pos, pos + ni ); 1388 1389 cvFindHomography( &_M, &_m, &_H ); 1390 memcpy( _allH->data.db + i*9, H, sizeof(H) ); 1391 1392 H[0] -= H[6]*a[2]; H[1] -= H[7]*a[2]; H[2] -= H[8]*a[2]; 1393 H[3] -= H[6]*a[5]; H[4] -= H[7]*a[5]; H[5] -= H[8]*a[5]; 1394 1395 for( j = 0; j < 3; j++ ) 1396 { 1397 double t0 = H[j*3], t1 = H[j*3+1]; 1398 h[j] = t0; v[j] = t1; 1399 d1[j] = (t0 + t1)*0.5; 1400 d2[j] = (t0 - t1)*0.5; 1401 n[0] += t0*t0; n[1] += t1*t1; 1402 n[2] += d1[j]*d1[j]; n[3] += d2[j]*d2[j]; 1403 } 1404 1405 for( j = 0; j < 4; j++ ) 1406 n[j] = 1./sqrt(n[j]); 1407 1408 for( j = 0; j < 3; j++ ) 1409 { 1410 h[j] *= n[0]; v[j] *= n[1]; 1411 d1[j] *= n[2]; d2[j] *= n[3]; 1412 } 1413 1414 Ap[0] = h[0]*v[0]; Ap[1] = h[1]*v[1]; 1415 Ap[2] = d1[0]*d2[0]; Ap[3] = d1[1]*d2[1]; 1416 bp[0] = -h[2]*v[2]; bp[1] = -d1[2]*d2[2]; 1417 } 1418 1419 cvSolve( _A, _b, &_f, CV_LSQ | CV_SVD ); 1420 a[0] = sqrt(fabs(1./f[0])); 1421 a[4] = sqrt(fabs(1./f[1])); 1422 if( aspectRatio != 0 ) 1423 { 1424 double tf = (a[0] + a[4])/(aspectRatio + 1.); 1425 a[0] = aspectRatio*tf; 1426 a[4] = tf; 1427 } 1428 1429 cvConvert( &_a, cameraMatrix ); 1430 1431 __END__; 1432 1433 cvReleaseMat( &_A ); 1434 cvReleaseMat( &_b ); 1435 cvReleaseMat( &_allH ); 1436 cvReleaseMat( &_allK ); 1437 } 1438 1439 1440 /* finds intrinsic and extrinsic camera parameters 1441 from a few views of known calibration pattern */ 1442 CV_IMPL void 1443 cvCalibrateCamera2( const CvMat* objectPoints, 1444 const CvMat* imagePoints, 1445 const CvMat* npoints, 1446 CvSize imageSize, 1447 CvMat* cameraMatrix, CvMat* distCoeffs, 1448 CvMat* rvecs, CvMat* tvecs, 1449 int flags ) 1450 { 1451 const int NINTRINSIC = 9; 1452 CvMat *_M = 0, *_m = 0, *_Ji = 0, *_Je = 0, *_err = 0; 1453 CvLevMarq solver; 1454 1455 CV_FUNCNAME( "cvCalibrateCamera2" ); 1456 1457 __BEGIN__; 1458 1459 double A[9], k[5] = {0,0,0,0,0}; 1460 CvMat _A = cvMat(3, 3, CV_64F, A), _k; 1461 int i, nimages, maxPoints = 0, ni = 0, pos, total = 0, nparams, npstep, cn; 1462 double aspectRatio = 0.; 1463 1464 // 0. check the parameters & allocate buffers 1465 if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(imagePoints) || 1466 !CV_IS_MAT(npoints) || !CV_IS_MAT(cameraMatrix) || !CV_IS_MAT(distCoeffs) ) 1467 CV_ERROR( CV_StsBadArg, "One of required vector arguments is not a valid matrix" ); 1468 1469 if( imageSize.width <= 0 || imageSize.height <= 0 ) 1470 CV_ERROR( CV_StsOutOfRange, "image width and height must be positive" ); 1471 1472 if( CV_MAT_TYPE(npoints->type) != CV_32SC1 || 1473 (npoints->rows != 1 && npoints->cols != 1) ) 1474 CV_ERROR( CV_StsUnsupportedFormat, 1475 "the array of point counters must be 1-dimensional integer vector" ); 1476 1477 nimages = npoints->rows*npoints->cols; 1478 npstep = npoints->rows == 1 ? 1 : npoints->step/CV_ELEM_SIZE(npoints->type); 1479 1480 if( rvecs ) 1481 { 1482 cn = CV_MAT_CN(rvecs->type); 1483 if( !CV_IS_MAT(rvecs) || 1484 (CV_MAT_DEPTH(rvecs->type) != CV_32F && CV_MAT_DEPTH(rvecs->type) != CV_64F) || 1485 ((rvecs->rows != nimages || (rvecs->cols*cn != 3 && rvecs->cols*cn != 9)) && 1486 (rvecs->rows != 1 || rvecs->cols != nimages || cn != 3)) ) 1487 CV_ERROR( CV_StsBadArg, "the output array of rotation vectors must be 3-channel " 1488 "1xn or nx1 array or 1-channel nx3 or nx9 array, where n is the number of views" ); 1489 } 1490 1491 if( tvecs ) 1492 { 1493 cn = CV_MAT_CN(tvecs->type); 1494 if( !CV_IS_MAT(tvecs) || 1495 (CV_MAT_DEPTH(tvecs->type) != CV_32F && CV_MAT_DEPTH(tvecs->type) != CV_64F) || 1496 ((tvecs->rows != nimages || tvecs->cols*cn != 3) && 1497 (tvecs->rows != 1 || tvecs->cols != nimages || cn != 3)) ) 1498 CV_ERROR( CV_StsBadArg, "the output array of translation vectors must be 3-channel " 1499 "1xn or nx1 array or 1-channel nx3 array, where n is the number of views" ); 1500 } 1501 1502 if( (CV_MAT_TYPE(cameraMatrix->type) != CV_32FC1 && 1503 CV_MAT_TYPE(cameraMatrix->type) != CV_64FC1) || 1504 cameraMatrix->rows != 3 || cameraMatrix->cols != 3 ) 1505 CV_ERROR( CV_StsBadArg, 1506 "Intrinsic parameters must be 3x3 floating-point matrix" ); 1507 1508 if( (CV_MAT_TYPE(distCoeffs->type) != CV_32FC1 && 1509 CV_MAT_TYPE(distCoeffs->type) != CV_64FC1) || 1510 (distCoeffs->cols != 1 && distCoeffs->rows != 1) || 1511 (distCoeffs->cols*distCoeffs->rows != 4 && 1512 distCoeffs->cols*distCoeffs->rows != 5) ) 1513 CV_ERROR( CV_StsBadArg, 1514 "Distortion coefficients must be 4x1, 1x4, 5x1 or 1x5 floating-point matrix" ); 1515 1516 for( i = 0; i < nimages; i++ ) 1517 { 1518 ni = npoints->data.i[i*npstep]; 1519 if( ni < 4 ) 1520 { 1521 char buf[100]; 1522 sprintf( buf, "The number of points in the view #%d is < 4", i ); 1523 CV_ERROR( CV_StsOutOfRange, buf ); 1524 } 1525 maxPoints = MAX( maxPoints, ni ); 1526 total += ni; 1527 } 1528 1529 CV_CALL( _M = cvCreateMat( 1, total, CV_64FC3 )); 1530 CV_CALL( _m = cvCreateMat( 1, total, CV_64FC2 )); 1531 1532 CV_CALL( cvConvertPointsHomogeneous( objectPoints, _M )); 1533 CV_CALL( cvConvertPointsHomogeneous( imagePoints, _m )); 1534 1535 nparams = NINTRINSIC + nimages*6; 1536 CV_CALL( _Ji = cvCreateMat( maxPoints*2, NINTRINSIC, CV_64FC1 )); 1537 CV_CALL( _Je = cvCreateMat( maxPoints*2, 6, CV_64FC1 )); 1538 CV_CALL( _err = cvCreateMat( maxPoints*2, 1, CV_64FC1 )); 1539 cvZero( _Ji ); 1540 1541 _k = cvMat( distCoeffs->rows, distCoeffs->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k); 1542 if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 4 ) 1543 flags |= CV_CALIB_FIX_K3; 1544 1545 // 1. initialize intrinsic parameters & LM solver 1546 if( flags & CV_CALIB_USE_INTRINSIC_GUESS ) 1547 { 1548 cvConvert( cameraMatrix, &_A ); 1549 if( A[0] <= 0 || A[4] <= 0 ) 1550 CV_ERROR( CV_StsOutOfRange, "Focal length (fx and fy) must be positive" ); 1551 if( A[2] < 0 || A[2] >= imageSize.width || 1552 A[5] < 0 || A[5] >= imageSize.height ) 1553 CV_ERROR( CV_StsOutOfRange, "Principal point must be within the image" ); 1554 if( fabs(A[1]) > 1e-5 ) 1555 CV_ERROR( CV_StsOutOfRange, "Non-zero skew is not supported by the function" ); 1556 if( fabs(A[3]) > 1e-5 || fabs(A[6]) > 1e-5 || 1557 fabs(A[7]) > 1e-5 || fabs(A[8]-1) > 1e-5 ) 1558 CV_ERROR( CV_StsOutOfRange, 1559 "The intrinsic matrix must have [fx 0 cx; 0 fy cy; 0 0 1] shape" ); 1560 A[1] = A[3] = A[6] = A[7] = 0.; 1561 A[8] = 1.; 1562 1563 if( flags & CV_CALIB_FIX_ASPECT_RATIO ) 1564 aspectRatio = A[0]/A[4]; 1565 cvConvert( distCoeffs, &_k ); 1566 } 1567 else 1568 { 1569 CvScalar mean, sdv; 1570 cvAvgSdv( _M, &mean, &sdv ); 1571 if( (fabs(mean.val[2]) > 1e-5 && fabs(mean.val[2] - 1) > 1e-5) || fabs(sdv.val[2]) > 1e-5 ) 1572 CV_ERROR( CV_StsBadArg, 1573 "For non-planar calibration rigs the initial intrinsic matrix must be specified" ); 1574 for( i = 0; i < total; i++ ) 1575 ((CvPoint3D64f*)_M->data.db)[i].z = 0.; 1576 1577 if( flags & CV_CALIB_FIX_ASPECT_RATIO ) 1578 { 1579 aspectRatio = cvmGet(cameraMatrix,0,0); 1580 aspectRatio /= cvmGet(cameraMatrix,1,1); 1581 if( aspectRatio < 0.01 || aspectRatio > 100 ) 1582 CV_ERROR( CV_StsOutOfRange, 1583 "The specified aspect ratio (=A[0][0]/A[1][1]) is incorrect" ); 1584 } 1585 cvInitIntrinsicParams2D( _M, _m, npoints, imageSize, &_A, aspectRatio ); 1586 } 1587 1588 solver.init( nparams, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,DBL_EPSILON) ); 1589 1590 { 1591 double* param = solver.param->data.db; 1592 uchar* mask = solver.mask->data.ptr; 1593 1594 param[0] = A[0]; param[1] = A[4]; param[2] = A[2]; param[3] = A[5]; 1595 param[4] = k[0]; param[5] = k[1]; param[6] = k[2]; param[7] = k[3]; 1596 param[8] = k[4]; 1597 1598 if( flags & CV_CALIB_FIX_FOCAL_LENGTH ) 1599 mask[0] = mask[1] = 0; 1600 if( flags & CV_CALIB_FIX_PRINCIPAL_POINT ) 1601 mask[2] = mask[3] = 0; 1602 if( flags & CV_CALIB_ZERO_TANGENT_DIST ) 1603 { 1604 param[6] = param[7] = 0; 1605 mask[6] = mask[7] = 0; 1606 } 1607 if( flags & CV_CALIB_FIX_K1 ) 1608 mask[4] = 0; 1609 if( flags & CV_CALIB_FIX_K2 ) 1610 mask[5] = 0; 1611 if( flags & CV_CALIB_FIX_K3 ) 1612 mask[8] = 0; 1613 } 1614 1615 // 2. initialize extrinsic parameters 1616 for( i = 0, pos = 0; i < nimages; i++, pos += ni ) 1617 { 1618 CvMat _Mi, _mi, _ri, _ti; 1619 ni = npoints->data.i[i*npstep]; 1620 1621 cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 ); 1622 cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 ); 1623 1624 cvGetCols( _M, &_Mi, pos, pos + ni ); 1625 cvGetCols( _m, &_mi, pos, pos + ni ); 1626 1627 cvFindExtrinsicCameraParams2( &_Mi, &_mi, &_A, &_k, &_ri, &_ti ); 1628 } 1629 1630 // 3. run the optimization 1631 for(;;) 1632 { 1633 const CvMat* _param = 0; 1634 CvMat *_JtJ = 0, *_JtErr = 0; 1635 double* _errNorm = 0; 1636 bool proceed = solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ); 1637 double *param = solver.param->data.db, *pparam = solver.prevParam->data.db; 1638 1639 if( flags & CV_CALIB_FIX_ASPECT_RATIO ) 1640 { 1641 param[0] = param[1]*aspectRatio; 1642 pparam[0] = pparam[1]*aspectRatio; 1643 } 1644 1645 A[0] = param[0]; A[4] = param[1]; 1646 A[2] = param[2]; A[5] = param[3]; 1647 k[0] = param[4]; k[1] = param[5]; k[2] = param[6]; 1648 k[3] = param[7]; 1649 k[4] = param[8]; 1650 1651 if( !proceed ) 1652 break; 1653 1654 for( i = 0, pos = 0; i < nimages; i++, pos += ni ) 1655 { 1656 CvMat _Mi, _mi, _ri, _ti, _dpdr, _dpdt, _dpdf, _dpdc, _dpdk, _mp, _part; 1657 ni = npoints->data.i[i*npstep]; 1658 1659 cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 ); 1660 cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 ); 1661 1662 cvGetCols( _M, &_Mi, pos, pos + ni ); 1663 cvGetCols( _m, &_mi, pos, pos + ni ); 1664 1665 _Je->rows = _Ji->rows = _err->rows = ni*2; 1666 cvGetCols( _Je, &_dpdr, 0, 3 ); 1667 cvGetCols( _Je, &_dpdt, 3, 6 ); 1668 cvGetCols( _Ji, &_dpdf, 0, 2 ); 1669 cvGetCols( _Ji, &_dpdc, 2, 4 ); 1670 cvGetCols( _Ji, &_dpdk, 4, NINTRINSIC ); 1671 cvReshape( _err, &_mp, 2, 1 ); 1672 1673 if( _JtJ || _JtErr ) 1674 { 1675 cvProjectPoints2( &_Mi, &_ri, &_ti, &_A, &_k, &_mp, &_dpdr, &_dpdt, 1676 (flags & CV_CALIB_FIX_FOCAL_LENGTH) ? 0 : &_dpdf, 1677 (flags & CV_CALIB_FIX_PRINCIPAL_POINT) ? 0 : &_dpdc, &_dpdk, 1678 (flags & CV_CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0); 1679 } 1680 else 1681 cvProjectPoints2( &_Mi, &_ri, &_ti, &_A, &_k, &_mp ); 1682 1683 cvSub( &_mp, &_mi, &_mp ); 1684 1685 if( _JtJ || _JtErr ) 1686 { 1687 cvGetSubRect( _JtJ, &_part, cvRect(0,0,NINTRINSIC,NINTRINSIC) ); 1688 cvGEMM( _Ji, _Ji, 1, &_part, 1, &_part, CV_GEMM_A_T ); 1689 1690 cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,NINTRINSIC+i*6,6,6) ); 1691 cvGEMM( _Je, _Je, 1, 0, 0, &_part, CV_GEMM_A_T ); 1692 1693 cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,0,6,NINTRINSIC) ); 1694 cvGEMM( _Ji, _Je, 1, 0, 0, &_part, CV_GEMM_A_T ); 1695 1696 cvGetRows( _JtErr, &_part, 0, NINTRINSIC ); 1697 cvGEMM( _Ji, _err, 1, &_part, 1, &_part, CV_GEMM_A_T ); 1698 1699 cvGetRows( _JtErr, &_part, NINTRINSIC + i*6, NINTRINSIC + (i+1)*6 ); 1700 cvGEMM( _Je, _err, 1, 0, 0, &_part, CV_GEMM_A_T ); 1701 } 1702 1703 if( _errNorm ) 1704 { 1705 double errNorm = cvNorm( &_mp, 0, CV_L2 ); 1706 *_errNorm += errNorm*errNorm; 1707 } 1708 } 1709 } 1710 1711 // 4. store the results 1712 cvConvert( &_A, cameraMatrix ); 1713 cvConvert( &_k, distCoeffs ); 1714 1715 for( i = 0; i < nimages; i++ ) 1716 { 1717 CvMat src, dst; 1718 if( rvecs ) 1719 { 1720 src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 ); 1721 if( rvecs->rows == nimages && rvecs->cols*CV_MAT_CN(rvecs->type) == 9 ) 1722 { 1723 dst = cvMat( 3, 3, CV_MAT_DEPTH(rvecs->type), 1724 rvecs->data.ptr + rvecs->step*i ); 1725 cvRodrigues2( &src, &_A ); 1726 cvConvert( &_A, &dst ); 1727 } 1728 else 1729 { 1730 dst = cvMat( 3, 1, CV_MAT_DEPTH(rvecs->type), rvecs->rows == 1 ? 1731 rvecs->data.ptr + i*CV_ELEM_SIZE(rvecs->type) : 1732 rvecs->data.ptr + rvecs->step*i ); 1733 cvConvert( &src, &dst ); 1734 } 1735 } 1736 if( tvecs ) 1737 { 1738 src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 + 3 ); 1739 dst = cvMat( 3, 1, CV_MAT_TYPE(tvecs->type), tvecs->rows == 1 ? 1740 tvecs->data.ptr + i*CV_ELEM_SIZE(tvecs->type) : 1741 tvecs->data.ptr + tvecs->step*i ); 1742 cvConvert( &src, &dst ); 1743 } 1744 } 1745 1746 __END__; 1747 1748 cvReleaseMat( &_M ); 1749 cvReleaseMat( &_m ); 1750 cvReleaseMat( &_Ji ); 1751 cvReleaseMat( &_Je ); 1752 cvReleaseMat( &_err ); 1753 } 1754 1755 1756 void cvCalibrationMatrixValues( const CvMat *calibMatr, CvSize imgSize, 1757 double apertureWidth, double apertureHeight, double *fovx, double *fovy, 1758 double *focalLength, CvPoint2D64f *principalPoint, double *pasp ) 1759 { 1760 double alphax, alphay, mx, my; 1761 int imgWidth = imgSize.width, imgHeight = imgSize.height; 1762 1763 CV_FUNCNAME("cvCalibrationMatrixValues"); 1764 __BEGIN__; 1765 1766 /* Validate parameters. */ 1767 1768 if(calibMatr == 0) 1769 CV_ERROR(CV_StsNullPtr, "Some of parameters is a NULL pointer!"); 1770 1771 if(!CV_IS_MAT(calibMatr)) 1772 CV_ERROR(CV_StsUnsupportedFormat, "Input parameters must be a matrices!"); 1773 1774 if(calibMatr->cols != 3 || calibMatr->rows != 3) 1775 CV_ERROR(CV_StsUnmatchedSizes, "Size of matrices must be 3x3!"); 1776 1777 alphax = cvmGet(calibMatr, 0, 0); 1778 alphay = cvmGet(calibMatr, 1, 1); 1779 assert(imgWidth != 0 && imgHeight != 0 && alphax != 0.0 && alphay != 0.0); 1780 1781 /* Calculate pixel aspect ratio. */ 1782 if(pasp) 1783 *pasp = alphay / alphax; 1784 1785 /* Calculate number of pixel per realworld unit. */ 1786 1787 if(apertureWidth != 0.0 && apertureHeight != 0.0) { 1788 mx = imgWidth / apertureWidth; 1789 my = imgHeight / apertureHeight; 1790 } else { 1791 mx = 1.0; 1792 my = *pasp; 1793 } 1794 1795 /* Calculate fovx and fovy. */ 1796 1797 if(fovx) 1798 *fovx = 2 * atan(imgWidth / (2 * alphax)) * 180.0 / CV_PI; 1799 1800 if(fovy) 1801 *fovy = 2 * atan(imgHeight / (2 * alphay)) * 180.0 / CV_PI; 1802 1803 /* Calculate focal length. */ 1804 1805 if(focalLength) 1806 *focalLength = alphax / mx; 1807 1808 /* Calculate principle point. */ 1809 1810 if(principalPoint) 1811 *principalPoint = cvPoint2D64f(cvmGet(calibMatr, 0, 2) / mx, cvmGet(calibMatr, 1, 2) / my); 1812 1813 __END__; 1814 } 1815 1816 1817 //////////////////////////////// Stereo Calibration /////////////////////////////////// 1818 1819 static int dbCmp( const void* _a, const void* _b ) 1820 { 1821 double a = *(const double*)_a; 1822 double b = *(const double*)_b; 1823 1824 return (a > b) - (a < b); 1825 } 1826 1827 1828 void cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1, 1829 const CvMat* _imagePoints2, const CvMat* _npoints, 1830 CvMat* _cameraMatrix1, CvMat* _distCoeffs1, 1831 CvMat* _cameraMatrix2, CvMat* _distCoeffs2, 1832 CvSize imageSize, CvMat* _R, CvMat* _T, 1833 CvMat* _E, CvMat* _F, 1834 CvTermCriteria termCrit, int flags ) 1835 { 1836 const int NINTRINSIC = 9; 1837 CvMat* npoints = 0; 1838 CvMat* err = 0; 1839 CvMat* J_LR = 0; 1840 CvMat* Je = 0; 1841 CvMat* Ji = 0; 1842 CvMat* imagePoints[2] = {0,0}; 1843 CvMat* objectPoints = 0; 1844 CvMat* RT0 = 0; 1845 CvLevMarq solver; 1846 1847 CV_FUNCNAME( "cvStereoCalibrate" ); 1848 1849 __BEGIN__; 1850 1851 double A[2][9], dk[2][5]={{0,0,0,0,0},{0,0,0,0,0}}, rlr[9]; 1852 CvMat K[2], Dist[2], om_LR, T_LR; 1853 CvMat R_LR = cvMat(3, 3, CV_64F, rlr); 1854 int i, k, p, ni = 0, ofs, nimages, pointsTotal, maxPoints = 0; 1855 int nparams; 1856 bool recomputeIntrinsics = false; 1857 double aspectRatio[2] = {0,0}; 1858 1859 CV_ASSERT( CV_IS_MAT(_imagePoints1) && CV_IS_MAT(_imagePoints2) && 1860 CV_IS_MAT(_objectPoints) && CV_IS_MAT(_npoints) && 1861 CV_IS_MAT(_R) && CV_IS_MAT(_T) ); 1862 1863 CV_ASSERT( CV_ARE_TYPES_EQ(_imagePoints1, _imagePoints2) && 1864 CV_ARE_DEPTHS_EQ(_imagePoints1, _objectPoints) ); 1865 1866 CV_ASSERT( (_npoints->cols == 1 || _npoints->rows == 1) && 1867 CV_MAT_TYPE(_npoints->type) == CV_32SC1 ); 1868 1869 nimages = _npoints->cols + _npoints->rows - 1; 1870 npoints = cvCreateMat( _npoints->rows, _npoints->cols, _npoints->type ); 1871 cvCopy( _npoints, npoints ); 1872 1873 for( i = 0, pointsTotal = 0; i < nimages; i++ ) 1874 { 1875 maxPoints = MAX(maxPoints, npoints->data.i[i]); 1876 pointsTotal += npoints->data.i[i]; 1877 } 1878 1879 objectPoints = cvCreateMat( _objectPoints->rows, _objectPoints->cols, 1880 CV_64FC(CV_MAT_CN(_objectPoints->type))); 1881 cvConvert( _objectPoints, objectPoints ); 1882 cvReshape( objectPoints, objectPoints, 3, 1 ); 1883 1884 for( k = 0; k < 2; k++ ) 1885 { 1886 const CvMat* points = k == 0 ? _imagePoints1 : _imagePoints2; 1887 const CvMat* cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2; 1888 const CvMat* distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2; 1889 1890 int cn = CV_MAT_CN(_imagePoints1->type); 1891 CV_ASSERT( (CV_MAT_DEPTH(_imagePoints1->type) == CV_32F || 1892 CV_MAT_DEPTH(_imagePoints1->type) == CV_64F) && 1893 ((_imagePoints1->rows == pointsTotal && _imagePoints1->cols*cn == 2) || 1894 (_imagePoints1->rows == 1 && _imagePoints1->cols == pointsTotal && cn == 2)) ); 1895 1896 K[k] = cvMat(3,3,CV_64F,A[k]); 1897 Dist[k] = cvMat(1,5,CV_64F,dk[k]); 1898 1899 imagePoints[k] = cvCreateMat( points->rows, points->cols, CV_64FC(CV_MAT_CN(points->type))); 1900 cvConvert( points, imagePoints[k] ); 1901 cvReshape( imagePoints[k], imagePoints[k], 2, 1 ); 1902 1903 if( flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS| 1904 CV_CALIB_FIX_ASPECT_RATIO|CV_CALIB_FIX_FOCAL_LENGTH) ) 1905 cvConvert( cameraMatrix, &K[k] ); 1906 1907 if( flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS| 1908 CV_CALIB_FIX_K1|CV_CALIB_FIX_K2|CV_CALIB_FIX_K3) ) 1909 { 1910 CvMat tdist = cvMat( distCoeffs->rows, distCoeffs->cols, 1911 CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), Dist[k].data.db ); 1912 cvConvert( distCoeffs, &tdist ); 1913 } 1914 1915 if( !(flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS))) 1916 { 1917 cvCalibrateCamera2( objectPoints, imagePoints[k], 1918 npoints, imageSize, &K[k], &Dist[k], 0, 0, flags ); 1919 } 1920 } 1921 1922 if( flags & CV_CALIB_SAME_FOCAL_LENGTH ) 1923 { 1924 static const int avg_idx[] = { 0, 4, 2, 5, -1 }; 1925 for( k = 0; avg_idx[k] >= 0; k++ ) 1926 A[0][avg_idx[k]] = A[1][avg_idx[k]] = (A[0][avg_idx[k]] + A[1][avg_idx[k]])*0.5; 1927 } 1928 1929 if( flags & CV_CALIB_FIX_ASPECT_RATIO ) 1930 { 1931 for( k = 0; k < 2; k++ ) 1932 aspectRatio[k] = A[k][0]/A[k][4]; 1933 } 1934 1935 recomputeIntrinsics = (flags & CV_CALIB_FIX_INTRINSIC) == 0; 1936 1937 err = cvCreateMat( maxPoints*2, 1, CV_64F ); 1938 Je = cvCreateMat( maxPoints*2, 6, CV_64F ); 1939 J_LR = cvCreateMat( maxPoints*2, 6, CV_64F ); 1940 Ji = cvCreateMat( maxPoints*2, NINTRINSIC, CV_64F ); 1941 cvZero( Ji ); 1942 1943 // we optimize for the inter-camera R(3),t(3), then, optionally, 1944 // for intrinisic parameters of each camera ((fx,fy,cx,cy,k1,k2,p1,p2) ~ 8 parameters). 1945 nparams = 6*(nimages+1) + (recomputeIntrinsics ? NINTRINSIC*2 : 0); 1946 1947 // storage for initial [om(R){i}|t{i}] (in order to compute the median for each component) 1948 RT0 = cvCreateMat( 6, nimages, CV_64F ); 1949 1950 solver.init( nparams, 0, termCrit ); 1951 if( recomputeIntrinsics ) 1952 { 1953 uchar* imask = solver.mask->data.ptr + nparams - NINTRINSIC*2; 1954 if( flags & CV_CALIB_FIX_ASPECT_RATIO ) 1955 imask[0] = imask[NINTRINSIC] = 0; 1956 if( flags & CV_CALIB_FIX_FOCAL_LENGTH ) 1957 imask[0] = imask[1] = imask[NINTRINSIC] = imask[NINTRINSIC+1] = 0; 1958 if( flags & CV_CALIB_FIX_PRINCIPAL_POINT ) 1959 imask[2] = imask[3] = imask[NINTRINSIC+2] = imask[NINTRINSIC+3] = 0; 1960 if( flags & CV_CALIB_ZERO_TANGENT_DIST ) 1961 imask[6] = imask[7] = imask[NINTRINSIC+6] = imask[NINTRINSIC+7] = 0; 1962 if( flags & CV_CALIB_FIX_K1 ) 1963 imask[4] = imask[NINTRINSIC+4] = 0; 1964 if( flags & CV_CALIB_FIX_K2 ) 1965 imask[5] = imask[NINTRINSIC+5] = 0; 1966 if( flags & CV_CALIB_FIX_K3 ) 1967 imask[8] = imask[NINTRINSIC+8] = 0; 1968 } 1969 1970 /* 1971 Compute initial estimate of pose 1972 1973 For each image, compute: 1974 R(om) is the rotation matrix of om 1975 om(R) is the rotation vector of R 1976 R_ref = R(om_right) * R(om_left)' 1977 T_ref_list = [T_ref_list; T_right - R_ref * T_left] 1978 om_ref_list = {om_ref_list; om(R_ref)] 1979 1980 om = median(om_ref_list) 1981 T = median(T_ref_list) 1982 */ 1983 for( i = ofs = 0; i < nimages; ofs += ni, i++ ) 1984 { 1985 ni = npoints->data.i[i]; 1986 CvMat objpt_i; 1987 double _om[2][3], r[2][9], t[2][3]; 1988 CvMat om[2], R[2], T[2], imgpt_i[2]; 1989 1990 objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3); 1991 for( k = 0; k < 2; k++ ) 1992 { 1993 imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2); 1994 om[k] = cvMat(3, 1, CV_64F, _om[k]); 1995 R[k] = cvMat(3, 3, CV_64F, r[k]); 1996 T[k] = cvMat(3, 1, CV_64F, t[k]); 1997 1998 // FIXME: here we ignore activePoints[k] because of 1999 // the limited API of cvFindExtrnisicCameraParams2 2000 cvFindExtrinsicCameraParams2( &objpt_i, &imgpt_i[k], &K[k], &Dist[k], &om[k], &T[k] ); 2001 cvRodrigues2( &om[k], &R[k] ); 2002 if( k == 0 ) 2003 { 2004 // save initial om_left and T_left 2005 solver.param->data.db[(i+1)*6] = _om[0][0]; 2006 solver.param->data.db[(i+1)*6 + 1] = _om[0][1]; 2007 solver.param->data.db[(i+1)*6 + 2] = _om[0][2]; 2008 solver.param->data.db[(i+1)*6 + 3] = t[0][0]; 2009 solver.param->data.db[(i+1)*6 + 4] = t[0][1]; 2010 solver.param->data.db[(i+1)*6 + 5] = t[0][2]; 2011 } 2012 } 2013 cvGEMM( &R[1], &R[0], 1, 0, 0, &R[0], CV_GEMM_B_T ); 2014 cvGEMM( &R[0], &T[0], -1, &T[1], 1, &T[1] ); 2015 cvRodrigues2( &R[0], &T[0] ); 2016 RT0->data.db[i] = t[0][0]; 2017 RT0->data.db[i + nimages] = t[0][1]; 2018 RT0->data.db[i + nimages*2] = t[0][2]; 2019 RT0->data.db[i + nimages*3] = t[1][0]; 2020 RT0->data.db[i + nimages*4] = t[1][1]; 2021 RT0->data.db[i + nimages*5] = t[1][2]; 2022 } 2023 2024 // find the medians and save the first 6 parameters 2025 for( i = 0; i < 6; i++ ) 2026 { 2027 qsort( RT0->data.db + i*nimages, nimages, CV_ELEM_SIZE(RT0->type), dbCmp ); 2028 solver.param->data.db[i] = nimages % 2 != 0 ? RT0->data.db[i*nimages + nimages/2] : 2029 (RT0->data.db[i*nimages + nimages/2 - 1] + RT0->data.db[i*nimages + nimages/2])*0.5; 2030 } 2031 2032 if( recomputeIntrinsics ) 2033 for( k = 0; k < 2; k++ ) 2034 { 2035 double* iparam = solver.param->data.db + (nimages+1)*6 + k*NINTRINSIC; 2036 if( flags & CV_CALIB_ZERO_TANGENT_DIST ) 2037 dk[k][2] = dk[k][3] = 0; 2038 iparam[0] = A[k][0]; iparam[1] = A[k][4]; iparam[2] = A[k][2]; iparam[3] = A[k][5]; 2039 iparam[4] = dk[k][0]; iparam[5] = dk[k][1]; iparam[6] = dk[k][2]; 2040 iparam[7] = dk[k][3]; iparam[8] = dk[k][4]; 2041 } 2042 2043 om_LR = cvMat(3, 1, CV_64F, solver.param->data.db); 2044 T_LR = cvMat(3, 1, CV_64F, solver.param->data.db + 3); 2045 2046 for(;;) 2047 { 2048 const CvMat* param = 0; 2049 CvMat tmpimagePoints; 2050 CvMat *JtJ = 0, *JtErr = 0; 2051 double* errNorm = 0; 2052 double _omR[3], _tR[3]; 2053 double _dr3dr1[9], _dr3dr2[9], /*_dt3dr1[9],*/ _dt3dr2[9], _dt3dt1[9], _dt3dt2[9]; 2054 CvMat dr3dr1 = cvMat(3, 3, CV_64F, _dr3dr1); 2055 CvMat dr3dr2 = cvMat(3, 3, CV_64F, _dr3dr2); 2056 //CvMat dt3dr1 = cvMat(3, 3, CV_64F, _dt3dr1); 2057 CvMat dt3dr2 = cvMat(3, 3, CV_64F, _dt3dr2); 2058 CvMat dt3dt1 = cvMat(3, 3, CV_64F, _dt3dt1); 2059 CvMat dt3dt2 = cvMat(3, 3, CV_64F, _dt3dt2); 2060 CvMat om[2], T[2], imgpt_i[2]; 2061 CvMat dpdrot_hdr, dpdt_hdr, dpdf_hdr, dpdc_hdr, dpdk_hdr; 2062 CvMat *dpdrot = &dpdrot_hdr, *dpdt = &dpdt_hdr, *dpdf = 0, *dpdc = 0, *dpdk = 0; 2063 2064 if( !solver.updateAlt( param, JtJ, JtErr, errNorm )) 2065 break; 2066 2067 cvRodrigues2( &om_LR, &R_LR ); 2068 om[1] = cvMat(3,1,CV_64F,_omR); 2069 T[1] = cvMat(3,1,CV_64F,_tR); 2070 2071 if( recomputeIntrinsics ) 2072 { 2073 double* iparam = solver.param->data.db + (nimages+1)*6; 2074 double* ipparam = solver.prevParam->data.db + (nimages+1)*6; 2075 dpdf = &dpdf_hdr; 2076 dpdc = &dpdc_hdr; 2077 dpdk = &dpdk_hdr; 2078 if( flags & CV_CALIB_SAME_FOCAL_LENGTH ) 2079 { 2080 iparam[NINTRINSIC] = iparam[0]; 2081 iparam[NINTRINSIC+1] = iparam[1]; 2082 ipparam[NINTRINSIC] = ipparam[0]; 2083 ipparam[NINTRINSIC+1] = ipparam[1]; 2084 } 2085 if( flags & CV_CALIB_FIX_ASPECT_RATIO ) 2086 { 2087 iparam[0] = iparam[1]*aspectRatio[0]; 2088 iparam[NINTRINSIC] = iparam[NINTRINSIC+1]*aspectRatio[1]; 2089 ipparam[0] = ipparam[1]*aspectRatio[0]; 2090 ipparam[NINTRINSIC] = ipparam[NINTRINSIC+1]*aspectRatio[1]; 2091 } 2092 for( k = 0; k < 2; k++ ) 2093 { 2094 A[k][0] = iparam[k*NINTRINSIC+0]; 2095 A[k][4] = iparam[k*NINTRINSIC+1]; 2096 A[k][2] = iparam[k*NINTRINSIC+2]; 2097 A[k][5] = iparam[k*NINTRINSIC+3]; 2098 dk[k][0] = iparam[k*NINTRINSIC+4]; 2099 dk[k][1] = iparam[k*NINTRINSIC+5]; 2100 dk[k][2] = iparam[k*NINTRINSIC+6]; 2101 dk[k][3] = iparam[k*NINTRINSIC+7]; 2102 dk[k][4] = iparam[k*NINTRINSIC+8]; 2103 } 2104 } 2105 2106 for( i = ofs = 0; i < nimages; ofs += ni, i++ ) 2107 { 2108 ni = npoints->data.i[i]; 2109 CvMat objpt_i, _part; 2110 2111 om[0] = cvMat(3,1,CV_64F,solver.param->data.db+(i+1)*6); 2112 T[0] = cvMat(3,1,CV_64F,solver.param->data.db+(i+1)*6+3); 2113 2114 if( JtJ || JtErr ) 2115 cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1], &dr3dr1, 0, 2116 &dr3dr2, 0, 0, &dt3dt1, &dt3dr2, &dt3dt2 ); 2117 else 2118 cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1] ); 2119 2120 objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3); 2121 err->rows = Je->rows = J_LR->rows = Ji->rows = ni*2; 2122 cvReshape( err, &tmpimagePoints, 2, 1 ); 2123 2124 cvGetCols( Ji, &dpdf_hdr, 0, 2 ); 2125 cvGetCols( Ji, &dpdc_hdr, 2, 4 ); 2126 cvGetCols( Ji, &dpdk_hdr, 4, NINTRINSIC ); 2127 cvGetCols( Je, &dpdrot_hdr, 0, 3 ); 2128 cvGetCols( Je, &dpdt_hdr, 3, 6 ); 2129 2130 for( k = 0; k < 2; k++ ) 2131 { 2132 double maxErr, l2err; 2133 imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2); 2134 2135 if( JtJ || JtErr ) 2136 cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k], 2137 &tmpimagePoints, dpdrot, dpdt, dpdf, dpdc, dpdk, 2138 (flags & CV_CALIB_FIX_ASPECT_RATIO) ? aspectRatio[k] : 0); 2139 else 2140 cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k], &tmpimagePoints ); 2141 cvSub( &tmpimagePoints, &imgpt_i[k], &tmpimagePoints ); 2142 2143 l2err = cvNorm( &tmpimagePoints, 0, CV_L2 ); 2144 maxErr = cvNorm( &tmpimagePoints, 0, CV_C ); 2145 2146 if( JtJ || JtErr ) 2147 { 2148 int iofs = (nimages+1)*6 + k*NINTRINSIC, eofs = (i+1)*6; 2149 assert( JtJ && JtErr ); 2150 2151 if( k == 1 ) 2152 { 2153 // d(err_{x|y}R) ~ de3 2154 // convert de3/{dr3,dt3} => de3{dr1,dt1} & de3{dr2,dt2} 2155 for( p = 0; p < ni*2; p++ ) 2156 { 2157 CvMat de3dr3 = cvMat( 1, 3, CV_64F, Je->data.ptr + Je->step*p ); 2158 CvMat de3dt3 = cvMat( 1, 3, CV_64F, de3dr3.data.db + 3 ); 2159 CvMat de3dr2 = cvMat( 1, 3, CV_64F, J_LR->data.ptr + J_LR->step*p ); 2160 CvMat de3dt2 = cvMat( 1, 3, CV_64F, de3dr2.data.db + 3 ); 2161 double _de3dr1[3], _de3dt1[3]; 2162 CvMat de3dr1 = cvMat( 1, 3, CV_64F, _de3dr1 ); 2163 CvMat de3dt1 = cvMat( 1, 3, CV_64F, _de3dt1 ); 2164 2165 cvMatMul( &de3dr3, &dr3dr1, &de3dr1 ); 2166 cvMatMul( &de3dt3, &dt3dt1, &de3dt1 ); 2167 2168 cvMatMul( &de3dr3, &dr3dr2, &de3dr2 ); 2169 cvMatMulAdd( &de3dt3, &dt3dr2, &de3dr2, &de3dr2 ); 2170 2171 cvMatMul( &de3dt3, &dt3dt2, &de3dt2 ); 2172 2173 cvCopy( &de3dr1, &de3dr3 ); 2174 cvCopy( &de3dt1, &de3dt3 ); 2175 } 2176 2177 cvGetSubRect( JtJ, &_part, cvRect(0, 0, 6, 6) ); 2178 cvGEMM( J_LR, J_LR, 1, &_part, 1, &_part, CV_GEMM_A_T ); 2179 2180 cvGetSubRect( JtJ, &_part, cvRect(eofs, 0, 6, 6) ); 2181 cvGEMM( J_LR, Je, 1, 0, 0, &_part, CV_GEMM_A_T ); 2182 2183 cvGetRows( JtErr, &_part, 0, 6 ); 2184 cvGEMM( J_LR, err, 1, &_part, 1, &_part, CV_GEMM_A_T ); 2185 } 2186 2187 cvGetSubRect( JtJ, &_part, cvRect(eofs, eofs, 6, 6) ); 2188 cvGEMM( Je, Je, 1, &_part, 1, &_part, CV_GEMM_A_T ); 2189 2190 cvGetRows( JtErr, &_part, eofs, eofs + 6 ); 2191 cvGEMM( Je, err, 1, &_part, 1, &_part, CV_GEMM_A_T ); 2192 2193 if( recomputeIntrinsics ) 2194 { 2195 cvGetSubRect( JtJ, &_part, cvRect(iofs, iofs, NINTRINSIC, NINTRINSIC) ); 2196 cvGEMM( Ji, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T ); 2197 cvGetSubRect( JtJ, &_part, cvRect(iofs, eofs, NINTRINSIC, 6) ); 2198 cvGEMM( Je, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T ); 2199 if( k == 1 ) 2200 { 2201 cvGetSubRect( JtJ, &_part, cvRect(iofs, 0, NINTRINSIC, 6) ); 2202 cvGEMM( J_LR, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T ); 2203 } 2204 cvGetRows( JtErr, &_part, iofs, iofs + NINTRINSIC ); 2205 cvGEMM( Ji, err, 1, &_part, 1, &_part, CV_GEMM_A_T ); 2206 } 2207 } 2208 2209 if( errNorm ) 2210 *errNorm += l2err*l2err; 2211 } 2212 } 2213 } 2214 2215 cvRodrigues2( &om_LR, &R_LR ); 2216 if( _R->rows == 1 || _R->cols == 1 ) 2217 cvConvert( &om_LR, _R ); 2218 else 2219 cvConvert( &R_LR, _R ); 2220 cvConvert( &T_LR, _T ); 2221 2222 if( recomputeIntrinsics ) 2223 { 2224 cvConvert( &K[0], _cameraMatrix1 ); 2225 cvConvert( &K[1], _cameraMatrix2 ); 2226 2227 for( k = 0; k < 2; k++ ) 2228 { 2229 CvMat* distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2; 2230 CvMat tdist = cvMat( distCoeffs->rows, distCoeffs->cols, 2231 CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), Dist[k].data.db ); 2232 cvConvert( &tdist, distCoeffs ); 2233 } 2234 } 2235 2236 if( _E || _F ) 2237 { 2238 double* t = T_LR.data.db; 2239 double tx[] = 2240 { 2241 0, -t[2], t[1], 2242 t[2], 0, -t[0], 2243 -t[1], t[0], 0 2244 }; 2245 CvMat Tx = cvMat(3, 3, CV_64F, tx); 2246 double e[9], f[9]; 2247 CvMat E = cvMat(3, 3, CV_64F, e); 2248 CvMat F = cvMat(3, 3, CV_64F, f); 2249 cvMatMul( &Tx, &R_LR, &E ); 2250 if( _E ) 2251 cvConvert( &E, _E ); 2252 if( _F ) 2253 { 2254 double ik[9]; 2255 CvMat iK = cvMat(3, 3, CV_64F, ik); 2256 cvInvert(&K[1], &iK); 2257 cvGEMM( &iK, &E, 1, 0, 0, &E, CV_GEMM_A_T ); 2258 cvInvert(&K[0], &iK); 2259 cvMatMul(&E, &iK, &F); 2260 cvConvertScale( &F, _F, fabs(f[8]) > 0 ? 1./f[8] : 1 ); 2261 } 2262 } 2263 2264 __END__; 2265 2266 cvReleaseMat( &npoints ); 2267 cvReleaseMat( &err ); 2268 cvReleaseMat( &J_LR ); 2269 cvReleaseMat( &Je ); 2270 cvReleaseMat( &Ji ); 2271 cvReleaseMat( &RT0 ); 2272 cvReleaseMat( &objectPoints ); 2273 cvReleaseMat( &imagePoints[0] ); 2274 cvReleaseMat( &imagePoints[1] ); 2275 } 2276 2277 2278 void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2, 2279 const CvMat* _distCoeffs1, const CvMat* _distCoeffs2, 2280 CvSize imageSize, const CvMat* _R, const CvMat* _T, 2281 CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, 2282 CvMat* _Q, int flags ) 2283 { 2284 double _om[3], _t[3], _uu[3]={0,0,0}, _r_r[3][3], _pp[3][4]; 2285 double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3]; 2286 CvMat om = cvMat(3, 1, CV_64F, _om); 2287 CvMat t = cvMat(3, 1, CV_64F, _t); 2288 CvMat uu = cvMat(3, 1, CV_64F, _uu); 2289 CvMat r_r = cvMat(3, 3, CV_64F, _r_r); 2290 CvMat pp = cvMat(3, 4, CV_64F, _pp); 2291 CvMat ww = cvMat(3, 1, CV_64F, _ww); // temps 2292 CvMat wR = cvMat(3, 3, CV_64F, _wr); 2293 CvMat Z = cvMat(3, 1, CV_64F, _z); 2294 CvMat Ri = cvMat(3, 3, CV_64F, _ri); 2295 double nx = imageSize.width, ny = imageSize.height; 2296 int i, k; 2297 2298 if( _R->rows == 3 && _R->cols == 3 ) 2299 cvRodrigues2(_R, &om); // get vector rotation 2300 else 2301 cvConvert(_R, &om); // it's already a rotation vector 2302 cvConvertScale(&om, &om, -0.5); // get average rotation 2303 cvRodrigues2(&om, &r_r); // rotate cameras to same orientation by averaging 2304 cvMatMul(&r_r, _T, &t); 2305 2306 int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1; 2307 double c = _t[idx], nt = cvNorm(&t, 0, CV_L2); 2308 _uu[idx] = c > 0 ? 1 : -1; 2309 2310 // calculate global Z rotation 2311 cvCrossProduct(&t,&uu,&ww); 2312 double nw = cvNorm(&ww, 0, CV_L2); 2313 cvConvertScale(&ww, &ww, acos(fabs(c)/nt)/nw); 2314 cvRodrigues2(&ww, &wR); 2315 2316 // apply to both views 2317 cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T); 2318 cvConvert( &Ri, _R1 ); 2319 cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, 0); 2320 cvConvert( &Ri, _R2 ); 2321 cvMatMul(&r_r, _T, &t); 2322 2323 // calculate projection/camera matrices 2324 // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy) 2325 double fc_new = DBL_MAX; 2326 CvPoint2D64f cc_new[2] = {{0,0}, {0,0}}; 2327 2328 for( k = 0; k < 2; k++ ) 2329 { 2330 const CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2; 2331 const CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2; 2332 CvPoint2D32f _pts[4]; 2333 CvPoint3D32f _pts_3[4]; 2334 CvMat pts = cvMat(1, 4, CV_32FC2, _pts); 2335 CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3); 2336 double fc, dk1 = Dk ? cvmGet(Dk, 0, 0) : 0; 2337 2338 fc = cvmGet(A,idx^1,idx^1); 2339 if( dk1 < 0 ) 2340 fc *= 1 + 0.2*dk1*(nx*nx + ny*ny)/(8*fc*fc); 2341 fc_new = MIN(fc_new, fc); 2342 2343 for( i = 0; i < 4; i++ ) 2344 { 2345 _pts[i].x = (float)(((i % 2) + 0.5)*nx*0.5); 2346 _pts[i].y = (float)(((i / 2) + 0.5)*ny*0.5); 2347 } 2348 cvUndistortPoints( &pts, &pts, A, Dk, 0, 0 ); 2349 cvConvertPointsHomogeneous( &pts, &pts_3 ); 2350 cvProjectPoints2( &pts_3, k == 0 ? _R1 : _R2, &Z, A, 0, &pts ); 2351 CvScalar avg = cvAvg(&pts); 2352 cc_new[k].x = avg.val[0]; 2353 cc_new[k].y = avg.val[1]; 2354 } 2355 2356 // vertical focal length must be the same for both images to keep the epipolar constraint 2357 // (for horizontal epipolar lines -- TBD: check for vertical epipolar lines) 2358 // use fy for fx also, for simplicity 2359 2360 // For simplicity, set the principal points for both cameras to be the average 2361 // of the two principal points (either one of or both x- and y- coordinates) 2362 if( flags & CV_CALIB_ZERO_DISPARITY ) 2363 { 2364 cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5; 2365 cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; 2366 } 2367 else if( idx == 0 ) // horizontal stereo 2368 cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; 2369 else // vertical stereo 2370 cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5; 2371 2372 cvZero( &pp ); 2373 _pp[0][0] = _pp[1][1] = fc_new; 2374 _pp[0][2] = cc_new[0].x; 2375 _pp[1][2] = cc_new[0].y; 2376 _pp[2][2] = 1; 2377 cvConvert(&pp, _P1); 2378 2379 _pp[0][2] = cc_new[1].x; 2380 _pp[1][2] = cc_new[1].y; 2381 _pp[idx][3] = _t[idx]*fc_new; // baseline * focal length 2382 cvConvert(&pp, _P2); 2383 2384 if( _Q ) 2385 { 2386 double q[] = 2387 { 2388 1, 0, 0, -cc_new[0].x, 2389 0, 1, 0, -cc_new[0].y, 2390 0, 0, 0, fc_new, 2391 0, 0, 1./_t[idx], 2392 (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/_t[idx] 2393 }; 2394 CvMat Q = cvMat(4, 4, CV_64F, q); 2395 cvConvert( &Q, _Q ); 2396 } 2397 } 2398 2399 2400 CV_IMPL int 2401 cvStereoRectifyUncalibrated( 2402 const CvMat* _points1, const CvMat* _points2, 2403 const CvMat* F0, CvSize imgSize, CvMat* _H1, CvMat* _H2, double threshold ) 2404 { 2405 int result = 0; 2406 CvMat* _m1 = 0; 2407 CvMat* _m2 = 0; 2408 CvMat* _lines1 = 0; 2409 CvMat* _lines2 = 0; 2410 2411 CV_FUNCNAME( "cvStereoCalcHomographiesFromF" ); 2412 2413 __BEGIN__; 2414 2415 int i, j, npoints; 2416 double cx, cy; 2417 double u[9], v[9], w[9], f[9], h1[9], h2[9], h0[9], e2[3]; 2418 CvMat E2 = cvMat( 3, 1, CV_64F, e2 ); 2419 CvMat U = cvMat( 3, 3, CV_64F, u ); 2420 CvMat V = cvMat( 3, 3, CV_64F, v ); 2421 CvMat W = cvMat( 3, 3, CV_64F, w ); 2422 CvMat F = cvMat( 3, 3, CV_64F, f ); 2423 CvMat H1 = cvMat( 3, 3, CV_64F, h1 ); 2424 CvMat H2 = cvMat( 3, 3, CV_64F, h2 ); 2425 CvMat H0 = cvMat( 3, 3, CV_64F, h0 ); 2426 2427 CvPoint2D64f* m1; 2428 CvPoint2D64f* m2; 2429 CvPoint3D64f* lines1; 2430 CvPoint3D64f* lines2; 2431 2432 CV_ASSERT( CV_IS_MAT(_points1) && CV_IS_MAT(_points2) && 2433 (_points1->rows == 1 || _points1->cols == 1) && 2434 (_points2->rows == 1 || _points2->cols == 1) && 2435 CV_ARE_SIZES_EQ(_points1, _points2) ); 2436 2437 npoints = _points1->rows * _points1->cols * CV_MAT_CN(_points1->type) / 2; 2438 2439 _m1 = cvCreateMat( _points1->rows, _points1->cols, CV_64FC(CV_MAT_CN(_points1->type)) ); 2440 _m2 = cvCreateMat( _points2->rows, _points2->cols, CV_64FC(CV_MAT_CN(_points2->type)) ); 2441 _lines1 = cvCreateMat( 1, npoints, CV_64FC3 ); 2442 _lines2 = cvCreateMat( 1, npoints, CV_64FC3 ); 2443 2444 cvConvert( F0, &F ); 2445 2446 cvSVD( (CvMat*)&F, &W, &U, &V, CV_SVD_U_T + CV_SVD_V_T ); 2447 W.data.db[8] = 0.; 2448 cvGEMM( &U, &W, 1, 0, 0, &W, CV_GEMM_A_T ); 2449 cvMatMul( &W, &V, &F ); 2450 2451 cx = cvRound( (imgSize.width-1)*0.5 ); 2452 cy = cvRound( (imgSize.height-1)*0.5 ); 2453 2454 cvZero( _H1 ); 2455 cvZero( _H2 ); 2456 2457 cvConvert( _points1, _m1 ); 2458 cvConvert( _points2, _m2 ); 2459 cvReshape( _m1, _m1, 2, 1 ); 2460 cvReshape( _m1, _m1, 2, 1 ); 2461 2462 m1 = (CvPoint2D64f*)_m1->data.ptr; 2463 m2 = (CvPoint2D64f*)_m2->data.ptr; 2464 lines1 = (CvPoint3D64f*)_lines1->data.ptr; 2465 lines2 = (CvPoint3D64f*)_lines2->data.ptr; 2466 2467 if( threshold > 0 ) 2468 { 2469 cvComputeCorrespondEpilines( _m1, 1, &F, _lines1 ); 2470 cvComputeCorrespondEpilines( _m2, 2, &F, _lines2 ); 2471 2472 // measure distance from points to the corresponding epilines, mark outliers 2473 for( i = j = 0; i < npoints; i++ ) 2474 { 2475 if( fabs(m1[i].x*lines2[i].x + 2476 m1[i].y*lines2[i].y + 2477 lines2[i].z) <= threshold && 2478 fabs(m2[i].x*lines1[i].x + 2479 m2[i].y*lines1[i].y + 2480 lines1[i].z) <= threshold ) 2481 { 2482 if( j > i ) 2483 { 2484 m1[j] = m1[i]; 2485 m2[j] = m2[i]; 2486 } 2487 j++; 2488 } 2489 } 2490 2491 npoints = j; 2492 if( npoints == 0 ) 2493 EXIT; 2494 } 2495 2496 { 2497 _m1->cols = _m2->cols = npoints; 2498 memcpy( E2.data.db, U.data.db + 6, sizeof(e2)); 2499 cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 ); 2500 2501 double t[] = 2502 { 2503 1, 0, -cx, 2504 0, 1, -cy, 2505 0, 0, 1 2506 }; 2507 CvMat T = cvMat(3, 3, CV_64F, t); 2508 cvMatMul( &T, &E2, &E2 ); 2509 2510 int mirror = e2[0] < 0; 2511 double d = MAX(sqrt(e2[0]*e2[0] + e2[1]*e2[1]),DBL_EPSILON); 2512 double alpha = e2[0]/d; 2513 double beta = e2[1]/d; 2514 double r[] = 2515 { 2516 alpha, beta, 0, 2517 -beta, alpha, 0, 2518 0, 0, 1 2519 }; 2520 CvMat R = cvMat(3, 3, CV_64F, r); 2521 cvMatMul( &R, &T, &T ); 2522 cvMatMul( &R, &E2, &E2 ); 2523 double invf = fabs(e2[2]) < 1e-6*fabs(e2[0]) ? 0 : -e2[2]/e2[0]; 2524 double k[] = 2525 { 2526 1, 0, 0, 2527 0, 1, 0, 2528 invf, 0, 1 2529 }; 2530 CvMat K = cvMat(3, 3, CV_64F, k); 2531 cvMatMul( &K, &T, &H2 ); 2532 cvMatMul( &K, &E2, &E2 ); 2533 2534 double it[] = 2535 { 2536 1, 0, cx, 2537 0, 1, cy, 2538 0, 0, 1 2539 }; 2540 CvMat iT = cvMat( 3, 3, CV_64F, it ); 2541 cvMatMul( &iT, &H2, &H2 ); 2542 2543 memcpy( E2.data.db, U.data.db + 6, sizeof(e2)); 2544 cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 ); 2545 2546 double e2_x[] = 2547 { 2548 0, -e2[2], e2[1], 2549 e2[2], 0, -e2[0], 2550 -e2[1], e2[0], 0 2551 }; 2552 double e2_111[] = 2553 { 2554 e2[0], e2[0], e2[0], 2555 e2[1], e2[1], e2[1], 2556 e2[2], e2[2], e2[2], 2557 }; 2558 CvMat E2_x = cvMat(3, 3, CV_64F, e2_x); 2559 CvMat E2_111 = cvMat(3, 3, CV_64F, e2_111); 2560 cvMatMulAdd(&E2_x, &F, &E2_111, &H0 ); 2561 cvMatMul(&H2, &H0, &H0); 2562 CvMat E1=cvMat(3, 1, CV_64F, V.data.db+6); 2563 cvMatMul(&H0, &E1, &E1); 2564 2565 cvPerspectiveTransform( _m1, _m1, &H0 ); 2566 cvPerspectiveTransform( _m2, _m2, &H2 ); 2567 CvMat A = cvMat( 1, npoints, CV_64FC3, lines1 ), BxBy, B; 2568 double a[9], atb[3], x[3]; 2569 CvMat AtA = cvMat( 3, 3, CV_64F, a ); 2570 CvMat AtB = cvMat( 3, 1, CV_64F, atb ); 2571 CvMat X = cvMat( 3, 1, CV_64F, x ); 2572 cvConvertPointsHomogeneous( _m1, &A ); 2573 cvReshape( &A, &A, 1, npoints ); 2574 cvReshape( _m2, &BxBy, 1, npoints ); 2575 cvGetCol( &BxBy, &B, 0 ); 2576 cvGEMM( &A, &A, 1, 0, 0, &AtA, CV_GEMM_A_T ); 2577 cvGEMM( &A, &B, 1, 0, 0, &AtB, CV_GEMM_A_T ); 2578 cvSolve( &AtA, &AtB, &X, CV_SVD_SYM ); 2579 2580 double ha[] = 2581 { 2582 x[0], x[1], x[2], 2583 0, 1, 0, 2584 0, 0, 1 2585 }; 2586 CvMat Ha = cvMat(3, 3, CV_64F, ha); 2587 cvMatMul( &Ha, &H0, &H1 ); 2588 cvPerspectiveTransform( _m1, _m1, &Ha ); 2589 2590 if( mirror ) 2591 { 2592 double mm[] = { -1, 0, cx*2, 0, -1, cy*2, 0, 0, 1 }; 2593 CvMat MM = cvMat(3, 3, CV_64F, mm); 2594 cvMatMul( &MM, &H1, &H1 ); 2595 cvMatMul( &MM, &H2, &H2 ); 2596 } 2597 2598 cvConvert( &H1, _H1 ); 2599 cvConvert( &H2, _H2 ); 2600 2601 result = 1; 2602 } 2603 2604 __END__; 2605 2606 cvReleaseMat( &_m1 ); 2607 cvReleaseMat( &_m2 ); 2608 cvReleaseMat( &_lines1 ); 2609 cvReleaseMat( &_lines2 ); 2610 2611 return result; 2612 } 2613 2614 2615 CV_IMPL void 2616 cvReprojectImageTo3D( 2617 const CvArr* disparityImage, 2618 CvArr* _3dImage, const CvMat* _Q ) 2619 { 2620 CV_FUNCNAME( "cvReprojectImageTo3D" ); 2621 2622 __BEGIN__; 2623 2624 double q[4][4]; 2625 CvMat Q = cvMat(4, 4, CV_64F, q); 2626 CvMat sstub, *src = cvGetMat( disparityImage, &sstub ); 2627 CvMat dstub, *dst = cvGetMat( _3dImage, &dstub ); 2628 int stype = CV_MAT_TYPE(src->type), dtype = CV_MAT_TYPE(dst->type); 2629 int x, y, rows = src->rows, cols = src->cols; 2630 float* sbuf = (float*)cvStackAlloc( cols*sizeof(sbuf[0]) ); 2631 float* dbuf = (float*)cvStackAlloc( cols*3*sizeof(dbuf[0]) ); 2632 2633 CV_ASSERT( CV_ARE_SIZES_EQ(src, dst) && 2634 (CV_MAT_TYPE(stype) == CV_16SC1 || CV_MAT_TYPE(stype) == CV_32FC1) && 2635 (CV_MAT_TYPE(dtype) == CV_16SC3 || CV_MAT_TYPE(dtype) == CV_32FC3) ); 2636 2637 cvConvert( _Q, &Q ); 2638 2639 for( y = 0; y < rows; y++ ) 2640 { 2641 const float* sptr = (const float*)(src->data.ptr + src->step*y); 2642 float* dptr0 = (float*)(dst->data.ptr + dst->step*y), *dptr = dptr0; 2643 double qx = q[0][1]*y + q[0][3], qy = q[1][1]*y + q[1][3]; 2644 double qz = q[2][1]*y + q[2][3], qw = q[3][1]*y + q[3][3]; 2645 2646 if( stype == CV_16SC1 ) 2647 { 2648 const short* sptr0 = (const short*)sptr; 2649 for( x = 0; x < cols; x++ ) 2650 sbuf[x] = (float)sptr0[x]; 2651 sptr = sbuf; 2652 } 2653 if( dtype != CV_32FC3 ) 2654 dptr = dbuf; 2655 2656 for( x = 0; x < cols; x++, qx += q[0][0], qy += q[1][0], qz += q[2][0], qw += q[3][0] ) 2657 { 2658 double d = sptr[x]; 2659 double iW = 1./(qw + q[3][2]*d); 2660 double X = (qx + q[0][2]*d)*iW; 2661 double Y = (qy + q[1][2]*d)*iW; 2662 double Z = (qz + q[2][2]*d)*iW; 2663 2664 dptr[x*3] = (float)X; 2665 dptr[x*3+1] = (float)Y; 2666 dptr[x*3+2] = (float)Z; 2667 } 2668 2669 if( dtype == CV_16SC3 ) 2670 { 2671 for( x = 0; x < cols*3; x++ ) 2672 { 2673 int ival = cvRound(dptr[x]); 2674 ((short*)dptr0)[x] = CV_CAST_16S(ival); 2675 } 2676 } 2677 } 2678 2679 __END__; 2680 } 2681 2682 2683 /* End of file. */ 2684
