vcs.maemo.org Git - opencv/blob - src/cv/cvcalibration.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                           License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
  14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
  15 // Third party copyrights are property of their respective owners.
  16 //
  17 // Redistribution and use in source and binary forms, with or without modification,
  18 // are permitted provided that the following conditions are met:
  19 //
  20 //   * Redistribution's of source code must retain the above copyright notice,
  21 //     this list of conditions and the following disclaimer.
  22 //
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,
  24 //     this list of conditions and the following disclaimer in the documentation
  25 //     and/or other materials provided with the distribution.
  26 //
  27 //   * The name of the copyright holders may not be used to endorse or promote products
  28 //     derived from this software without specific prior written permission.
  29 //
  30 // This software is provided by the copyright holders and contributors "as is" and
  31 // any express or implied warranties, including, but not limited to, the implied
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,
  34 // indirect, incidental, special, exemplary, or consequential damages
  35 // (including, but not limited to, procurement of substitute goods or services;
  36 // loss of use, data, or profits; or business interruption) however caused
  37 // and on any theory of liability, whether in contract, strict liability,
  38 // or tort (including negligence or otherwise) arising in any way out of
  39 // the use of this software, even if advised of the possibility of such damage.
  40 //
  41 //M*/
  42
  43 #include "_cv.h"
  44
  45 /*
  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.
  50
  51     The 1st initial port was done by Valery Mosyagin.
  52 */
  53
  54 CvLevMarq::CvLevMarq()
  55 {
  56     mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = 0;
  57     lambdaLg10 = 0; state = DONE;
  58     criteria = cvTermCriteria(0,0,0);
  59     iters = 0;
  60     completeSymmFlag = false;
  61 }
  62
  63 CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
  64 {
  65     mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = 0;
  66     init(nparams, nerrs, criteria0, _completeSymmFlag);
  67 }
  68
  69 void CvLevMarq::clear()
  70 {
  71     cvReleaseMat(&mask);
  72     cvReleaseMat(&prevParam);
  73     cvReleaseMat(&param);
  74     cvReleaseMat(&J);
  75     cvReleaseMat(&err);
  76     cvReleaseMat(&JtJ);
  77     cvReleaseMat(&JtJN);
  78     cvReleaseMat(&JtErr);
  79     cvReleaseMat(&JtJV);
  80     cvReleaseMat(&JtJW);
  81 }
  82
  83 CvLevMarq::~CvLevMarq()
  84 {
  85     clear();
  86 }
  87
  88 void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
  89 {
  90     if( !param || param->rows != nparams || nerrs != (err ? err->rows : 0) )
  91         clear();
  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 );
 101     if( nerrs > 0 )
 102     {
 103         J = cvCreateMat( nerrs, nparams, CV_64F );
 104         err = cvCreateMat( nerrs, 1, CV_64F );
 105     }
 106     prevErrNorm = DBL_MAX;
 107     lambdaLg10 = -3;
 108     criteria = criteria0;
 109     if( criteria.type & CV_TERMCRIT_ITER )
 110         criteria.max_iter = MIN(MAX(criteria.max_iter,1),1000);
 111     else
 112         criteria.max_iter = 30;
 113     if( criteria.type & CV_TERMCRIT_EPS )
 114         criteria.epsilon = MAX(criteria.epsilon, 0);
 115     else
 116         criteria.epsilon = DBL_EPSILON;
 117     state = STARTED;
 118     iters = 0;
 119     completeSymmFlag = _completeSymmFlag;
 120 }
 121
 122 bool CvLevMarq::update( const CvMat*& _param, CvMat*& _J, CvMat*& _err )
 123 {
 124     double change;
 125
 126     _J = _err = 0;
 127
 128     assert( err != 0 );
 129     if( state == DONE )
 130     {
 131         _param = param;
 132         return false;
 133     }
 134
 135     if( state == STARTED )
 136     {
 137         _param = param;
 138         cvZero( J );
 139         cvZero( err );
 140         _J = J;
 141         _err = err;
 142         state = CALC_J;
 143         return true;
 144     }
 145
 146     if( state == CALC_J )
 147     {
 148         cvMulTransposed( J, JtJ, 1 );
 149         cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );
 150         cvCopy( param, prevParam );
 151         step();
 152         if( iters == 0 )
 153             prevErrNorm = cvNorm(err, 0, CV_L2);
 154         _param = param;
 155         cvZero( err );
 156         _err = err;
 157         state = CHECK_ERR;
 158         return true;
 159     }
 160
 161     assert( state == CHECK_ERR );
 162     errNorm = cvNorm( err, 0, CV_L2 );
 163     if( errNorm > prevErrNorm )
 164     {
 165         lambdaLg10++;
 166         step();
 167         _param = param;
 168         cvZero( err );
 169         _err = err;
 170         state = CHECK_ERR;
 171         return true;
 172     }
 173
 174     lambdaLg10 = MAX(lambdaLg10-1, -16);
 175     if( ++iters >= criteria.max_iter ||
 176         (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
 177     {
 178         _param = param;
 179         state = DONE;
 180         return true;
 181     }
 182
 183     prevErrNorm = errNorm;
 184     _param = param;
 185     cvZero(J);
 186     _J = J;
 187     _err = err;
 188     state = CALC_J;
 189     return true;
 190 }
 191
 192
 193 bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm )
 194 {
 195     double change;
 196
 197     assert( err == 0 );
 198     if( state == DONE )
 199     {
 200         _param = param;
 201         return false;
 202     }
 203
 204     if( state == STARTED )
 205     {
 206         _param = param;
 207         cvZero( JtJ );
 208         cvZero( JtErr );
 209         errNorm = 0;
 210         _JtJ = JtJ;
 211         _JtErr = JtErr;
 212         _errNorm = &errNorm;
 213         state = CALC_J;
 214         return true;
 215     }
 216
 217     if( state == CALC_J )
 218     {
 219         cvCopy( param, prevParam );
 220         step();
 221         _param = param;
 222         prevErrNorm = errNorm;
 223         errNorm = 0;
 224         _errNorm = &errNorm;
 225         state = CHECK_ERR;
 226         return true;
 227     }
 228
 229     assert( state == CHECK_ERR );
 230     if( errNorm > prevErrNorm )
 231     {
 232         lambdaLg10++;
 233         step();
 234         _param = param;
 235         errNorm = 0;
 236         _errNorm = &errNorm;
 237         state = CHECK_ERR;
 238         return true;
 239     }
 240
 241     lambdaLg10 = MAX(lambdaLg10-1, -16);
 242     if( ++iters >= criteria.max_iter ||
 243         (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
 244     {
 245         _param = param;
 246         state = DONE;
 247         return false;
 248     }
 249
 250     prevErrNorm = errNorm;
 251     cvZero( JtJ );
 252     cvZero( JtErr );
 253     _param = param;
 254     _JtJ = JtJ;
 255     _JtErr = JtErr;
 256     state = CALC_J;
 257     return true;
 258 }
 259
 260 void CvLevMarq::step()
 261 {
 262     const double LOG10 = log(10.);
 263     double lambda = exp(lambdaLg10*LOG10);
 264     int i, j, nparams = param->rows;
 265
 266     for( i = 0; i < nparams; i++ )
 267         if( mask->data.ptr[i] == 0 )
 268         {
 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;
 273         }
 274
 275     if( !err )
 276         cvCompleteSymm( JtJ, completeSymmFlag );
 277 #if 1
 278     cvCopy( JtJ, JtJN );
 279     for( i = 0; i < nparams; i++ )
 280         JtJN->data.db[(nparams+1)*i] *= 1. + lambda;
 281 #else
 282     cvSetIdentity(JtJN, cvRealScalar(lambda));
 283     cvAdd( JtJ, JtJN, JtJN );
 284 #endif
 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);
 289 }
 290
 291 // reimplementation of dAB.m
 292 CV_IMPL void
 293 cvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB )
 294 {
 295     CV_FUNCNAME( "cvCalcMatMulDeriv" );
 296
 297     __BEGIN__;
 298
 299     int i, j, M, N, L;
 300     int bstep;
 301
 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 );
 306
 307     M = A->rows;
 308     L = A->cols;
 309     N = B->cols;
 310     bstep = B->step/CV_ELEM_SIZE(B->type);
 311
 312     if( dABdA )
 313     {
 314         CV_ASSERT( CV_ARE_TYPES_EQ(A, dABdA) &&
 315             dABdA->rows == A->rows*B->cols && dABdA->cols == A->rows*A->cols );
 316     }
 317
 318     if( dABdB )
 319     {
 320         CV_ASSERT( CV_ARE_TYPES_EQ(A, dABdB) &&
 321             dABdB->rows == A->rows*B->cols && dABdB->cols == B->rows*B->cols );
 322     }
 323
 324     if( CV_MAT_TYPE(A->type) == CV_32F )
 325     {
 326         for( i = 0; i < M*N; i++ )
 327         {
 328             int i1 = i / N,  i2 = i % N;
 329
 330             if( dABdA )
 331             {
 332                 float* dcda = (float*)(dABdA->data.ptr + dABdA->step*i);
 333                 const float* b = (const float*)B->data.ptr + i2;
 334
 335                 for( j = 0; j < M*L; j++ )
 336                     dcda[j] = 0;
 337                 for( j = 0; j < L; j++ )
 338                     dcda[i1*L + j] = b[j*bstep];
 339             }
 340
 341             if( dABdB )
 342             {
 343                 float* dcdb = (float*)(dABdB->data.ptr + dABdB->step*i);
 344                 const float* a = (const float*)(A->data.ptr + A->step*i1);
 345
 346                 for( j = 0; j < L*N; j++ )
 347                     dcdb[j] = 0;
 348                 for( j = 0; j < L; j++ )
 349                     dcdb[j*N + i2] = a[j];
 350             }
 351         }
 352     }
 353     else
 354     {
 355         for( i = 0; i < M*N; i++ )
 356         {
 357             int i1 = i / N,  i2 = i % N;
 358
 359             if( dABdA )
 360             {
 361                 double* dcda = (double*)(dABdA->data.ptr + dABdA->step*i);
 362                 const double* b = (const double*)B->data.ptr + i2;
 363
 364                 for( j = 0; j < M*L; j++ )
 365                     dcda[j] = 0;
 366                 for( j = 0; j < L; j++ )
 367                     dcda[i1*L + j] = b[j*bstep];
 368             }
 369
 370             if( dABdB )
 371             {
 372                 double* dcdb = (double*)(dABdB->data.ptr + dABdB->step*i);
 373                 const double* a = (const double*)(A->data.ptr + A->step*i1);
 374
 375                 for( j = 0; j < L*N; j++ )
 376                     dcdb[j] = 0;
 377                 for( j = 0; j < L; j++ )
 378                     dcdb[j*N + i2] = a[j];
 379             }
 380         }
 381     }
 382
 383     __END__;
 384 }
 385
 386 // reimplementation of compose_motion.m
 387 CV_IMPL void
 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 )
 395 {
 396     CV_FUNCNAME( "cvComposeRT" );
 397
 398     __BEGIN__;
 399
 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);
 405
 406     CV_ASSERT( CV_IS_MAT(_rvec1) && CV_IS_MAT(_rvec2) );
 407
 408     CV_ASSERT( CV_MAT_TYPE(_rvec1->type) == CV_32F ||
 409                CV_MAT_TYPE(_rvec1->type) == CV_64F );
 410
 411     CV_ASSERT( _rvec1->rows == 3 && _rvec1->cols == 1 && CV_ARE_SIZES_EQ(_rvec1, _rvec2) );
 412
 413     cvConvert( _rvec1, &r1 );
 414     cvConvert( _rvec2, &r2 );
 415
 416     cvRodrigues2( &r1, &R1, &dR1dr1 );
 417     cvRodrigues2( &r2, &R2, &dR2dr2 );
 418
 419     if( _rvec3 || dr3dr1 || dr3dr1 )
 420     {
 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);
 427
 428         cvMatMul( &R2, &R1, &R3 );
 429         cvCalcMatMulDeriv( &R2, &R1, &dR3dR2, &dR3dR1 );
 430
 431         cvRodrigues2( &R3, &r3, &dr3dR3 );
 432
 433         if( _rvec3 )
 434             cvConvert( &r3, _rvec3 );
 435
 436         if( dr3dr1 )
 437         {
 438             cvMatMul( &dr3dR3, &dR3dR1, &W1 );
 439             cvMatMul( &W1, &dR1dr1, &W2 );
 440             cvConvert( &W2, dr3dr1 );
 441         }
 442
 443         if( dr3dr2 )
 444         {
 445             cvMatMul( &dr3dR3, &dR3dR2, &W1 );
 446             cvMatMul( &W1, &dR2dr2, &W2 );
 447             cvConvert( &W2, dr3dr2 );
 448         }
 449     }
 450
 451     if( dr3dt1 )
 452         cvZero( dr3dt1 );
 453     if( dr3dt2 )
 454         cvZero( dr3dt2 );
 455
 456     if( _tvec3 || dt3dr2 || dt3dt1 )
 457     {
 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);
 464
 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) );
 467
 468         cvConvert( _tvec1, &t1 );
 469         cvConvert( _tvec2, &t2 );
 470         cvMatMulAdd( &R2, &t1, &t2, &t3 );
 471
 472         if( _tvec3 )
 473             cvConvert( &t3, _tvec3 );
 474
 475         if( dt3dr2 || dt3dt1 )
 476         {
 477             cvCalcMatMulDeriv( &R2, &t1, &dxdR2, &dxdt1 );
 478             if( dt3dr2 )
 479             {
 480                 cvMatMul( &dxdR2, &dR2dr2, &W3 );
 481                 cvConvert( &W3, dt3dr2 );
 482             }
 483             if( dt3dt1 )
 484                 cvConvert( &dxdt1, dt3dt1 );
 485         }
 486     }
 487
 488     if( dt3dt2 )
 489         cvSetIdentity( dt3dt2 );
 490     if( dt3dr1 )
 491         cvZero( dt3dr1 );
 492
 493     __END__;
 494 }
 495
 496 CV_IMPL int
 497 cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
 498 {
 499     int result = 0;
 500
 501     CV_FUNCNAME( "cvRogrigues2" );
 502
 503     __BEGIN__;
 504
 505     int depth, elem_size;
 506     int i, k;
 507     double J[27];
 508     CvMat _J = cvMat( 3, 9, CV_64F, J );
 509
 510     if( !CV_IS_MAT(src) )
 511         CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg, "Input argument is not a valid matrix" );
 512
 513     if( !CV_IS_MAT(dst) )
 514         CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg,
 515         "The first output argument is not a valid matrix" );
 516
 517     depth = CV_MAT_DEPTH(src->type);
 518     elem_size = CV_ELEM_SIZE(depth);
 519
 520     if( depth != CV_32F && depth != CV_64F )
 521         CV_ERROR( CV_StsUnsupportedFormat, "The matrices must have 32f or 64f data type" );
 522
 523     if( !CV_ARE_DEPTHS_EQ(src, dst) )
 524         CV_ERROR( CV_StsUnmatchedFormats, "All the matrices must have the same data type" );
 525
 526     if( jacobian )
 527     {
 528         if( !CV_IS_MAT(jacobian) )
 529             CV_ERROR( CV_StsBadArg, "Jacobian is not a valid matrix" );
 530
 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" );
 533
 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" );
 537     }
 538
 539     if( src->cols == 1 || src->rows == 1 )
 540     {
 541         double rx, ry, rz, theta;
 542         int step = src->rows > 1 ? src->step / elem_size : 1;
 543
 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" );
 546
 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" );
 549
 550         if( depth == CV_32F )
 551         {
 552             rx = src->data.fl[0];
 553             ry = src->data.fl[step];
 554             rz = src->data.fl[step*2];
 555         }
 556         else
 557         {
 558             rx = src->data.db[0];
 559             ry = src->data.db[step];
 560             rz = src->data.db[step*2];
 561         }
 562         theta = sqrt(rx*rx + ry*ry + rz*rz);
 563
 564         if( theta < DBL_EPSILON )
 565         {
 566             cvSetIdentity( dst );
 567
 568             if( jacobian )
 569             {
 570                 memset( J, 0, sizeof(J) );
 571                 J[5] = J[15] = J[19] = -1;
 572                 J[7] = J[11] = J[21] = 1;
 573             }
 574         }
 575         else
 576         {
 577             const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
 578
 579             double c = cos(theta);
 580             double s = sin(theta);
 581             double c1 = 1. - c;
 582             double itheta = theta ? 1./theta : 0.;
 583
 584             rx *= itheta; ry *= itheta; rz *= itheta;
 585
 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 };
 588             double R[9];
 589             CvMat _R = cvMat( 3, 3, CV_64F, R );
 590
 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];
 595
 596             cvConvert( &_R, dst );
 597
 598             if( jacobian )
 599             {
 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++ )
 607                 {
 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];
 614                 }
 615             }
 616         }
 617     }
 618     else if( src->cols == 3 && src->rows == 3 )
 619     {
 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 );
 625         double theta, s, c;
 626         int step = dst->rows > 1 ? dst->step / elem_size : 1;
 627
 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" );
 631
 632         cvConvert( src, &_R );
 633         if( !cvCheckArr( &_R, CV_CHECK_RANGE+CV_CHECK_QUIET, -100, 100 ) )
 634         {
 635             cvZero(dst);
 636             if( jacobian )
 637                 cvZero(jacobian);
 638             EXIT;
 639         }
 640
 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 );
 643
 644         rx = R[7] - R[5];
 645         ry = R[2] - R[6];
 646         rz = R[3] - R[1];
 647
 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;
 651         theta = acos(c);
 652
 653         if( s < 1e-5 )
 654         {
 655             double t;
 656
 657             if( c > 0 )
 658                 rx = ry = rz = 0;
 659             else
 660             {
 661                 t = (R[0] + 1)*0.5;
 662                 rx = sqrt(MAX(t,0.));
 663                 t = (R[4] + 1)*0.5;
 664                 ry = sqrt(MAX(t,0.))*(R[1] < 0 ? -1. : 1.);
 665                 t = (R[8] + 1)*0.5;
 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) )
 668                     rz = -rz;
 669                 theta /= sqrt(rx*rx + ry*ry + rz*rz);
 670                 rx *= theta;
 671                 ry *= theta;
 672                 rz *= theta;
 673             }
 674
 675             if( jacobian )
 676             {
 677                 memset( J, 0, sizeof(J) );
 678                 if( c > 0 )
 679                 {
 680                     J[5] = J[15] = J[19] = -0.5;
 681                     J[7] = J[11] = J[21] = 0.5;
 682                 }
 683             }
 684         }
 685         else
 686         {
 687             double vth = 1/(2*s);
 688
 689             if( jacobian )
 690             {
 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
 698                 double dvardR[5*9] =
 699                 {
 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
 705                 };
 706                 // var2 = [om;theta]
 707                 double dvar2dvar[] =
 708                 {
 709                     vth, 0, 0, rx, 0,
 710                     0, vth, 0, ry, 0,
 711                     0, 0, vth, rz, 0,
 712                     0, 0, 0, 0, 1
 713                 };
 714                 double domegadvar2[] =
 715                 {
 716                     theta, 0, 0, rx*vth,
 717                     0, theta, 0, ry*vth,
 718                     0, 0, theta, rz*vth
 719                 };
 720
 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 );
 724                 double t0[3*5];
 725                 CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 );
 726
 727                 cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 );
 728                 cvMatMul( &_t0, &_dvardR, &_J );
 729
 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);
 734             }
 735
 736             vth *= theta;
 737             rx *= vth; ry *= vth; rz *= vth;
 738         }
 739
 740         if( depth == CV_32F )
 741         {
 742             dst->data.fl[0] = (float)rx;
 743             dst->data.fl[step] = (float)ry;
 744             dst->data.fl[step*2] = (float)rz;
 745         }
 746         else
 747         {
 748             dst->data.db[0] = rx;
 749             dst->data.db[step] = ry;
 750             dst->data.db[step*2] = rz;
 751         }
 752     }
 753
 754     if( jacobian )
 755     {
 756         if( depth == CV_32F )
 757         {
 758             if( jacobian->rows == _J.rows )
 759                 cvConvert( &_J, jacobian );
 760             else
 761             {
 762                 float Jf[3*9];
 763                 CvMat _Jf = cvMat( _J.rows, _J.cols, CV_32FC1, Jf );
 764                 cvConvert( &_J, &_Jf );
 765                 cvTranspose( &_Jf, jacobian );
 766             }
 767         }
 768         else if( jacobian->rows == _J.rows )
 769             cvCopy( &_J, jacobian );
 770         else
 771             cvTranspose( &_J, jacobian );
 772     }
 773
 774     result = 1;
 775
 776     __END__;
 777
 778     return result;
 779 }
 780
 781
 782 CV_IMPL void
 783 cvProjectPoints2( const CvMat* objectPoints,
 784                   const CvMat* r_vec,
 785                   const CvMat* t_vec,
 786                   const CvMat* A,
 787                   const CvMat* distCoeffs,
 788                   CvMat* imagePoints, CvMat* dpdr,
 789                   CvMat* dpdt, CvMat* dpdf,
 790                   CvMat* dpdc, CvMat* dpdk,
 791                   double aspectRatio )
 792 {
 793     CvMat *_M = 0, *_m = 0;
 794     CvMat *_dpdr = 0, *_dpdt = 0, *_dpdc = 0, *_dpdf = 0, *_dpdk = 0;
 795
 796     CV_FUNCNAME( "cvProjectPoints2" );
 797
 798     __BEGIN__;
 799
 800     int i, j, count;
 801     int calc_derivatives;
 802     const CvPoint3D64f* M;
 803     CvPoint2D64f* 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;
 810
 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" );
 815
 816     count = MAX(objectPoints->rows, objectPoints->cols);
 817
 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;
 822     else
 823     {
 824         CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 ));
 825         CV_CALL( cvConvertPointsHomogeneous( objectPoints, _M ));
 826     }
 827
 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)))
 831         _m = imagePoints;
 832     else
 833         CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
 834
 835     M = (CvPoint3D64f*)_M->data.db;
 836     m = (CvPoint2D64f*)_m->data.db;
 837
 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" );
 844
 845     if( r_vec->rows == 3 && r_vec->cols == 3 )
 846     {
 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 );
 851     }
 852     else
 853     {
 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 ) );
 857     }
 858
 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" );
 864
 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 ));
 867
 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" );
 871
 872     CV_CALL( cvConvert( A, &_a ));
 873     fx = a[0]; fy = a[4];
 874     cx = a[2]; cy = a[5];
 875
 876     if( fixedAspectRatio )
 877         fx = fy*aspectRatio;
 878
 879     if( distCoeffs )
 880     {
 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" );
 889
 890         _k = cvMat( distCoeffs->rows, distCoeffs->cols,
 891                     CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
 892         CV_CALL( cvConvert( distCoeffs, &_k ));
 893     }
 894
 895     if( dpdr )
 896     {
 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" );
 902
 903         if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
 904             _dpdr = dpdr;
 905         else
 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]);
 909     }
 910
 911     if( dpdt )
 912     {
 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" );
 918
 919         if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
 920             _dpdt = dpdt;
 921         else
 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]);
 925     }
 926
 927     if( dpdf )
 928     {
 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" );
 933
 934         if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
 935             _dpdf = dpdf;
 936         else
 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]);
 940     }
 941
 942     if( dpdc )
 943     {
 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" );
 948
 949         if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
 950             _dpdc = dpdc;
 951         else
 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]);
 955     }
 956
 957     if( dpdk )
 958     {
 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" );
 963
 964         if( !distCoeffs )
 965             CV_ERROR( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
 966
 967         if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
 968             _dpdk = dpdk;
 969         else
 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]);
 973     }
 974
 975     calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk;
 976
 977     for( i = 0; i < count; i++ )
 978     {
 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;
 984         double xd, yd;
 985
 986         z = z ? 1./z : 1;
 987         x *= z; y *= z;
 988
 989         r2 = x*x + y*y;
 990         r4 = r2*r2;
 991         r6 = r4*r2;
 992         a1 = 2*x*y;
 993         a2 = r2 + 2*x*x;
 994         a3 = r2 + 2*y*y;
 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;
 998
 999         m[i].x = xd*fx + cx;
1000         m[i].y = yd*fy + cy;
1001
1002         if( calc_derivatives )
1003         {
1004             if( dpdc_p )
1005             {
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;
1010             }
1011
1012             if( dpdf_p )
1013             {
1014                 if( fixedAspectRatio )
1015                 {
1016                     dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio;
1017                     dpdf_p[dpdf_step] = 0;
1018                     dpdf_p[dpdf_step+1] = yd;
1019                 }
1020                 else
1021                 {
1022                     dpdf_p[0] = xd; dpdf_p[1] = 0;
1023                     dpdf_p[dpdf_step] = 0;
1024                     dpdf_p[dpdf_step+1] = yd;
1025                 }
1026                 dpdf_p += dpdf_step*2;
1027             }
1028
1029             if( dpdk_p )
1030             {
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 )
1036                 {
1037                     dpdk_p[2] = fx*a1;
1038                     dpdk_p[3] = fx*a2;
1039                     dpdk_p[dpdk_step+2] = fy*a3;
1040                     dpdk_p[dpdk_step+3] = fy*a1;
1041                     if( _dpdk->cols > 4 )
1042                     {
1043                         dpdk_p[4] = fx*x*r6;
1044                         dpdk_p[dpdk_step+4] = fy*y*r6;
1045                     }
1046                 }
1047                 dpdk_p += dpdk_step*2;
1048             }
1049
1050             if( dpdt_p )
1051             {
1052                 double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
1053                 for( j = 0; j < 3; j++ )
1054                 {
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);
1062                     dpdt_p[j] = dmxdt;
1063                     dpdt_p[dpdt_step+j] = dmydt;
1064                 }
1065                 dpdt_p += dpdt_step*2;
1066             }
1067
1068             if( dpdr_p )
1069             {
1070                 double dx0dr[] =
1071                 {
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]
1075                 };
1076                 double dy0dr[] =
1077                 {
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]
1081                 };
1082                 double dz0dr[] =
1083                 {
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]
1087                 };
1088                 for( j = 0; j < 3; j++ )
1089                 {
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);
1099                     dpdr_p[j] = dmxdr;
1100                     dpdr_p[dpdr_step+j] = dmydr;
1101                 }
1102                 dpdr_p += dpdr_step*2;
1103             }
1104         }
1105     }
1106
1107     if( _m != imagePoints )
1108         cvConvertPointsHomogeneous( _m, imagePoints );
1109     if( _dpdr != dpdr )
1110         cvConvert( _dpdr, dpdr );
1111     if( _dpdt != dpdt )
1112         cvConvert( _dpdt, dpdt );
1113     if( _dpdf != dpdf )
1114         cvConvert( _dpdf, dpdf );
1115     if( _dpdc != dpdc )
1116         cvConvert( _dpdc, dpdc );
1117     if( _dpdk != dpdk )
1118         cvConvert( _dpdk, dpdk );
1119
1120     __END__;
1121
1122     if( _M != objectPoints )
1123         cvReleaseMat( &_M );
1124     if( _m != imagePoints )
1125         cvReleaseMat( &_m );
1126     if( _dpdr != dpdr )
1127         cvReleaseMat( &_dpdr );
1128     if( _dpdt != dpdt )
1129         cvReleaseMat( &_dpdt );
1130     if( _dpdf != dpdf )
1131         cvReleaseMat( &_dpdf );
1132     if( _dpdc != dpdc )
1133         cvReleaseMat( &_dpdc );
1134     if( _dpdk != dpdk )
1135         cvReleaseMat( &_dpdk );
1136 }
1137
1138
1139 CV_IMPL void
1140 cvFindExtrinsicCameraParams2( const CvMat* objectPoints,
1141                   const CvMat* imagePoints, const CvMat* A,
1142                   const CvMat* distCoeffs,
1143                   CvMat* rvec, CvMat* tvec,
1144                   int useExtrinsicGuess )
1145 {
1146     const int max_iter = 20;
1147     CvMat *_M = 0, *_Mxy = 0, *_m = 0, *_mn = 0, *_L = 0, *_J = 0;
1148
1149     CV_FUNCNAME( "cvFindExtrinsicCameraParams2" );
1150
1151     __BEGIN__;
1152
1153     int i, count;
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];
1156     CvScalar Mc;
1157     double param[6];
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 );
1169     CvMat _dpdr, _dpdt;
1170
1171     CV_ASSERT( CV_IS_MAT(objectPoints) && CV_IS_MAT(imagePoints) &&
1172         CV_IS_MAT(A) && CV_IS_MAT(rvec) && CV_IS_MAT(tvec) );
1173
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 ));
1177
1178     CV_CALL( cvConvertPointsHomogeneous( objectPoints, _M ));
1179     CV_CALL( cvConvertPointsHomogeneous( imagePoints, _m ));
1180     CV_CALL( cvConvert( A, &_A ));
1181
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 );
1184
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 );
1187
1188     CV_CALL( _mn = cvCreateMat( 1, count, CV_64FC2 ));
1189     CV_CALL( _Mxy = cvCreateMat( 1, count, CV_64FC2 ));
1190
1191     // normalize image points
1192     // (unapply the intrinsic matrix transformation and distortion)
1193     cvUndistortPoints( _m, _mn, &_A, distCoeffs, 0, &_Ar );
1194
1195     if( useExtrinsicGuess )
1196     {
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 );
1203     }
1204     else
1205     {
1206         Mc = cvAvg(_M);
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 );
1210
1211         // initialize extrinsic parameters
1212         if( W[2]/W[1] < 1e-3 || count < 4 )
1213         {
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;
1220
1221             if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
1222                 cvSetIdentity( R_transform );
1223
1224             if( cvDet(R_transform) < 0 )
1225                 cvScale( R_transform, R_transform, -1 );
1226
1227             cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );
1228
1229             for( i = 0; i < count; i++ )
1230             {
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;
1235
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];
1238             }
1239
1240             cvFindHomography( _Mxy, _mn, &_H );
1241
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]);
1247
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 );
1252
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 );
1258         }
1259         else
1260         {
1261             // non-planar structure. Use DLT method
1262             double* L;
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;
1270
1271             CV_CALL( _L = cvCreateMat( 2*count, 12, CV_64F ));
1272             L = _L->data.db;
1273
1274             for( i = 0; i < count; i++, L += 24 )
1275             {
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;
1280                 L[3] = L[19] = 1.;
1281                 L[4] = L[5] = L[6] = L[7] = 0.;
1282                 L[12] = L[13] = L[14] = L[15] = 0.;
1283                 L[8] = x*M[i].x;
1284                 L[9] = x*M[i].y;
1285                 L[10] = x*M[i].z;
1286                 L[11] = x;
1287                 L[20] = y*M[i].x;
1288                 L[21] = y*M[i].y;
1289                 L[22] = y*M[i].z;
1290                 L[23] = y;
1291             }
1292
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 );
1300             sc = cvNorm(&_RR);
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 );
1306         }
1307     }
1308
1309     cvReshape( _M, _M, 3, 1 );
1310     cvReshape( _mn, _mn, 2, 1 );
1311
1312     // refine extrinsic parameters using iterative algorithm
1313     {
1314     CvLevMarq solver( 6, count*2, cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,max_iter,FLT_EPSILON), true);
1315     cvCopy( &_param, solver.param );
1316
1317     for(;;)
1318     {
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 )
1324             break;
1325         cvReshape( _err, _err, 2, 1 );
1326         if( _J )
1327         {
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 );
1332         }
1333         else
1334         {
1335             cvProjectPoints2( _M, &_r, &_t, &_A, distCoeffs,
1336                               _err, 0, 0, 0, 0, 0 );
1337         }
1338         cvSub(_err, _m, _err);
1339         cvReshape( _err, _err, 1, 2*count );
1340     }
1341     cvCopy( solver.param, &_param );
1342     }
1343
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 );
1348
1349     cvConvert( &_r, rvec );
1350     cvConvert( &_t, tvec );
1351
1352     __END__;
1353
1354     cvReleaseMat( &_M );
1355     cvReleaseMat( &_Mxy );
1356     cvReleaseMat( &_m );
1357     cvReleaseMat( &_mn );
1358     cvReleaseMat( &_L );
1359     cvReleaseMat( &_J );
1360 }
1361
1362
1363 CV_IMPL void
1364 cvInitIntrinsicParams2D( const CvMat* objectPoints,
1365                          const CvMat* imagePoints,
1366                          const CvMat* npoints,
1367                          CvSize imageSize,
1368                          CvMat* cameraMatrix,
1369                          double aspectRatio )
1370 {
1371     CvMat *_A = 0, *_b = 0, *_allH = 0, *_allK = 0;
1372
1373     CV_FUNCNAME( "cvInitIntrinsicParams2D" );
1374
1375     __BEGIN__;
1376
1377     int i, j, pos, nimages, total, ni = 0;
1378     double a[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 };
1379     double H[9], f[2];
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 );
1383
1384     assert( CV_MAT_TYPE(npoints->type) == CV_32SC1 &&
1385             CV_IS_MAT_CONT(npoints->type) );
1386     nimages = npoints->rows + npoints->cols - 1;
1387
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" );
1393
1394     if( objectPoints->rows != 1 || imagePoints->rows != 1 )
1395         CV_ERROR( CV_StsBadSize, "object points and image points must be a single-row matrices" );
1396
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 );
1402
1403     total = cvRound(cvSum(npoints).val[0]);
1404
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 )
1407     {
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};
1413         CvMat _m, _M;
1414         cvGetCols( objectPoints, &_M, pos, pos + ni );
1415         cvGetCols( imagePoints, &_m, pos, pos + ni );
1416
1417         cvFindHomography( &_M, &_m, &_H );
1418         memcpy( _allH->data.db + i*9, H, sizeof(H) );
1419
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];
1422
1423         for( j = 0; j < 3; j++ )
1424         {
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];
1431         }
1432
1433         for( j = 0; j < 4; j++ )
1434             n[j] = 1./sqrt(n[j]);
1435
1436         for( j = 0; j < 3; j++ )
1437         {
1438             h[j] *= n[0]; v[j] *= n[1];
1439             d1[j] *= n[2]; d2[j] *= n[3];
1440         }
1441
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];
1445     }
1446
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 )
1451     {
1452         double tf = (a[0] + a[4])/(aspectRatio + 1.);
1453         a[0] = aspectRatio*tf;
1454         a[4] = tf;
1455     }
1456
1457     cvConvert( &_a, cameraMatrix );
1458
1459     __END__;
1460
1461     cvReleaseMat( &_A );
1462     cvReleaseMat( &_b );
1463     cvReleaseMat( &_allH );
1464     cvReleaseMat( &_allK );
1465 }
1466
1467
1468 /* finds intrinsic and extrinsic camera parameters
1469    from a few views of known calibration pattern */
1470 CV_IMPL void
1471 cvCalibrateCamera2( const CvMat* objectPoints,
1472                     const CvMat* imagePoints,
1473                     const CvMat* npoints,
1474                     CvSize imageSize,
1475                     CvMat* cameraMatrix, CvMat* distCoeffs,
1476                     CvMat* rvecs, CvMat* tvecs,
1477                     int flags )
1478 {
1479     const int NINTRINSIC = 9;
1480     CvMat *_M = 0, *_m = 0, *_Ji = 0, *_Je = 0, *_err = 0;
1481     CvLevMarq solver;
1482
1483     CV_FUNCNAME( "cvCalibrateCamera2" );
1484
1485     __BEGIN__;
1486
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.;
1491
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" );
1496
1497     if( imageSize.width <= 0 || imageSize.height <= 0 )
1498         CV_ERROR( CV_StsOutOfRange, "image width and height must be positive" );
1499
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" );
1504
1505     nimages = npoints->rows*npoints->cols;
1506     npstep = npoints->rows == 1 ? 1 : npoints->step/CV_ELEM_SIZE(npoints->type);
1507
1508     if( rvecs )
1509     {
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" );
1517     }
1518
1519     if( tvecs )
1520     {
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" );
1528     }
1529
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" );
1535
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" );
1543
1544     for( i = 0; i < nimages; i++ )
1545     {
1546         ni = npoints->data.i[i*npstep];
1547         if( ni < 4 )
1548         {
1549             char buf[100];
1550             sprintf( buf, "The number of points in the view #%d is < 4", i );
1551             CV_ERROR( CV_StsOutOfRange, buf );
1552         }
1553         maxPoints = MAX( maxPoints, ni );
1554         total += ni;
1555     }
1556
1557     CV_CALL( _M = cvCreateMat( 1, total, CV_64FC3 ));
1558     CV_CALL( _m = cvCreateMat( 1, total, CV_64FC2 ));
1559
1560     CV_CALL( cvConvertPointsHomogeneous( objectPoints, _M ));
1561     CV_CALL( cvConvertPointsHomogeneous( imagePoints, _m ));
1562
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 ));
1567     cvZero( _Ji );
1568
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;
1572
1573     // 1. initialize intrinsic parameters & LM solver
1574     if( flags & CV_CALIB_USE_INTRINSIC_GUESS )
1575     {
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.;
1589         A[8] = 1.;
1590
1591         if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1592             aspectRatio = A[0]/A[4];
1593         cvConvert( distCoeffs, &_k );
1594     }
1595     else
1596     {
1597         CvScalar mean, sdv;
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.;
1604
1605         if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1606         {
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" );
1612         }
1613         cvInitIntrinsicParams2D( _M, _m, npoints, imageSize, &_A, aspectRatio );
1614     }
1615
1616     solver.init( nparams, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,DBL_EPSILON) );
1617
1618     {
1619     double* param = solver.param->data.db;
1620     uchar* mask = solver.mask->data.ptr;
1621
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];
1624     param[8] = k[4];
1625
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 )
1631     {
1632         param[6] = param[7] = 0;
1633         mask[6] = mask[7] = 0;
1634     }
1635     if( flags & CV_CALIB_FIX_K1 )
1636         mask[4] = 0;
1637     if( flags & CV_CALIB_FIX_K2 )
1638         mask[5] = 0;
1639     if( flags & CV_CALIB_FIX_K3 )
1640         mask[8] = 0;
1641     }
1642
1643     // 2. initialize extrinsic parameters
1644     for( i = 0, pos = 0; i < nimages; i++, pos += ni )
1645     {
1646         CvMat _Mi, _mi, _ri, _ti;
1647         ni = npoints->data.i[i*npstep];
1648
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 );
1651
1652         cvGetCols( _M, &_Mi, pos, pos + ni );
1653         cvGetCols( _m, &_mi, pos, pos + ni );
1654
1655         cvFindExtrinsicCameraParams2( &_Mi, &_mi, &_A, &_k, &_ri, &_ti );
1656     }
1657
1658     // 3. run the optimization
1659     for(;;)
1660     {
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;
1666
1667         if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1668         {
1669             param[0] = param[1]*aspectRatio;
1670             pparam[0] = pparam[1]*aspectRatio;
1671         }
1672
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];
1676         k[3] = param[7];
1677         k[4] = param[8];
1678
1679         if( !proceed )
1680             break;
1681
1682         for( i = 0, pos = 0; i < nimages; i++, pos += ni )
1683         {
1684             CvMat _Mi, _mi, _ri, _ti, _dpdr, _dpdt, _dpdf, _dpdc, _dpdk, _mp, _part;
1685             ni = npoints->data.i[i*npstep];
1686
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 );
1689
1690             cvGetCols( _M, &_Mi, pos, pos + ni );
1691             cvGetCols( _m, &_mi, pos, pos + ni );
1692
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 );
1700
1701             if( _JtJ || _JtErr )
1702             {
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);
1707             }
1708             else
1709                 cvProjectPoints2( &_Mi, &_ri, &_ti, &_A, &_k, &_mp );
1710
1711             cvSub( &_mp, &_mi, &_mp );
1712
1713             if( _JtJ || _JtErr )
1714             {
1715                 cvGetSubRect( _JtJ, &_part, cvRect(0,0,NINTRINSIC,NINTRINSIC) );
1716                 cvGEMM( _Ji, _Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
1717
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 );
1720
1721                 cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,0,6,NINTRINSIC) );
1722                 cvGEMM( _Ji, _Je, 1, 0, 0, &_part, CV_GEMM_A_T );
1723
1724                 cvGetRows( _JtErr, &_part, 0, NINTRINSIC );
1725                 cvGEMM( _Ji, _err, 1, &_part, 1, &_part, CV_GEMM_A_T );
1726
1727                 cvGetRows( _JtErr, &_part, NINTRINSIC + i*6, NINTRINSIC + (i+1)*6 );
1728                 cvGEMM( _Je, _err, 1, 0, 0, &_part, CV_GEMM_A_T );
1729             }
1730
1731             if( _errNorm )
1732             {
1733                 double errNorm = cvNorm( &_mp, 0, CV_L2 );
1734                 *_errNorm += errNorm*errNorm;
1735             }
1736         }
1737     }
1738
1739     // 4. store the results
1740     cvConvert( &_A, cameraMatrix );
1741     cvConvert( &_k, distCoeffs );
1742
1743     for( i = 0; i < nimages; i++ )
1744     {
1745         CvMat src, dst;
1746         if( rvecs )
1747         {
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 )
1750             {
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 );
1755             }
1756             else
1757             {
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 );
1762             }
1763         }
1764         if( tvecs )
1765         {
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 );
1771          }
1772     }
1773
1774     __END__;
1775
1776     cvReleaseMat( &_M );
1777     cvReleaseMat( &_m );
1778     cvReleaseMat( &_Ji );
1779     cvReleaseMat( &_Je );
1780     cvReleaseMat( &_err );
1781 }
1782
1783
1784 void cvCalibrationMatrixValues( const CvMat *calibMatr, CvSize imgSize,
1785     double apertureWidth, double apertureHeight, double *fovx, double *fovy,
1786     double *focalLength, CvPoint2D64f *principalPoint, double *pasp )
1787 {
1788     double alphax, alphay, mx, my;
1789     int imgWidth = imgSize.width, imgHeight = imgSize.height;
1790
1791     CV_FUNCNAME("cvCalibrationMatrixValues");
1792     __BEGIN__;
1793
1794     /* Validate parameters. */
1795
1796     if(calibMatr == 0)
1797         CV_ERROR(CV_StsNullPtr, "Some of parameters is a NULL pointer!");
1798
1799     if(!CV_IS_MAT(calibMatr))
1800         CV_ERROR(CV_StsUnsupportedFormat, "Input parameters must be a matrices!");
1801
1802     if(calibMatr->cols != 3 || calibMatr->rows != 3)
1803         CV_ERROR(CV_StsUnmatchedSizes, "Size of matrices must be 3x3!");
1804
1805     alphax = cvmGet(calibMatr, 0, 0);
1806     alphay = cvmGet(calibMatr, 1, 1);
1807     assert(imgWidth != 0 && imgHeight != 0 && alphax != 0.0 && alphay != 0.0);
1808
1809     /* Calculate pixel aspect ratio. */
1810     if(pasp)
1811         *pasp = alphay / alphax;
1812
1813     /* Calculate number of pixel per realworld unit. */
1814
1815     if(apertureWidth != 0.0 && apertureHeight != 0.0) {
1816         mx = imgWidth / apertureWidth;
1817         my = imgHeight / apertureHeight;
1818     } else {
1819         mx = 1.0;
1820         my = *pasp;
1821     }
1822
1823     /* Calculate fovx and fovy. */
1824
1825     if(fovx)
1826         *fovx = 2 * atan(imgWidth / (2 * alphax)) * 180.0 / CV_PI;
1827
1828     if(fovy)
1829         *fovy = 2 * atan(imgHeight / (2 * alphay)) * 180.0 / CV_PI;
1830
1831     /* Calculate focal length. */
1832
1833     if(focalLength)
1834         *focalLength = alphax / mx;
1835
1836     /* Calculate principle point. */
1837
1838     if(principalPoint)
1839         *principalPoint = cvPoint2D64f(cvmGet(calibMatr, 0, 2) / mx, cvmGet(calibMatr, 1, 2) / my);
1840
1841     __END__;
1842 }
1843
1844
1845 //////////////////////////////// Stereo Calibration ///////////////////////////////////
1846
1847 static int dbCmp( const void* _a, const void* _b )
1848 {
1849     double a = *(const double*)_a;
1850     double b = *(const double*)_b;
1851
1852     return (a > b) - (a < b);
1853 }
1854
1855
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 )
1863 {
1864     const int NINTRINSIC = 9;
1865     CvMat* npoints = 0;
1866     CvMat* err = 0;
1867     CvMat* J_LR = 0;
1868     CvMat* Je = 0;
1869     CvMat* Ji = 0;
1870     CvMat* imagePoints[2] = {0,0};
1871     CvMat* objectPoints = 0;
1872     CvMat* RT0 = 0;
1873     CvLevMarq solver;
1874
1875     CV_FUNCNAME( "cvStereoCalibrate" );
1876
1877     __BEGIN__;
1878
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;
1883     int nparams;
1884     bool recomputeIntrinsics = false;
1885     double aspectRatio[2] = {0,0};
1886
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) );
1890
1891     CV_ASSERT( CV_ARE_TYPES_EQ(_imagePoints1, _imagePoints2) &&
1892                CV_ARE_DEPTHS_EQ(_imagePoints1, _objectPoints) );
1893
1894     CV_ASSERT( (_npoints->cols == 1 || _npoints->rows == 1) &&
1895                CV_MAT_TYPE(_npoints->type) == CV_32SC1 );
1896
1897     nimages = _npoints->cols + _npoints->rows - 1;
1898     npoints = cvCreateMat( _npoints->rows, _npoints->cols, _npoints->type );
1899     cvCopy( _npoints, npoints );
1900
1901     for( i = 0, pointsTotal = 0; i < nimages; i++ )
1902     {
1903         maxPoints = MAX(maxPoints, npoints->data.i[i]);
1904         pointsTotal += npoints->data.i[i];
1905     }
1906
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 );
1911
1912     for( k = 0; k < 2; k++ )
1913     {
1914         const CvMat* points = k == 0 ? _imagePoints1 : _imagePoints2;
1915         const CvMat* cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
1916         const CvMat* distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2;
1917
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)) );
1923
1924         K[k] = cvMat(3,3,CV_64F,A[k]);
1925         Dist[k] = cvMat(1,5,CV_64F,dk[k]);
1926
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 );
1930
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] );
1934
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) )
1937         {
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 );
1941         }
1942
1943         if( !(flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS)))
1944         {
1945             cvCalibrateCamera2( objectPoints, imagePoints[k],
1946                 npoints, imageSize, &K[k], &Dist[k], 0, 0, flags );
1947         }
1948     }
1949
1950     if( flags & CV_CALIB_SAME_FOCAL_LENGTH )
1951     {
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;
1955     }
1956
1957     if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1958     {
1959         for( k = 0; k < 2; k++ )
1960             aspectRatio[k] = A[k][0]/A[k][4];
1961     }
1962
1963     recomputeIntrinsics = (flags & CV_CALIB_FIX_INTRINSIC) == 0;
1964
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 );
1969     cvZero( Ji );
1970
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);
1974
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 );
1977
1978     solver.init( nparams, 0, termCrit );
1979     if( recomputeIntrinsics )
1980     {
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;
1996     }
1997
1998     /*
1999        Compute initial estimate of pose
2000
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)]
2007
2008        om = median(om_ref_list)
2009        T = median(T_ref_list)
2010     */
2011     for( i = ofs = 0; i < nimages; ofs += ni, i++ )
2012     {
2013         ni = npoints->data.i[i];
2014         CvMat objpt_i;
2015         double _om[2][3], r[2][9], t[2][3];
2016         CvMat om[2], R[2], T[2], imgpt_i[2];
2017
2018         objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3);
2019         for( k = 0; k < 2; k++ )
2020         {
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]);
2025
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] );
2030             if( k == 0 )
2031             {
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];
2039             }
2040         }
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];
2050     }
2051
2052     // find the medians and save the first 6 parameters
2053     for( i = 0; i < 6; i++ )
2054     {
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;
2058     }
2059
2060     if( recomputeIntrinsics )
2061         for( k = 0; k < 2; k++ )
2062         {
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];
2069         }
2070
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);
2073
2074     for(;;)
2075     {
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;
2091
2092         if( !solver.updateAlt( param, JtJ, JtErr, errNorm ))
2093             break;
2094
2095         cvRodrigues2( &om_LR, &R_LR );
2096         om[1] = cvMat(3,1,CV_64F,_omR);
2097         T[1] = cvMat(3,1,CV_64F,_tR);
2098
2099         if( recomputeIntrinsics )
2100         {
2101             double* iparam = solver.param->data.db + (nimages+1)*6;
2102             double* ipparam = solver.prevParam->data.db + (nimages+1)*6;
2103             dpdf = &dpdf_hdr;
2104             dpdc = &dpdc_hdr;
2105             dpdk = &dpdk_hdr;
2106             if( flags & CV_CALIB_SAME_FOCAL_LENGTH )
2107             {
2108                 iparam[NINTRINSIC] = iparam[0];
2109                 iparam[NINTRINSIC+1] = iparam[1];
2110                 ipparam[NINTRINSIC] = ipparam[0];
2111                 ipparam[NINTRINSIC+1] = ipparam[1];
2112             }
2113             if( flags & CV_CALIB_FIX_ASPECT_RATIO )
2114             {
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];
2119             }
2120             for( k = 0; k < 2; k++ )
2121             {
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];
2131             }
2132         }
2133
2134         for( i = ofs = 0; i < nimages; ofs += ni, i++ )
2135         {
2136             ni = npoints->data.i[i];
2137             CvMat objpt_i, _part;
2138
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);
2141
2142             if( JtJ || JtErr )
2143                 cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1], &dr3dr1, 0,
2144                              &dr3dr2, 0, 0, &dt3dt1, &dt3dr2, &dt3dt2 );
2145             else
2146                 cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1] );
2147
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 );
2151
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 );
2157
2158             for( k = 0; k < 2; k++ )
2159             {
2160                 double maxErr, l2err;
2161                 imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2);
2162
2163                 if( JtJ || JtErr )
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);
2167                 else
2168                     cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k], &tmpimagePoints );
2169                 cvSub( &tmpimagePoints, &imgpt_i[k], &tmpimagePoints );
2170
2171                 l2err = cvNorm( &tmpimagePoints, 0, CV_L2 );
2172                 maxErr = cvNorm( &tmpimagePoints, 0, CV_C );
2173
2174                 if( JtJ || JtErr )
2175                 {
2176                     int iofs = (nimages+1)*6 + k*NINTRINSIC, eofs = (i+1)*6;
2177                     assert( JtJ && JtErr );
2178
2179                     if( k == 1 )
2180                     {
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++ )
2184                         {
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 );
2192
2193                             cvMatMul( &de3dr3, &dr3dr1, &de3dr1 );
2194                             cvMatMul( &de3dt3, &dt3dt1, &de3dt1 );
2195
2196                             cvMatMul( &de3dr3, &dr3dr2, &de3dr2 );
2197                             cvMatMulAdd( &de3dt3, &dt3dr2, &de3dr2, &de3dr2 );
2198
2199                             cvMatMul( &de3dt3, &dt3dt2, &de3dt2 );
2200
2201                             cvCopy( &de3dr1, &de3dr3 );
2202                             cvCopy( &de3dt1, &de3dt3 );
2203                         }
2204
2205                         cvGetSubRect( JtJ, &_part, cvRect(0, 0, 6, 6) );
2206                         cvGEMM( J_LR, J_LR, 1, &_part, 1, &_part, CV_GEMM_A_T );
2207
2208                         cvGetSubRect( JtJ, &_part, cvRect(eofs, 0, 6, 6) );
2209                         cvGEMM( J_LR, Je, 1, 0, 0, &_part, CV_GEMM_A_T );
2210
2211                         cvGetRows( JtErr, &_part, 0, 6 );
2212                         cvGEMM( J_LR, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
2213                     }
2214
2215                     cvGetSubRect( JtJ, &_part, cvRect(eofs, eofs, 6, 6) );
2216                     cvGEMM( Je, Je, 1, &_part, 1, &_part, CV_GEMM_A_T );
2217
2218                     cvGetRows( JtErr, &_part, eofs, eofs + 6 );
2219                     cvGEMM( Je, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
2220
2221                     if( recomputeIntrinsics )
2222                     {
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 );
2227                         if( k == 1 )
2228                         {
2229                             cvGetSubRect( JtJ, &_part, cvRect(iofs, 0, NINTRINSIC, 6) );
2230                             cvGEMM( J_LR, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
2231                         }
2232                         cvGetRows( JtErr, &_part, iofs, iofs + NINTRINSIC );
2233                         cvGEMM( Ji, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
2234                     }
2235                 }
2236
2237                 if( errNorm )
2238                     *errNorm += l2err*l2err;
2239             }
2240         }
2241     }
2242
2243     cvRodrigues2( &om_LR, &R_LR );
2244     if( _R->rows == 1 || _R->cols == 1 )
2245         cvConvert( &om_LR, _R );
2246     else
2247         cvConvert( &R_LR, _R );
2248     cvConvert( &T_LR, _T );
2249
2250     if( recomputeIntrinsics )
2251     {
2252         cvConvert( &K[0], _cameraMatrix1 );
2253         cvConvert( &K[1], _cameraMatrix2 );
2254
2255         for( k = 0; k < 2; k++ )
2256         {
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 );
2261         }
2262     }
2263
2264     if( _E || _F )
2265     {
2266         double* t = T_LR.data.db;
2267         double tx[] =
2268         {
2269             0, -t[2], t[1],
2270             t[2], 0, -t[0],
2271             -t[1], t[0], 0
2272         };
2273         CvMat Tx = cvMat(3, 3, CV_64F, tx);
2274         double e[9], f[9];
2275         CvMat E = cvMat(3, 3, CV_64F, e);
2276         CvMat F = cvMat(3, 3, CV_64F, f);
2277         cvMatMul( &Tx, &R_LR, &E );
2278         if( _E )
2279             cvConvert( &E, _E );
2280         if( _F )
2281         {
2282             double ik[9];
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 );
2289         }
2290     }
2291
2292     __END__;
2293
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] );
2303 }
2304
2305
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 )
2311 {
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;
2324     int i, k;
2325
2326     if( _R->rows == 3 && _R->cols == 3 )
2327         cvRodrigues2(_R, &om);          // get vector rotation
2328     else
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);
2333
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;
2337
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);
2343
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);
2350
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}};
2355
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);
2361                 if( dk1 < 0 ) {
2362                         fc *= 1 + dk1*(nx*nx + ny*ny)/(4*fc*fc);
2363                 }
2364         fc_new = MIN(fc_new, fc);
2365         }
2366
2367     for( k = 0; k < 2; k++ )
2368     {
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);
2375
2376         for( i = 0; i < 4; i++ )
2377         {
2378                         int j = (i<2) ? 0 : 1;
2379             _pts[i].x = (float)((i % 2)*(nx-1));
2380                 _pts[i].y = (float)(j*(ny-1));
2381         }
2382         cvUndistortPoints( &pts, &pts, A, Dk, 0, 0 );
2383         cvConvertPointsHomogeneous( &pts, &pts_3 );
2384
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;
2390                 _a_tmp[0][2]=0.0;
2391                 _a_tmp[1][2]=0.0;
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];
2396     }
2397
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
2401
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 )
2405     {
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;
2408     }
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;
2413
2414     cvZero( &pp );
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;
2418     _pp[2][2] = 1;
2419     cvConvert(&pp, _P1);
2420
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);
2425
2426     if( _Q )
2427     {
2428         double q[] =
2429         {
2430             1, 0, 0, -cc_new[0].x,
2431             0, 1, 0, -cc_new[0].y,
2432             0, 0, 0, fc_new,
2433             0, 0, 1./_t[idx],
2434             (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/_t[idx]
2435         };
2436         CvMat Q = cvMat(4, 4, CV_64F, q);
2437         cvConvert( &Q, _Q );
2438     }
2439 }
2440
2441
2442 CV_IMPL int
2443 cvStereoRectifyUncalibrated(
2444     const CvMat* _points1, const CvMat* _points2,
2445     const CvMat* F0, CvSize imgSize, CvMat* _H1, CvMat* _H2, double threshold )
2446 {
2447     int result = 0;
2448     CvMat* _m1 = 0;
2449     CvMat* _m2 = 0;
2450     CvMat* _lines1 = 0;
2451     CvMat* _lines2 = 0;
2452
2453     CV_FUNCNAME( "cvStereoCalcHomographiesFromF" );
2454
2455     __BEGIN__;
2456
2457     int i, j, npoints;
2458     double cx, cy;
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 );
2468
2469     CvPoint2D64f* m1;
2470     CvPoint2D64f* m2;
2471     CvPoint3D64f* lines1;
2472     CvPoint3D64f* lines2;
2473
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) );
2478
2479     npoints = _points1->rows * _points1->cols * CV_MAT_CN(_points1->type) / 2;
2480
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 );
2485
2486     cvConvert( F0, &F );
2487
2488     cvSVD( (CvMat*)&F, &W, &U, &V, CV_SVD_U_T + CV_SVD_V_T );
2489     W.data.db[8] = 0.;
2490     cvGEMM( &U, &W, 1, 0, 0, &W, CV_GEMM_A_T );
2491     cvMatMul( &W, &V, &F );
2492
2493     cx = cvRound( (imgSize.width-1)*0.5 );
2494     cy = cvRound( (imgSize.height-1)*0.5 );
2495
2496     cvZero( _H1 );
2497     cvZero( _H2 );
2498
2499     cvConvert( _points1, _m1 );
2500     cvConvert( _points2, _m2 );
2501     cvReshape( _m1, _m1, 2, 1 );
2502     cvReshape( _m2, _m2, 2, 1 );
2503
2504     m1 = (CvPoint2D64f*)_m1->data.ptr;
2505     m2 = (CvPoint2D64f*)_m2->data.ptr;
2506     lines1 = (CvPoint3D64f*)_lines1->data.ptr;
2507     lines2 = (CvPoint3D64f*)_lines2->data.ptr;
2508
2509     if( threshold > 0 )
2510     {
2511         cvComputeCorrespondEpilines( _m1, 1, &F, _lines1 );
2512         cvComputeCorrespondEpilines( _m2, 2, &F, _lines2 );
2513
2514         // measure distance from points to the corresponding epilines, mark outliers
2515         for( i = j = 0; i < npoints; i++ )
2516         {
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 )
2523             {
2524                 if( j > i )
2525                 {
2526                     m1[j] = m1[i];
2527                     m2[j] = m2[i];
2528                 }
2529                 j++;
2530             }
2531         }
2532
2533         npoints = j;
2534         if( npoints == 0 )
2535             EXIT;
2536     }
2537
2538     {
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 );
2542
2543     double t[] =
2544     {
2545         1, 0, -cx,
2546         0, 1, -cy,
2547         0, 0, 1
2548     };
2549     CvMat T = cvMat(3, 3, CV_64F, t);
2550     cvMatMul( &T, &E2, &E2 );
2551
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;
2556     double r[] =
2557     {
2558         alpha, beta, 0,
2559         -beta, alpha, 0,
2560         0, 0, 1
2561     };
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];
2566     double k[] =
2567     {
2568         1, 0, 0,
2569         0, 1, 0,
2570         invf, 0, 1
2571     };
2572     CvMat K = cvMat(3, 3, CV_64F, k);
2573     cvMatMul( &K, &T, &H2 );
2574     cvMatMul( &K, &E2, &E2 );
2575
2576     double it[] =
2577     {
2578         1, 0, cx,
2579         0, 1, cy,
2580         0, 0, 1
2581     };
2582     CvMat iT = cvMat( 3, 3, CV_64F, it );
2583     cvMatMul( &iT, &H2, &H2 );
2584
2585     memcpy( E2.data.db, U.data.db + 6, sizeof(e2));
2586     cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 );
2587
2588     double e2_x[] =
2589     {
2590         0, -e2[2], e2[1],
2591        e2[2], 0, -e2[0],
2592        -e2[1], e2[0], 0
2593     };
2594     double e2_111[] =
2595     {
2596         e2[0], e2[0], e2[0],
2597         e2[1], e2[1], e2[1],
2598         e2[2], e2[2], e2[2],
2599     };
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);
2606
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 );
2621
2622     double ha[] =
2623     {
2624         x[0], x[1], x[2],
2625         0, 1, 0,
2626         0, 0, 1
2627     };
2628     CvMat Ha = cvMat(3, 3, CV_64F, ha);
2629     cvMatMul( &Ha, &H0, &H1 );
2630     cvPerspectiveTransform( _m1, _m1, &Ha );
2631
2632     if( mirror )
2633     {
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 );
2638     }
2639
2640     cvConvert( &H1, _H1 );
2641     cvConvert( &H2, _H2 );
2642
2643     result = 1;
2644     }
2645
2646     __END__;
2647
2648     cvReleaseMat( &_m1 );
2649     cvReleaseMat( &_m2 );
2650     cvReleaseMat( &_lines1 );
2651     cvReleaseMat( &_lines2 );
2652
2653     return result;
2654 }
2655
2656
2657 CV_IMPL void
2658 cvReprojectImageTo3D(
2659     const CvArr* disparityImage,
2660     CvArr* _3dImage, const CvMat* _Q,
2661     int handleMissingValues )
2662 {
2663     const double bigZ = 10000.;
2664     CV_FUNCNAME( "cvReprojectImageTo3D" );
2665
2666     __BEGIN__;
2667
2668     double q[4][4];
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;
2677
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) );
2683
2684     cvConvert( _Q, &Q );
2685
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 );
2690
2691     for( y = 0; y < rows; y++ )
2692     {
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];
2697
2698         if( stype == CV_8UC1 )
2699         {
2700             const uchar* sptr0 = (const uchar*)sptr;
2701             for( x = 0; x < cols; x++ )
2702                 sbuf[x] = (float)sptr0[x];
2703             sptr = sbuf;
2704         }
2705         else if( stype == CV_16SC1 )
2706         {
2707             const short* sptr0 = (const short*)sptr;
2708             for( x = 0; x < cols; x++ )
2709                 sbuf[x] = (float)sptr0[x];
2710             sptr = sbuf;
2711         }
2712         else if( stype == CV_32SC1 )
2713         {
2714             const int* sptr0 = (const int*)sptr;
2715             for( x = 0; x < cols; x++ )
2716                 sbuf[x] = (float)sptr0[x];
2717             sptr = sbuf;
2718         }
2719
2720         if( dtype != CV_32FC3 )
2721             dptr = dbuf;
2722
2723         for( x = 0; x < cols; x++, qx += q[0][0], qy += q[1][0], qz += q[2][0], qw += q[3][0] )
2724         {
2725             double d = sptr[x];
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 )
2731                 Z = bigZ;
2732
2733             dptr[x*3] = (float)X;
2734             dptr[x*3+1] = (float)Y;
2735             dptr[x*3+2] = (float)Z;
2736         }
2737
2738         if( dtype == CV_16SC3 )
2739         {
2740             for( x = 0; x < cols*3; x++ )
2741             {
2742                 int ival = cvRound(dptr[x]);
2743                 ((short*)dptr0)[x] = CV_CAST_16S(ival);
2744             }
2745         }
2746         else if( dtype == CV_32SC3 )
2747         {
2748             for( x = 0; x < cols*3; x++ )
2749             {
2750                 int ival = cvRound(dptr[x]);
2751                 ((int*)dptr0)[x] = ival;
2752             }
2753         }
2754     }
2755
2756     __END__;
2757 }
2758
2759
2760 namespace cv
2761 {
2762
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,
2767                                     Mat& npoints )
2768 {
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()));
2772
2773     for( i = 0; i < nimages; i++ )
2774     {
2775         ni = objectPoints[i].size();
2776         CV_Assert(ni == imagePoints[i].size() && (!imgPtMat2 || ni == imagePoints2[i].size()));
2777         total += ni;
2778     }
2779
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;
2784
2785     if( imgPtMat2 )
2786     {
2787         imgPtMat2->create(1, (int)total, DataType<Point2f>::type);
2788         imgPtData2 = imgPtMat2->ptr<Point2f>();
2789     }
2790
2791     Point3f* objPtData = objPtMat.ptr<Point3f>();
2792     Point2f* imgPtData = imgPtMat.ptr<Point2f>();
2793
2794     for( i = 0; i < nimages; i++, j += ni )
2795     {
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);
2800         if( imgPtMat2 )
2801             std::copy(imagePoints2[i].begin(), imagePoints2[i].end(), imgPtData2 + j);
2802     }
2803 }
2804
2805 static Mat prepareCameraMatrix(Mat& cameraMatrix0, int rtype)
2806 {
2807     Mat cameraMatrix = Mat::eye(3, 3, rtype);
2808     if( cameraMatrix0.size() == cameraMatrix.size() )
2809         cameraMatrix0.convertTo(cameraMatrix, rtype);
2810     return cameraMatrix;
2811 }
2812
2813 static Mat prepareDistCoeffs(Mat& distCoeffs0, int rtype)
2814 {
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) )
2820     {
2821         Mat dstCoeffs(distCoeffs, Rect(0, 0, distCoeffs0.cols, distCoeffs0.rows));
2822         distCoeffs0.convertTo(dstCoeffs, rtype);
2823     }
2824     return distCoeffs;
2825 }
2826
2827 }
2828
2829
2830 void cv::Rodrigues(const Mat& src, Mat& dst)
2831 {
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;
2836     if( !ok )
2837         dst = Scalar(0);
2838 }
2839
2840 void cv::Rodrigues(const Mat& src, Mat& dst, Mat& jacobian)
2841 {
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;
2847     if( !ok )
2848         dst = Scalar(0);
2849 }
2850
2851 void cv::matMulDeriv( const Mat& A, const Mat& B, Mat& dABdA, Mat& dABdB )
2852 {
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);
2857 }
2858
2859 void cv::composeRT( const Mat& rvec1, const Mat& tvec1,
2860                     const Mat& rvec2, const Mat& tvec2,
2861                     Mat& rvec3, Mat& tvec3 )
2862 {
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);
2868 }
2869
2870
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 )
2878 {
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);
2886
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);
2894 }
2895
2896
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 )
2902 {
2903     CV_Assert(opoints.isContinuous() && opoints.depth() == CV_32F &&
2904               ((opoints.rows == 1 && opoints.channels() == 3) ||
2905                opoints.cols*opoints.channels() == 3));
2906
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;
2910
2911     cvProjectPoints2( &_objectPoints, &_rvec, &_tvec, &_cameraMatrix, &_distCoeffs,
2912                      &_imagePoints, 0, 0, 0, 0, 0, 0 );
2913 }
2914
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 )
2923 {
2924     CV_Assert(opoints.isContinuous() && opoints.depth() == CV_32F &&
2925               ((opoints.rows == 1 && opoints.channels() == 3) ||
2926                opoints.cols*opoints.channels() == 3));
2927
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;
2938
2939     cvProjectPoints2( &_objectPoints, &_rvec, &_tvec, &_cameraMatrix, &_distCoeffs,
2940                       &_imagePoints, &_dpdrot, &_dpdt, &_dpdf, &_dpdc, &_dpddist, aspectRatio );
2941 }
2942
2943 void cv::solvePnP( const Mat& opoints, const Mat& ipoints,
2944                    const Mat& cameraMatrix, const Mat& distCoeffs,
2945                    Mat& rvec, Mat& tvec, bool useExtrinsicGuess )
2946 {
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));
2953
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 );
2961 }
2962
2963
2964 cv::Mat cv::initCameraMatrix2D( const vector<vector<Point3f> >& objectPoints,
2965                                 const vector<vector<Point2f> >& imagePoints,
2966                                 Size imageSize, double aspectRatio )
2967 {
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;
2975 }
2976
2977
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 )
2982 {
2983     int rtype = CV_64F;
2984     cameraMatrix = prepareCameraMatrix(cameraMatrix, rtype);
2985     distCoeffs = prepareDistCoeffs(distCoeffs, rtype);
2986
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;
2995
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++ )
3001     {
3002         rvecM.row((int)i).copyTo(rvecs[i]);
3003         tvecM.row((int)i).copyTo(tvecs[i]);
3004     }
3005 }
3006
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 )
3011 {
3012     CvMat _cameraMatrix = cameraMatrix;
3013     cvCalibrationMatrixValues( &_cameraMatrix, imageSize, apertureWidth, apertureHeight,
3014         &fovx, &fovy, &focalLength, (CvPoint2D64f*)&principalPoint, &aspectRatio );
3015 }
3016
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,
3024                           int flags )
3025 {
3026     int rtype = CV_64F;
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);
3033
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;
3041
3042     cvStereoCalibrate(&_objPt, &_imgPt, &_imgPt2, &_npoints, &_cameraMatrix1,
3043         &_cameraMatrix2, &_distCoeffs1, &_distCoeffs2, imageSize,
3044         &_R, &_T, &_E, &_F, criteria, flags );
3045 }
3046
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,
3051                         int flags )
3052 {
3053     int rtype = CV_64F;
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 );
3064 }
3065
3066 bool cv::stereoRectifyUncalibrated( const Mat& points1, const Mat& points2,
3067                                     const Mat& F, Size imgSize,
3068                                     Mat& H1, Mat& H2, double threshold )
3069 {
3070     int rtype = CV_64F;
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) )
3075         pF = &(_F = F);
3076     return cvStereoRectifyUncalibrated(&_pt1, &_pt2, pF, imgSize, &_H1, &_H2, threshold) > 0;
3077 }
3078
3079 void cv::reprojectImageTo3D( const Mat& disparity,
3080                              Mat& _3dImage, const Mat& Q,
3081                              bool handleMissingValues )
3082 {
3083     _3dImage.create(disparity.size(), CV_32FC3);
3084     CvMat _disparity = disparity, __3dImage = _3dImage, _Q = Q;
3085     cvReprojectImageTo3D( &_disparity, &__3dImage, &_Q, handleMissingValues );
3086 }
3087
3088 /* End of file. */