1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
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.
11 // For Open Source Computer Vision Library
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.
17 // Redistribution and use in source and binary forms, with or without modification,
18 // are permitted provided that the following conditions are met:
20 // * Redistribution's of source code must retain the above copyright notice,
21 // this list of conditions and the following disclaimer.
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.
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.
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.
46 This is stright-forward port v3 of Matlab calibration engine by Jean-Yves Bouguet
47 that is (in a large extent) based on the paper:
48 Z. Zhang. "A flexible new technique for camera calibration".
49 IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000.
51 The 1st initial port was done by Valery Mosyagin.
54 CvLevMarq::CvLevMarq()
56 mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = 0;
57 lambdaLg10 = 0; state = DONE;
58 criteria = cvTermCriteria(0,0,0);
60 completeSymmFlag = false;
63 CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
65 mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = 0;
66 init(nparams, nerrs, criteria0, _completeSymmFlag);
69 void CvLevMarq::clear()
72 cvReleaseMat(&prevParam);
83 CvLevMarq::~CvLevMarq()
88 void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
90 if( !param || param->rows != nparams || nerrs != (err ? err->rows : 0) )
92 mask = cvCreateMat( nparams, 1, CV_8U );
93 cvSet(mask, cvScalarAll(1));
94 prevParam = cvCreateMat( nparams, 1, CV_64F );
95 param = cvCreateMat( nparams, 1, CV_64F );
96 JtJ = cvCreateMat( nparams, nparams, CV_64F );
97 JtJN = cvCreateMat( nparams, nparams, CV_64F );
98 JtJV = cvCreateMat( nparams, nparams, CV_64F );
99 JtJW = cvCreateMat( nparams, 1, CV_64F );
100 JtErr = cvCreateMat( nparams, 1, CV_64F );
103 J = cvCreateMat( nerrs, nparams, CV_64F );
104 err = cvCreateMat( nerrs, 1, CV_64F );
106 prevErrNorm = DBL_MAX;
108 criteria = criteria0;
109 if( criteria.type & CV_TERMCRIT_ITER )
110 criteria.max_iter = MIN(MAX(criteria.max_iter,1),1000);
112 criteria.max_iter = 30;
113 if( criteria.type & CV_TERMCRIT_EPS )
114 criteria.epsilon = MAX(criteria.epsilon, 0);
116 criteria.epsilon = DBL_EPSILON;
119 completeSymmFlag = _completeSymmFlag;
122 bool CvLevMarq::update( const CvMat*& _param, CvMat*& _J, CvMat*& _err )
135 if( state == STARTED )
146 if( state == CALC_J )
148 cvMulTransposed( J, JtJ, 1 );
149 cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );
150 cvCopy( param, prevParam );
153 prevErrNorm = cvNorm(err, 0, CV_L2);
161 assert( state == CHECK_ERR );
162 errNorm = cvNorm( err, 0, CV_L2 );
163 if( errNorm > prevErrNorm )
174 lambdaLg10 = MAX(lambdaLg10-1, -16);
175 if( ++iters >= criteria.max_iter ||
176 (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
183 prevErrNorm = errNorm;
193 bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm )
204 if( state == STARTED )
217 if( state == CALC_J )
219 cvCopy( param, prevParam );
222 prevErrNorm = errNorm;
229 assert( state == CHECK_ERR );
230 if( errNorm > prevErrNorm )
241 lambdaLg10 = MAX(lambdaLg10-1, -16);
242 if( ++iters >= criteria.max_iter ||
243 (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
250 prevErrNorm = errNorm;
260 void CvLevMarq::step()
262 const double LOG10 = log(10.);
263 double lambda = exp(lambdaLg10*LOG10);
264 int i, j, nparams = param->rows;
266 for( i = 0; i < nparams; i++ )
267 if( mask->data.ptr[i] == 0 )
269 double *row = JtJ->data.db + i*nparams, *col = JtJ->data.db + i;
270 for( j = 0; j < nparams; j++ )
271 row[j] = col[j*nparams] = 0;
272 JtErr->data.db[i] = 0;
276 cvCompleteSymm( JtJ, completeSymmFlag );
279 for( i = 0; i < nparams; i++ )
280 JtJN->data.db[(nparams+1)*i] *= 1. + lambda;
282 cvSetIdentity(JtJN, cvRealScalar(lambda));
283 cvAdd( JtJ, JtJN, JtJN );
285 cvSVD( JtJN, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
286 cvSVBkSb( JtJW, JtJV, JtJV, JtErr, param, CV_SVD_U_T + CV_SVD_V_T );
287 for( i = 0; i < nparams; i++ )
288 param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? param->data.db[i] : 0);
291 // reimplementation of dAB.m
293 cvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB )
295 CV_FUNCNAME( "cvCalcMatMulDeriv" );
302 CV_ASSERT( CV_IS_MAT(A) && CV_IS_MAT(B) );
303 CV_ASSERT( CV_ARE_TYPES_EQ(A, B) &&
304 (CV_MAT_TYPE(A->type) == CV_32F || CV_MAT_TYPE(A->type) == CV_64F) );
305 CV_ASSERT( A->cols == B->rows );
310 bstep = B->step/CV_ELEM_SIZE(B->type);
314 CV_ASSERT( CV_ARE_TYPES_EQ(A, dABdA) &&
315 dABdA->rows == A->rows*B->cols && dABdA->cols == A->rows*A->cols );
320 CV_ASSERT( CV_ARE_TYPES_EQ(A, dABdB) &&
321 dABdB->rows == A->rows*B->cols && dABdB->cols == B->rows*B->cols );
324 if( CV_MAT_TYPE(A->type) == CV_32F )
326 for( i = 0; i < M*N; i++ )
328 int i1 = i / N, i2 = i % N;
332 float* dcda = (float*)(dABdA->data.ptr + dABdA->step*i);
333 const float* b = (const float*)B->data.ptr + i2;
335 for( j = 0; j < M*L; j++ )
337 for( j = 0; j < L; j++ )
338 dcda[i1*L + j] = b[j*bstep];
343 float* dcdb = (float*)(dABdB->data.ptr + dABdB->step*i);
344 const float* a = (const float*)(A->data.ptr + A->step*i1);
346 for( j = 0; j < L*N; j++ )
348 for( j = 0; j < L; j++ )
349 dcdb[j*N + i2] = a[j];
355 for( i = 0; i < M*N; i++ )
357 int i1 = i / N, i2 = i % N;
361 double* dcda = (double*)(dABdA->data.ptr + dABdA->step*i);
362 const double* b = (const double*)B->data.ptr + i2;
364 for( j = 0; j < M*L; j++ )
366 for( j = 0; j < L; j++ )
367 dcda[i1*L + j] = b[j*bstep];
372 double* dcdb = (double*)(dABdB->data.ptr + dABdB->step*i);
373 const double* a = (const double*)(A->data.ptr + A->step*i1);
375 for( j = 0; j < L*N; j++ )
377 for( j = 0; j < L; j++ )
378 dcdb[j*N + i2] = a[j];
386 // reimplementation of compose_motion.m
388 cvComposeRT( const CvMat* _rvec1, const CvMat* _tvec1,
389 const CvMat* _rvec2, const CvMat* _tvec2,
390 CvMat* _rvec3, CvMat* _tvec3,
391 CvMat* dr3dr1, CvMat* dr3dt1,
392 CvMat* dr3dr2, CvMat* dr3dt2,
393 CvMat* dt3dr1, CvMat* dt3dt1,
394 CvMat* dt3dr2, CvMat* dt3dt2 )
396 CV_FUNCNAME( "cvComposeRT" );
400 double _r1[3], _r2[3];
401 double _R1[9], _d1[9*3], _R2[9], _d2[9*3];
402 CvMat r1 = cvMat(3,1,CV_64F,_r1), r2 = cvMat(3,1,CV_64F,_r2);
403 CvMat R1 = cvMat(3,3,CV_64F,_R1), R2 = cvMat(3,3,CV_64F,_R2);
404 CvMat dR1dr1 = cvMat(9,3,CV_64F,_d1), dR2dr2 = cvMat(9,3,CV_64F,_d2);
406 CV_ASSERT( CV_IS_MAT(_rvec1) && CV_IS_MAT(_rvec2) );
408 CV_ASSERT( CV_MAT_TYPE(_rvec1->type) == CV_32F ||
409 CV_MAT_TYPE(_rvec1->type) == CV_64F );
411 CV_ASSERT( _rvec1->rows == 3 && _rvec1->cols == 1 && CV_ARE_SIZES_EQ(_rvec1, _rvec2) );
413 cvConvert( _rvec1, &r1 );
414 cvConvert( _rvec2, &r2 );
416 cvRodrigues2( &r1, &R1, &dR1dr1 );
417 cvRodrigues2( &r2, &R2, &dR2dr2 );
419 if( _rvec3 || dr3dr1 || dr3dr1 )
421 double _r3[3], _R3[9], _dR3dR1[9*9], _dR3dR2[9*9], _dr3dR3[9*3];
422 double _W1[9*3], _W2[3*3];
423 CvMat r3 = cvMat(3,1,CV_64F,_r3), R3 = cvMat(3,3,CV_64F,_R3);
424 CvMat dR3dR1 = cvMat(9,9,CV_64F,_dR3dR1), dR3dR2 = cvMat(9,9,CV_64F,_dR3dR2);
425 CvMat dr3dR3 = cvMat(3,9,CV_64F,_dr3dR3);
426 CvMat W1 = cvMat(3,9,CV_64F,_W1), W2 = cvMat(3,3,CV_64F,_W2);
428 cvMatMul( &R2, &R1, &R3 );
429 cvCalcMatMulDeriv( &R2, &R1, &dR3dR2, &dR3dR1 );
431 cvRodrigues2( &R3, &r3, &dr3dR3 );
434 cvConvert( &r3, _rvec3 );
438 cvMatMul( &dr3dR3, &dR3dR1, &W1 );
439 cvMatMul( &W1, &dR1dr1, &W2 );
440 cvConvert( &W2, dr3dr1 );
445 cvMatMul( &dr3dR3, &dR3dR2, &W1 );
446 cvMatMul( &W1, &dR2dr2, &W2 );
447 cvConvert( &W2, dr3dr2 );
456 if( _tvec3 || dt3dr2 || dt3dt1 )
458 double _t1[3], _t2[3], _t3[3], _dxdR2[3*9], _dxdt1[3*3], _W3[3*3];
459 CvMat t1 = cvMat(3,1,CV_64F,_t1), t2 = cvMat(3,1,CV_64F,_t2);
460 CvMat t3 = cvMat(3,1,CV_64F,_t3);
461 CvMat dxdR2 = cvMat(3, 9, CV_64F, _dxdR2);
462 CvMat dxdt1 = cvMat(3, 3, CV_64F, _dxdt1);
463 CvMat W3 = cvMat(3, 3, CV_64F, _W3);
465 CV_ASSERT( CV_IS_MAT(_tvec1) && CV_IS_MAT(_tvec2) );
466 CV_ASSERT( CV_ARE_SIZES_EQ(_tvec1, _tvec2) && CV_ARE_SIZES_EQ(_tvec1, _rvec1) );
468 cvConvert( _tvec1, &t1 );
469 cvConvert( _tvec2, &t2 );
470 cvMatMulAdd( &R2, &t1, &t2, &t3 );
473 cvConvert( &t3, _tvec3 );
475 if( dt3dr2 || dt3dt1 )
477 cvCalcMatMulDeriv( &R2, &t1, &dxdR2, &dxdt1 );
480 cvMatMul( &dxdR2, &dR2dr2, &W3 );
481 cvConvert( &W3, dt3dr2 );
484 cvConvert( &dxdt1, dt3dt1 );
489 cvSetIdentity( dt3dt2 );
497 cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
501 CV_FUNCNAME( "cvRogrigues2" );
505 int depth, elem_size;
508 CvMat _J = cvMat( 3, 9, CV_64F, J );
510 if( !CV_IS_MAT(src) )
511 CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg, "Input argument is not a valid matrix" );
513 if( !CV_IS_MAT(dst) )
514 CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg,
515 "The first output argument is not a valid matrix" );
517 depth = CV_MAT_DEPTH(src->type);
518 elem_size = CV_ELEM_SIZE(depth);
520 if( depth != CV_32F && depth != CV_64F )
521 CV_ERROR( CV_StsUnsupportedFormat, "The matrices must have 32f or 64f data type" );
523 if( !CV_ARE_DEPTHS_EQ(src, dst) )
524 CV_ERROR( CV_StsUnmatchedFormats, "All the matrices must have the same data type" );
528 if( !CV_IS_MAT(jacobian) )
529 CV_ERROR( CV_StsBadArg, "Jacobian is not a valid matrix" );
531 if( !CV_ARE_DEPTHS_EQ(src, jacobian) || CV_MAT_CN(jacobian->type) != 1 )
532 CV_ERROR( CV_StsUnmatchedFormats, "Jacobian must have 32fC1 or 64fC1 datatype" );
534 if( (jacobian->rows != 9 || jacobian->cols != 3) &&
535 (jacobian->rows != 3 || jacobian->cols != 9))
536 CV_ERROR( CV_StsBadSize, "Jacobian must be 3x9 or 9x3" );
539 if( src->cols == 1 || src->rows == 1 )
541 double rx, ry, rz, theta;
542 int step = src->rows > 1 ? src->step / elem_size : 1;
544 if( src->rows + src->cols*CV_MAT_CN(src->type) - 1 != 3 )
545 CV_ERROR( CV_StsBadSize, "Input matrix must be 1x3, 3x1 or 3x3" );
547 if( dst->rows != 3 || dst->cols != 3 || CV_MAT_CN(dst->type) != 1 )
548 CV_ERROR( CV_StsBadSize, "Output matrix must be 3x3, single-channel floating point matrix" );
550 if( depth == CV_32F )
552 rx = src->data.fl[0];
553 ry = src->data.fl[step];
554 rz = src->data.fl[step*2];
558 rx = src->data.db[0];
559 ry = src->data.db[step];
560 rz = src->data.db[step*2];
562 theta = sqrt(rx*rx + ry*ry + rz*rz);
564 if( theta < DBL_EPSILON )
566 cvSetIdentity( dst );
570 memset( J, 0, sizeof(J) );
571 J[5] = J[15] = J[19] = -1;
572 J[7] = J[11] = J[21] = 1;
577 const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
579 double c = cos(theta);
580 double s = sin(theta);
582 double itheta = theta ? 1./theta : 0.;
584 rx *= itheta; ry *= itheta; rz *= itheta;
586 double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz };
587 double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 };
589 CvMat _R = cvMat( 3, 3, CV_64F, R );
591 // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
592 // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0]
593 for( k = 0; k < 9; k++ )
594 R[k] = c*I[k] + c1*rrt[k] + s*_r_x_[k];
596 cvConvert( &_R, dst );
600 double drrt[] = { rx+rx, ry, rz, ry, 0, 0, rz, 0, 0,
601 0, rx, 0, rx, ry+ry, rz, 0, rz, 0,
602 0, 0, rx, 0, 0, ry, rx, ry, rz+rz };
603 double d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0,
604 0, 0, 1, 0, 0, 0, -1, 0, 0,
605 0, -1, 0, 1, 0, 0, 0, 0, 0 };
606 for( i = 0; i < 3; i++ )
608 double ri = i == 0 ? rx : i == 1 ? ry : rz;
609 double a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta;
610 double a3 = (c - s*itheta)*ri, a4 = s*itheta;
611 for( k = 0; k < 9; k++ )
612 J[i*9+k] = a0*I[k] + a1*rrt[k] + a2*drrt[i*9+k] +
613 a3*_r_x_[k] + a4*d_r_x_[i*9+k];
618 else if( src->cols == 3 && src->rows == 3 )
620 double R[9], U[9], V[9], W[3], rx, ry, rz;
621 CvMat _R = cvMat( 3, 3, CV_64F, R );
622 CvMat _U = cvMat( 3, 3, CV_64F, U );
623 CvMat _V = cvMat( 3, 3, CV_64F, V );
624 CvMat _W = cvMat( 3, 1, CV_64F, W );
626 int step = dst->rows > 1 ? dst->step / elem_size : 1;
628 if( (dst->rows != 1 || dst->cols*CV_MAT_CN(dst->type) != 3) &&
629 (dst->rows != 3 || dst->cols != 1 || CV_MAT_CN(dst->type) != 1))
630 CV_ERROR( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" );
632 cvConvert( src, &_R );
633 if( !cvCheckArr( &_R, CV_CHECK_RANGE+CV_CHECK_QUIET, -100, 100 ) )
641 cvSVD( &_R, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
642 cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T );
648 s = sqrt((rx*rx + ry*ry + rz*rz)*0.25);
649 c = (R[0] + R[4] + R[8] - 1)*0.5;
650 c = c > 1. ? 1. : c < -1. ? -1. : c;
662 rx = sqrt(MAX(t,0.));
664 ry = sqrt(MAX(t,0.))*(R[1] < 0 ? -1. : 1.);
666 rz = sqrt(MAX(t,0.))*(R[2] < 0 ? -1. : 1.);
667 if( fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R[5] > 0) != (ry*rz > 0) )
669 theta /= sqrt(rx*rx + ry*ry + rz*rz);
677 memset( J, 0, sizeof(J) );
680 J[5] = J[15] = J[19] = -0.5;
681 J[7] = J[11] = J[21] = 0.5;
687 double vth = 1/(2*s);
691 double t, dtheta_dtr = -1./s;
692 // var1 = [vth;theta]
693 // var = [om1;var1] = [om1;vth;theta]
694 double dvth_dtheta = -vth*c/s;
695 double d1 = 0.5*dvth_dtheta*dtheta_dtr;
696 double d2 = 0.5*dtheta_dtr;
697 // dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR
700 0, 0, 0, 0, 0, 1, 0, -1, 0,
701 0, 0, -1, 0, 0, 0, 1, 0, 0,
702 0, 1, 0, -1, 0, 0, 0, 0, 0,
703 d1, 0, 0, 0, d1, 0, 0, 0, d1,
704 d2, 0, 0, 0, d2, 0, 0, 0, d2
714 double domegadvar2[] =
721 CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR );
722 CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar );
723 CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 );
725 CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 );
727 cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 );
728 cvMatMul( &_t0, &_dvardR, &_J );
730 // transpose every row of _J (treat the rows as 3x3 matrices)
731 CV_SWAP(J[1], J[3], t); CV_SWAP(J[2], J[6], t); CV_SWAP(J[5], J[7], t);
732 CV_SWAP(J[10], J[12], t); CV_SWAP(J[11], J[15], t); CV_SWAP(J[14], J[16], t);
733 CV_SWAP(J[19], J[21], t); CV_SWAP(J[20], J[24], t); CV_SWAP(J[23], J[25], t);
737 rx *= vth; ry *= vth; rz *= vth;
740 if( depth == CV_32F )
742 dst->data.fl[0] = (float)rx;
743 dst->data.fl[step] = (float)ry;
744 dst->data.fl[step*2] = (float)rz;
748 dst->data.db[0] = rx;
749 dst->data.db[step] = ry;
750 dst->data.db[step*2] = rz;
756 if( depth == CV_32F )
758 if( jacobian->rows == _J.rows )
759 cvConvert( &_J, jacobian );
763 CvMat _Jf = cvMat( _J.rows, _J.cols, CV_32FC1, Jf );
764 cvConvert( &_J, &_Jf );
765 cvTranspose( &_Jf, jacobian );
768 else if( jacobian->rows == _J.rows )
769 cvCopy( &_J, jacobian );
771 cvTranspose( &_J, jacobian );
783 cvProjectPoints2( const CvMat* objectPoints,
787 const CvMat* distCoeffs,
788 CvMat* imagePoints, CvMat* dpdr,
789 CvMat* dpdt, CvMat* dpdf,
790 CvMat* dpdc, CvMat* dpdk,
793 CvMat *_M = 0, *_m = 0;
794 CvMat *_dpdr = 0, *_dpdt = 0, *_dpdc = 0, *_dpdf = 0, *_dpdk = 0;
796 CV_FUNCNAME( "cvProjectPoints2" );
801 int calc_derivatives;
802 const CvPoint3D64f* M;
804 double r[3], R[9], dRdr[27], t[3], a[9], k[5] = {0,0,0,0,0}, fx, fy, cx, cy;
805 CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
806 CvMat _R = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
807 double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
808 int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
809 bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
811 if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||
812 !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
813 /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) )
814 CV_ERROR( CV_StsBadArg, "One of required arguments is not a valid matrix" );
816 count = MAX(objectPoints->rows, objectPoints->cols);
818 if( CV_IS_CONT_MAT(objectPoints->type) && CV_MAT_DEPTH(objectPoints->type) == CV_64F &&
819 ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) ||
820 (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3)))
821 _M = (CvMat*)objectPoints;
824 CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 ));
825 CV_CALL( cvConvertPointsHomogeneous( objectPoints, _M ));
828 if( CV_IS_CONT_MAT(imagePoints->type) && CV_MAT_DEPTH(imagePoints->type) == CV_64F &&
829 ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 2) ||
830 (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 2)))
833 CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
835 M = (CvPoint3D64f*)_M->data.db;
836 m = (CvPoint2D64f*)_m->data.db;
838 if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) ||
839 (((r_vec->rows != 1 && r_vec->cols != 1) ||
840 r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
841 ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1)))
842 CV_ERROR( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
843 "floating-point rotation vector, or 3x3 rotation matrix" );
845 if( r_vec->rows == 3 && r_vec->cols == 3 )
847 _r = cvMat( 3, 1, CV_64FC1, r );
848 CV_CALL( cvRodrigues2( r_vec, &_r ));
849 CV_CALL( cvRodrigues2( &_r, &_R, &_dRdr ));
850 cvCopy( r_vec, &_R );
854 _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
855 CV_CALL( cvConvert( r_vec, &_r ));
856 CV_CALL( cvRodrigues2( &_r, &_R, &_dRdr ) );
859 if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) ||
860 (t_vec->rows != 1 && t_vec->cols != 1) ||
861 t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
862 CV_ERROR( CV_StsBadArg,
863 "Translation vector must be 1x3 or 3x1 floating-point vector" );
865 _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
866 CV_CALL( cvConvert( t_vec, &_t ));
868 if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) ||
869 A->rows != 3 || A->cols != 3 )
870 CV_ERROR( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" );
872 CV_CALL( cvConvert( A, &_a ));
873 fx = a[0]; fy = a[4];
874 cx = a[2]; cy = a[5];
876 if( fixedAspectRatio )
881 if( !CV_IS_MAT(distCoeffs) ||
882 (CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
883 CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
884 (distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
885 (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
886 distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5) )
887 CV_ERROR( CV_StsBadArg,
888 "Distortion coefficients must be 1x4, 4x1, 1x5 or 5x1 floating-point vector" );
890 _k = cvMat( distCoeffs->rows, distCoeffs->cols,
891 CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
892 CV_CALL( cvConvert( distCoeffs, &_k ));
897 if( !CV_IS_MAT(dpdr) ||
898 (CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
899 CV_MAT_TYPE(dpdr->type) != CV_64FC1) ||
900 dpdr->rows != count*2 || dpdr->cols != 3 )
901 CV_ERROR( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
903 if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
906 CV_CALL( _dpdr = cvCreateMat( 2*count, 3, CV_64FC1 ));
907 dpdr_p = _dpdr->data.db;
908 dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
913 if( !CV_IS_MAT(dpdt) ||
914 (CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
915 CV_MAT_TYPE(dpdt->type) != CV_64FC1) ||
916 dpdt->rows != count*2 || dpdt->cols != 3 )
917 CV_ERROR( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
919 if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
922 CV_CALL( _dpdt = cvCreateMat( 2*count, 3, CV_64FC1 ));
923 dpdt_p = _dpdt->data.db;
924 dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
929 if( !CV_IS_MAT(dpdf) ||
930 (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) ||
931 dpdf->rows != count*2 || dpdf->cols != 2 )
932 CV_ERROR( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
934 if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
937 CV_CALL( _dpdf = cvCreateMat( 2*count, 2, CV_64FC1 ));
938 dpdf_p = _dpdf->data.db;
939 dpdf_step = _dpdf->step/sizeof(dpdf_p[0]);
944 if( !CV_IS_MAT(dpdc) ||
945 (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) ||
946 dpdc->rows != count*2 || dpdc->cols != 2 )
947 CV_ERROR( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" );
949 if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
952 CV_CALL( _dpdc = cvCreateMat( 2*count, 2, CV_64FC1 ));
953 dpdc_p = _dpdc->data.db;
954 dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
959 if( !CV_IS_MAT(dpdk) ||
960 (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
961 dpdk->rows != count*2 || (dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
962 CV_ERROR( CV_StsBadArg, "dp/df must be 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
965 CV_ERROR( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
967 if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
970 CV_CALL( _dpdk = cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 ));
971 dpdk_p = _dpdk->data.db;
972 dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
975 calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk;
977 for( i = 0; i < count; i++ )
979 double X = M[i].x, Y = M[i].y, Z = M[i].z;
980 double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
981 double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
982 double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
983 double r2, r4, r6, a1, a2, a3, cdist;
995 cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
996 xd = x*cdist + k[2]*a1 + k[3]*a2;
997 yd = y*cdist + k[2]*a3 + k[3]*a1;
1000 m[i].y = yd*fy + cy;
1002 if( calc_derivatives )
1006 dpdc_p[0] = 1; dpdc_p[1] = 0;
1007 dpdc_p[dpdc_step] = 0;
1008 dpdc_p[dpdc_step+1] = 1;
1009 dpdc_p += dpdc_step*2;
1014 if( fixedAspectRatio )
1016 dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio;
1017 dpdf_p[dpdf_step] = 0;
1018 dpdf_p[dpdf_step+1] = yd;
1022 dpdf_p[0] = xd; dpdf_p[1] = 0;
1023 dpdf_p[dpdf_step] = 0;
1024 dpdf_p[dpdf_step+1] = yd;
1026 dpdf_p += dpdf_step*2;
1031 dpdk_p[0] = fx*x*r2;
1032 dpdk_p[1] = fx*x*r4;
1033 dpdk_p[dpdk_step] = fy*y*r2;
1034 dpdk_p[dpdk_step+1] = fy*y*r4;
1035 if( _dpdk->cols > 2 )
1039 dpdk_p[dpdk_step+2] = fy*a3;
1040 dpdk_p[dpdk_step+3] = fy*a1;
1041 if( _dpdk->cols > 4 )
1043 dpdk_p[4] = fx*x*r6;
1044 dpdk_p[dpdk_step+4] = fy*y*r6;
1047 dpdk_p += dpdk_step*2;
1052 double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
1053 for( j = 0; j < 3; j++ )
1055 double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
1056 double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
1057 double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
1058 double dmxdt = fx*(dxdt[j]*cdist + x*dcdist_dt +
1059 k[2]*da1dt + k[3]*(dr2dt + 2*x*dxdt[j]));
1060 double dmydt = fy*(dydt[j]*cdist + y*dcdist_dt +
1061 k[2]*(dr2dt + 2*y*dydt[j]) + k[3]*da1dt);
1063 dpdt_p[dpdt_step+j] = dmydt;
1065 dpdt_p += dpdt_step*2;
1072 X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
1073 X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
1074 X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
1078 X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
1079 X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
1080 X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
1084 X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
1085 X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
1086 X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
1088 for( j = 0; j < 3; j++ )
1090 double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
1091 double dydr = z*(dy0dr[j] - y*dz0dr[j]);
1092 double dr2dr = 2*x*dxdr + 2*y*dydr;
1093 double dcdist_dr = k[0]*dr2dr + 2*k[1]*r2*dr2dr + 3*k[4]*r4*dr2dr;
1094 double da1dr = 2*(x*dydr + y*dxdr);
1095 double dmxdr = fx*(dxdr*cdist + x*dcdist_dr +
1096 k[2]*da1dr + k[3]*(dr2dr + 2*x*dxdr));
1097 double dmydr = fy*(dydr*cdist + y*dcdist_dr +
1098 k[2]*(dr2dr + 2*y*dydr) + k[3]*da1dr);
1100 dpdr_p[dpdr_step+j] = dmydr;
1102 dpdr_p += dpdr_step*2;
1107 if( _m != imagePoints )
1108 cvConvertPointsHomogeneous( _m, imagePoints );
1110 cvConvert( _dpdr, dpdr );
1112 cvConvert( _dpdt, dpdt );
1114 cvConvert( _dpdf, dpdf );
1116 cvConvert( _dpdc, dpdc );
1118 cvConvert( _dpdk, dpdk );
1122 if( _M != objectPoints )
1123 cvReleaseMat( &_M );
1124 if( _m != imagePoints )
1125 cvReleaseMat( &_m );
1127 cvReleaseMat( &_dpdr );
1129 cvReleaseMat( &_dpdt );
1131 cvReleaseMat( &_dpdf );
1133 cvReleaseMat( &_dpdc );
1135 cvReleaseMat( &_dpdk );
1140 cvFindExtrinsicCameraParams2( const CvMat* objectPoints,
1141 const CvMat* imagePoints, const CvMat* A,
1142 const CvMat* distCoeffs,
1143 CvMat* rvec, CvMat* tvec,
1144 int useExtrinsicGuess )
1146 const int max_iter = 20;
1147 CvMat *_M = 0, *_Mxy = 0, *_m = 0, *_mn = 0, *_L = 0, *_J = 0;
1149 CV_FUNCNAME( "cvFindExtrinsicCameraParams2" );
1154 double a[9], ar[9]={1,0,0,0,1,0,0,0,1}, R[9];
1155 double MM[9], U[9], V[9], W[3];
1158 CvMat _A = cvMat( 3, 3, CV_64F, a );
1159 CvMat _Ar = cvMat( 3, 3, CV_64F, ar );
1160 CvMat _R = cvMat( 3, 3, CV_64F, R );
1161 CvMat _r = cvMat( 3, 1, CV_64F, param );
1162 CvMat _t = cvMat( 3, 1, CV_64F, param + 3 );
1163 CvMat _Mc = cvMat( 1, 3, CV_64F, Mc.val );
1164 CvMat _MM = cvMat( 3, 3, CV_64F, MM );
1165 CvMat _U = cvMat( 3, 3, CV_64F, U );
1166 CvMat _V = cvMat( 3, 3, CV_64F, V );
1167 CvMat _W = cvMat( 3, 1, CV_64F, W );
1168 CvMat _param = cvMat( 6, 1, CV_64F, param );
1171 CV_ASSERT( CV_IS_MAT(objectPoints) && CV_IS_MAT(imagePoints) &&
1172 CV_IS_MAT(A) && CV_IS_MAT(rvec) && CV_IS_MAT(tvec) );
1174 count = MAX(objectPoints->cols, objectPoints->rows);
1175 CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 ));
1176 CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
1178 CV_CALL( cvConvertPointsHomogeneous( objectPoints, _M ));
1179 CV_CALL( cvConvertPointsHomogeneous( imagePoints, _m ));
1180 CV_CALL( cvConvert( A, &_A ));
1182 CV_ASSERT( (CV_MAT_DEPTH(rvec->type) == CV_64F || CV_MAT_DEPTH(rvec->type) == CV_32F) &&
1183 (rvec->rows == 1 || rvec->cols == 1) && rvec->rows*rvec->cols*CV_MAT_CN(rvec->type) == 3 );
1185 CV_ASSERT( (CV_MAT_DEPTH(tvec->type) == CV_64F || CV_MAT_DEPTH(tvec->type) == CV_32F) &&
1186 (tvec->rows == 1 || tvec->cols == 1) && tvec->rows*tvec->cols*CV_MAT_CN(tvec->type) == 3 );
1188 CV_CALL( _mn = cvCreateMat( 1, count, CV_64FC2 ));
1189 CV_CALL( _Mxy = cvCreateMat( 1, count, CV_64FC2 ));
1191 // normalize image points
1192 // (unapply the intrinsic matrix transformation and distortion)
1193 cvUndistortPoints( _m, _mn, &_A, distCoeffs, 0, &_Ar );
1195 if( useExtrinsicGuess )
1197 CvMat _r_temp = cvMat(rvec->rows, rvec->cols,
1198 CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param );
1199 CvMat _t_temp = cvMat(tvec->rows, tvec->cols,
1200 CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3);
1201 cvConvert( rvec, &_r_temp );
1202 cvConvert( tvec, &_t_temp );
1207 cvReshape( _M, _M, 1, count );
1208 cvMulTransposed( _M, &_MM, 1, &_Mc );
1209 cvSVD( &_MM, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T );
1211 // initialize extrinsic parameters
1212 if( W[2]/W[1] < 1e-3 || count < 4 )
1214 // a planar structure case (all M's lie in the same plane)
1215 double tt[3], h[9], h1_norm, h2_norm;
1216 CvMat* R_transform = &_V;
1217 CvMat T_transform = cvMat( 3, 1, CV_64F, tt );
1218 CvMat _H = cvMat( 3, 3, CV_64F, h );
1219 CvMat _h1, _h2, _h3;
1221 if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
1222 cvSetIdentity( R_transform );
1224 if( cvDet(R_transform) < 0 )
1225 cvScale( R_transform, R_transform, -1 );
1227 cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );
1229 for( i = 0; i < count; i++ )
1231 const double* Rp = R_transform->data.db;
1232 const double* Tp = T_transform.data.db;
1233 const double* src = _M->data.db + i*3;
1234 double* dst = _Mxy->data.db + i*2;
1236 dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0];
1237 dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1];
1240 cvFindHomography( _Mxy, _mn, &_H );
1242 cvGetCol( &_H, &_h1, 0 );
1243 _h2 = _h1; _h2.data.db++;
1244 _h3 = _h2; _h3.data.db++;
1245 h1_norm = sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]);
1246 h2_norm = sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]);
1248 cvScale( &_h1, &_h1, 1./h1_norm );
1249 cvScale( &_h2, &_h2, 1./h2_norm );
1250 cvScale( &_h3, &_t, 2./(h1_norm + h2_norm));
1251 cvCrossProduct( &_h1, &_h2, &_h3 );
1253 cvRodrigues2( &_H, &_r );
1254 cvRodrigues2( &_r, &_H );
1255 cvMatMulAdd( &_H, &T_transform, &_t, &_t );
1256 cvMatMul( &_H, R_transform, &_R );
1257 cvRodrigues2( &_R, &_r );
1261 // non-planar structure. Use DLT method
1263 double LL[12*12], LW[12], LV[12*12], sc;
1264 CvMat _LL = cvMat( 12, 12, CV_64F, LL );
1265 CvMat _LW = cvMat( 12, 1, CV_64F, LW );
1266 CvMat _LV = cvMat( 12, 12, CV_64F, LV );
1267 CvMat _RRt, _RR, _tt;
1268 CvPoint3D64f* M = (CvPoint3D64f*)_M->data.db;
1269 CvPoint2D64f* mn = (CvPoint2D64f*)_mn->data.db;
1271 CV_CALL( _L = cvCreateMat( 2*count, 12, CV_64F ));
1274 for( i = 0; i < count; i++, L += 24 )
1276 double x = -mn[i].x, y = -mn[i].y;
1277 L[0] = L[16] = M[i].x;
1278 L[1] = L[17] = M[i].y;
1279 L[2] = L[18] = M[i].z;
1281 L[4] = L[5] = L[6] = L[7] = 0.;
1282 L[12] = L[13] = L[14] = L[15] = 0.;
1293 cvMulTransposed( _L, &_LL, 1 );
1294 cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
1295 _RRt = cvMat( 3, 4, CV_64F, LV + 11*12 );
1296 cvGetCols( &_RRt, &_RR, 0, 3 );
1297 cvGetCol( &_RRt, &_tt, 3 );
1298 if( cvDet(&_RR) < 0 )
1299 cvScale( &_RRt, &_RRt, -1 );
1301 cvSVD( &_RR, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
1302 cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T );
1303 cvScale( &_tt, &_t, cvNorm(&_R)/sc );
1304 cvRodrigues2( &_R, &_r );
1305 cvReleaseMat( &_L );
1309 cvReshape( _M, _M, 3, 1 );
1310 cvReshape( _mn, _mn, 2, 1 );
1312 // refine extrinsic parameters using iterative algorithm
1314 CvLevMarq solver( 6, count*2, cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,max_iter,FLT_EPSILON), true);
1315 cvCopy( &_param, solver.param );
1319 CvMat *_J = 0, *_err = 0;
1320 const CvMat *__param = 0;
1321 bool proceed = solver.update( __param, _J, _err );
1322 cvCopy( __param, &_param );
1323 if( !proceed || !_err )
1325 cvReshape( _err, _err, 2, 1 );
1328 cvGetCols( _J, &_dpdr, 0, 3 );
1329 cvGetCols( _J, &_dpdt, 3, 6 );
1330 cvProjectPoints2( _M, &_r, &_t, &_A, distCoeffs,
1331 _err, &_dpdr, &_dpdt, 0, 0, 0 );
1335 cvProjectPoints2( _M, &_r, &_t, &_A, distCoeffs,
1336 _err, 0, 0, 0, 0, 0 );
1338 cvSub(_err, _m, _err);
1339 cvReshape( _err, _err, 1, 2*count );
1341 cvCopy( solver.param, &_param );
1344 _r = cvMat( rvec->rows, rvec->cols,
1345 CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param );
1346 _t = cvMat( tvec->rows, tvec->cols,
1347 CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3 );
1349 cvConvert( &_r, rvec );
1350 cvConvert( &_t, tvec );
1354 cvReleaseMat( &_M );
1355 cvReleaseMat( &_Mxy );
1356 cvReleaseMat( &_m );
1357 cvReleaseMat( &_mn );
1358 cvReleaseMat( &_L );
1359 cvReleaseMat( &_J );
1364 cvInitIntrinsicParams2D( const CvMat* objectPoints,
1365 const CvMat* imagePoints,
1366 const CvMat* npoints,
1368 CvMat* cameraMatrix,
1369 double aspectRatio )
1371 CvMat *_A = 0, *_b = 0, *_allH = 0, *_allK = 0;
1373 CV_FUNCNAME( "cvInitIntrinsicParams2D" );
1377 int i, j, pos, nimages, total, ni = 0;
1378 double a[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 };
1380 CvMat _a = cvMat( 3, 3, CV_64F, a );
1381 CvMat _H = cvMat( 3, 3, CV_64F, H );
1382 CvMat _f = cvMat( 2, 1, CV_64F, f );
1384 assert( CV_MAT_TYPE(npoints->type) == CV_32SC1 &&
1385 CV_IS_MAT_CONT(npoints->type) );
1386 nimages = npoints->rows + npoints->cols - 1;
1388 if( (CV_MAT_TYPE(objectPoints->type) != CV_32FC3 &&
1389 CV_MAT_TYPE(objectPoints->type) != CV_64FC3) ||
1390 (CV_MAT_TYPE(imagePoints->type) != CV_32FC2 &&
1391 CV_MAT_TYPE(imagePoints->type) != CV_64FC2) )
1392 CV_ERROR( CV_StsUnsupportedFormat, "Both object points and image points must be 2D" );
1394 if( objectPoints->rows != 1 || imagePoints->rows != 1 )
1395 CV_ERROR( CV_StsBadSize, "object points and image points must be a single-row matrices" );
1397 _A = cvCreateMat( 2*nimages, 2, CV_64F );
1398 _b = cvCreateMat( 2*nimages, 1, CV_64F );
1399 a[2] = (imageSize.width - 1)*0.5;
1400 a[5] = (imageSize.height - 1)*0.5;
1401 _allH = cvCreateMat( nimages, 9, CV_64F );
1403 total = cvRound(cvSum(npoints).val[0]);
1405 // extract vanishing points in order to obtain initial value for the focal length
1406 for( i = 0, pos = 0; i < nimages; i++, pos += ni )
1408 double* Ap = _A->data.db + i*4;
1409 double* bp = _b->data.db + i*2;
1410 ni = npoints->data.i[i];
1411 double h[3], v[3], d1[3], d2[3];
1412 double n[4] = {0,0,0,0};
1414 cvGetCols( objectPoints, &_M, pos, pos + ni );
1415 cvGetCols( imagePoints, &_m, pos, pos + ni );
1417 cvFindHomography( &_M, &_m, &_H );
1418 memcpy( _allH->data.db + i*9, H, sizeof(H) );
1420 H[0] -= H[6]*a[2]; H[1] -= H[7]*a[2]; H[2] -= H[8]*a[2];
1421 H[3] -= H[6]*a[5]; H[4] -= H[7]*a[5]; H[5] -= H[8]*a[5];
1423 for( j = 0; j < 3; j++ )
1425 double t0 = H[j*3], t1 = H[j*3+1];
1426 h[j] = t0; v[j] = t1;
1427 d1[j] = (t0 + t1)*0.5;
1428 d2[j] = (t0 - t1)*0.5;
1429 n[0] += t0*t0; n[1] += t1*t1;
1430 n[2] += d1[j]*d1[j]; n[3] += d2[j]*d2[j];
1433 for( j = 0; j < 4; j++ )
1434 n[j] = 1./sqrt(n[j]);
1436 for( j = 0; j < 3; j++ )
1438 h[j] *= n[0]; v[j] *= n[1];
1439 d1[j] *= n[2]; d2[j] *= n[3];
1442 Ap[0] = h[0]*v[0]; Ap[1] = h[1]*v[1];
1443 Ap[2] = d1[0]*d2[0]; Ap[3] = d1[1]*d2[1];
1444 bp[0] = -h[2]*v[2]; bp[1] = -d1[2]*d2[2];
1447 cvSolve( _A, _b, &_f, CV_NORMAL + CV_SVD );
1448 a[0] = sqrt(fabs(1./f[0]));
1449 a[4] = sqrt(fabs(1./f[1]));
1450 if( aspectRatio != 0 )
1452 double tf = (a[0] + a[4])/(aspectRatio + 1.);
1453 a[0] = aspectRatio*tf;
1457 cvConvert( &_a, cameraMatrix );
1461 cvReleaseMat( &_A );
1462 cvReleaseMat( &_b );
1463 cvReleaseMat( &_allH );
1464 cvReleaseMat( &_allK );
1468 /* finds intrinsic and extrinsic camera parameters
1469 from a few views of known calibration pattern */
1471 cvCalibrateCamera2( const CvMat* objectPoints,
1472 const CvMat* imagePoints,
1473 const CvMat* npoints,
1475 CvMat* cameraMatrix, CvMat* distCoeffs,
1476 CvMat* rvecs, CvMat* tvecs,
1479 const int NINTRINSIC = 9;
1480 CvMat *_M = 0, *_m = 0, *_Ji = 0, *_Je = 0, *_err = 0;
1483 CV_FUNCNAME( "cvCalibrateCamera2" );
1487 double A[9], k[5] = {0,0,0,0,0};
1488 CvMat _A = cvMat(3, 3, CV_64F, A), _k;
1489 int i, nimages, maxPoints = 0, ni = 0, pos, total = 0, nparams, npstep, cn;
1490 double aspectRatio = 0.;
1492 // 0. check the parameters & allocate buffers
1493 if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(imagePoints) ||
1494 !CV_IS_MAT(npoints) || !CV_IS_MAT(cameraMatrix) || !CV_IS_MAT(distCoeffs) )
1495 CV_ERROR( CV_StsBadArg, "One of required vector arguments is not a valid matrix" );
1497 if( imageSize.width <= 0 || imageSize.height <= 0 )
1498 CV_ERROR( CV_StsOutOfRange, "image width and height must be positive" );
1500 if( CV_MAT_TYPE(npoints->type) != CV_32SC1 ||
1501 (npoints->rows != 1 && npoints->cols != 1) )
1502 CV_ERROR( CV_StsUnsupportedFormat,
1503 "the array of point counters must be 1-dimensional integer vector" );
1505 nimages = npoints->rows*npoints->cols;
1506 npstep = npoints->rows == 1 ? 1 : npoints->step/CV_ELEM_SIZE(npoints->type);
1510 cn = CV_MAT_CN(rvecs->type);
1511 if( !CV_IS_MAT(rvecs) ||
1512 (CV_MAT_DEPTH(rvecs->type) != CV_32F && CV_MAT_DEPTH(rvecs->type) != CV_64F) ||
1513 ((rvecs->rows != nimages || (rvecs->cols*cn != 3 && rvecs->cols*cn != 9)) &&
1514 (rvecs->rows != 1 || rvecs->cols != nimages || cn != 3)) )
1515 CV_ERROR( CV_StsBadArg, "the output array of rotation vectors must be 3-channel "
1516 "1xn or nx1 array or 1-channel nx3 or nx9 array, where n is the number of views" );
1521 cn = CV_MAT_CN(tvecs->type);
1522 if( !CV_IS_MAT(tvecs) ||
1523 (CV_MAT_DEPTH(tvecs->type) != CV_32F && CV_MAT_DEPTH(tvecs->type) != CV_64F) ||
1524 ((tvecs->rows != nimages || tvecs->cols*cn != 3) &&
1525 (tvecs->rows != 1 || tvecs->cols != nimages || cn != 3)) )
1526 CV_ERROR( CV_StsBadArg, "the output array of translation vectors must be 3-channel "
1527 "1xn or nx1 array or 1-channel nx3 array, where n is the number of views" );
1530 if( (CV_MAT_TYPE(cameraMatrix->type) != CV_32FC1 &&
1531 CV_MAT_TYPE(cameraMatrix->type) != CV_64FC1) ||
1532 cameraMatrix->rows != 3 || cameraMatrix->cols != 3 )
1533 CV_ERROR( CV_StsBadArg,
1534 "Intrinsic parameters must be 3x3 floating-point matrix" );
1536 if( (CV_MAT_TYPE(distCoeffs->type) != CV_32FC1 &&
1537 CV_MAT_TYPE(distCoeffs->type) != CV_64FC1) ||
1538 (distCoeffs->cols != 1 && distCoeffs->rows != 1) ||
1539 (distCoeffs->cols*distCoeffs->rows != 4 &&
1540 distCoeffs->cols*distCoeffs->rows != 5) )
1541 CV_ERROR( CV_StsBadArg,
1542 "Distortion coefficients must be 4x1, 1x4, 5x1 or 1x5 floating-point matrix" );
1544 for( i = 0; i < nimages; i++ )
1546 ni = npoints->data.i[i*npstep];
1550 sprintf( buf, "The number of points in the view #%d is < 4", i );
1551 CV_ERROR( CV_StsOutOfRange, buf );
1553 maxPoints = MAX( maxPoints, ni );
1557 CV_CALL( _M = cvCreateMat( 1, total, CV_64FC3 ));
1558 CV_CALL( _m = cvCreateMat( 1, total, CV_64FC2 ));
1560 CV_CALL( cvConvertPointsHomogeneous( objectPoints, _M ));
1561 CV_CALL( cvConvertPointsHomogeneous( imagePoints, _m ));
1563 nparams = NINTRINSIC + nimages*6;
1564 CV_CALL( _Ji = cvCreateMat( maxPoints*2, NINTRINSIC, CV_64FC1 ));
1565 CV_CALL( _Je = cvCreateMat( maxPoints*2, 6, CV_64FC1 ));
1566 CV_CALL( _err = cvCreateMat( maxPoints*2, 1, CV_64FC1 ));
1569 _k = cvMat( distCoeffs->rows, distCoeffs->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k);
1570 if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 4 )
1571 flags |= CV_CALIB_FIX_K3;
1573 // 1. initialize intrinsic parameters & LM solver
1574 if( flags & CV_CALIB_USE_INTRINSIC_GUESS )
1576 cvConvert( cameraMatrix, &_A );
1577 if( A[0] <= 0 || A[4] <= 0 )
1578 CV_ERROR( CV_StsOutOfRange, "Focal length (fx and fy) must be positive" );
1579 if( A[2] < 0 || A[2] >= imageSize.width ||
1580 A[5] < 0 || A[5] >= imageSize.height )
1581 CV_ERROR( CV_StsOutOfRange, "Principal point must be within the image" );
1582 if( fabs(A[1]) > 1e-5 )
1583 CV_ERROR( CV_StsOutOfRange, "Non-zero skew is not supported by the function" );
1584 if( fabs(A[3]) > 1e-5 || fabs(A[6]) > 1e-5 ||
1585 fabs(A[7]) > 1e-5 || fabs(A[8]-1) > 1e-5 )
1586 CV_ERROR( CV_StsOutOfRange,
1587 "The intrinsic matrix must have [fx 0 cx; 0 fy cy; 0 0 1] shape" );
1588 A[1] = A[3] = A[6] = A[7] = 0.;
1591 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1592 aspectRatio = A[0]/A[4];
1593 cvConvert( distCoeffs, &_k );
1598 cvAvgSdv( _M, &mean, &sdv );
1599 if( fabs(mean.val[2]) > 1e-5 || fabs(sdv.val[2]) > 1e-5 )
1600 CV_ERROR( CV_StsBadArg,
1601 "For non-planar calibration rigs the initial intrinsic matrix must be specified" );
1602 for( i = 0; i < total; i++ )
1603 ((CvPoint3D64f*)_M->data.db)[i].z = 0.;
1605 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1607 aspectRatio = cvmGet(cameraMatrix,0,0);
1608 aspectRatio /= cvmGet(cameraMatrix,1,1);
1609 if( aspectRatio < 0.01 || aspectRatio > 100 )
1610 CV_ERROR( CV_StsOutOfRange,
1611 "The specified aspect ratio (=A[0][0]/A[1][1]) is incorrect" );
1613 cvInitIntrinsicParams2D( _M, _m, npoints, imageSize, &_A, aspectRatio );
1616 solver.init( nparams, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,DBL_EPSILON) );
1619 double* param = solver.param->data.db;
1620 uchar* mask = solver.mask->data.ptr;
1622 param[0] = A[0]; param[1] = A[4]; param[2] = A[2]; param[3] = A[5];
1623 param[4] = k[0]; param[5] = k[1]; param[6] = k[2]; param[7] = k[3];
1626 if( flags & CV_CALIB_FIX_FOCAL_LENGTH )
1627 mask[0] = mask[1] = 0;
1628 if( flags & CV_CALIB_FIX_PRINCIPAL_POINT )
1629 mask[2] = mask[3] = 0;
1630 if( flags & CV_CALIB_ZERO_TANGENT_DIST )
1632 param[6] = param[7] = 0;
1633 mask[6] = mask[7] = 0;
1635 if( flags & CV_CALIB_FIX_K1 )
1637 if( flags & CV_CALIB_FIX_K2 )
1639 if( flags & CV_CALIB_FIX_K3 )
1643 // 2. initialize extrinsic parameters
1644 for( i = 0, pos = 0; i < nimages; i++, pos += ni )
1646 CvMat _Mi, _mi, _ri, _ti;
1647 ni = npoints->data.i[i*npstep];
1649 cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 );
1650 cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 );
1652 cvGetCols( _M, &_Mi, pos, pos + ni );
1653 cvGetCols( _m, &_mi, pos, pos + ni );
1655 cvFindExtrinsicCameraParams2( &_Mi, &_mi, &_A, &_k, &_ri, &_ti );
1658 // 3. run the optimization
1661 const CvMat* _param = 0;
1662 CvMat *_JtJ = 0, *_JtErr = 0;
1663 double* _errNorm = 0;
1664 bool proceed = solver.updateAlt( _param, _JtJ, _JtErr, _errNorm );
1665 double *param = solver.param->data.db, *pparam = solver.prevParam->data.db;
1667 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1669 param[0] = param[1]*aspectRatio;
1670 pparam[0] = pparam[1]*aspectRatio;
1673 A[0] = param[0]; A[4] = param[1];
1674 A[2] = param[2]; A[5] = param[3];
1675 k[0] = param[4]; k[1] = param[5]; k[2] = param[6];
1682 for( i = 0, pos = 0; i < nimages; i++, pos += ni )
1684 CvMat _Mi, _mi, _ri, _ti, _dpdr, _dpdt, _dpdf, _dpdc, _dpdk, _mp, _part;
1685 ni = npoints->data.i[i*npstep];
1687 cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 );
1688 cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 );
1690 cvGetCols( _M, &_Mi, pos, pos + ni );
1691 cvGetCols( _m, &_mi, pos, pos + ni );
1693 _Je->rows = _Ji->rows = _err->rows = ni*2;
1694 cvGetCols( _Je, &_dpdr, 0, 3 );
1695 cvGetCols( _Je, &_dpdt, 3, 6 );
1696 cvGetCols( _Ji, &_dpdf, 0, 2 );
1697 cvGetCols( _Ji, &_dpdc, 2, 4 );
1698 cvGetCols( _Ji, &_dpdk, 4, NINTRINSIC );
1699 cvReshape( _err, &_mp, 2, 1 );
1701 if( _JtJ || _JtErr )
1703 cvProjectPoints2( &_Mi, &_ri, &_ti, &_A, &_k, &_mp, &_dpdr, &_dpdt,
1704 (flags & CV_CALIB_FIX_FOCAL_LENGTH) ? 0 : &_dpdf,
1705 (flags & CV_CALIB_FIX_PRINCIPAL_POINT) ? 0 : &_dpdc, &_dpdk,
1706 (flags & CV_CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0);
1709 cvProjectPoints2( &_Mi, &_ri, &_ti, &_A, &_k, &_mp );
1711 cvSub( &_mp, &_mi, &_mp );
1713 if( _JtJ || _JtErr )
1715 cvGetSubRect( _JtJ, &_part, cvRect(0,0,NINTRINSIC,NINTRINSIC) );
1716 cvGEMM( _Ji, _Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
1718 cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,NINTRINSIC+i*6,6,6) );
1719 cvGEMM( _Je, _Je, 1, 0, 0, &_part, CV_GEMM_A_T );
1721 cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,0,6,NINTRINSIC) );
1722 cvGEMM( _Ji, _Je, 1, 0, 0, &_part, CV_GEMM_A_T );
1724 cvGetRows( _JtErr, &_part, 0, NINTRINSIC );
1725 cvGEMM( _Ji, _err, 1, &_part, 1, &_part, CV_GEMM_A_T );
1727 cvGetRows( _JtErr, &_part, NINTRINSIC + i*6, NINTRINSIC + (i+1)*6 );
1728 cvGEMM( _Je, _err, 1, 0, 0, &_part, CV_GEMM_A_T );
1733 double errNorm = cvNorm( &_mp, 0, CV_L2 );
1734 *_errNorm += errNorm*errNorm;
1739 // 4. store the results
1740 cvConvert( &_A, cameraMatrix );
1741 cvConvert( &_k, distCoeffs );
1743 for( i = 0; i < nimages; i++ )
1748 src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 );
1749 if( rvecs->rows == nimages && rvecs->cols*CV_MAT_CN(rvecs->type) == 9 )
1751 dst = cvMat( 3, 3, CV_MAT_DEPTH(rvecs->type),
1752 rvecs->data.ptr + rvecs->step*i );
1753 cvRodrigues2( &src, &_A );
1754 cvConvert( &_A, &dst );
1758 dst = cvMat( 3, 1, CV_MAT_DEPTH(rvecs->type), rvecs->rows == 1 ?
1759 rvecs->data.ptr + i*CV_ELEM_SIZE(rvecs->type) :
1760 rvecs->data.ptr + rvecs->step*i );
1761 cvConvert( &src, &dst );
1766 src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 + 3 );
1767 dst = cvMat( 3, 1, CV_MAT_TYPE(tvecs->type), tvecs->rows == 1 ?
1768 tvecs->data.ptr + i*CV_ELEM_SIZE(tvecs->type) :
1769 tvecs->data.ptr + tvecs->step*i );
1770 cvConvert( &src, &dst );
1776 cvReleaseMat( &_M );
1777 cvReleaseMat( &_m );
1778 cvReleaseMat( &_Ji );
1779 cvReleaseMat( &_Je );
1780 cvReleaseMat( &_err );
1784 void cvCalibrationMatrixValues( const CvMat *calibMatr, CvSize imgSize,
1785 double apertureWidth, double apertureHeight, double *fovx, double *fovy,
1786 double *focalLength, CvPoint2D64f *principalPoint, double *pasp )
1788 double alphax, alphay, mx, my;
1789 int imgWidth = imgSize.width, imgHeight = imgSize.height;
1791 CV_FUNCNAME("cvCalibrationMatrixValues");
1794 /* Validate parameters. */
1797 CV_ERROR(CV_StsNullPtr, "Some of parameters is a NULL pointer!");
1799 if(!CV_IS_MAT(calibMatr))
1800 CV_ERROR(CV_StsUnsupportedFormat, "Input parameters must be a matrices!");
1802 if(calibMatr->cols != 3 || calibMatr->rows != 3)
1803 CV_ERROR(CV_StsUnmatchedSizes, "Size of matrices must be 3x3!");
1805 alphax = cvmGet(calibMatr, 0, 0);
1806 alphay = cvmGet(calibMatr, 1, 1);
1807 assert(imgWidth != 0 && imgHeight != 0 && alphax != 0.0 && alphay != 0.0);
1809 /* Calculate pixel aspect ratio. */
1811 *pasp = alphay / alphax;
1813 /* Calculate number of pixel per realworld unit. */
1815 if(apertureWidth != 0.0 && apertureHeight != 0.0) {
1816 mx = imgWidth / apertureWidth;
1817 my = imgHeight / apertureHeight;
1823 /* Calculate fovx and fovy. */
1826 *fovx = 2 * atan(imgWidth / (2 * alphax)) * 180.0 / CV_PI;
1829 *fovy = 2 * atan(imgHeight / (2 * alphay)) * 180.0 / CV_PI;
1831 /* Calculate focal length. */
1834 *focalLength = alphax / mx;
1836 /* Calculate principle point. */
1839 *principalPoint = cvPoint2D64f(cvmGet(calibMatr, 0, 2) / mx, cvmGet(calibMatr, 1, 2) / my);
1845 //////////////////////////////// Stereo Calibration ///////////////////////////////////
1847 static int dbCmp( const void* _a, const void* _b )
1849 double a = *(const double*)_a;
1850 double b = *(const double*)_b;
1852 return (a > b) - (a < b);
1856 void cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1,
1857 const CvMat* _imagePoints2, const CvMat* _npoints,
1858 CvMat* _cameraMatrix1, CvMat* _distCoeffs1,
1859 CvMat* _cameraMatrix2, CvMat* _distCoeffs2,
1860 CvSize imageSize, CvMat* _R, CvMat* _T,
1861 CvMat* _E, CvMat* _F,
1862 CvTermCriteria termCrit, int flags )
1864 const int NINTRINSIC = 9;
1870 CvMat* imagePoints[2] = {0,0};
1871 CvMat* objectPoints = 0;
1875 CV_FUNCNAME( "cvStereoCalibrate" );
1879 double A[2][9], dk[2][5]={{0,0,0,0,0},{0,0,0,0,0}}, rlr[9];
1880 CvMat K[2], Dist[2], om_LR, T_LR;
1881 CvMat R_LR = cvMat(3, 3, CV_64F, rlr);
1882 int i, k, p, ni = 0, ofs, nimages, pointsTotal, maxPoints = 0;
1884 bool recomputeIntrinsics = false;
1885 double aspectRatio[2] = {0,0};
1887 CV_ASSERT( CV_IS_MAT(_imagePoints1) && CV_IS_MAT(_imagePoints2) &&
1888 CV_IS_MAT(_objectPoints) && CV_IS_MAT(_npoints) &&
1889 CV_IS_MAT(_R) && CV_IS_MAT(_T) );
1891 CV_ASSERT( CV_ARE_TYPES_EQ(_imagePoints1, _imagePoints2) &&
1892 CV_ARE_DEPTHS_EQ(_imagePoints1, _objectPoints) );
1894 CV_ASSERT( (_npoints->cols == 1 || _npoints->rows == 1) &&
1895 CV_MAT_TYPE(_npoints->type) == CV_32SC1 );
1897 nimages = _npoints->cols + _npoints->rows - 1;
1898 npoints = cvCreateMat( _npoints->rows, _npoints->cols, _npoints->type );
1899 cvCopy( _npoints, npoints );
1901 for( i = 0, pointsTotal = 0; i < nimages; i++ )
1903 maxPoints = MAX(maxPoints, npoints->data.i[i]);
1904 pointsTotal += npoints->data.i[i];
1907 objectPoints = cvCreateMat( _objectPoints->rows, _objectPoints->cols,
1908 CV_64FC(CV_MAT_CN(_objectPoints->type)));
1909 cvConvert( _objectPoints, objectPoints );
1910 cvReshape( objectPoints, objectPoints, 3, 1 );
1912 for( k = 0; k < 2; k++ )
1914 const CvMat* points = k == 0 ? _imagePoints1 : _imagePoints2;
1915 const CvMat* cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
1916 const CvMat* distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2;
1918 int cn = CV_MAT_CN(_imagePoints1->type);
1919 CV_ASSERT( (CV_MAT_DEPTH(_imagePoints1->type) == CV_32F ||
1920 CV_MAT_DEPTH(_imagePoints1->type) == CV_64F) &&
1921 ((_imagePoints1->rows == pointsTotal && _imagePoints1->cols*cn == 2) ||
1922 (_imagePoints1->rows == 1 && _imagePoints1->cols == pointsTotal && cn == 2)) );
1924 K[k] = cvMat(3,3,CV_64F,A[k]);
1925 Dist[k] = cvMat(1,5,CV_64F,dk[k]);
1927 imagePoints[k] = cvCreateMat( points->rows, points->cols, CV_64FC(CV_MAT_CN(points->type)));
1928 cvConvert( points, imagePoints[k] );
1929 cvReshape( imagePoints[k], imagePoints[k], 2, 1 );
1931 if( flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS|
1932 CV_CALIB_FIX_ASPECT_RATIO|CV_CALIB_FIX_FOCAL_LENGTH) )
1933 cvConvert( cameraMatrix, &K[k] );
1935 if( flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS|
1936 CV_CALIB_FIX_K1|CV_CALIB_FIX_K2|CV_CALIB_FIX_K3) )
1938 CvMat tdist = cvMat( distCoeffs->rows, distCoeffs->cols,
1939 CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), Dist[k].data.db );
1940 cvConvert( distCoeffs, &tdist );
1943 if( !(flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS)))
1945 cvCalibrateCamera2( objectPoints, imagePoints[k],
1946 npoints, imageSize, &K[k], &Dist[k], 0, 0, flags );
1950 if( flags & CV_CALIB_SAME_FOCAL_LENGTH )
1952 static const int avg_idx[] = { 0, 4, 2, 5, -1 };
1953 for( k = 0; avg_idx[k] >= 0; k++ )
1954 A[0][avg_idx[k]] = A[1][avg_idx[k]] = (A[0][avg_idx[k]] + A[1][avg_idx[k]])*0.5;
1957 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1959 for( k = 0; k < 2; k++ )
1960 aspectRatio[k] = A[k][0]/A[k][4];
1963 recomputeIntrinsics = (flags & CV_CALIB_FIX_INTRINSIC) == 0;
1965 err = cvCreateMat( maxPoints*2, 1, CV_64F );
1966 Je = cvCreateMat( maxPoints*2, 6, CV_64F );
1967 J_LR = cvCreateMat( maxPoints*2, 6, CV_64F );
1968 Ji = cvCreateMat( maxPoints*2, NINTRINSIC, CV_64F );
1971 // we optimize for the inter-camera R(3),t(3), then, optionally,
1972 // for intrinisic parameters of each camera ((fx,fy,cx,cy,k1,k2,p1,p2) ~ 8 parameters).
1973 nparams = 6*(nimages+1) + (recomputeIntrinsics ? NINTRINSIC*2 : 0);
1975 // storage for initial [om(R){i}|t{i}] (in order to compute the median for each component)
1976 RT0 = cvCreateMat( 6, nimages, CV_64F );
1978 solver.init( nparams, 0, termCrit );
1979 if( recomputeIntrinsics )
1981 uchar* imask = solver.mask->data.ptr + nparams - NINTRINSIC*2;
1982 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1983 imask[0] = imask[NINTRINSIC] = 0;
1984 if( flags & CV_CALIB_FIX_FOCAL_LENGTH )
1985 imask[0] = imask[1] = imask[NINTRINSIC] = imask[NINTRINSIC+1] = 0;
1986 if( flags & CV_CALIB_FIX_PRINCIPAL_POINT )
1987 imask[2] = imask[3] = imask[NINTRINSIC+2] = imask[NINTRINSIC+3] = 0;
1988 if( flags & CV_CALIB_ZERO_TANGENT_DIST )
1989 imask[6] = imask[7] = imask[NINTRINSIC+6] = imask[NINTRINSIC+7] = 0;
1990 if( flags & CV_CALIB_FIX_K1 )
1991 imask[4] = imask[NINTRINSIC+4] = 0;
1992 if( flags & CV_CALIB_FIX_K2 )
1993 imask[5] = imask[NINTRINSIC+5] = 0;
1994 if( flags & CV_CALIB_FIX_K3 )
1995 imask[8] = imask[NINTRINSIC+8] = 0;
1999 Compute initial estimate of pose
2001 For each image, compute:
2002 R(om) is the rotation matrix of om
2003 om(R) is the rotation vector of R
2004 R_ref = R(om_right) * R(om_left)'
2005 T_ref_list = [T_ref_list; T_right - R_ref * T_left]
2006 om_ref_list = {om_ref_list; om(R_ref)]
2008 om = median(om_ref_list)
2009 T = median(T_ref_list)
2011 for( i = ofs = 0; i < nimages; ofs += ni, i++ )
2013 ni = npoints->data.i[i];
2015 double _om[2][3], r[2][9], t[2][3];
2016 CvMat om[2], R[2], T[2], imgpt_i[2];
2018 objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3);
2019 for( k = 0; k < 2; k++ )
2021 imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2);
2022 om[k] = cvMat(3, 1, CV_64F, _om[k]);
2023 R[k] = cvMat(3, 3, CV_64F, r[k]);
2024 T[k] = cvMat(3, 1, CV_64F, t[k]);
2026 // FIXME: here we ignore activePoints[k] because of
2027 // the limited API of cvFindExtrnisicCameraParams2
2028 cvFindExtrinsicCameraParams2( &objpt_i, &imgpt_i[k], &K[k], &Dist[k], &om[k], &T[k] );
2029 cvRodrigues2( &om[k], &R[k] );
2032 // save initial om_left and T_left
2033 solver.param->data.db[(i+1)*6] = _om[0][0];
2034 solver.param->data.db[(i+1)*6 + 1] = _om[0][1];
2035 solver.param->data.db[(i+1)*6 + 2] = _om[0][2];
2036 solver.param->data.db[(i+1)*6 + 3] = t[0][0];
2037 solver.param->data.db[(i+1)*6 + 4] = t[0][1];
2038 solver.param->data.db[(i+1)*6 + 5] = t[0][2];
2041 cvGEMM( &R[1], &R[0], 1, 0, 0, &R[0], CV_GEMM_B_T );
2042 cvGEMM( &R[0], &T[0], -1, &T[1], 1, &T[1] );
2043 cvRodrigues2( &R[0], &T[0] );
2044 RT0->data.db[i] = t[0][0];
2045 RT0->data.db[i + nimages] = t[0][1];
2046 RT0->data.db[i + nimages*2] = t[0][2];
2047 RT0->data.db[i + nimages*3] = t[1][0];
2048 RT0->data.db[i + nimages*4] = t[1][1];
2049 RT0->data.db[i + nimages*5] = t[1][2];
2052 // find the medians and save the first 6 parameters
2053 for( i = 0; i < 6; i++ )
2055 qsort( RT0->data.db + i*nimages, nimages, CV_ELEM_SIZE(RT0->type), dbCmp );
2056 solver.param->data.db[i] = nimages % 2 != 0 ? RT0->data.db[i*nimages + nimages/2] :
2057 (RT0->data.db[i*nimages + nimages/2 - 1] + RT0->data.db[i*nimages + nimages/2])*0.5;
2060 if( recomputeIntrinsics )
2061 for( k = 0; k < 2; k++ )
2063 double* iparam = solver.param->data.db + (nimages+1)*6 + k*NINTRINSIC;
2064 if( flags & CV_CALIB_ZERO_TANGENT_DIST )
2065 dk[k][2] = dk[k][3] = 0;
2066 iparam[0] = A[k][0]; iparam[1] = A[k][4]; iparam[2] = A[k][2]; iparam[3] = A[k][5];
2067 iparam[4] = dk[k][0]; iparam[5] = dk[k][1]; iparam[6] = dk[k][2];
2068 iparam[7] = dk[k][3]; iparam[8] = dk[k][4];
2071 om_LR = cvMat(3, 1, CV_64F, solver.param->data.db);
2072 T_LR = cvMat(3, 1, CV_64F, solver.param->data.db + 3);
2076 const CvMat* param = 0;
2077 CvMat tmpimagePoints;
2078 CvMat *JtJ = 0, *JtErr = 0;
2079 double* errNorm = 0;
2080 double _omR[3], _tR[3];
2081 double _dr3dr1[9], _dr3dr2[9], /*_dt3dr1[9],*/ _dt3dr2[9], _dt3dt1[9], _dt3dt2[9];
2082 CvMat dr3dr1 = cvMat(3, 3, CV_64F, _dr3dr1);
2083 CvMat dr3dr2 = cvMat(3, 3, CV_64F, _dr3dr2);
2084 //CvMat dt3dr1 = cvMat(3, 3, CV_64F, _dt3dr1);
2085 CvMat dt3dr2 = cvMat(3, 3, CV_64F, _dt3dr2);
2086 CvMat dt3dt1 = cvMat(3, 3, CV_64F, _dt3dt1);
2087 CvMat dt3dt2 = cvMat(3, 3, CV_64F, _dt3dt2);
2088 CvMat om[2], T[2], imgpt_i[2];
2089 CvMat dpdrot_hdr, dpdt_hdr, dpdf_hdr, dpdc_hdr, dpdk_hdr;
2090 CvMat *dpdrot = &dpdrot_hdr, *dpdt = &dpdt_hdr, *dpdf = 0, *dpdc = 0, *dpdk = 0;
2092 if( !solver.updateAlt( param, JtJ, JtErr, errNorm ))
2095 cvRodrigues2( &om_LR, &R_LR );
2096 om[1] = cvMat(3,1,CV_64F,_omR);
2097 T[1] = cvMat(3,1,CV_64F,_tR);
2099 if( recomputeIntrinsics )
2101 double* iparam = solver.param->data.db + (nimages+1)*6;
2102 double* ipparam = solver.prevParam->data.db + (nimages+1)*6;
2106 if( flags & CV_CALIB_SAME_FOCAL_LENGTH )
2108 iparam[NINTRINSIC] = iparam[0];
2109 iparam[NINTRINSIC+1] = iparam[1];
2110 ipparam[NINTRINSIC] = ipparam[0];
2111 ipparam[NINTRINSIC+1] = ipparam[1];
2113 if( flags & CV_CALIB_FIX_ASPECT_RATIO )
2115 iparam[0] = iparam[1]*aspectRatio[0];
2116 iparam[NINTRINSIC] = iparam[NINTRINSIC+1]*aspectRatio[1];
2117 ipparam[0] = ipparam[1]*aspectRatio[0];
2118 ipparam[NINTRINSIC] = ipparam[NINTRINSIC+1]*aspectRatio[1];
2120 for( k = 0; k < 2; k++ )
2122 A[k][0] = iparam[k*NINTRINSIC+0];
2123 A[k][4] = iparam[k*NINTRINSIC+1];
2124 A[k][2] = iparam[k*NINTRINSIC+2];
2125 A[k][5] = iparam[k*NINTRINSIC+3];
2126 dk[k][0] = iparam[k*NINTRINSIC+4];
2127 dk[k][1] = iparam[k*NINTRINSIC+5];
2128 dk[k][2] = iparam[k*NINTRINSIC+6];
2129 dk[k][3] = iparam[k*NINTRINSIC+7];
2130 dk[k][4] = iparam[k*NINTRINSIC+8];
2134 for( i = ofs = 0; i < nimages; ofs += ni, i++ )
2136 ni = npoints->data.i[i];
2137 CvMat objpt_i, _part;
2139 om[0] = cvMat(3,1,CV_64F,solver.param->data.db+(i+1)*6);
2140 T[0] = cvMat(3,1,CV_64F,solver.param->data.db+(i+1)*6+3);
2143 cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1], &dr3dr1, 0,
2144 &dr3dr2, 0, 0, &dt3dt1, &dt3dr2, &dt3dt2 );
2146 cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1] );
2148 objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3);
2149 err->rows = Je->rows = J_LR->rows = Ji->rows = ni*2;
2150 cvReshape( err, &tmpimagePoints, 2, 1 );
2152 cvGetCols( Ji, &dpdf_hdr, 0, 2 );
2153 cvGetCols( Ji, &dpdc_hdr, 2, 4 );
2154 cvGetCols( Ji, &dpdk_hdr, 4, NINTRINSIC );
2155 cvGetCols( Je, &dpdrot_hdr, 0, 3 );
2156 cvGetCols( Je, &dpdt_hdr, 3, 6 );
2158 for( k = 0; k < 2; k++ )
2160 double maxErr, l2err;
2161 imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2);
2164 cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k],
2165 &tmpimagePoints, dpdrot, dpdt, dpdf, dpdc, dpdk,
2166 (flags & CV_CALIB_FIX_ASPECT_RATIO) ? aspectRatio[k] : 0);
2168 cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k], &tmpimagePoints );
2169 cvSub( &tmpimagePoints, &imgpt_i[k], &tmpimagePoints );
2171 l2err = cvNorm( &tmpimagePoints, 0, CV_L2 );
2172 maxErr = cvNorm( &tmpimagePoints, 0, CV_C );
2176 int iofs = (nimages+1)*6 + k*NINTRINSIC, eofs = (i+1)*6;
2177 assert( JtJ && JtErr );
2181 // d(err_{x|y}R) ~ de3
2182 // convert de3/{dr3,dt3} => de3{dr1,dt1} & de3{dr2,dt2}
2183 for( p = 0; p < ni*2; p++ )
2185 CvMat de3dr3 = cvMat( 1, 3, CV_64F, Je->data.ptr + Je->step*p );
2186 CvMat de3dt3 = cvMat( 1, 3, CV_64F, de3dr3.data.db + 3 );
2187 CvMat de3dr2 = cvMat( 1, 3, CV_64F, J_LR->data.ptr + J_LR->step*p );
2188 CvMat de3dt2 = cvMat( 1, 3, CV_64F, de3dr2.data.db + 3 );
2189 double _de3dr1[3], _de3dt1[3];
2190 CvMat de3dr1 = cvMat( 1, 3, CV_64F, _de3dr1 );
2191 CvMat de3dt1 = cvMat( 1, 3, CV_64F, _de3dt1 );
2193 cvMatMul( &de3dr3, &dr3dr1, &de3dr1 );
2194 cvMatMul( &de3dt3, &dt3dt1, &de3dt1 );
2196 cvMatMul( &de3dr3, &dr3dr2, &de3dr2 );
2197 cvMatMulAdd( &de3dt3, &dt3dr2, &de3dr2, &de3dr2 );
2199 cvMatMul( &de3dt3, &dt3dt2, &de3dt2 );
2201 cvCopy( &de3dr1, &de3dr3 );
2202 cvCopy( &de3dt1, &de3dt3 );
2205 cvGetSubRect( JtJ, &_part, cvRect(0, 0, 6, 6) );
2206 cvGEMM( J_LR, J_LR, 1, &_part, 1, &_part, CV_GEMM_A_T );
2208 cvGetSubRect( JtJ, &_part, cvRect(eofs, 0, 6, 6) );
2209 cvGEMM( J_LR, Je, 1, 0, 0, &_part, CV_GEMM_A_T );
2211 cvGetRows( JtErr, &_part, 0, 6 );
2212 cvGEMM( J_LR, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
2215 cvGetSubRect( JtJ, &_part, cvRect(eofs, eofs, 6, 6) );
2216 cvGEMM( Je, Je, 1, &_part, 1, &_part, CV_GEMM_A_T );
2218 cvGetRows( JtErr, &_part, eofs, eofs + 6 );
2219 cvGEMM( Je, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
2221 if( recomputeIntrinsics )
2223 cvGetSubRect( JtJ, &_part, cvRect(iofs, iofs, NINTRINSIC, NINTRINSIC) );
2224 cvGEMM( Ji, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
2225 cvGetSubRect( JtJ, &_part, cvRect(iofs, eofs, NINTRINSIC, 6) );
2226 cvGEMM( Je, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
2229 cvGetSubRect( JtJ, &_part, cvRect(iofs, 0, NINTRINSIC, 6) );
2230 cvGEMM( J_LR, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
2232 cvGetRows( JtErr, &_part, iofs, iofs + NINTRINSIC );
2233 cvGEMM( Ji, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
2238 *errNorm += l2err*l2err;
2243 cvRodrigues2( &om_LR, &R_LR );
2244 if( _R->rows == 1 || _R->cols == 1 )
2245 cvConvert( &om_LR, _R );
2247 cvConvert( &R_LR, _R );
2248 cvConvert( &T_LR, _T );
2250 if( recomputeIntrinsics )
2252 cvConvert( &K[0], _cameraMatrix1 );
2253 cvConvert( &K[1], _cameraMatrix2 );
2255 for( k = 0; k < 2; k++ )
2257 CvMat* distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2;
2258 CvMat tdist = cvMat( distCoeffs->rows, distCoeffs->cols,
2259 CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), Dist[k].data.db );
2260 cvConvert( &tdist, distCoeffs );
2266 double* t = T_LR.data.db;
2273 CvMat Tx = cvMat(3, 3, CV_64F, tx);
2275 CvMat E = cvMat(3, 3, CV_64F, e);
2276 CvMat F = cvMat(3, 3, CV_64F, f);
2277 cvMatMul( &Tx, &R_LR, &E );
2279 cvConvert( &E, _E );
2283 CvMat iK = cvMat(3, 3, CV_64F, ik);
2284 cvInvert(&K[1], &iK);
2285 cvGEMM( &iK, &E, 1, 0, 0, &E, CV_GEMM_A_T );
2286 cvInvert(&K[0], &iK);
2287 cvMatMul(&E, &iK, &F);
2288 cvConvertScale( &F, _F, fabs(f[8]) > 0 ? 1./f[8] : 1 );
2294 cvReleaseMat( &npoints );
2295 cvReleaseMat( &err );
2296 cvReleaseMat( &J_LR );
2297 cvReleaseMat( &Je );
2298 cvReleaseMat( &Ji );
2299 cvReleaseMat( &RT0 );
2300 cvReleaseMat( &objectPoints );
2301 cvReleaseMat( &imagePoints[0] );
2302 cvReleaseMat( &imagePoints[1] );
2306 void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2,
2307 const CvMat* _distCoeffs1, const CvMat* _distCoeffs2,
2308 CvSize imageSize, const CvMat* _R, const CvMat* _T,
2309 CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2,
2310 CvMat* _Q, int flags )
2312 double _om[3], _t[3], _uu[3]={0,0,0}, _r_r[3][3], _pp[3][4];
2313 double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3];
2314 CvMat om = cvMat(3, 1, CV_64F, _om);
2315 CvMat t = cvMat(3, 1, CV_64F, _t);
2316 CvMat uu = cvMat(3, 1, CV_64F, _uu);
2317 CvMat r_r = cvMat(3, 3, CV_64F, _r_r);
2318 CvMat pp = cvMat(3, 4, CV_64F, _pp);
2319 CvMat ww = cvMat(3, 1, CV_64F, _ww); // temps
2320 CvMat wR = cvMat(3, 3, CV_64F, _wr);
2321 CvMat Z = cvMat(3, 1, CV_64F, _z);
2322 CvMat Ri = cvMat(3, 3, CV_64F, _ri);
2323 double nx = imageSize.width, ny = imageSize.height;
2326 if( _R->rows == 3 && _R->cols == 3 )
2327 cvRodrigues2(_R, &om); // get vector rotation
2329 cvConvert(_R, &om); // it's already a rotation vector
2330 cvConvertScale(&om, &om, -0.5); // get average rotation
2331 cvRodrigues2(&om, &r_r); // rotate cameras to same orientation by averaging
2332 cvMatMul(&r_r, _T, &t);
2334 int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1;
2335 double c = _t[idx], nt = cvNorm(&t, 0, CV_L2);
2336 _uu[idx] = c > 0 ? 1 : -1;
2338 // calculate global Z rotation
2339 cvCrossProduct(&t,&uu,&ww);
2340 double nw = cvNorm(&ww, 0, CV_L2);
2341 cvConvertScale(&ww, &ww, acos(fabs(c)/nt)/nw);
2342 cvRodrigues2(&ww, &wR);
2344 // apply to both views
2345 cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T);
2346 cvConvert( &Ri, _R1 );
2347 cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, 0);
2348 cvConvert( &Ri, _R2 );
2349 cvMatMul(&r_r, _T, &t);
2351 // calculate projection/camera matrices
2352 // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy)
2353 double fc_new = DBL_MAX;
2354 CvPoint2D64f cc_new[2] = {{0,0}, {0,0}};
2356 for( k = 0; k < 2; k++ ) {
2357 const CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
2358 const CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2;
2359 double dk1 = Dk ? cvmGet(Dk, 0, 0) : 0;
2360 double fc = cvmGet(A,idx^1,idx^1);
2362 fc *= 1 + dk1*(nx*nx + ny*ny)/(4*fc*fc);
2364 fc_new = MIN(fc_new, fc);
2367 for( k = 0; k < 2; k++ )
2369 const CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
2370 const CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2;
2371 CvPoint2D32f _pts[4];
2372 CvPoint3D32f _pts_3[4];
2373 CvMat pts = cvMat(1, 4, CV_32FC2, _pts);
2374 CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3);
2376 for( i = 0; i < 4; i++ )
2378 int j = (i<2) ? 0 : 1;
2379 _pts[i].x = (float)((i % 2)*(nx-1));
2380 _pts[i].y = (float)(j*(ny-1));
2382 cvUndistortPoints( &pts, &pts, A, Dk, 0, 0 );
2383 cvConvertPointsHomogeneous( &pts, &pts_3 );
2385 //Change camera matrix to have cc=[0,0] and fc = fc_new
2386 double _a_tmp[3][3];
2387 CvMat A_tmp = cvMat(3, 3, CV_64F, _a_tmp);
2388 _a_tmp[0][0]=fc_new;
2389 _a_tmp[1][1]=fc_new;
2392 cvProjectPoints2( &pts_3, k == 0 ? _R1 : _R2, &Z, &A_tmp, 0, &pts );
2393 CvScalar avg = cvAvg(&pts);
2394 cc_new[k].x = (nx-1)/2 - avg.val[0];
2395 cc_new[k].y = (ny-1)/2 - avg.val[1];
2398 // vertical focal length must be the same for both images to keep the epipolar constraint
2399 // (for horizontal epipolar lines -- TBD: check for vertical epipolar lines)
2400 // use fy for fx also, for simplicity
2402 // For simplicity, set the principal points for both cameras to be the average
2403 // of the two principal points (either one of or both x- and y- coordinates)
2404 if( flags & CV_CALIB_ZERO_DISPARITY )
2406 cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;
2407 cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
2409 else if( idx == 0 ) // horizontal stereo
2410 cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
2411 else // vertical stereo
2412 cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;
2415 _pp[0][0] = _pp[1][1] = fc_new;
2416 _pp[0][2] = cc_new[0].x;
2417 _pp[1][2] = cc_new[0].y;
2419 cvConvert(&pp, _P1);
2421 _pp[0][2] = cc_new[1].x;
2422 _pp[1][2] = cc_new[1].y;
2423 _pp[idx][3] = _t[idx]*fc_new; // baseline * focal length
2424 cvConvert(&pp, _P2);
2430 1, 0, 0, -cc_new[0].x,
2431 0, 1, 0, -cc_new[0].y,
2434 (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/_t[idx]
2436 CvMat Q = cvMat(4, 4, CV_64F, q);
2437 cvConvert( &Q, _Q );
2443 cvStereoRectifyUncalibrated(
2444 const CvMat* _points1, const CvMat* _points2,
2445 const CvMat* F0, CvSize imgSize, CvMat* _H1, CvMat* _H2, double threshold )
2453 CV_FUNCNAME( "cvStereoCalcHomographiesFromF" );
2459 double u[9], v[9], w[9], f[9], h1[9], h2[9], h0[9], e2[3];
2460 CvMat E2 = cvMat( 3, 1, CV_64F, e2 );
2461 CvMat U = cvMat( 3, 3, CV_64F, u );
2462 CvMat V = cvMat( 3, 3, CV_64F, v );
2463 CvMat W = cvMat( 3, 3, CV_64F, w );
2464 CvMat F = cvMat( 3, 3, CV_64F, f );
2465 CvMat H1 = cvMat( 3, 3, CV_64F, h1 );
2466 CvMat H2 = cvMat( 3, 3, CV_64F, h2 );
2467 CvMat H0 = cvMat( 3, 3, CV_64F, h0 );
2471 CvPoint3D64f* lines1;
2472 CvPoint3D64f* lines2;
2474 CV_ASSERT( CV_IS_MAT(_points1) && CV_IS_MAT(_points2) &&
2475 (_points1->rows == 1 || _points1->cols == 1) &&
2476 (_points2->rows == 1 || _points2->cols == 1) &&
2477 CV_ARE_SIZES_EQ(_points1, _points2) );
2479 npoints = _points1->rows * _points1->cols * CV_MAT_CN(_points1->type) / 2;
2481 _m1 = cvCreateMat( _points1->rows, _points1->cols, CV_64FC(CV_MAT_CN(_points1->type)) );
2482 _m2 = cvCreateMat( _points2->rows, _points2->cols, CV_64FC(CV_MAT_CN(_points2->type)) );
2483 _lines1 = cvCreateMat( 1, npoints, CV_64FC3 );
2484 _lines2 = cvCreateMat( 1, npoints, CV_64FC3 );
2486 cvConvert( F0, &F );
2488 cvSVD( (CvMat*)&F, &W, &U, &V, CV_SVD_U_T + CV_SVD_V_T );
2490 cvGEMM( &U, &W, 1, 0, 0, &W, CV_GEMM_A_T );
2491 cvMatMul( &W, &V, &F );
2493 cx = cvRound( (imgSize.width-1)*0.5 );
2494 cy = cvRound( (imgSize.height-1)*0.5 );
2499 cvConvert( _points1, _m1 );
2500 cvConvert( _points2, _m2 );
2501 cvReshape( _m1, _m1, 2, 1 );
2502 cvReshape( _m2, _m2, 2, 1 );
2504 m1 = (CvPoint2D64f*)_m1->data.ptr;
2505 m2 = (CvPoint2D64f*)_m2->data.ptr;
2506 lines1 = (CvPoint3D64f*)_lines1->data.ptr;
2507 lines2 = (CvPoint3D64f*)_lines2->data.ptr;
2511 cvComputeCorrespondEpilines( _m1, 1, &F, _lines1 );
2512 cvComputeCorrespondEpilines( _m2, 2, &F, _lines2 );
2514 // measure distance from points to the corresponding epilines, mark outliers
2515 for( i = j = 0; i < npoints; i++ )
2517 if( fabs(m1[i].x*lines2[i].x +
2518 m1[i].y*lines2[i].y +
2519 lines2[i].z) <= threshold &&
2520 fabs(m2[i].x*lines1[i].x +
2521 m2[i].y*lines1[i].y +
2522 lines1[i].z) <= threshold )
2539 _m1->cols = _m2->cols = npoints;
2540 memcpy( E2.data.db, U.data.db + 6, sizeof(e2));
2541 cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 );
2549 CvMat T = cvMat(3, 3, CV_64F, t);
2550 cvMatMul( &T, &E2, &E2 );
2552 int mirror = e2[0] < 0;
2553 double d = MAX(sqrt(e2[0]*e2[0] + e2[1]*e2[1]),DBL_EPSILON);
2554 double alpha = e2[0]/d;
2555 double beta = e2[1]/d;
2562 CvMat R = cvMat(3, 3, CV_64F, r);
2563 cvMatMul( &R, &T, &T );
2564 cvMatMul( &R, &E2, &E2 );
2565 double invf = fabs(e2[2]) < 1e-6*fabs(e2[0]) ? 0 : -e2[2]/e2[0];
2572 CvMat K = cvMat(3, 3, CV_64F, k);
2573 cvMatMul( &K, &T, &H2 );
2574 cvMatMul( &K, &E2, &E2 );
2582 CvMat iT = cvMat( 3, 3, CV_64F, it );
2583 cvMatMul( &iT, &H2, &H2 );
2585 memcpy( E2.data.db, U.data.db + 6, sizeof(e2));
2586 cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 );
2596 e2[0], e2[0], e2[0],
2597 e2[1], e2[1], e2[1],
2598 e2[2], e2[2], e2[2],
2600 CvMat E2_x = cvMat(3, 3, CV_64F, e2_x);
2601 CvMat E2_111 = cvMat(3, 3, CV_64F, e2_111);
2602 cvMatMulAdd(&E2_x, &F, &E2_111, &H0 );
2603 cvMatMul(&H2, &H0, &H0);
2604 CvMat E1=cvMat(3, 1, CV_64F, V.data.db+6);
2605 cvMatMul(&H0, &E1, &E1);
2607 cvPerspectiveTransform( _m1, _m1, &H0 );
2608 cvPerspectiveTransform( _m2, _m2, &H2 );
2609 CvMat A = cvMat( 1, npoints, CV_64FC3, lines1 ), BxBy, B;
2610 double a[9], atb[3], x[3];
2611 CvMat AtA = cvMat( 3, 3, CV_64F, a );
2612 CvMat AtB = cvMat( 3, 1, CV_64F, atb );
2613 CvMat X = cvMat( 3, 1, CV_64F, x );
2614 cvConvertPointsHomogeneous( _m1, &A );
2615 cvReshape( &A, &A, 1, npoints );
2616 cvReshape( _m2, &BxBy, 1, npoints );
2617 cvGetCol( &BxBy, &B, 0 );
2618 cvGEMM( &A, &A, 1, 0, 0, &AtA, CV_GEMM_A_T );
2619 cvGEMM( &A, &B, 1, 0, 0, &AtB, CV_GEMM_A_T );
2620 cvSolve( &AtA, &AtB, &X, CV_SVD_SYM );
2628 CvMat Ha = cvMat(3, 3, CV_64F, ha);
2629 cvMatMul( &Ha, &H0, &H1 );
2630 cvPerspectiveTransform( _m1, _m1, &Ha );
2634 double mm[] = { -1, 0, cx*2, 0, -1, cy*2, 0, 0, 1 };
2635 CvMat MM = cvMat(3, 3, CV_64F, mm);
2636 cvMatMul( &MM, &H1, &H1 );
2637 cvMatMul( &MM, &H2, &H2 );
2640 cvConvert( &H1, _H1 );
2641 cvConvert( &H2, _H2 );
2648 cvReleaseMat( &_m1 );
2649 cvReleaseMat( &_m2 );
2650 cvReleaseMat( &_lines1 );
2651 cvReleaseMat( &_lines2 );
2658 cvReprojectImageTo3D(
2659 const CvArr* disparityImage,
2660 CvArr* _3dImage, const CvMat* _Q,
2661 int handleMissingValues )
2663 const double bigZ = 10000.;
2664 CV_FUNCNAME( "cvReprojectImageTo3D" );
2669 CvMat Q = cvMat(4, 4, CV_64F, q);
2670 CvMat sstub, *src = cvGetMat( disparityImage, &sstub );
2671 CvMat dstub, *dst = cvGetMat( _3dImage, &dstub );
2672 int stype = CV_MAT_TYPE(src->type), dtype = CV_MAT_TYPE(dst->type);
2673 int x, y, rows = src->rows, cols = src->cols;
2674 float* sbuf = (float*)cvStackAlloc( cols*sizeof(sbuf[0]) );
2675 float* dbuf = (float*)cvStackAlloc( cols*3*sizeof(dbuf[0]) );
2676 double minDisparity = FLT_MAX;
2678 CV_ASSERT( CV_ARE_SIZES_EQ(src, dst) &&
2679 (CV_MAT_TYPE(stype) == CV_8UC1 || CV_MAT_TYPE(stype) == CV_16SC1 ||
2680 CV_MAT_TYPE(stype) == CV_32SC1 || CV_MAT_TYPE(stype) == CV_32FC1) &&
2681 (CV_MAT_TYPE(dtype) == CV_16SC3 || CV_MAT_TYPE(dtype) == CV_32SC3 ||
2682 CV_MAT_TYPE(dtype) == CV_32FC3) );
2684 cvConvert( _Q, &Q );
2686 // NOTE: here we quietly assume that at least one pixel in the disparity map is not defined.
2687 // and we set the corresponding Z's to some fixed big value.
2688 if( handleMissingValues )
2689 cvMinMaxLoc( disparityImage, &minDisparity, 0, 0, 0 );
2691 for( y = 0; y < rows; y++ )
2693 const float* sptr = (const float*)(src->data.ptr + src->step*y);
2694 float* dptr0 = (float*)(dst->data.ptr + dst->step*y), *dptr = dptr0;
2695 double qx = q[0][1]*y + q[0][3], qy = q[1][1]*y + q[1][3];
2696 double qz = q[2][1]*y + q[2][3], qw = q[3][1]*y + q[3][3];
2698 if( stype == CV_8UC1 )
2700 const uchar* sptr0 = (const uchar*)sptr;
2701 for( x = 0; x < cols; x++ )
2702 sbuf[x] = (float)sptr0[x];
2705 else if( stype == CV_16SC1 )
2707 const short* sptr0 = (const short*)sptr;
2708 for( x = 0; x < cols; x++ )
2709 sbuf[x] = (float)sptr0[x];
2712 else if( stype == CV_32SC1 )
2714 const int* sptr0 = (const int*)sptr;
2715 for( x = 0; x < cols; x++ )
2716 sbuf[x] = (float)sptr0[x];
2720 if( dtype != CV_32FC3 )
2723 for( x = 0; x < cols; x++, qx += q[0][0], qy += q[1][0], qz += q[2][0], qw += q[3][0] )
2726 double iW = 1./(qw + q[3][2]*d);
2727 double X = (qx + q[0][2]*d)*iW;
2728 double Y = (qy + q[1][2]*d)*iW;
2729 double Z = (qz + q[2][2]*d)*iW;
2730 if( fabs(d-minDisparity) <= FLT_EPSILON )
2733 dptr[x*3] = (float)X;
2734 dptr[x*3+1] = (float)Y;
2735 dptr[x*3+2] = (float)Z;
2738 if( dtype == CV_16SC3 )
2740 for( x = 0; x < cols*3; x++ )
2742 int ival = cvRound(dptr[x]);
2743 ((short*)dptr0)[x] = CV_CAST_16S(ival);
2746 else if( dtype == CV_32SC3 )
2748 for( x = 0; x < cols*3; x++ )
2750 int ival = cvRound(dptr[x]);
2751 ((int*)dptr0)[x] = ival;
2763 static void collectCalibrationData( const vector<vector<Point3f> >& objectPoints,
2764 const vector<vector<Point2f> >& imagePoints,
2765 const vector<vector<Point2f> >& imagePoints2,
2766 Mat& objPtMat, Mat& imgPtMat, Mat* imgPtMat2,
2769 size_t i, j = 0, ni = 0, nimages = objectPoints.size(), total = 0;
2770 CV_Assert(nimages > 0 && nimages == imagePoints.size() &&
2771 (!imgPtMat2 || nimages == imagePoints2.size()));
2773 for( i = 0; i < nimages; i++ )
2775 ni = objectPoints[i].size();
2776 CV_Assert(ni == imagePoints[i].size() && (!imgPtMat2 || ni == imagePoints2[i].size()));
2780 npoints.create(1, (int)nimages, CV_32S);
2781 objPtMat.create(1, (int)total, DataType<Point3f>::type);
2782 imgPtMat.create(1, (int)total, DataType<Point2f>::type);
2783 Point2f* imgPtData2 = 0;
2787 imgPtMat2->create(1, (int)total, DataType<Point2f>::type);
2788 imgPtData2 = imgPtMat2->ptr<Point2f>();
2791 Point3f* objPtData = objPtMat.ptr<Point3f>();
2792 Point2f* imgPtData = imgPtMat.ptr<Point2f>();
2794 for( i = 0; i < nimages; i++, j += ni )
2796 ni = objectPoints[i].size();
2797 ((int*)npoints.data)[i] = (int)ni;
2798 std::copy(objectPoints[i].begin(), objectPoints[i].end(), objPtData + j);
2799 std::copy(imagePoints[i].begin(), imagePoints[i].end(), imgPtData + j);
2801 std::copy(imagePoints2[i].begin(), imagePoints2[i].end(), imgPtData2 + j);
2805 static Mat prepareCameraMatrix(Mat& cameraMatrix0, int rtype)
2807 Mat cameraMatrix = Mat::eye(3, 3, rtype);
2808 if( cameraMatrix0.size() == cameraMatrix.size() )
2809 cameraMatrix0.convertTo(cameraMatrix, rtype);
2810 return cameraMatrix;
2813 static Mat prepareDistCoeffs(Mat& distCoeffs0, int rtype)
2815 Mat distCoeffs = Mat::zeros(distCoeffs0.cols == 1 ? Size(1, 5) : Size(5, 1), rtype);
2816 if( distCoeffs0.size() == Size(1, 4) ||
2817 distCoeffs0.size() == Size(1, 5) ||
2818 distCoeffs0.size() == Size(4, 1) ||
2819 distCoeffs0.size() == Size(5, 1) )
2821 Mat dstCoeffs(distCoeffs, Rect(0, 0, distCoeffs0.cols, distCoeffs0.rows));
2822 distCoeffs0.convertTo(dstCoeffs, rtype);
2830 void cv::Rodrigues(const Mat& src, Mat& dst)
2832 bool v2m = src.cols == 1 || src.rows == 1;
2833 dst.create(3, v2m ? 3 : 1, src.type());
2834 CvMat _src = src, _dst = dst;
2835 bool ok = cvRodrigues2(&_src, &_dst, 0) > 0;
2840 void cv::Rodrigues(const Mat& src, Mat& dst, Mat& jacobian)
2842 bool v2m = src.cols == 1 || src.rows == 1;
2843 dst.create(3, v2m ? 3 : 1, src.type());
2844 jacobian.create(v2m ? Size(9, 3) : Size(3, 9), src.type());
2845 CvMat _src = src, _dst = dst, _jacobian = jacobian;
2846 bool ok = cvRodrigues2(&_src, &_dst, &_jacobian) > 0;
2851 void cv::matMulDeriv( const Mat& A, const Mat& B, Mat& dABdA, Mat& dABdB )
2853 dABdA.create(A.rows*B.cols, A.rows*A.cols, A.type());
2854 dABdB.create(A.rows*B.cols, B.rows*B.cols, A.type());
2855 CvMat _A = A, _B = B, _dABdA = dABdA, _dABdB = dABdB;
2856 cvCalcMatMulDeriv(&_A, &_B, &_dABdA, &_dABdB);
2859 void cv::composeRT( const Mat& rvec1, const Mat& tvec1,
2860 const Mat& rvec2, const Mat& tvec2,
2861 Mat& rvec3, Mat& tvec3 )
2863 rvec3.create(rvec1.size(), rvec1.type());
2864 tvec3.create(tvec1.size(), tvec1.type());
2865 CvMat _rvec1 = rvec1, _tvec1 = tvec1, _rvec2 = rvec2,
2866 _tvec2 = tvec2, _rvec3 = rvec3, _tvec3 = tvec3;
2867 cvComposeRT(&_rvec1, &_tvec1, &_rvec2, &_tvec2, &_rvec3, &_tvec3, 0, 0, 0, 0, 0, 0, 0, 0);
2871 void cv::composeRT( const Mat& rvec1, const Mat& tvec1,
2872 const Mat& rvec2, const Mat& tvec2,
2873 Mat& rvec3, Mat& tvec3,
2874 Mat& dr3dr1, Mat& dr3dt1,
2875 Mat& dr3dr2, Mat& dr3dt2,
2876 Mat& dt3dr1, Mat& dt3dt1,
2877 Mat& dt3dr2, Mat& dt3dt2 )
2879 int rtype = rvec1.type();
2880 rvec3.create(rvec1.size(), rtype);
2881 tvec3.create(tvec1.size(), rtype);
2882 dr3dr1.create(3, 3, rtype); dr3dt1.create(3, 3, rtype);
2883 dr3dr2.create(3, 3, rtype); dr3dt2.create(3, 3, rtype);
2884 dt3dr1.create(3, 3, rtype); dt3dt1.create(3, 3, rtype);
2885 dt3dr2.create(3, 3, rtype); dt3dt2.create(3, 3, rtype);
2887 CvMat _rvec1 = rvec1, _tvec1 = tvec1, _rvec2 = rvec2,
2888 _tvec2 = tvec2, _rvec3 = rvec3, _tvec3 = tvec3;
2889 CvMat _dr3dr1 = dr3dr1, _dr3dt1 = dr3dt1, _dr3dr2 = dr3dr2, _dr3dt2 = dr3dt2;
2890 CvMat _dt3dr1 = dt3dr1, _dt3dt1 = dt3dt1, _dt3dr2 = dt3dr2, _dt3dt2 = dt3dt2;
2891 cvComposeRT(&_rvec1, &_tvec1, &_rvec2, &_tvec2, &_rvec3, &_tvec3,
2892 &_dr3dr1, &_dr3dt1, &_dr3dr2, &_dr3dt2,
2893 &_dt3dr1, &_dt3dt1, &_dt3dr2, &_dt3dt2);
2897 void cv::projectPoints( const Mat& opoints,
2898 const Mat& rvec, const Mat& tvec,
2899 const Mat& cameraMatrix,
2900 const Mat& distCoeffs,
2901 vector<Point2f>& ipoints )
2903 CV_Assert(opoints.isContinuous() && opoints.depth() == CV_32F &&
2904 ((opoints.rows == 1 && opoints.channels() == 3) ||
2905 opoints.cols*opoints.channels() == 3));
2907 ipoints.resize(opoints.cols*opoints.rows*opoints.channels()/3);
2908 CvMat _objectPoints = opoints, _imagePoints = Mat(ipoints);
2909 CvMat _rvec = rvec, _tvec = tvec, _cameraMatrix = cameraMatrix, _distCoeffs = distCoeffs;
2911 cvProjectPoints2( &_objectPoints, &_rvec, &_tvec, &_cameraMatrix, &_distCoeffs,
2912 &_imagePoints, 0, 0, 0, 0, 0, 0 );
2915 void cv::projectPoints( const Mat& opoints,
2916 const Mat& rvec, const Mat& tvec,
2917 const Mat& cameraMatrix,
2918 const Mat& distCoeffs,
2919 vector<Point2f>& ipoints,
2920 Mat& dpdrot, Mat& dpdt, Mat& dpdf,
2921 Mat& dpdc, Mat& dpddist,
2922 double aspectRatio )
2924 CV_Assert(opoints.isContinuous() && opoints.depth() == CV_32F &&
2925 ((opoints.rows == 1 && opoints.channels() == 3) ||
2926 opoints.cols*opoints.channels() == 3));
2928 int npoints = opoints.cols*opoints.rows*opoints.channels()/3;
2929 ipoints.resize(npoints);
2930 dpdrot.create(npoints*2, 3, CV_64F);
2931 dpdt.create(npoints*2, 3, CV_64F);
2932 dpdf.create(npoints*2, 2, CV_64F);
2933 dpdc.create(npoints*2, 3, CV_64F);
2934 dpddist.create(npoints*2, distCoeffs.rows + distCoeffs.cols - 1, CV_64F);
2935 CvMat _objectPoints = opoints, _imagePoints = Mat(ipoints);
2936 CvMat _rvec = rvec, _tvec = tvec, _cameraMatrix = cameraMatrix, _distCoeffs = distCoeffs;
2937 CvMat _dpdrot = dpdrot, _dpdt = dpdt, _dpdf = dpdf, _dpdc = dpdc, _dpddist = dpddist;
2939 cvProjectPoints2( &_objectPoints, &_rvec, &_tvec, &_cameraMatrix, &_distCoeffs,
2940 &_imagePoints, &_dpdrot, &_dpdt, &_dpdf, &_dpdc, &_dpddist, aspectRatio );
2943 void cv::solvePnP( const Mat& opoints, const Mat& ipoints,
2944 const Mat& cameraMatrix, const Mat& distCoeffs,
2945 Mat& rvec, Mat& tvec, bool useExtrinsicGuess )
2947 CV_Assert(opoints.isContinuous() && opoints.depth() == CV_32F &&
2948 ((opoints.rows == 1 && opoints.channels() == 3) ||
2949 opoints.cols*opoints.channels() == 3) &&
2950 ipoints.isContinuous() && ipoints.depth() == CV_32F &&
2951 ((ipoints.rows == 1 && ipoints.channels() == 2) ||
2952 ipoints.cols*ipoints.channels() == 2));
2954 rvec.create(3, 1, CV_64F);
2955 tvec.create(3, 1, CV_64F);
2956 CvMat _objectPoints = opoints, _imagePoints = ipoints;
2957 CvMat _cameraMatrix = cameraMatrix, _distCoeffs = distCoeffs;
2958 CvMat _rvec = rvec, _tvec = tvec;
2959 cvFindExtrinsicCameraParams2(&_objectPoints, &_imagePoints, &_cameraMatrix,
2960 &_distCoeffs, &_rvec, &_tvec, useExtrinsicGuess );
2964 cv::Mat cv::initCameraMatrix2D( const vector<vector<Point3f> >& objectPoints,
2965 const vector<vector<Point2f> >& imagePoints,
2966 Size imageSize, double aspectRatio )
2968 Mat objPt, imgPt, npoints, cameraMatrix(3, 3, CV_64F);
2969 collectCalibrationData( objectPoints, imagePoints, vector<vector<Point2f> >(),
2970 objPt, imgPt, 0, npoints );
2971 CvMat _objPt = objPt, _imgPt = imgPt, _npoints = npoints, _cameraMatrix = cameraMatrix;
2972 cvInitIntrinsicParams2D( &_objPt, &_imgPt, &_npoints,
2973 imageSize, &_cameraMatrix, aspectRatio );
2974 return cameraMatrix;
2978 void cv::calibrateCamera( const vector<vector<Point3f> >& objectPoints,
2979 const vector<vector<Point2f> >& imagePoints,
2980 Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,
2981 vector<Mat>& rvecs, vector<Mat>& tvecs, int flags )
2984 cameraMatrix = prepareCameraMatrix(cameraMatrix, rtype);
2985 distCoeffs = prepareDistCoeffs(distCoeffs, rtype);
2987 size_t i, nimages = objectPoints.size();
2988 CV_Assert( nimages > 0 );
2989 Mat objPt, imgPt, npoints, rvecM((int)nimages, 3, CV_64FC1), tvecM((int)nimages, 3, CV_64FC1);
2990 collectCalibrationData( objectPoints, imagePoints, vector<vector<Point2f> >(),
2991 objPt, imgPt, 0, npoints );
2992 CvMat _objPt = objPt, _imgPt = imgPt, _npoints = npoints;
2993 CvMat _cameraMatrix = cameraMatrix, _distCoeffs = distCoeffs;
2994 CvMat _rvecM = rvecM, _tvecM = tvecM;
2996 cvCalibrateCamera2(&_objPt, &_imgPt, &_npoints, imageSize, &_cameraMatrix,
2997 &_distCoeffs, &_rvecM, &_tvecM, flags );
2998 rvecs.resize(nimages);
2999 tvecs.resize(nimages);
3000 for( i = 0; i < nimages; i++ )
3002 rvecM.row((int)i).copyTo(rvecs[i]);
3003 tvecM.row((int)i).copyTo(tvecs[i]);
3007 void cv::calibrationMatrixValues( const Mat& cameraMatrix, Size imageSize,
3008 double apertureWidth, double apertureHeight,
3009 double& fovx, double& fovy, double& focalLength,
3010 Point2d& principalPoint, double& aspectRatio )
3012 CvMat _cameraMatrix = cameraMatrix;
3013 cvCalibrationMatrixValues( &_cameraMatrix, imageSize, apertureWidth, apertureHeight,
3014 &fovx, &fovy, &focalLength, (CvPoint2D64f*)&principalPoint, &aspectRatio );
3017 void cv::stereoCalibrate( const vector<vector<Point3f> >& objectPoints,
3018 const vector<vector<Point2f> >& imagePoints1,
3019 const vector<vector<Point2f> >& imagePoints2,
3020 Mat& cameraMatrix1, Mat& distCoeffs1,
3021 Mat& cameraMatrix2, Mat& distCoeffs2,
3022 Size imageSize, Mat& R, Mat& T,
3023 Mat& E, Mat& F, TermCriteria criteria,
3027 cameraMatrix1 = prepareCameraMatrix(cameraMatrix1, rtype);
3028 cameraMatrix2 = prepareCameraMatrix(cameraMatrix2, rtype);
3029 distCoeffs1 = prepareDistCoeffs(distCoeffs1, rtype);
3030 distCoeffs2 = prepareDistCoeffs(distCoeffs2, rtype);
3031 R.create(3, 3, rtype);
3032 T.create(3, 1, rtype);
3034 Mat objPt, imgPt, imgPt2, npoints;
3035 collectCalibrationData( objectPoints, imagePoints1, imagePoints2,
3036 objPt, imgPt, &imgPt2, npoints );
3037 CvMat _objPt = objPt, _imgPt = imgPt, _imgPt2 = imgPt2, _npoints = npoints;
3038 CvMat _cameraMatrix1 = cameraMatrix1, _distCoeffs1 = distCoeffs1;
3039 CvMat _cameraMatrix2 = cameraMatrix2, _distCoeffs2 = distCoeffs2;
3040 CvMat _R = R, _T = T, _E = E, _F = F;
3042 cvStereoCalibrate(&_objPt, &_imgPt, &_imgPt2, &_npoints, &_cameraMatrix1,
3043 &_cameraMatrix2, &_distCoeffs1, &_distCoeffs2, imageSize,
3044 &_R, &_T, &_E, &_F, criteria, flags );
3047 void cv::stereoRectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,
3048 const Mat& cameraMatrix2, const Mat& distCoeffs2,
3049 Size imageSize, const Mat& R, const Mat& T,
3050 Mat& R1, Mat& R2, Mat& P1, Mat& P2, Mat& Q,
3054 R1.create(3, 3, rtype);
3055 R2.create(3, 3, rtype);
3056 P1.create(3, 4, rtype);
3057 P2.create(3, 4, rtype);
3058 Q.create(4, 4, rtype);
3059 CvMat _cameraMatrix1 = cameraMatrix1, _distCoeffs1 = distCoeffs1;
3060 CvMat _cameraMatrix2 = cameraMatrix2, _distCoeffs2 = distCoeffs2;
3061 CvMat _R = R, _T = T, _R1 = R1, _R2 = R2, _P1 = P1, _P2 = P2, _Q = Q;
3062 cvStereoRectify( &_cameraMatrix1, &_cameraMatrix2, &_distCoeffs1, &_distCoeffs2,
3063 imageSize, &_R, &_T, &_R1, &_R2, &_P1, &_P2, &_Q, flags );
3066 bool cv::stereoRectifyUncalibrated( const Mat& points1, const Mat& points2,
3067 const Mat& F, Size imgSize,
3068 Mat& H1, Mat& H2, double threshold )
3071 H1.create(3, 3, rtype);
3072 H2.create(3, 3, rtype);
3073 CvMat _pt1 = points1, _pt2 = points2, _F, *pF=0, _H1 = H1, _H2 = H2;
3074 if( F.size() == Size(3, 3) )
3076 return cvStereoRectifyUncalibrated(&_pt1, &_pt2, pF, imgSize, &_H1, &_H2, threshold) > 0;
3079 void cv::reprojectImageTo3D( const Mat& disparity,
3080 Mat& _3dImage, const Mat& Q,
3081 bool handleMissingValues )
3083 _3dImage.create(disparity.size(), CV_32FC3);
3084 CvMat _disparity = disparity, __3dImage = _3dImage, _Q = Q;
3085 cvReprojectImageTo3D( &_disparity, &__3dImage, &_Q, handleMissingValues );