modules/calib3d/src/calibration.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 "precomp.hpp"
  44 #include <stdio.h>
  45 #include <iterator>
  46
  47 /*
  48     This is stright-forward port v3 of Matlab calibration engine by Jean-Yves Bouguet
  49     that is (in a large extent) based on the paper:
  50     Z. Zhang. "A flexible new technique for camera calibration".
  51     IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000.
  52
  53     The 1st initial port was done by Valery Mosyagin.
  54 */
  55
  56 using namespace cv;
  57
  58 CvLevMarq::CvLevMarq()
  59 {
  60     mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = Ptr<CvMat>();
  61     lambdaLg10 = 0; state = DONE;
  62     criteria = cvTermCriteria(0,0,0);
  63     iters = 0;
  64     completeSymmFlag = false;
  65 }
  66
  67 CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
  68 {
  69     mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = Ptr<CvMat>();
  70     init(nparams, nerrs, criteria0, _completeSymmFlag);
  71 }
  72
  73 void CvLevMarq::clear()
  74 {
  75     mask.release();
  76     prevParam.release();
  77     param.release();
  78     J.release();
  79     err.release();
  80     JtJ.release();
  81     JtJN.release();
  82     JtErr.release();
  83     JtJV.release();
  84     JtJW.release();
  85 }
  86
  87 CvLevMarq::~CvLevMarq()
  88 {
  89     clear();
  90 }
  91
  92 void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
  93 {
  94     if( !param || param->rows != nparams || nerrs != (err ? err->rows : 0) )
  95         clear();
  96     mask = cvCreateMat( nparams, 1, CV_8U );
  97     cvSet(mask, cvScalarAll(1));
  98     prevParam = cvCreateMat( nparams, 1, CV_64F );
  99     param = cvCreateMat( nparams, 1, CV_64F );
 100     JtJ = cvCreateMat( nparams, nparams, CV_64F );
 101     JtJN = cvCreateMat( nparams, nparams, CV_64F );
 102     JtJV = cvCreateMat( nparams, nparams, CV_64F );
 103     JtJW = cvCreateMat( nparams, 1, CV_64F );
 104     JtErr = cvCreateMat( nparams, 1, CV_64F );
 105     if( nerrs > 0 )
 106     {
 107         J = cvCreateMat( nerrs, nparams, CV_64F );
 108         err = cvCreateMat( nerrs, 1, CV_64F );
 109     }
 110     prevErrNorm = DBL_MAX;
 111     lambdaLg10 = -3;
 112     criteria = criteria0;
 113     if( criteria.type & CV_TERMCRIT_ITER )
 114         criteria.max_iter = MIN(MAX(criteria.max_iter,1),1000);
 115     else
 116         criteria.max_iter = 30;
 117     if( criteria.type & CV_TERMCRIT_EPS )
 118         criteria.epsilon = MAX(criteria.epsilon, 0);
 119     else
 120         criteria.epsilon = DBL_EPSILON;
 121     state = STARTED;
 122     iters = 0;
 123     completeSymmFlag = _completeSymmFlag;
 124 }
 125
 126 bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err )
 127 {
 128     double change;
 129
 130     matJ = _err = 0;
 131
 132     assert( !err.empty() );
 133     if( state == DONE )
 134     {
 135         _param = param;
 136         return false;
 137     }
 138
 139     if( state == STARTED )
 140     {
 141         _param = param;
 142         cvZero( J );
 143         cvZero( err );
 144         matJ = J;
 145         _err = err;
 146         state = CALC_J;
 147         return true;
 148     }
 149
 150     if( state == CALC_J )
 151     {
 152         cvMulTransposed( J, JtJ, 1 );
 153         cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );
 154         cvCopy( param, prevParam );
 155         step();
 156         if( iters == 0 )
 157             prevErrNorm = cvNorm(err, 0, CV_L2);
 158         _param = param;
 159         cvZero( err );
 160         _err = err;
 161         state = CHECK_ERR;
 162         return true;
 163     }
 164
 165     assert( state == CHECK_ERR );
 166     errNorm = cvNorm( err, 0, CV_L2 );
 167     if( errNorm > prevErrNorm )
 168     {
 169         lambdaLg10++;
 170         step();
 171         _param = param;
 172         cvZero( err );
 173         _err = err;
 174         state = CHECK_ERR;
 175         return true;
 176     }
 177
 178     lambdaLg10 = MAX(lambdaLg10-1, -16);
 179     if( ++iters >= criteria.max_iter ||
 180         (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
 181     {
 182         _param = param;
 183         state = DONE;
 184         return true;
 185     }
 186
 187     prevErrNorm = errNorm;
 188     _param = param;
 189     cvZero(J);
 190     matJ = J;
 191     _err = err;
 192     state = CALC_J;
 193     return true;
 194 }
 195
 196
 197 bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm )
 198 {
 199     double change;
 200
 201     CV_Assert( err.empty() );
 202     if( state == DONE )
 203     {
 204         _param = param;
 205         return false;
 206     }
 207
 208     if( state == STARTED )
 209     {
 210         _param = param;
 211         cvZero( JtJ );
 212         cvZero( JtErr );
 213         errNorm = 0;
 214         _JtJ = JtJ;
 215         _JtErr = JtErr;
 216         _errNorm = &errNorm;
 217         state = CALC_J;
 218         return true;
 219     }
 220
 221     if( state == CALC_J )
 222     {
 223         cvCopy( param, prevParam );
 224         step();
 225         _param = param;
 226         prevErrNorm = errNorm;
 227         errNorm = 0;
 228         _errNorm = &errNorm;
 229         state = CHECK_ERR;
 230         return true;
 231     }
 232
 233     assert( state == CHECK_ERR );
 234     if( errNorm > prevErrNorm )
 235     {
 236         lambdaLg10++;
 237         step();
 238         _param = param;
 239         errNorm = 0;
 240         _errNorm = &errNorm;
 241         state = CHECK_ERR;
 242         return true;
 243     }
 244
 245     lambdaLg10 = MAX(lambdaLg10-1, -16);
 246     if( ++iters >= criteria.max_iter ||
 247         (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon )
 248     {
 249         _param = param;
 250         state = DONE;
 251         return false;
 252     }
 253
 254     prevErrNorm = errNorm;
 255     cvZero( JtJ );
 256     cvZero( JtErr );
 257     _param = param;
 258     _JtJ = JtJ;
 259     _JtErr = JtErr;
 260     state = CALC_J;
 261     return true;
 262 }
 263
 264 void CvLevMarq::step()
 265 {
 266     const double LOG10 = log(10.);
 267     double lambda = exp(lambdaLg10*LOG10);
 268     int i, j, nparams = param->rows;
 269
 270     for( i = 0; i < nparams; i++ )
 271         if( mask->data.ptr[i] == 0 )
 272         {
 273             double *row = JtJ->data.db + i*nparams, *col = JtJ->data.db + i;
 274             for( j = 0; j < nparams; j++ )
 275                 row[j] = col[j*nparams] = 0;
 276             JtErr->data.db[i] = 0;
 277         }
 278
 279     if( !err )
 280         cvCompleteSymm( JtJ, completeSymmFlag );
 281 #if 1
 282     cvCopy( JtJ, JtJN );
 283     for( i = 0; i < nparams; i++ )
 284         JtJN->data.db[(nparams+1)*i] *= 1. + lambda;
 285 #else
 286     cvSetIdentity(JtJN, cvRealScalar(lambda));
 287     cvAdd( JtJ, JtJN, JtJN );
 288 #endif
 289     cvSVD( JtJN, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
 290     cvSVBkSb( JtJW, JtJV, JtJV, JtErr, param, CV_SVD_U_T + CV_SVD_V_T );
 291     for( i = 0; i < nparams; i++ )
 292         param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? param->data.db[i] : 0);
 293 }
 294
 295 // reimplementation of dAB.m
 296 CV_IMPL void cvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB )
 297 {
 298     int i, j, M, N, L;
 299     int bstep;
 300
 301     CV_Assert( CV_IS_MAT(A) && CV_IS_MAT(B) );
 302     CV_Assert( CV_ARE_TYPES_EQ(A, B) &&
 303         (CV_MAT_TYPE(A->type) == CV_32F || CV_MAT_TYPE(A->type) == CV_64F) );
 304     CV_Assert( A->cols == B->rows );
 305
 306     M = A->rows;
 307     L = A->cols;
 308     N = B->cols;
 309     bstep = B->step/CV_ELEM_SIZE(B->type);
 310
 311     if( dABdA )
 312     {
 313         CV_Assert( CV_ARE_TYPES_EQ(A, dABdA) &&
 314             dABdA->rows == A->rows*B->cols && dABdA->cols == A->rows*A->cols );
 315     }
 316
 317     if( dABdB )
 318     {
 319         CV_Assert( CV_ARE_TYPES_EQ(A, dABdB) &&
 320             dABdB->rows == A->rows*B->cols && dABdB->cols == B->rows*B->cols );
 321     }
 322
 323     if( CV_MAT_TYPE(A->type) == CV_32F )
 324     {
 325         for( i = 0; i < M*N; i++ )
 326         {
 327             int i1 = i / N,  i2 = i % N;
 328
 329             if( dABdA )
 330             {
 331                 float* dcda = (float*)(dABdA->data.ptr + dABdA->step*i);
 332                 const float* b = (const float*)B->data.ptr + i2;
 333
 334                 for( j = 0; j < M*L; j++ )
 335                     dcda[j] = 0;
 336                 for( j = 0; j < L; j++ )
 337                     dcda[i1*L + j] = b[j*bstep];
 338             }
 339
 340             if( dABdB )
 341             {
 342                 float* dcdb = (float*)(dABdB->data.ptr + dABdB->step*i);
 343                 const float* a = (const float*)(A->data.ptr + A->step*i1);
 344
 345                 for( j = 0; j < L*N; j++ )
 346                     dcdb[j] = 0;
 347                 for( j = 0; j < L; j++ )
 348                     dcdb[j*N + i2] = a[j];
 349             }
 350         }
 351     }
 352     else
 353     {
 354         for( i = 0; i < M*N; i++ )
 355         {
 356             int i1 = i / N,  i2 = i % N;
 357
 358             if( dABdA )
 359             {
 360                 double* dcda = (double*)(dABdA->data.ptr + dABdA->step*i);
 361                 const double* b = (const double*)B->data.ptr + i2;
 362
 363                 for( j = 0; j < M*L; j++ )
 364                     dcda[j] = 0;
 365                 for( j = 0; j < L; j++ )
 366                     dcda[i1*L + j] = b[j*bstep];
 367             }
 368
 369             if( dABdB )
 370             {
 371                 double* dcdb = (double*)(dABdB->data.ptr + dABdB->step*i);
 372                 const double* a = (const double*)(A->data.ptr + A->step*i1);
 373
 374                 for( j = 0; j < L*N; j++ )
 375                     dcdb[j] = 0;
 376                 for( j = 0; j < L; j++ )
 377                     dcdb[j*N + i2] = a[j];
 378             }
 379         }
 380     }
 381 }
 382
 383 // reimplementation of compose_motion.m
 384 CV_IMPL void cvComposeRT( const CvMat* _rvec1, const CvMat* _tvec1,
 385              const CvMat* _rvec2, const CvMat* _tvec2,
 386              CvMat* _rvec3, CvMat* _tvec3,
 387              CvMat* dr3dr1, CvMat* dr3dt1,
 388              CvMat* dr3dr2, CvMat* dr3dt2,
 389              CvMat* dt3dr1, CvMat* dt3dt1,
 390              CvMat* dt3dr2, CvMat* dt3dt2 )
 391 {
 392     double _r1[3], _r2[3];
 393     double _R1[9], _d1[9*3], _R2[9], _d2[9*3];
 394     CvMat r1 = cvMat(3,1,CV_64F,_r1), r2 = cvMat(3,1,CV_64F,_r2);
 395     CvMat R1 = cvMat(3,3,CV_64F,_R1), R2 = cvMat(3,3,CV_64F,_R2);
 396     CvMat dR1dr1 = cvMat(9,3,CV_64F,_d1), dR2dr2 = cvMat(9,3,CV_64F,_d2);
 397
 398     CV_Assert( CV_IS_MAT(_rvec1) && CV_IS_MAT(_rvec2) );
 399
 400     CV_Assert( CV_MAT_TYPE(_rvec1->type) == CV_32F ||
 401                CV_MAT_TYPE(_rvec1->type) == CV_64F );
 402
 403     CV_Assert( _rvec1->rows == 3 && _rvec1->cols == 1 && CV_ARE_SIZES_EQ(_rvec1, _rvec2) );
 404
 405     cvConvert( _rvec1, &r1 );
 406     cvConvert( _rvec2, &r2 );
 407
 408     cvRodrigues2( &r1, &R1, &dR1dr1 );
 409     cvRodrigues2( &r2, &R2, &dR2dr2 );
 410
 411     if( _rvec3 || dr3dr1 || dr3dr1 )
 412     {
 413         double _r3[3], _R3[9], _dR3dR1[9*9], _dR3dR2[9*9], _dr3dR3[9*3];
 414         double _W1[9*3], _W2[3*3];
 415         CvMat r3 = cvMat(3,1,CV_64F,_r3), R3 = cvMat(3,3,CV_64F,_R3);
 416         CvMat dR3dR1 = cvMat(9,9,CV_64F,_dR3dR1), dR3dR2 = cvMat(9,9,CV_64F,_dR3dR2);
 417         CvMat dr3dR3 = cvMat(3,9,CV_64F,_dr3dR3);
 418         CvMat W1 = cvMat(3,9,CV_64F,_W1), W2 = cvMat(3,3,CV_64F,_W2);
 419
 420         cvMatMul( &R2, &R1, &R3 );
 421         cvCalcMatMulDeriv( &R2, &R1, &dR3dR2, &dR3dR1 );
 422
 423         cvRodrigues2( &R3, &r3, &dr3dR3 );
 424
 425         if( _rvec3 )
 426             cvConvert( &r3, _rvec3 );
 427
 428         if( dr3dr1 )
 429         {
 430             cvMatMul( &dr3dR3, &dR3dR1, &W1 );
 431             cvMatMul( &W1, &dR1dr1, &W2 );
 432             cvConvert( &W2, dr3dr1 );
 433         }
 434
 435         if( dr3dr2 )
 436         {
 437             cvMatMul( &dr3dR3, &dR3dR2, &W1 );
 438             cvMatMul( &W1, &dR2dr2, &W2 );
 439             cvConvert( &W2, dr3dr2 );
 440         }
 441     }
 442
 443     if( dr3dt1 )
 444         cvZero( dr3dt1 );
 445     if( dr3dt2 )
 446         cvZero( dr3dt2 );
 447
 448     if( _tvec3 || dt3dr2 || dt3dt1 )
 449     {
 450         double _t1[3], _t2[3], _t3[3], _dxdR2[3*9], _dxdt1[3*3], _W3[3*3];
 451         CvMat t1 = cvMat(3,1,CV_64F,_t1), t2 = cvMat(3,1,CV_64F,_t2);
 452         CvMat t3 = cvMat(3,1,CV_64F,_t3);
 453         CvMat dxdR2 = cvMat(3, 9, CV_64F, _dxdR2);
 454         CvMat dxdt1 = cvMat(3, 3, CV_64F, _dxdt1);
 455         CvMat W3 = cvMat(3, 3, CV_64F, _W3);
 456
 457         CV_Assert( CV_IS_MAT(_tvec1) && CV_IS_MAT(_tvec2) );
 458         CV_Assert( CV_ARE_SIZES_EQ(_tvec1, _tvec2) && CV_ARE_SIZES_EQ(_tvec1, _rvec1) );
 459
 460         cvConvert( _tvec1, &t1 );
 461         cvConvert( _tvec2, &t2 );
 462         cvMatMulAdd( &R2, &t1, &t2, &t3 );
 463
 464         if( _tvec3 )
 465             cvConvert( &t3, _tvec3 );
 466
 467         if( dt3dr2 || dt3dt1 )
 468         {
 469             cvCalcMatMulDeriv( &R2, &t1, &dxdR2, &dxdt1 );
 470             if( dt3dr2 )
 471             {
 472                 cvMatMul( &dxdR2, &dR2dr2, &W3 );
 473                 cvConvert( &W3, dt3dr2 );
 474             }
 475             if( dt3dt1 )
 476                 cvConvert( &dxdt1, dt3dt1 );
 477         }
 478     }
 479
 480     if( dt3dt2 )
 481         cvSetIdentity( dt3dt2 );
 482     if( dt3dr1 )
 483         cvZero( dt3dr1 );
 484 }
 485
 486 CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
 487 {
 488     int depth, elem_size;
 489     int i, k;
 490     double J[27];
 491     CvMat matJ = cvMat( 3, 9, CV_64F, J );
 492
 493     if( !CV_IS_MAT(src) )
 494         CV_Error( !src ? CV_StsNullPtr : CV_StsBadArg, "Input argument is not a valid matrix" );
 495
 496     if( !CV_IS_MAT(dst) )
 497         CV_Error( !dst ? CV_StsNullPtr : CV_StsBadArg,
 498         "The first output argument is not a valid matrix" );
 499
 500     depth = CV_MAT_DEPTH(src->type);
 501     elem_size = CV_ELEM_SIZE(depth);
 502
 503     if( depth != CV_32F && depth != CV_64F )
 504         CV_Error( CV_StsUnsupportedFormat, "The matrices must have 32f or 64f data type" );
 505
 506     if( !CV_ARE_DEPTHS_EQ(src, dst) )
 507         CV_Error( CV_StsUnmatchedFormats, "All the matrices must have the same data type" );
 508
 509     if( jacobian )
 510     {
 511         if( !CV_IS_MAT(jacobian) )
 512             CV_Error( CV_StsBadArg, "Jacobian is not a valid matrix" );
 513
 514         if( !CV_ARE_DEPTHS_EQ(src, jacobian) || CV_MAT_CN(jacobian->type) != 1 )
 515             CV_Error( CV_StsUnmatchedFormats, "Jacobian must have 32fC1 or 64fC1 datatype" );
 516
 517         if( (jacobian->rows != 9 || jacobian->cols != 3) &&
 518             (jacobian->rows != 3 || jacobian->cols != 9))
 519             CV_Error( CV_StsBadSize, "Jacobian must be 3x9 or 9x3" );
 520     }
 521
 522     if( src->cols == 1 || src->rows == 1 )
 523     {
 524         double rx, ry, rz, theta;
 525         int step = src->rows > 1 ? src->step / elem_size : 1;
 526
 527         if( src->rows + src->cols*CV_MAT_CN(src->type) - 1 != 3 )
 528             CV_Error( CV_StsBadSize, "Input matrix must be 1x3, 3x1 or 3x3" );
 529
 530         if( dst->rows != 3 || dst->cols != 3 || CV_MAT_CN(dst->type) != 1 )
 531             CV_Error( CV_StsBadSize, "Output matrix must be 3x3, single-channel floating point matrix" );
 532
 533         if( depth == CV_32F )
 534         {
 535             rx = src->data.fl[0];
 536             ry = src->data.fl[step];
 537             rz = src->data.fl[step*2];
 538         }
 539         else
 540         {
 541             rx = src->data.db[0];
 542             ry = src->data.db[step];
 543             rz = src->data.db[step*2];
 544         }
 545         theta = sqrt(rx*rx + ry*ry + rz*rz);
 546
 547         if( theta < DBL_EPSILON )
 548         {
 549             cvSetIdentity( dst );
 550
 551             if( jacobian )
 552             {
 553                 memset( J, 0, sizeof(J) );
 554                 J[5] = J[15] = J[19] = -1;
 555                 J[7] = J[11] = J[21] = 1;
 556             }
 557         }
 558         else
 559         {
 560             const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
 561
 562             double c = cos(theta);
 563             double s = sin(theta);
 564             double c1 = 1. - c;
 565             double itheta = theta ? 1./theta : 0.;
 566
 567             rx *= itheta; ry *= itheta; rz *= itheta;
 568
 569             double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz };
 570             double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 };
 571             double R[9];
 572             CvMat matR = cvMat( 3, 3, CV_64F, R );
 573
 574             // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
 575             // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0]
 576             for( k = 0; k < 9; k++ )
 577                 R[k] = c*I[k] + c1*rrt[k] + s*_r_x_[k];
 578
 579             cvConvert( &matR, dst );
 580
 581             if( jacobian )
 582             {
 583                 double drrt[] = { rx+rx, ry, rz, ry, 0, 0, rz, 0, 0,
 584                                   0, rx, 0, rx, ry+ry, rz, 0, rz, 0,
 585                                   0, 0, rx, 0, 0, ry, rx, ry, rz+rz };
 586                 double d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0,
 587                                     0, 0, 1, 0, 0, 0, -1, 0, 0,
 588                                     0, -1, 0, 1, 0, 0, 0, 0, 0 };
 589                 for( i = 0; i < 3; i++ )
 590                 {
 591                     double ri = i == 0 ? rx : i == 1 ? ry : rz;
 592                     double a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta;
 593                     double a3 = (c - s*itheta)*ri, a4 = s*itheta;
 594                     for( k = 0; k < 9; k++ )
 595                         J[i*9+k] = a0*I[k] + a1*rrt[k] + a2*drrt[i*9+k] +
 596                                    a3*_r_x_[k] + a4*d_r_x_[i*9+k];
 597                 }
 598             }
 599         }
 600     }
 601     else if( src->cols == 3 && src->rows == 3 )
 602     {
 603         double R[9], U[9], V[9], W[3], rx, ry, rz;
 604         CvMat matR = cvMat( 3, 3, CV_64F, R );
 605         CvMat matU = cvMat( 3, 3, CV_64F, U );
 606         CvMat matV = cvMat( 3, 3, CV_64F, V );
 607         CvMat matW = cvMat( 3, 1, CV_64F, W );
 608         double theta, s, c;
 609         int step = dst->rows > 1 ? dst->step / elem_size : 1;
 610
 611         if( (dst->rows != 1 || dst->cols*CV_MAT_CN(dst->type) != 3) &&
 612             (dst->rows != 3 || dst->cols != 1 || CV_MAT_CN(dst->type) != 1))
 613             CV_Error( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" );
 614
 615         cvConvert( src, &matR );
 616         if( !cvCheckArr( &matR, CV_CHECK_RANGE+CV_CHECK_QUIET, -100, 100 ) )
 617         {
 618             cvZero(dst);
 619             if( jacobian )
 620                 cvZero(jacobian);
 621             return 0;
 622         }
 623
 624         cvSVD( &matR, &matW, &matU, &matV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
 625         cvGEMM( &matU, &matV, 1, 0, 0, &matR, CV_GEMM_A_T );
 626
 627         rx = R[7] - R[5];
 628         ry = R[2] - R[6];
 629         rz = R[3] - R[1];
 630
 631         s = sqrt((rx*rx + ry*ry + rz*rz)*0.25);
 632         c = (R[0] + R[4] + R[8] - 1)*0.5;
 633         c = c > 1. ? 1. : c < -1. ? -1. : c;
 634         theta = acos(c);
 635
 636         if( s < 1e-5 )
 637         {
 638             double t;
 639
 640             if( c > 0 )
 641                 rx = ry = rz = 0;
 642             else
 643             {
 644                 t = (R[0] + 1)*0.5;
 645                 rx = sqrt(MAX(t,0.));
 646                 t = (R[4] + 1)*0.5;
 647                 ry = sqrt(MAX(t,0.))*(R[1] < 0 ? -1. : 1.);
 648                 t = (R[8] + 1)*0.5;
 649                 rz = sqrt(MAX(t,0.))*(R[2] < 0 ? -1. : 1.);
 650                 if( fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R[5] > 0) != (ry*rz > 0) )
 651                     rz = -rz;
 652                 theta /= sqrt(rx*rx + ry*ry + rz*rz);
 653                 rx *= theta;
 654                 ry *= theta;
 655                 rz *= theta;
 656             }
 657
 658             if( jacobian )
 659             {
 660                 memset( J, 0, sizeof(J) );
 661                 if( c > 0 )
 662                 {
 663                     J[5] = J[15] = J[19] = -0.5;
 664                     J[7] = J[11] = J[21] = 0.5;
 665                 }
 666             }
 667         }
 668         else
 669         {
 670             double vth = 1/(2*s);
 671
 672             if( jacobian )
 673             {
 674                 double t, dtheta_dtr = -1./s;
 675                 // var1 = [vth;theta]
 676                 // var = [om1;var1] = [om1;vth;theta]
 677                 double dvth_dtheta = -vth*c/s;
 678                 double d1 = 0.5*dvth_dtheta*dtheta_dtr;
 679                 double d2 = 0.5*dtheta_dtr;
 680                 // dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR
 681                 double dvardR[5*9] =
 682                 {
 683                     0, 0, 0, 0, 0, 1, 0, -1, 0,
 684                     0, 0, -1, 0, 0, 0, 1, 0, 0,
 685                     0, 1, 0, -1, 0, 0, 0, 0, 0,
 686                     d1, 0, 0, 0, d1, 0, 0, 0, d1,
 687                     d2, 0, 0, 0, d2, 0, 0, 0, d2
 688                 };
 689                 // var2 = [om;theta]
 690                 double dvar2dvar[] =
 691                 {
 692                     vth, 0, 0, rx, 0,
 693                     0, vth, 0, ry, 0,
 694                     0, 0, vth, rz, 0,
 695                     0, 0, 0, 0, 1
 696                 };
 697                 double domegadvar2[] =
 698                 {
 699                     theta, 0, 0, rx*vth,
 700                     0, theta, 0, ry*vth,
 701                     0, 0, theta, rz*vth
 702                 };
 703
 704                 CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR );
 705                 CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar );
 706                 CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 );
 707                 double t0[3*5];
 708                 CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 );
 709
 710                 cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 );
 711                 cvMatMul( &_t0, &_dvardR, &matJ );
 712
 713                 // transpose every row of matJ (treat the rows as 3x3 matrices)
 714                 CV_SWAP(J[1], J[3], t); CV_SWAP(J[2], J[6], t); CV_SWAP(J[5], J[7], t);
 715                 CV_SWAP(J[10], J[12], t); CV_SWAP(J[11], J[15], t); CV_SWAP(J[14], J[16], t);
 716                 CV_SWAP(J[19], J[21], t); CV_SWAP(J[20], J[24], t); CV_SWAP(J[23], J[25], t);
 717             }
 718
 719             vth *= theta;
 720             rx *= vth; ry *= vth; rz *= vth;
 721         }
 722
 723         if( depth == CV_32F )
 724         {
 725             dst->data.fl[0] = (float)rx;
 726             dst->data.fl[step] = (float)ry;
 727             dst->data.fl[step*2] = (float)rz;
 728         }
 729         else
 730         {
 731             dst->data.db[0] = rx;
 732             dst->data.db[step] = ry;
 733             dst->data.db[step*2] = rz;
 734         }
 735     }
 736
 737     if( jacobian )
 738     {
 739         if( depth == CV_32F )
 740         {
 741             if( jacobian->rows == matJ.rows )
 742                 cvConvert( &matJ, jacobian );
 743             else
 744             {
 745                 float Jf[3*9];
 746                 CvMat _Jf = cvMat( matJ.rows, matJ.cols, CV_32FC1, Jf );
 747                 cvConvert( &matJ, &_Jf );
 748                 cvTranspose( &_Jf, jacobian );
 749             }
 750         }
 751         else if( jacobian->rows == matJ.rows )
 752             cvCopy( &matJ, jacobian );
 753         else
 754             cvTranspose( &matJ, jacobian );
 755     }
 756
 757     return 1;
 758 }
 759
 760
 761 static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8 or 8x1 floating-point vector";
 762
 763 CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
 764                   const CvMat* r_vec,
 765                   const CvMat* t_vec,
 766                   const CvMat* A,
 767                   const CvMat* distCoeffs,
 768                   CvMat* imagePoints, CvMat* dpdr,
 769                   CvMat* dpdt, CvMat* dpdf,
 770                   CvMat* dpdc, CvMat* dpdk,
 771                   double aspectRatio )
 772 {
 773     Ptr<CvMat> matM, _m;
 774     Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
 775
 776     int i, j, count;
 777     int calc_derivatives;
 778     const CvPoint3D64f* M;
 779     CvPoint2D64f* m;
 780     double r[3], R[9], dRdr[27], t[3], a[9], k[8] = {0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
 781     CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
 782     CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
 783     double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
 784     int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
 785     bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
 786
 787     if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||
 788         !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
 789         /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) )
 790         CV_Error( CV_StsBadArg, "One of required arguments is not a valid matrix" );
 791
 792     int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type);
 793     if(total % 3 != 0)
 794     {
 795         //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data
 796         CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
 797     }
 798     count = total / 3;
 799
 800     if( CV_IS_CONT_MAT(objectPoints->type) &&
 801         (CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&&
 802         ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) ||
 803         (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3)))
 804     {
 805         matM = cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) );
 806         cvConvert(objectPoints, matM);
 807     }
 808     else
 809     {
 810 //        matM = cvCreateMat( 1, count, CV_64FC3 );
 811 //        cvConvertPointsHomogeneous( objectPoints, matM );
 812         CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
 813     }
 814
 815     if( CV_IS_CONT_MAT(imagePoints->type) &&
 816         (CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) &&
 817         ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 2) ||
 818         (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 2)))
 819     {
 820         _m = cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) );
 821         cvConvert(imagePoints, _m);
 822     }
 823     else
 824     {
 825 //        _m = cvCreateMat( 1, count, CV_64FC2 );
 826         CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
 827     }
 828
 829     M = (CvPoint3D64f*)matM->data.db;
 830     m = (CvPoint2D64f*)_m->data.db;
 831
 832     if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) ||
 833         (((r_vec->rows != 1 && r_vec->cols != 1) ||
 834         r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
 835         ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1)))
 836         CV_Error( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
 837                   "floating-point rotation vector, or 3x3 rotation matrix" );
 838
 839     if( r_vec->rows == 3 && r_vec->cols == 3 )
 840     {
 841         _r = cvMat( 3, 1, CV_64FC1, r );
 842         cvRodrigues2( r_vec, &_r );
 843         cvRodrigues2( &_r, &matR, &_dRdr );
 844         cvCopy( r_vec, &matR );
 845     }
 846     else
 847     {
 848         _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
 849         cvConvert( r_vec, &_r );
 850         cvRodrigues2( &_r, &matR, &_dRdr );
 851     }
 852
 853     if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) ||
 854         (t_vec->rows != 1 && t_vec->cols != 1) ||
 855         t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
 856         CV_Error( CV_StsBadArg,
 857             "Translation vector must be 1x3 or 3x1 floating-point vector" );
 858
 859     _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
 860     cvConvert( t_vec, &_t );
 861
 862     if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) ||
 863         A->rows != 3 || A->cols != 3 )
 864         CV_Error( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" );
 865
 866     cvConvert( A, &_a );
 867     fx = a[0]; fy = a[4];
 868     cx = a[2]; cy = a[5];
 869
 870     if( fixedAspectRatio )
 871         fx = fy*aspectRatio;
 872
 873     if( distCoeffs )
 874     {
 875         if( !CV_IS_MAT(distCoeffs) ||
 876             (CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
 877             CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
 878             (distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
 879             (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
 880             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 &&
 881             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8) )
 882             CV_Error( CV_StsBadArg, cvDistCoeffErr );
 883
 884         _k = cvMat( distCoeffs->rows, distCoeffs->cols,
 885                     CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
 886         cvConvert( distCoeffs, &_k );
 887     }
 888
 889     if( dpdr )
 890     {
 891         if( !CV_IS_MAT(dpdr) ||
 892             (CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
 893             CV_MAT_TYPE(dpdr->type) != CV_64FC1) ||
 894             dpdr->rows != count*2 || dpdr->cols != 3 )
 895             CV_Error( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
 896
 897         if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
 898         {
 899             _dpdr = cvCloneMat(dpdr);
 900         }
 901         else
 902             _dpdr = cvCreateMat( 2*count, 3, CV_64FC1 );
 903         dpdr_p = _dpdr->data.db;
 904         dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
 905     }
 906
 907     if( dpdt )
 908     {
 909         if( !CV_IS_MAT(dpdt) ||
 910             (CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
 911             CV_MAT_TYPE(dpdt->type) != CV_64FC1) ||
 912             dpdt->rows != count*2 || dpdt->cols != 3 )
 913             CV_Error( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
 914
 915         if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
 916         {
 917             _dpdt = cvCloneMat(dpdt);
 918         }
 919         else
 920             _dpdt = cvCreateMat( 2*count, 3, CV_64FC1 );
 921         dpdt_p = _dpdt->data.db;
 922         dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
 923     }
 924
 925     if( dpdf )
 926     {
 927         if( !CV_IS_MAT(dpdf) ||
 928             (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) ||
 929             dpdf->rows != count*2 || dpdf->cols != 2 )
 930             CV_Error( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
 931
 932         if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
 933         {
 934             _dpdf = cvCloneMat(dpdf);
 935         }
 936         else
 937             _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         {
 951             _dpdc = cvCloneMat(dpdc);
 952         }
 953         else
 954             _dpdc = cvCreateMat( 2*count, 2, CV_64FC1 );
 955         dpdc_p = _dpdc->data.db;
 956         dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
 957     }
 958
 959     if( dpdk )
 960     {
 961         if( !CV_IS_MAT(dpdk) ||
 962             (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
 963             dpdk->rows != count*2 || (dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
 964             CV_Error( CV_StsBadArg, "dp/df must be 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
 965
 966         if( !distCoeffs )
 967             CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
 968
 969         if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
 970         {
 971             _dpdk = cvCloneMat(dpdk);
 972         }
 973         else
 974             _dpdk = cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 );
 975         dpdk_p = _dpdk->data.db;
 976         dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
 977     }
 978
 979     calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk;
 980
 981     for( i = 0; i < count; i++ )
 982     {
 983         double X = M[i].x, Y = M[i].y, Z = M[i].z;
 984         double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
 985         double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
 986         double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
 987         double r2, r4, r6, a1, a2, a3, cdist, icdist2;
 988         double xd, yd;
 989
 990         z = z ? 1./z : 1;
 991         x *= z; y *= z;
 992
 993         r2 = x*x + y*y;
 994         r4 = r2*r2;
 995         r6 = r4*r2;
 996         a1 = 2*x*y;
 997         a2 = r2 + 2*x*x;
 998         a3 = r2 + 2*y*y;
 999         cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
1000         icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
1001         xd = x*cdist*icdist2 + k[2]*a1 + k[3]*a2;
1002         yd = y*cdist*icdist2 + k[2]*a3 + k[3]*a1;
1003
1004         m[i].x = xd*fx + cx;
1005         m[i].y = yd*fy + cy;
1006
1007         if( calc_derivatives )
1008         {
1009             if( dpdc_p )
1010             {
1011                 dpdc_p[0] = 1; dpdc_p[1] = 0;
1012                 dpdc_p[dpdc_step] = 0;
1013                 dpdc_p[dpdc_step+1] = 1;
1014                 dpdc_p += dpdc_step*2;
1015             }
1016
1017             if( dpdf_p )
1018             {
1019                 if( fixedAspectRatio )
1020                 {
1021                     dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio;
1022                     dpdf_p[dpdf_step] = 0;
1023                     dpdf_p[dpdf_step+1] = yd;
1024                 }
1025                 else
1026                 {
1027                     dpdf_p[0] = xd; dpdf_p[1] = 0;
1028                     dpdf_p[dpdf_step] = 0;
1029                     dpdf_p[dpdf_step+1] = yd;
1030                 }
1031                 dpdf_p += dpdf_step*2;
1032             }
1033
1034             if( dpdk_p )
1035             {
1036                 dpdk_p[0] = fx*x*icdist2*r2;
1037                 dpdk_p[1] = fx*x*icdist2*r4;
1038                 dpdk_p[dpdk_step] = fy*y*icdist2*r2;
1039                 dpdk_p[dpdk_step+1] = fy*y*icdist2*r4;
1040                 if( _dpdk->cols > 2 )
1041                 {
1042                     dpdk_p[2] = fx*a1;
1043                     dpdk_p[3] = fx*a2;
1044                     dpdk_p[dpdk_step+2] = fy*a3;
1045                     dpdk_p[dpdk_step+3] = fy*a1;
1046                     if( _dpdk->cols > 4 )
1047                     {
1048                         dpdk_p[4] = fx*x*icdist2*r6;
1049                         dpdk_p[dpdk_step+4] = fy*y*icdist2*r6;
1050
1051                         if( _dpdk->cols > 5 )
1052                         {
1053                             dpdk_p[5] = fx*x*cdist*(-icdist2)*icdist2*r2;
1054                             dpdk_p[dpdk_step+5] = fy*y*cdist*(-icdist2)*icdist2*r2;
1055                             dpdk_p[6] = fx*x*icdist2*cdist*(-icdist2)*icdist2*r4;
1056                             dpdk_p[dpdk_step+6] = fy*y*cdist*(-icdist2)*icdist2*r4;
1057                             dpdk_p[7] = fx*x*icdist2*cdist*(-icdist2)*icdist2*r6;
1058                             dpdk_p[dpdk_step+7] = fy*y*cdist*(-icdist2)*icdist2*r6;
1059                         }
1060                     }
1061                 }
1062                 dpdk_p += dpdk_step*2;
1063             }
1064
1065             if( dpdt_p )
1066             {
1067                 double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
1068                 for( j = 0; j < 3; j++ )
1069                 {
1070                     double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
1071                     double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
1072                     double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
1073                     double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
1074                     double dmxdt = fx*(dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
1075                                        k[2]*da1dt + k[3]*(dr2dt + 2*x*dxdt[j]));
1076                     double dmydt = fy*(dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
1077                                        k[2]*(dr2dt + 2*y*dydt[j]) + k[3]*da1dt);
1078                     dpdt_p[j] = dmxdt;
1079                     dpdt_p[dpdt_step+j] = dmydt;
1080                 }
1081                 dpdt_p += dpdt_step*2;
1082             }
1083
1084             if( dpdr_p )
1085             {
1086                 double dx0dr[] =
1087                 {
1088                     X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
1089                     X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
1090                     X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
1091                 };
1092                 double dy0dr[] =
1093                 {
1094                     X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
1095                     X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
1096                     X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
1097                 };
1098                 double dz0dr[] =
1099                 {
1100                     X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
1101                     X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
1102                     X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
1103                 };
1104                 for( j = 0; j < 3; j++ )
1105                 {
1106                     double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
1107                     double dydr = z*(dy0dr[j] - y*dz0dr[j]);
1108                     double dr2dr = 2*x*dxdr + 2*y*dydr;
1109                     double dcdist_dr = k[0]*dr2dr + 2*k[1]*r2*dr2dr + 3*k[4]*r4*dr2dr;
1110                     double dicdist2_dr = -icdist2*icdist2*(k[5]*dr2dr + 2*k[6]*r2*dr2dr + 3*k[7]*r4*dr2dr);
1111                     double da1dr = 2*(x*dydr + y*dxdr);
1112                     double dmxdr = fx*(dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
1113                                        k[2]*da1dr + k[3]*(dr2dr + 2*x*dxdr));
1114                     double dmydr = fy*(dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
1115                                        k[2]*(dr2dr + 2*y*dydr) + k[3]*da1dr);
1116                     dpdr_p[j] = dmxdr;
1117                     dpdr_p[dpdr_step+j] = dmydr;
1118                 }
1119                 dpdr_p += dpdr_step*2;
1120             }
1121         }
1122     }
1123
1124     if( _m != imagePoints )
1125         cvConvert( _m, imagePoints );
1126
1127     if( _dpdr != dpdr )
1128         cvConvert( _dpdr, dpdr );
1129
1130     if( _dpdt != dpdt )
1131         cvConvert( _dpdt, dpdt );
1132
1133     if( _dpdf != dpdf )
1134         cvConvert( _dpdf, dpdf );
1135
1136     if( _dpdc != dpdc )
1137         cvConvert( _dpdc, dpdc );
1138
1139     if( _dpdk != dpdk )
1140         cvConvert( _dpdk, dpdk );
1141 }
1142
1143
1144 CV_IMPL void cvFindExtrinsicCameraParams2( const CvMat* objectPoints,
1145                   const CvMat* imagePoints, const CvMat* A,
1146                   const CvMat* distCoeffs, CvMat* rvec, CvMat* tvec,
1147                   int useExtrinsicGuess )
1148 {
1149     const int max_iter = 20;
1150     Ptr<CvMat> matM, _Mxy, _m, _mn, matL, matJ;
1151
1152     int i, count;
1153     double a[9], ar[9]={1,0,0,0,1,0,0,0,1}, R[9];
1154     double MM[9], U[9], V[9], W[3];
1155     CvScalar Mc;
1156     double param[6];
1157     CvMat matA = cvMat( 3, 3, CV_64F, a );
1158     CvMat _Ar = cvMat( 3, 3, CV_64F, ar );
1159     CvMat matR = cvMat( 3, 3, CV_64F, R );
1160     CvMat _r = cvMat( 3, 1, CV_64F, param );
1161     CvMat _t = cvMat( 3, 1, CV_64F, param + 3 );
1162     CvMat _Mc = cvMat( 1, 3, CV_64F, Mc.val );
1163     CvMat _MM = cvMat( 3, 3, CV_64F, MM );
1164     CvMat matU = cvMat( 3, 3, CV_64F, U );
1165     CvMat matV = cvMat( 3, 3, CV_64F, V );
1166     CvMat matW = cvMat( 3, 1, CV_64F, W );
1167     CvMat _param = cvMat( 6, 1, CV_64F, param );
1168     CvMat _dpdr, _dpdt;
1169
1170     CV_Assert( CV_IS_MAT(objectPoints) && CV_IS_MAT(imagePoints) &&
1171         CV_IS_MAT(A) && CV_IS_MAT(rvec) && CV_IS_MAT(tvec) );
1172
1173     count = MAX(objectPoints->cols, objectPoints->rows);
1174     matM = cvCreateMat( 1, count, CV_64FC3 );
1175     _m = cvCreateMat( 1, count, CV_64FC2 );
1176
1177     cvConvertPointsHomogeneous( objectPoints, matM );
1178     cvConvertPointsHomogeneous( imagePoints, _m );
1179     cvConvert( A, &matA );
1180
1181     CV_Assert( (CV_MAT_DEPTH(rvec->type) == CV_64F || CV_MAT_DEPTH(rvec->type) == CV_32F) &&
1182         (rvec->rows == 1 || rvec->cols == 1) && rvec->rows*rvec->cols*CV_MAT_CN(rvec->type) == 3 );
1183
1184     CV_Assert( (CV_MAT_DEPTH(tvec->type) == CV_64F || CV_MAT_DEPTH(tvec->type) == CV_32F) &&
1185         (tvec->rows == 1 || tvec->cols == 1) && tvec->rows*tvec->cols*CV_MAT_CN(tvec->type) == 3 );
1186
1187     _mn = cvCreateMat( 1, count, CV_64FC2 );
1188     _Mxy = cvCreateMat( 1, count, CV_64FC2 );
1189
1190     // normalize image points
1191     // (unapply the intrinsic matrix transformation and distortion)
1192     cvUndistortPoints( _m, _mn, &matA, distCoeffs, 0, &_Ar );
1193
1194     if( useExtrinsicGuess )
1195     {
1196         CvMat _r_temp = cvMat(rvec->rows, rvec->cols,
1197             CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param );
1198         CvMat _t_temp = cvMat(tvec->rows, tvec->cols,
1199             CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3);
1200         cvConvert( rvec, &_r_temp );
1201         cvConvert( tvec, &_t_temp );
1202     }
1203     else
1204     {
1205         Mc = cvAvg(matM);
1206         cvReshape( matM, matM, 1, count );
1207         cvMulTransposed( matM, &_MM, 1, &_Mc );
1208         cvSVD( &_MM, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T );
1209
1210         // initialize extrinsic parameters
1211         if( W[2]/W[1] < 1e-3 || count < 4 )
1212         {
1213             // a planar structure case (all M's lie in the same plane)
1214             double tt[3], h[9], h1_norm, h2_norm;
1215             CvMat* R_transform = &matV;
1216             CvMat T_transform = cvMat( 3, 1, CV_64F, tt );
1217             CvMat matH = cvMat( 3, 3, CV_64F, h );
1218             CvMat _h1, _h2, _h3;
1219
1220             if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
1221                 cvSetIdentity( R_transform );
1222
1223             if( cvDet(R_transform) < 0 )
1224                 cvScale( R_transform, R_transform, -1 );
1225
1226             cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );
1227
1228             for( i = 0; i < count; i++ )
1229             {
1230                 const double* Rp = R_transform->data.db;
1231                 const double* Tp = T_transform.data.db;
1232                 const double* src = matM->data.db + i*3;
1233                 double* dst = _Mxy->data.db + i*2;
1234
1235                 dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0];
1236                 dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1];
1237             }
1238
1239             cvFindHomography( _Mxy, _mn, &matH );
1240
1241             if( cvCheckArr(&matH, CV_CHECK_QUIET) )
1242             {
1243                 cvGetCol( &matH, &_h1, 0 );
1244                 _h2 = _h1; _h2.data.db++;
1245                 _h3 = _h2; _h3.data.db++;
1246                 h1_norm = sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]);
1247                 h2_norm = sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]);
1248
1249                 cvScale( &_h1, &_h1, 1./MAX(h1_norm, DBL_EPSILON) );
1250                 cvScale( &_h2, &_h2, 1./MAX(h2_norm, DBL_EPSILON) );
1251                 cvScale( &_h3, &_t, 2./MAX(h1_norm + h2_norm, DBL_EPSILON));
1252                 cvCrossProduct( &_h1, &_h2, &_h3 );
1253
1254                 cvRodrigues2( &matH, &_r );
1255                 cvRodrigues2( &_r, &matH );
1256                 cvMatMulAdd( &matH, &T_transform, &_t, &_t );
1257                 cvMatMul( &matH, R_transform, &matR );
1258             }
1259             else
1260             {
1261                 cvSetIdentity( &matR );
1262                 cvZero( &_t );
1263             }
1264
1265             cvRodrigues2( &matR, &_r );
1266         }
1267         else
1268         {
1269             // non-planar structure. Use DLT method
1270             double* L;
1271             double LL[12*12], LW[12], LV[12*12], sc;
1272             CvMat _LL = cvMat( 12, 12, CV_64F, LL );
1273             CvMat _LW = cvMat( 12, 1, CV_64F, LW );
1274             CvMat _LV = cvMat( 12, 12, CV_64F, LV );
1275             CvMat _RRt, _RR, _tt;
1276             CvPoint3D64f* M = (CvPoint3D64f*)matM->data.db;
1277             CvPoint2D64f* mn = (CvPoint2D64f*)_mn->data.db;
1278
1279             matL = cvCreateMat( 2*count, 12, CV_64F );
1280             L = matL->data.db;
1281
1282             for( i = 0; i < count; i++, L += 24 )
1283             {
1284                 double x = -mn[i].x, y = -mn[i].y;
1285                 L[0] = L[16] = M[i].x;
1286                 L[1] = L[17] = M[i].y;
1287                 L[2] = L[18] = M[i].z;
1288                 L[3] = L[19] = 1.;
1289                 L[4] = L[5] = L[6] = L[7] = 0.;
1290                 L[12] = L[13] = L[14] = L[15] = 0.;
1291                 L[8] = x*M[i].x;
1292                 L[9] = x*M[i].y;
1293                 L[10] = x*M[i].z;
1294                 L[11] = x;
1295                 L[20] = y*M[i].x;
1296                 L[21] = y*M[i].y;
1297                 L[22] = y*M[i].z;
1298                 L[23] = y;
1299             }
1300
1301             cvMulTransposed( matL, &_LL, 1 );
1302             cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
1303             _RRt = cvMat( 3, 4, CV_64F, LV + 11*12 );
1304             cvGetCols( &_RRt, &_RR, 0, 3 );
1305             cvGetCol( &_RRt, &_tt, 3 );
1306             if( cvDet(&_RR) < 0 )
1307                 cvScale( &_RRt, &_RRt, -1 );
1308             sc = cvNorm(&_RR);
1309             cvSVD( &_RR, &matW, &matU, &matV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
1310             cvGEMM( &matU, &matV, 1, 0, 0, &matR, CV_GEMM_A_T );
1311             cvScale( &_tt, &_t, cvNorm(&matR)/sc );
1312             cvRodrigues2( &matR, &_r );
1313         }
1314     }
1315
1316     cvReshape( matM, matM, 3, 1 );
1317     cvReshape( _mn, _mn, 2, 1 );
1318
1319     // refine extrinsic parameters using iterative algorithm
1320     CvLevMarq solver( 6, count*2, cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,max_iter,FLT_EPSILON), true);
1321     cvCopy( &_param, solver.param );
1322
1323     for(;;)
1324     {
1325         CvMat *matJ = 0, *_err = 0;
1326         const CvMat *__param = 0;
1327         bool proceed = solver.update( __param, matJ, _err );
1328         cvCopy( __param, &_param );
1329         if( !proceed || !_err )
1330             break;
1331         cvReshape( _err, _err, 2, 1 );
1332         if( matJ )
1333         {
1334             cvGetCols( matJ, &_dpdr, 0, 3 );
1335             cvGetCols( matJ, &_dpdt, 3, 6 );
1336             cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs,
1337                               _err, &_dpdr, &_dpdt, 0, 0, 0 );
1338         }
1339         else
1340         {
1341             cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs,
1342                               _err, 0, 0, 0, 0, 0 );
1343         }
1344         cvSub(_err, _m, _err);
1345         cvReshape( _err, _err, 1, 2*count );
1346     }
1347     cvCopy( solver.param, &_param );
1348
1349     _r = cvMat( rvec->rows, rvec->cols,
1350         CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param );
1351     _t = cvMat( tvec->rows, tvec->cols,
1352         CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3 );
1353
1354     cvConvert( &_r, rvec );
1355     cvConvert( &_t, tvec );
1356 }
1357
1358
1359 CV_IMPL void cvInitIntrinsicParams2D( const CvMat* objectPoints,
1360                          const CvMat* imagePoints, const CvMat* npoints,
1361                          CvSize imageSize, CvMat* cameraMatrix,
1362                          double aspectRatio )
1363 {
1364     Ptr<CvMat> matA, _b, _allH, _allK;
1365
1366     int i, j, pos, nimages, ni = 0;
1367     double a[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 };
1368     double H[9], f[2];
1369     CvMat _a = cvMat( 3, 3, CV_64F, a );
1370     CvMat matH = cvMat( 3, 3, CV_64F, H );
1371     CvMat _f = cvMat( 2, 1, CV_64F, f );
1372
1373     assert( CV_MAT_TYPE(npoints->type) == CV_32SC1 &&
1374             CV_IS_MAT_CONT(npoints->type) );
1375     nimages = npoints->rows + npoints->cols - 1;
1376
1377     if( (CV_MAT_TYPE(objectPoints->type) != CV_32FC3 &&
1378         CV_MAT_TYPE(objectPoints->type) != CV_64FC3) ||
1379         (CV_MAT_TYPE(imagePoints->type) != CV_32FC2 &&
1380         CV_MAT_TYPE(imagePoints->type) != CV_64FC2) )
1381         CV_Error( CV_StsUnsupportedFormat, "Both object points and image points must be 2D" );
1382
1383     if( objectPoints->rows != 1 || imagePoints->rows != 1 )
1384         CV_Error( CV_StsBadSize, "object points and image points must be a single-row matrices" );
1385
1386     matA = cvCreateMat( 2*nimages, 2, CV_64F );
1387     _b = cvCreateMat( 2*nimages, 1, CV_64F );
1388     a[2] = (imageSize.width - 1)*0.5;
1389     a[5] = (imageSize.height - 1)*0.5;
1390     _allH = cvCreateMat( nimages, 9, CV_64F );
1391
1392     // extract vanishing points in order to obtain initial value for the focal length
1393     for( i = 0, pos = 0; i < nimages; i++, pos += ni )
1394     {
1395         double* Ap = matA->data.db + i*4;
1396         double* bp = _b->data.db + i*2;
1397         ni = npoints->data.i[i];
1398         double h[3], v[3], d1[3], d2[3];
1399         double n[4] = {0,0,0,0};
1400         CvMat _m, matM;
1401         cvGetCols( objectPoints, &matM, pos, pos + ni );
1402         cvGetCols( imagePoints, &_m, pos, pos + ni );
1403
1404         cvFindHomography( &matM, &_m, &matH );
1405         memcpy( _allH->data.db + i*9, H, sizeof(H) );
1406
1407         H[0] -= H[6]*a[2]; H[1] -= H[7]*a[2]; H[2] -= H[8]*a[2];
1408         H[3] -= H[6]*a[5]; H[4] -= H[7]*a[5]; H[5] -= H[8]*a[5];
1409
1410         for( j = 0; j < 3; j++ )
1411         {
1412             double t0 = H[j*3], t1 = H[j*3+1];
1413             h[j] = t0; v[j] = t1;
1414             d1[j] = (t0 + t1)*0.5;
1415             d2[j] = (t0 - t1)*0.5;
1416             n[0] += t0*t0; n[1] += t1*t1;
1417             n[2] += d1[j]*d1[j]; n[3] += d2[j]*d2[j];
1418         }
1419
1420         for( j = 0; j < 4; j++ )
1421             n[j] = 1./sqrt(n[j]);
1422
1423         for( j = 0; j < 3; j++ )
1424         {
1425             h[j] *= n[0]; v[j] *= n[1];
1426             d1[j] *= n[2]; d2[j] *= n[3];
1427         }
1428
1429         Ap[0] = h[0]*v[0]; Ap[1] = h[1]*v[1];
1430         Ap[2] = d1[0]*d2[0]; Ap[3] = d1[1]*d2[1];
1431         bp[0] = -h[2]*v[2]; bp[1] = -d1[2]*d2[2];
1432     }
1433
1434     cvSolve( matA, _b, &_f, CV_NORMAL + CV_SVD );
1435     a[0] = sqrt(fabs(1./f[0]));
1436     a[4] = sqrt(fabs(1./f[1]));
1437     if( aspectRatio != 0 )
1438     {
1439         double tf = (a[0] + a[4])/(aspectRatio + 1.);
1440         a[0] = aspectRatio*tf;
1441         a[4] = tf;
1442     }
1443
1444     cvConvert( &_a, cameraMatrix );
1445 }
1446
1447
1448 /* finds intrinsic and extrinsic camera parameters
1449    from a few views of known calibration pattern */
1450 CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
1451                     const CvMat* imagePoints, const CvMat* npoints,
1452                     CvSize imageSize, CvMat* cameraMatrix, CvMat* distCoeffs,
1453                     CvMat* rvecs, CvMat* tvecs, int flags )
1454 {
1455     const int NINTRINSIC = 12;
1456     Ptr<CvMat> matM, _m, _Ji, _Je, _err;
1457     CvLevMarq solver;
1458     double reprojErr = 0;
1459
1460     double A[9], k[8] = {0,0,0,0,0,0,0,0};
1461     CvMat matA = cvMat(3, 3, CV_64F, A), _k;
1462     int i, nimages, maxPoints = 0, ni = 0, pos, total = 0, nparams, npstep, cn;
1463     double aspectRatio = 0.;
1464
1465     // 0. check the parameters & allocate buffers
1466     if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(imagePoints) ||
1467         !CV_IS_MAT(npoints) || !CV_IS_MAT(cameraMatrix) || !CV_IS_MAT(distCoeffs) )
1468         CV_Error( CV_StsBadArg, "One of required vector arguments is not a valid matrix" );
1469
1470     if( imageSize.width <= 0 || imageSize.height <= 0 )
1471         CV_Error( CV_StsOutOfRange, "image width and height must be positive" );
1472
1473     if( CV_MAT_TYPE(npoints->type) != CV_32SC1 ||
1474         (npoints->rows != 1 && npoints->cols != 1) )
1475         CV_Error( CV_StsUnsupportedFormat,
1476             "the array of point counters must be 1-dimensional integer vector" );
1477
1478     nimages = npoints->rows*npoints->cols;
1479     npstep = npoints->rows == 1 ? 1 : npoints->step/CV_ELEM_SIZE(npoints->type);
1480
1481     if( rvecs )
1482     {
1483         cn = CV_MAT_CN(rvecs->type);
1484         if( !CV_IS_MAT(rvecs) ||
1485             (CV_MAT_DEPTH(rvecs->type) != CV_32F && CV_MAT_DEPTH(rvecs->type) != CV_64F) ||
1486             ((rvecs->rows != nimages || (rvecs->cols*cn != 3 && rvecs->cols*cn != 9)) &&
1487             (rvecs->rows != 1 || rvecs->cols != nimages || cn != 3)) )
1488             CV_Error( CV_StsBadArg, "the output array of rotation vectors must be 3-channel "
1489                 "1xn or nx1 array or 1-channel nx3 or nx9 array, where n is the number of views" );
1490     }
1491
1492     if( tvecs )
1493     {
1494         cn = CV_MAT_CN(tvecs->type);
1495         if( !CV_IS_MAT(tvecs) ||
1496             (CV_MAT_DEPTH(tvecs->type) != CV_32F && CV_MAT_DEPTH(tvecs->type) != CV_64F) ||
1497             ((tvecs->rows != nimages || tvecs->cols*cn != 3) &&
1498             (tvecs->rows != 1 || tvecs->cols != nimages || cn != 3)) )
1499             CV_Error( CV_StsBadArg, "the output array of translation vectors must be 3-channel "
1500                 "1xn or nx1 array or 1-channel nx3 array, where n is the number of views" );
1501     }
1502
1503     if( (CV_MAT_TYPE(cameraMatrix->type) != CV_32FC1 &&
1504         CV_MAT_TYPE(cameraMatrix->type) != CV_64FC1) ||
1505         cameraMatrix->rows != 3 || cameraMatrix->cols != 3 )
1506         CV_Error( CV_StsBadArg,
1507             "Intrinsic parameters must be 3x3 floating-point matrix" );
1508
1509     if( (CV_MAT_TYPE(distCoeffs->type) != CV_32FC1 &&
1510         CV_MAT_TYPE(distCoeffs->type) != CV_64FC1) ||
1511         (distCoeffs->cols != 1 && distCoeffs->rows != 1) ||
1512         (distCoeffs->cols*distCoeffs->rows != 4 &&
1513         distCoeffs->cols*distCoeffs->rows != 5 &&
1514         distCoeffs->cols*distCoeffs->rows != 8) )
1515         CV_Error( CV_StsBadArg, cvDistCoeffErr );
1516
1517     for( i = 0; i < nimages; i++ )
1518     {
1519         ni = npoints->data.i[i*npstep];
1520         if( ni < 4 )
1521         {
1522             char buf[100];
1523             sprintf( buf, "The number of points in the view #%d is < 4", i );
1524             CV_Error( CV_StsOutOfRange, buf );
1525         }
1526         maxPoints = MAX( maxPoints, ni );
1527         total += ni;
1528     }
1529
1530     matM = cvCreateMat( 1, total, CV_64FC3 );
1531     _m = cvCreateMat( 1, total, CV_64FC2 );
1532
1533     cvConvertPointsHomogeneous( objectPoints, matM );
1534     cvConvertPointsHomogeneous( imagePoints, _m );
1535
1536     nparams = NINTRINSIC + nimages*6;
1537     _Ji = cvCreateMat( maxPoints*2, NINTRINSIC, CV_64FC1 );
1538     _Je = cvCreateMat( maxPoints*2, 6, CV_64FC1 );
1539     _err = cvCreateMat( maxPoints*2, 1, CV_64FC1 );
1540     cvZero( _Ji );
1541
1542     _k = cvMat( distCoeffs->rows, distCoeffs->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k);
1543     if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) < 8 )
1544     {
1545         if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) < 5 )
1546             flags |= CV_CALIB_FIX_K3;
1547         flags |= CV_CALIB_FIX_K4 | CV_CALIB_FIX_K5 | CV_CALIB_FIX_K6;
1548     }
1549
1550     // 1. initialize intrinsic parameters & LM solver
1551     if( flags & CV_CALIB_USE_INTRINSIC_GUESS )
1552     {
1553         cvConvert( cameraMatrix, &matA );
1554         if( A[0] <= 0 || A[4] <= 0 )
1555             CV_Error( CV_StsOutOfRange, "Focal length (fx and fy) must be positive" );
1556         if( A[2] < 0 || A[2] >= imageSize.width ||
1557             A[5] < 0 || A[5] >= imageSize.height )
1558             CV_Error( CV_StsOutOfRange, "Principal point must be within the image" );
1559         if( fabs(A[1]) > 1e-5 )
1560             CV_Error( CV_StsOutOfRange, "Non-zero skew is not supported by the function" );
1561         if( fabs(A[3]) > 1e-5 || fabs(A[6]) > 1e-5 ||
1562             fabs(A[7]) > 1e-5 || fabs(A[8]-1) > 1e-5 )
1563             CV_Error( CV_StsOutOfRange,
1564                 "The intrinsic matrix must have [fx 0 cx; 0 fy cy; 0 0 1] shape" );
1565         A[1] = A[3] = A[6] = A[7] = 0.;
1566         A[8] = 1.;
1567
1568         if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1569             aspectRatio = A[0]/A[4];
1570         cvConvert( distCoeffs, &_k );
1571     }
1572     else
1573     {
1574         CvScalar mean, sdv;
1575         cvAvgSdv( matM, &mean, &sdv );
1576         if( fabs(mean.val[2]) > 1e-5 || fabs(sdv.val[2]) > 1e-5 )
1577             CV_Error( CV_StsBadArg,
1578             "For non-planar calibration rigs the initial intrinsic matrix must be specified" );
1579         for( i = 0; i < total; i++ )
1580             ((CvPoint3D64f*)matM->data.db)[i].z = 0.;
1581
1582         if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1583         {
1584             aspectRatio = cvmGet(cameraMatrix,0,0);
1585             aspectRatio /= cvmGet(cameraMatrix,1,1);
1586             if( aspectRatio < 0.01 || aspectRatio > 100 )
1587                 CV_Error( CV_StsOutOfRange,
1588                     "The specified aspect ratio (=A[0][0]/A[1][1]) is incorrect" );
1589         }
1590         cvInitIntrinsicParams2D( matM, _m, npoints, imageSize, &matA, aspectRatio );
1591     }
1592
1593     solver.init( nparams, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,DBL_EPSILON) );
1594
1595     {
1596     double* param = solver.param->data.db;
1597     uchar* mask = solver.mask->data.ptr;
1598
1599     param[0] = A[0]; param[1] = A[4]; param[2] = A[2]; param[3] = A[5];
1600     param[4] = k[0]; param[5] = k[1]; param[6] = k[2]; param[7] = k[3];
1601     param[8] = k[4]; param[9] = k[5]; param[10] = k[6]; param[11] = k[7];
1602
1603     if( flags & CV_CALIB_FIX_FOCAL_LENGTH )
1604         mask[0] = mask[1] = 0;
1605     if( flags & CV_CALIB_FIX_PRINCIPAL_POINT )
1606         mask[2] = mask[3] = 0;
1607     if( flags & CV_CALIB_ZERO_TANGENT_DIST )
1608     {
1609         param[6] = param[7] = 0;
1610         mask[6] = mask[7] = 0;
1611     }
1612     if( !(flags & CV_CALIB_RATIONAL_MODEL) )
1613         flags |= CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5 + CV_CALIB_FIX_K6;
1614     if( flags & CV_CALIB_FIX_K1 )
1615         mask[4] = 0;
1616     if( flags & CV_CALIB_FIX_K2 )
1617         mask[5] = 0;
1618     if( flags & CV_CALIB_FIX_K3 )
1619         mask[8] = 0;
1620     if( flags & CV_CALIB_FIX_K4 )
1621         mask[9] = 0;
1622     if( flags & CV_CALIB_FIX_K5 )
1623         mask[10] = 0;
1624     if( flags & CV_CALIB_FIX_K6 )
1625         mask[11] = 0;
1626     }
1627
1628     // 2. initialize extrinsic parameters
1629     for( i = 0, pos = 0; i < nimages; i++, pos += ni )
1630     {
1631         CvMat _Mi, _mi, _ri, _ti;
1632         ni = npoints->data.i[i*npstep];
1633
1634         cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 );
1635         cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 );
1636
1637         cvGetCols( matM, &_Mi, pos, pos + ni );
1638         cvGetCols( _m, &_mi, pos, pos + ni );
1639
1640         cvFindExtrinsicCameraParams2( &_Mi, &_mi, &matA, &_k, &_ri, &_ti );
1641     }
1642
1643     // 3. run the optimization
1644     for(;;)
1645     {
1646         const CvMat* _param = 0;
1647         CvMat *_JtJ = 0, *_JtErr = 0;
1648         double* _errNorm = 0;
1649         bool proceed = solver.updateAlt( _param, _JtJ, _JtErr, _errNorm );
1650         double *param = solver.param->data.db, *pparam = solver.prevParam->data.db;
1651
1652         if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1653         {
1654             param[0] = param[1]*aspectRatio;
1655             pparam[0] = pparam[1]*aspectRatio;
1656         }
1657
1658         A[0] = param[0]; A[4] = param[1]; A[2] = param[2]; A[5] = param[3];
1659         k[0] = param[4]; k[1] = param[5]; k[2] = param[6]; k[3] = param[7];
1660         k[4] = param[8]; k[5] = param[9]; k[6] = param[10]; k[7] = param[11];
1661
1662         if( !proceed )
1663             break;
1664
1665         reprojErr = 0;
1666
1667         for( i = 0, pos = 0; i < nimages; i++, pos += ni )
1668         {
1669             CvMat _Mi, _mi, _ri, _ti, _dpdr, _dpdt, _dpdf, _dpdc, _dpdk, _mp, _part;
1670             ni = npoints->data.i[i*npstep];
1671
1672             cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 );
1673             cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 );
1674
1675             cvGetCols( matM, &_Mi, pos, pos + ni );
1676             cvGetCols( _m, &_mi, pos, pos + ni );
1677
1678             _Je->rows = _Ji->rows = _err->rows = ni*2;
1679             cvGetCols( _Je, &_dpdr, 0, 3 );
1680             cvGetCols( _Je, &_dpdt, 3, 6 );
1681             cvGetCols( _Ji, &_dpdf, 0, 2 );
1682             cvGetCols( _Ji, &_dpdc, 2, 4 );
1683             cvGetCols( _Ji, &_dpdk, 4, NINTRINSIC );
1684             cvReshape( _err, &_mp, 2, 1 );
1685
1686             if( _JtJ || _JtErr )
1687             {
1688                 cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, &_k, &_mp, &_dpdr, &_dpdt,
1689                                   (flags & CV_CALIB_FIX_FOCAL_LENGTH) ? 0 : &_dpdf,
1690                                   (flags & CV_CALIB_FIX_PRINCIPAL_POINT) ? 0 : &_dpdc, &_dpdk,
1691                                   (flags & CV_CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0);
1692             }
1693             else
1694                 cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, &_k, &_mp );
1695
1696             cvSub( &_mp, &_mi, &_mp );
1697
1698             if( _JtJ || _JtErr )
1699             {
1700                 cvGetSubRect( _JtJ, &_part, cvRect(0,0,NINTRINSIC,NINTRINSIC) );
1701                 cvGEMM( _Ji, _Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
1702
1703                 cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,NINTRINSIC+i*6,6,6) );
1704                 cvGEMM( _Je, _Je, 1, 0, 0, &_part, CV_GEMM_A_T );
1705
1706                 cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,0,6,NINTRINSIC) );
1707                 cvGEMM( _Ji, _Je, 1, 0, 0, &_part, CV_GEMM_A_T );
1708
1709                 cvGetRows( _JtErr, &_part, 0, NINTRINSIC );
1710                 cvGEMM( _Ji, _err, 1, &_part, 1, &_part, CV_GEMM_A_T );
1711
1712                 cvGetRows( _JtErr, &_part, NINTRINSIC + i*6, NINTRINSIC + (i+1)*6 );
1713                 cvGEMM( _Je, _err, 1, 0, 0, &_part, CV_GEMM_A_T );
1714             }
1715
1716             double errNorm = cvNorm( &_mp, 0, CV_L2 );
1717             reprojErr += errNorm*errNorm;
1718         }
1719         if( _errNorm )
1720             *_errNorm = reprojErr;
1721     }
1722
1723     // 4. store the results
1724     cvConvert( &matA, cameraMatrix );
1725     cvConvert( &_k, distCoeffs );
1726
1727     for( i = 0; i < nimages; i++ )
1728     {
1729         CvMat src, dst;
1730         if( rvecs )
1731         {
1732             src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 );
1733             if( rvecs->rows == nimages && rvecs->cols*CV_MAT_CN(rvecs->type) == 9 )
1734             {
1735                 dst = cvMat( 3, 3, CV_MAT_DEPTH(rvecs->type),
1736                     rvecs->data.ptr + rvecs->step*i );
1737                 cvRodrigues2( &src, &matA );
1738                 cvConvert( &matA, &dst );
1739             }
1740             else
1741             {
1742                 dst = cvMat( 3, 1, CV_MAT_DEPTH(rvecs->type), rvecs->rows == 1 ?
1743                     rvecs->data.ptr + i*CV_ELEM_SIZE(rvecs->type) :
1744                     rvecs->data.ptr + rvecs->step*i );
1745                 cvConvert( &src, &dst );
1746             }
1747         }
1748         if( tvecs )
1749         {
1750             src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 + 3 );
1751             dst = cvMat( 3, 1, CV_MAT_TYPE(tvecs->type), tvecs->rows == 1 ?
1752                     tvecs->data.ptr + i*CV_ELEM_SIZE(tvecs->type) :
1753                     tvecs->data.ptr + tvecs->step*i );
1754             cvConvert( &src, &dst );
1755          }
1756     }
1757
1758     return std::sqrt(reprojErr/total);
1759 }
1760
1761
1762 void cvCalibrationMatrixValues( const CvMat *calibMatr, CvSize imgSize,
1763     double apertureWidth, double apertureHeight, double *fovx, double *fovy,
1764     double *focalLength, CvPoint2D64f *principalPoint, double *pasp )
1765 {
1766     double alphax, alphay, mx, my;
1767     int imgWidth = imgSize.width, imgHeight = imgSize.height;
1768
1769     /* Validate parameters. */
1770
1771     if(calibMatr == 0)
1772         CV_Error(CV_StsNullPtr, "Some of parameters is a NULL pointer!");
1773
1774     if(!CV_IS_MAT(calibMatr))
1775         CV_Error(CV_StsUnsupportedFormat, "Input parameters must be a matrices!");
1776
1777     if(calibMatr->cols != 3 || calibMatr->rows != 3)
1778         CV_Error(CV_StsUnmatchedSizes, "Size of matrices must be 3x3!");
1779
1780     alphax = cvmGet(calibMatr, 0, 0);
1781     alphay = cvmGet(calibMatr, 1, 1);
1782     assert(imgWidth != 0 && imgHeight != 0 && alphax != 0.0 && alphay != 0.0);
1783
1784     /* Calculate pixel aspect ratio. */
1785     if(pasp)
1786         *pasp = alphay / alphax;
1787
1788     /* Calculate number of pixel per realworld unit. */
1789
1790     if(apertureWidth != 0.0 && apertureHeight != 0.0) {
1791         mx = imgWidth / apertureWidth;
1792         my = imgHeight / apertureHeight;
1793     } else {
1794         mx = 1.0;
1795         my = *pasp;
1796     }
1797
1798     /* Calculate fovx and fovy. */
1799
1800     if(fovx)
1801         *fovx = 2 * atan(imgWidth / (2 * alphax)) * 180.0 / CV_PI;
1802
1803     if(fovy)
1804         *fovy = 2 * atan(imgHeight / (2 * alphay)) * 180.0 / CV_PI;
1805
1806     /* Calculate focal length. */
1807
1808     if(focalLength)
1809         *focalLength = alphax / mx;
1810
1811     /* Calculate principle point. */
1812
1813     if(principalPoint)
1814         *principalPoint = cvPoint2D64f(cvmGet(calibMatr, 0, 2) / mx, cvmGet(calibMatr, 1, 2) / my);
1815 }
1816
1817
1818 //////////////////////////////// Stereo Calibration ///////////////////////////////////
1819
1820 static int dbCmp( const void* _a, const void* _b )
1821 {
1822     double a = *(const double*)_a;
1823     double b = *(const double*)_b;
1824
1825     return (a > b) - (a < b);
1826 }
1827
1828
1829 double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1,
1830                         const CvMat* _imagePoints2, const CvMat* _npoints,
1831                         CvMat* _cameraMatrix1, CvMat* _distCoeffs1,
1832                         CvMat* _cameraMatrix2, CvMat* _distCoeffs2,
1833                         CvSize imageSize, CvMat* matR, CvMat* matT,
1834                         CvMat* matE, CvMat* matF,
1835                         CvTermCriteria termCrit,
1836                         int flags )
1837 {
1838     const int NINTRINSIC = 12;
1839     Ptr<CvMat> npoints, err, J_LR, Je, Ji, imagePoints[2], objectPoints, RT0;
1840     CvLevMarq solver;
1841     double reprojErr = 0;
1842
1843     double A[2][9], dk[2][8]={{0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0}}, rlr[9];
1844     CvMat K[2], Dist[2], om_LR, T_LR;
1845     CvMat R_LR = cvMat(3, 3, CV_64F, rlr);
1846     int i, k, p, ni = 0, ofs, nimages, pointsTotal, maxPoints = 0;
1847     int nparams;
1848     bool recomputeIntrinsics = false;
1849     double aspectRatio[2] = {0,0};
1850
1851     CV_Assert( CV_IS_MAT(_imagePoints1) && CV_IS_MAT(_imagePoints2) &&
1852                CV_IS_MAT(_objectPoints) && CV_IS_MAT(_npoints) &&
1853                CV_IS_MAT(matR) && CV_IS_MAT(matT) );
1854
1855     CV_Assert( CV_ARE_TYPES_EQ(_imagePoints1, _imagePoints2) &&
1856                CV_ARE_DEPTHS_EQ(_imagePoints1, _objectPoints) );
1857
1858     CV_Assert( (_npoints->cols == 1 || _npoints->rows == 1) &&
1859                CV_MAT_TYPE(_npoints->type) == CV_32SC1 );
1860
1861     nimages = _npoints->cols + _npoints->rows - 1;
1862     npoints = cvCreateMat( _npoints->rows, _npoints->cols, _npoints->type );
1863     cvCopy( _npoints, npoints );
1864
1865     for( i = 0, pointsTotal = 0; i < nimages; i++ )
1866     {
1867         maxPoints = MAX(maxPoints, npoints->data.i[i]);
1868         pointsTotal += npoints->data.i[i];
1869     }
1870
1871     objectPoints = cvCreateMat( _objectPoints->rows, _objectPoints->cols,
1872                                 CV_64FC(CV_MAT_CN(_objectPoints->type)));
1873     cvConvert( _objectPoints, objectPoints );
1874     cvReshape( objectPoints, objectPoints, 3, 1 );
1875
1876     for( k = 0; k < 2; k++ )
1877     {
1878         const CvMat* points = k == 0 ? _imagePoints1 : _imagePoints2;
1879         const CvMat* cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
1880         const CvMat* distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2;
1881
1882         int cn = CV_MAT_CN(_imagePoints1->type);
1883         CV_Assert( (CV_MAT_DEPTH(_imagePoints1->type) == CV_32F ||
1884                 CV_MAT_DEPTH(_imagePoints1->type) == CV_64F) &&
1885                ((_imagePoints1->rows == pointsTotal && _imagePoints1->cols*cn == 2) ||
1886                 (_imagePoints1->rows == 1 && _imagePoints1->cols == pointsTotal && cn == 2)) );
1887
1888         K[k] = cvMat(3,3,CV_64F,A[k]);
1889         Dist[k] = cvMat(1,8,CV_64F,dk[k]);
1890
1891         imagePoints[k] = cvCreateMat( points->rows, points->cols, CV_64FC(CV_MAT_CN(points->type)));
1892         cvConvert( points, imagePoints[k] );
1893         cvReshape( imagePoints[k], imagePoints[k], 2, 1 );
1894
1895         if( flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS|
1896             CV_CALIB_FIX_ASPECT_RATIO|CV_CALIB_FIX_FOCAL_LENGTH) )
1897             cvConvert( cameraMatrix, &K[k] );
1898
1899         if( flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS|
1900             CV_CALIB_FIX_K1|CV_CALIB_FIX_K2|CV_CALIB_FIX_K3|CV_CALIB_FIX_K4|CV_CALIB_FIX_K5|CV_CALIB_FIX_K6) )
1901         {
1902             CvMat tdist = cvMat( distCoeffs->rows, distCoeffs->cols,
1903                 CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), Dist[k].data.db );
1904             cvConvert( distCoeffs, &tdist );
1905         }
1906
1907         if( !(flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS)))
1908         {
1909             cvCalibrateCamera2( objectPoints, imagePoints[k],
1910                 npoints, imageSize, &K[k], &Dist[k], 0, 0, flags );
1911         }
1912     }
1913
1914     if( flags & CV_CALIB_SAME_FOCAL_LENGTH )
1915     {
1916         static const int avg_idx[] = { 0, 4, 2, 5, -1 };
1917         for( k = 0; avg_idx[k] >= 0; k++ )
1918             A[0][avg_idx[k]] = A[1][avg_idx[k]] = (A[0][avg_idx[k]] + A[1][avg_idx[k]])*0.5;
1919     }
1920
1921     if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1922     {
1923         for( k = 0; k < 2; k++ )
1924             aspectRatio[k] = A[k][0]/A[k][4];
1925     }
1926
1927     recomputeIntrinsics = (flags & CV_CALIB_FIX_INTRINSIC) == 0;
1928
1929     err = cvCreateMat( maxPoints*2, 1, CV_64F );
1930     Je = cvCreateMat( maxPoints*2, 6, CV_64F );
1931     J_LR = cvCreateMat( maxPoints*2, 6, CV_64F );
1932     Ji = cvCreateMat( maxPoints*2, NINTRINSIC, CV_64F );
1933     cvZero( Ji );
1934
1935     // we optimize for the inter-camera R(3),t(3), then, optionally,
1936     // for intrinisic parameters of each camera ((fx,fy,cx,cy,k1,k2,p1,p2) ~ 8 parameters).
1937     nparams = 6*(nimages+1) + (recomputeIntrinsics ? NINTRINSIC*2 : 0);
1938
1939     // storage for initial [om(R){i}|t{i}] (in order to compute the median for each component)
1940     RT0 = cvCreateMat( 6, nimages, CV_64F );
1941
1942     solver.init( nparams, 0, termCrit );
1943     if( recomputeIntrinsics )
1944     {
1945         uchar* imask = solver.mask->data.ptr + nparams - NINTRINSIC*2;
1946         if( !(flags & CV_CALIB_RATIONAL_MODEL) )
1947             flags |= CV_CALIB_FIX_K4 | CV_CALIB_FIX_K5 | CV_CALIB_FIX_K6;
1948         if( flags & CV_CALIB_FIX_ASPECT_RATIO )
1949             imask[0] = imask[NINTRINSIC] = 0;
1950         if( flags & CV_CALIB_FIX_FOCAL_LENGTH )
1951             imask[0] = imask[1] = imask[NINTRINSIC] = imask[NINTRINSIC+1] = 0;
1952         if( flags & CV_CALIB_FIX_PRINCIPAL_POINT )
1953             imask[2] = imask[3] = imask[NINTRINSIC+2] = imask[NINTRINSIC+3] = 0;
1954         if( flags & CV_CALIB_ZERO_TANGENT_DIST )
1955             imask[6] = imask[7] = imask[NINTRINSIC+6] = imask[NINTRINSIC+7] = 0;
1956         if( flags & CV_CALIB_FIX_K1 )
1957             imask[4] = imask[NINTRINSIC+4] = 0;
1958         if( flags & CV_CALIB_FIX_K2 )
1959             imask[5] = imask[NINTRINSIC+5] = 0;
1960         if( flags & CV_CALIB_FIX_K3 )
1961             imask[8] = imask[NINTRINSIC+8] = 0;
1962         if( flags & CV_CALIB_FIX_K4 )
1963             imask[9] = imask[NINTRINSIC+9] = 0;
1964         if( flags & CV_CALIB_FIX_K5 )
1965             imask[10] = imask[NINTRINSIC+10] = 0;
1966         if( flags & CV_CALIB_FIX_K6 )
1967             imask[11] = imask[NINTRINSIC+11] = 0;
1968     }
1969
1970     /*
1971        Compute initial estimate of pose
1972
1973        For each image, compute:
1974           R(om) is the rotation matrix of om
1975           om(R) is the rotation vector of R
1976           R_ref = R(om_right) * R(om_left)'
1977           T_ref_list = [T_ref_list; T_right - R_ref * T_left]
1978           om_ref_list = {om_ref_list; om(R_ref)]
1979
1980        om = median(om_ref_list)
1981        T = median(T_ref_list)
1982     */
1983     for( i = ofs = 0; i < nimages; ofs += ni, i++ )
1984     {
1985         ni = npoints->data.i[i];
1986         CvMat objpt_i;
1987         double _om[2][3], r[2][9], t[2][3];
1988         CvMat om[2], R[2], T[2], imgpt_i[2];
1989
1990         objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3);
1991         for( k = 0; k < 2; k++ )
1992         {
1993             imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2);
1994             om[k] = cvMat(3, 1, CV_64F, _om[k]);
1995             R[k] = cvMat(3, 3, CV_64F, r[k]);
1996             T[k] = cvMat(3, 1, CV_64F, t[k]);
1997
1998             // FIXME: here we ignore activePoints[k] because of
1999             // the limited API of cvFindExtrnisicCameraParams2
2000             cvFindExtrinsicCameraParams2( &objpt_i, &imgpt_i[k], &K[k], &Dist[k], &om[k], &T[k] );
2001             cvRodrigues2( &om[k], &R[k] );
2002             if( k == 0 )
2003             {
2004                 // save initial om_left and T_left
2005                 solver.param->data.db[(i+1)*6] = _om[0][0];
2006                 solver.param->data.db[(i+1)*6 + 1] = _om[0][1];
2007                 solver.param->data.db[(i+1)*6 + 2] = _om[0][2];
2008                 solver.param->data.db[(i+1)*6 + 3] = t[0][0];
2009                 solver.param->data.db[(i+1)*6 + 4] = t[0][1];
2010                 solver.param->data.db[(i+1)*6 + 5] = t[0][2];
2011             }
2012         }
2013         cvGEMM( &R[1], &R[0], 1, 0, 0, &R[0], CV_GEMM_B_T );
2014         cvGEMM( &R[0], &T[0], -1, &T[1], 1, &T[1] );
2015         cvRodrigues2( &R[0], &T[0] );
2016         RT0->data.db[i] = t[0][0];
2017         RT0->data.db[i + nimages] = t[0][1];
2018         RT0->data.db[i + nimages*2] = t[0][2];
2019         RT0->data.db[i + nimages*3] = t[1][0];
2020         RT0->data.db[i + nimages*4] = t[1][1];
2021         RT0->data.db[i + nimages*5] = t[1][2];
2022     }
2023
2024     // find the medians and save the first 6 parameters
2025     for( i = 0; i < 6; i++ )
2026     {
2027         qsort( RT0->data.db + i*nimages, nimages, CV_ELEM_SIZE(RT0->type), dbCmp );
2028         solver.param->data.db[i] = nimages % 2 != 0 ? RT0->data.db[i*nimages + nimages/2] :
2029             (RT0->data.db[i*nimages + nimages/2 - 1] + RT0->data.db[i*nimages + nimages/2])*0.5;
2030     }
2031
2032     if( recomputeIntrinsics )
2033         for( k = 0; k < 2; k++ )
2034         {
2035             double* iparam = solver.param->data.db + (nimages+1)*6 + k*NINTRINSIC;
2036             if( flags & CV_CALIB_ZERO_TANGENT_DIST )
2037                 dk[k][2] = dk[k][3] = 0;
2038             iparam[0] = A[k][0]; iparam[1] = A[k][4]; iparam[2] = A[k][2]; iparam[3] = A[k][5];
2039             iparam[4] = dk[k][0]; iparam[5] = dk[k][1]; iparam[6] = dk[k][2];
2040             iparam[7] = dk[k][3]; iparam[8] = dk[k][4]; iparam[9] = dk[k][5];
2041             iparam[10] = dk[k][6]; iparam[11] = dk[k][7];
2042         }
2043
2044     om_LR = cvMat(3, 1, CV_64F, solver.param->data.db);
2045     T_LR = cvMat(3, 1, CV_64F, solver.param->data.db + 3);
2046
2047     for(;;)
2048     {
2049         const CvMat* param = 0;
2050         CvMat tmpimagePoints;
2051         CvMat *JtJ = 0, *JtErr = 0;
2052         double *_errNorm = 0;
2053         double _omR[3], _tR[3];
2054         double _dr3dr1[9], _dr3dr2[9], /*_dt3dr1[9],*/ _dt3dr2[9], _dt3dt1[9], _dt3dt2[9];
2055         CvMat dr3dr1 = cvMat(3, 3, CV_64F, _dr3dr1);
2056         CvMat dr3dr2 = cvMat(3, 3, CV_64F, _dr3dr2);
2057         //CvMat dt3dr1 = cvMat(3, 3, CV_64F, _dt3dr1);
2058         CvMat dt3dr2 = cvMat(3, 3, CV_64F, _dt3dr2);
2059         CvMat dt3dt1 = cvMat(3, 3, CV_64F, _dt3dt1);
2060         CvMat dt3dt2 = cvMat(3, 3, CV_64F, _dt3dt2);
2061         CvMat om[2], T[2], imgpt_i[2];
2062         CvMat dpdrot_hdr, dpdt_hdr, dpdf_hdr, dpdc_hdr, dpdk_hdr;
2063         CvMat *dpdrot = &dpdrot_hdr, *dpdt = &dpdt_hdr, *dpdf = 0, *dpdc = 0, *dpdk = 0;
2064
2065         if( !solver.updateAlt( param, JtJ, JtErr, _errNorm ))
2066             break;
2067         reprojErr = 0;
2068
2069         cvRodrigues2( &om_LR, &R_LR );
2070         om[1] = cvMat(3,1,CV_64F,_omR);
2071         T[1] = cvMat(3,1,CV_64F,_tR);
2072
2073         if( recomputeIntrinsics )
2074         {
2075             double* iparam = solver.param->data.db + (nimages+1)*6;
2076             double* ipparam = solver.prevParam->data.db + (nimages+1)*6;
2077             dpdf = &dpdf_hdr;
2078             dpdc = &dpdc_hdr;
2079             dpdk = &dpdk_hdr;
2080             if( flags & CV_CALIB_SAME_FOCAL_LENGTH )
2081             {
2082                 iparam[NINTRINSIC] = iparam[0];
2083                 iparam[NINTRINSIC+1] = iparam[1];
2084                 ipparam[NINTRINSIC] = ipparam[0];
2085                 ipparam[NINTRINSIC+1] = ipparam[1];
2086             }
2087             if( flags & CV_CALIB_FIX_ASPECT_RATIO )
2088             {
2089                 iparam[0] = iparam[1]*aspectRatio[0];
2090                 iparam[NINTRINSIC] = iparam[NINTRINSIC+1]*aspectRatio[1];
2091                 ipparam[0] = ipparam[1]*aspectRatio[0];
2092                 ipparam[NINTRINSIC] = ipparam[NINTRINSIC+1]*aspectRatio[1];
2093             }
2094             for( k = 0; k < 2; k++ )
2095             {
2096                 A[k][0] = iparam[k*NINTRINSIC+0];
2097                 A[k][4] = iparam[k*NINTRINSIC+1];
2098                 A[k][2] = iparam[k*NINTRINSIC+2];
2099                 A[k][5] = iparam[k*NINTRINSIC+3];
2100                 dk[k][0] = iparam[k*NINTRINSIC+4];
2101                 dk[k][1] = iparam[k*NINTRINSIC+5];
2102                 dk[k][2] = iparam[k*NINTRINSIC+6];
2103                 dk[k][3] = iparam[k*NINTRINSIC+7];
2104                 dk[k][4] = iparam[k*NINTRINSIC+8];
2105                 dk[k][5] = iparam[k*NINTRINSIC+9];
2106                 dk[k][6] = iparam[k*NINTRINSIC+10];
2107                 dk[k][7] = iparam[k*NINTRINSIC+11];
2108             }
2109         }
2110
2111         for( i = ofs = 0; i < nimages; ofs += ni, i++ )
2112         {
2113             ni = npoints->data.i[i];
2114             CvMat objpt_i, _part;
2115
2116             om[0] = cvMat(3,1,CV_64F,solver.param->data.db+(i+1)*6);
2117             T[0] = cvMat(3,1,CV_64F,solver.param->data.db+(i+1)*6+3);
2118
2119             if( JtJ || JtErr )
2120                 cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1], &dr3dr1, 0,
2121                              &dr3dr2, 0, 0, &dt3dt1, &dt3dr2, &dt3dt2 );
2122             else
2123                 cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1] );
2124
2125             objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3);
2126             err->rows = Je->rows = J_LR->rows = Ji->rows = ni*2;
2127             cvReshape( err, &tmpimagePoints, 2, 1 );
2128
2129             cvGetCols( Ji, &dpdf_hdr, 0, 2 );
2130             cvGetCols( Ji, &dpdc_hdr, 2, 4 );
2131             cvGetCols( Ji, &dpdk_hdr, 4, NINTRINSIC );
2132             cvGetCols( Je, &dpdrot_hdr, 0, 3 );
2133             cvGetCols( Je, &dpdt_hdr, 3, 6 );
2134
2135             for( k = 0; k < 2; k++ )
2136             {
2137                 double l2err;
2138                 imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2);
2139
2140                 if( JtJ || JtErr )
2141                     cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k],
2142                             &tmpimagePoints, dpdrot, dpdt, dpdf, dpdc, dpdk,
2143                             (flags & CV_CALIB_FIX_ASPECT_RATIO) ? aspectRatio[k] : 0);
2144                 else
2145                     cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k], &tmpimagePoints );
2146                 cvSub( &tmpimagePoints, &imgpt_i[k], &tmpimagePoints );
2147
2148                 l2err = cvNorm( &tmpimagePoints, 0, CV_L2 );
2149
2150                 if( JtJ || JtErr )
2151                 {
2152                     int iofs = (nimages+1)*6 + k*NINTRINSIC, eofs = (i+1)*6;
2153                     assert( JtJ && JtErr );
2154
2155                     if( k == 1 )
2156                     {
2157                         // d(err_{x|y}R) ~ de3
2158                         // convert de3/{dr3,dt3} => de3{dr1,dt1} & de3{dr2,dt2}
2159                         for( p = 0; p < ni*2; p++ )
2160                         {
2161                             CvMat de3dr3 = cvMat( 1, 3, CV_64F, Je->data.ptr + Je->step*p );
2162                             CvMat de3dt3 = cvMat( 1, 3, CV_64F, de3dr3.data.db + 3 );
2163                             CvMat de3dr2 = cvMat( 1, 3, CV_64F, J_LR->data.ptr + J_LR->step*p );
2164                             CvMat de3dt2 = cvMat( 1, 3, CV_64F, de3dr2.data.db + 3 );
2165                             double _de3dr1[3], _de3dt1[3];
2166                             CvMat de3dr1 = cvMat( 1, 3, CV_64F, _de3dr1 );
2167                             CvMat de3dt1 = cvMat( 1, 3, CV_64F, _de3dt1 );
2168
2169                             cvMatMul( &de3dr3, &dr3dr1, &de3dr1 );
2170                             cvMatMul( &de3dt3, &dt3dt1, &de3dt1 );
2171
2172                             cvMatMul( &de3dr3, &dr3dr2, &de3dr2 );
2173                             cvMatMulAdd( &de3dt3, &dt3dr2, &de3dr2, &de3dr2 );
2174
2175                             cvMatMul( &de3dt3, &dt3dt2, &de3dt2 );
2176
2177                             cvCopy( &de3dr1, &de3dr3 );
2178                             cvCopy( &de3dt1, &de3dt3 );
2179                         }
2180
2181                         cvGetSubRect( JtJ, &_part, cvRect(0, 0, 6, 6) );
2182                         cvGEMM( J_LR, J_LR, 1, &_part, 1, &_part, CV_GEMM_A_T );
2183
2184                         cvGetSubRect( JtJ, &_part, cvRect(eofs, 0, 6, 6) );
2185                         cvGEMM( J_LR, Je, 1, 0, 0, &_part, CV_GEMM_A_T );
2186
2187                         cvGetRows( JtErr, &_part, 0, 6 );
2188                         cvGEMM( J_LR, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
2189                     }
2190
2191                     cvGetSubRect( JtJ, &_part, cvRect(eofs, eofs, 6, 6) );
2192                     cvGEMM( Je, Je, 1, &_part, 1, &_part, CV_GEMM_A_T );
2193
2194                     cvGetRows( JtErr, &_part, eofs, eofs + 6 );
2195                     cvGEMM( Je, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
2196
2197                     if( recomputeIntrinsics )
2198                     {
2199                         cvGetSubRect( JtJ, &_part, cvRect(iofs, iofs, NINTRINSIC, NINTRINSIC) );
2200                         cvGEMM( Ji, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
2201                         cvGetSubRect( JtJ, &_part, cvRect(iofs, eofs, NINTRINSIC, 6) );
2202                         cvGEMM( Je, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
2203                         if( k == 1 )
2204                         {
2205                             cvGetSubRect( JtJ, &_part, cvRect(iofs, 0, NINTRINSIC, 6) );
2206                             cvGEMM( J_LR, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T );
2207                         }
2208                         cvGetRows( JtErr, &_part, iofs, iofs + NINTRINSIC );
2209                         cvGEMM( Ji, err, 1, &_part, 1, &_part, CV_GEMM_A_T );
2210                     }
2211                 }
2212
2213                 reprojErr += l2err*l2err;
2214             }
2215         }
2216         if(_errNorm)
2217             *_errNorm = reprojErr;
2218     }
2219
2220     cvRodrigues2( &om_LR, &R_LR );
2221     if( matR->rows == 1 || matR->cols == 1 )
2222         cvConvert( &om_LR, matR );
2223     else
2224         cvConvert( &R_LR, matR );
2225     cvConvert( &T_LR, matT );
2226
2227     if( recomputeIntrinsics )
2228     {
2229         cvConvert( &K[0], _cameraMatrix1 );
2230         cvConvert( &K[1], _cameraMatrix2 );
2231
2232         for( k = 0; k < 2; k++ )
2233         {
2234             CvMat* distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2;
2235             CvMat tdist = cvMat( distCoeffs->rows, distCoeffs->cols,
2236                 CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), Dist[k].data.db );
2237             cvConvert( &tdist, distCoeffs );
2238         }
2239     }
2240
2241     if( matE || matF )
2242     {
2243         double* t = T_LR.data.db;
2244         double tx[] =
2245         {
2246             0, -t[2], t[1],
2247             t[2], 0, -t[0],
2248             -t[1], t[0], 0
2249         };
2250         CvMat Tx = cvMat(3, 3, CV_64F, tx);
2251         double e[9], f[9];
2252         CvMat E = cvMat(3, 3, CV_64F, e);
2253         CvMat F = cvMat(3, 3, CV_64F, f);
2254         cvMatMul( &Tx, &R_LR, &E );
2255         if( matE )
2256             cvConvert( &E, matE );
2257         if( matF )
2258         {
2259             double ik[9];
2260             CvMat iK = cvMat(3, 3, CV_64F, ik);
2261             cvInvert(&K[1], &iK);
2262             cvGEMM( &iK, &E, 1, 0, 0, &E, CV_GEMM_A_T );
2263             cvInvert(&K[0], &iK);
2264             cvMatMul(&E, &iK, &F);
2265             cvConvertScale( &F, matF, fabs(f[8]) > 0 ? 1./f[8] : 1 );
2266         }
2267     }
2268
2269     return std::sqrt(reprojErr/(pointsTotal*2));
2270 }
2271
2272
2273 static void
2274 icvGetRectangles( const CvMat* cameraMatrix, const CvMat* distCoeffs,
2275                  const CvMat* R, const CvMat* newCameraMatrix, CvSize imgSize,
2276                  cv::Rect_<float>& inner, cv::Rect_<float>& outer )
2277 {
2278     const int N = 9;
2279     int x, y, k;
2280     cv::Ptr<CvMat> _pts = cvCreateMat(1, N*N, CV_32FC2);
2281     CvPoint2D32f* pts = (CvPoint2D32f*)(_pts->data.ptr);
2282
2283     for( y = k = 0; y < N; y++ )
2284         for( x = 0; x < N; x++ )
2285             pts[k++] = cvPoint2D32f((float)x*imgSize.width/(N-1),
2286                                     (float)y*imgSize.height/(N-1));
2287
2288     cvUndistortPoints(_pts, _pts, cameraMatrix, distCoeffs, R, newCameraMatrix);
2289
2290     float iX0=-FLT_MAX, iX1=FLT_MAX, iY0=-FLT_MAX, iY1=FLT_MAX;
2291     float oX0=FLT_MAX, oX1=-FLT_MAX, oY0=FLT_MAX, oY1=-FLT_MAX;
2292     // find the inscribed rectangle.
2293     // the code will likely not work with extreme rotation matrices (R) (>45%)
2294     for( y = k = 0; y < N; y++ )
2295         for( x = 0; x < N; x++ )
2296         {
2297             CvPoint2D32f p = pts[k++];
2298             oX0 = MIN(oX0, p.x);
2299             oX1 = MAX(oX1, p.x);
2300             oY0 = MIN(oY0, p.y);
2301             oY1 = MAX(oY1, p.y);
2302
2303             if( x == 0 )
2304                 iX0 = MAX(iX0, p.x);
2305             if( x == N-1 )
2306                 iX1 = MIN(iX1, p.x);
2307             if( y == 0 )
2308                 iY0 = MAX(iY0, p.y);
2309             if( y == N-1 )
2310                 iY1 = MIN(iY1, p.y);
2311         }
2312     inner = cv::Rect_<float>(iX0, iY0, iX1-iX0, iY1-iY0);
2313     outer = cv::Rect_<float>(oX0, oY0, oX1-oX0, oY1-oY0);
2314 }
2315
2316
2317 void cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2,
2318                       const CvMat* _distCoeffs1, const CvMat* _distCoeffs2,
2319                       CvSize imageSize, const CvMat* matR, const CvMat* matT,
2320                       CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2,
2321                       CvMat* matQ, int flags, double alpha, CvSize newImgSize,
2322                       CvRect* roi1, CvRect* roi2 )
2323 {
2324     double _om[3], _t[3], _uu[3]={0,0,0}, _r_r[3][3], _pp[3][4];
2325     double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3];
2326     cv::Rect_<float> inner1, inner2, outer1, outer2;
2327
2328     CvMat om  = cvMat(3, 1, CV_64F, _om);
2329     CvMat t   = cvMat(3, 1, CV_64F, _t);
2330     CvMat uu  = cvMat(3, 1, CV_64F, _uu);
2331     CvMat r_r = cvMat(3, 3, CV_64F, _r_r);
2332     CvMat pp  = cvMat(3, 4, CV_64F, _pp);
2333     CvMat ww  = cvMat(3, 1, CV_64F, _ww); // temps
2334     CvMat wR  = cvMat(3, 3, CV_64F, _wr);
2335     CvMat Z   = cvMat(3, 1, CV_64F, _z);
2336     CvMat Ri  = cvMat(3, 3, CV_64F, _ri);
2337     double nx = imageSize.width, ny = imageSize.height;
2338     int i, k;
2339
2340     if( matR->rows == 3 && matR->cols == 3 )
2341         cvRodrigues2(matR, &om);          // get vector rotation
2342     else
2343         cvConvert(matR, &om); // it's already a rotation vector
2344     cvConvertScale(&om, &om, -0.5); // get average rotation
2345     cvRodrigues2(&om, &r_r);        // rotate cameras to same orientation by averaging
2346     cvMatMul(&r_r, matT, &t);
2347
2348     int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1;
2349     double c = _t[idx], nt = cvNorm(&t, 0, CV_L2);
2350     _uu[idx] = c > 0 ? 1 : -1;
2351
2352     // calculate global Z rotation
2353     cvCrossProduct(&t,&uu,&ww);
2354     double nw = cvNorm(&ww, 0, CV_L2);
2355     cvConvertScale(&ww, &ww, acos(fabs(c)/nt)/nw);
2356     cvRodrigues2(&ww, &wR);
2357
2358     // apply to both views
2359     cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T);
2360     cvConvert( &Ri, _R1 );
2361     cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, 0);
2362     cvConvert( &Ri, _R2 );
2363     cvMatMul(&Ri, matT, &t);
2364
2365     // calculate projection/camera matrices
2366     // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy)
2367     double fc_new = DBL_MAX;
2368     CvPoint2D64f cc_new[2] = {{0,0}, {0,0}};
2369
2370     for( k = 0; k < 2; k++ ) {
2371         const CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
2372         const CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2;
2373         double dk1 = Dk ? cvmGet(Dk, 0, 0) : 0;
2374         double fc = cvmGet(A,idx^1,idx^1);
2375         if( dk1 < 0 ) {
2376             fc *= 1 + dk1*(nx*nx + ny*ny)/(4*fc*fc);
2377         }
2378         fc_new = MIN(fc_new, fc);
2379     }
2380
2381     for( k = 0; k < 2; k++ )
2382     {
2383         const CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
2384         const CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2;
2385         CvPoint2D32f _pts[4];
2386         CvPoint3D32f _pts_3[4];
2387         CvMat pts = cvMat(1, 4, CV_32FC2, _pts);
2388         CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3);
2389
2390         for( i = 0; i < 4; i++ )
2391         {
2392             int j = (i<2) ? 0 : 1;
2393             _pts[i].x = (float)((i % 2)*(nx-1));
2394             _pts[i].y = (float)(j*(ny-1));
2395         }
2396         cvUndistortPoints( &pts, &pts, A, Dk, 0, 0 );
2397         cvConvertPointsHomogeneous( &pts, &pts_3 );
2398
2399         //Change camera matrix to have cc=[0,0] and fc = fc_new
2400         double _a_tmp[3][3];
2401         CvMat A_tmp  = cvMat(3, 3, CV_64F, _a_tmp);
2402         _a_tmp[0][0]=fc_new;
2403         _a_tmp[1][1]=fc_new;
2404         _a_tmp[0][2]=0.0;
2405         _a_tmp[1][2]=0.0;
2406         cvProjectPoints2( &pts_3, k == 0 ? _R1 : _R2, &Z, &A_tmp, 0, &pts );
2407         CvScalar avg = cvAvg(&pts);
2408         cc_new[k].x = (nx-1)/2 - avg.val[0];
2409         cc_new[k].y = (ny-1)/2 - avg.val[1];
2410     }
2411
2412     // vertical focal length must be the same for both images to keep the epipolar constraint
2413     // (for horizontal epipolar lines -- TBD: check for vertical epipolar lines)
2414     // use fy for fx also, for simplicity
2415
2416     // For simplicity, set the principal points for both cameras to be the average
2417     // of the two principal points (either one of or both x- and y- coordinates)
2418     if( flags & CV_CALIB_ZERO_DISPARITY )
2419     {
2420         cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;
2421         cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
2422     }
2423     else if( idx == 0 ) // horizontal stereo
2424         cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
2425     else // vertical stereo
2426         cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;
2427
2428     cvZero( &pp );
2429     _pp[0][0] = _pp[1][1] = fc_new;
2430     _pp[0][2] = cc_new[0].x;
2431     _pp[1][2] = cc_new[0].y;
2432     _pp[2][2] = 1;
2433     cvConvert(&pp, _P1);
2434
2435     _pp[0][2] = cc_new[1].x;
2436     _pp[1][2] = cc_new[1].y;
2437     _pp[idx][3] = _t[idx]*fc_new; // baseline * focal length
2438     cvConvert(&pp, _P2);
2439
2440     alpha = MIN(alpha, 1.);
2441
2442     icvGetRectangles( _cameraMatrix1, _distCoeffs1, _R1, _P1, imageSize, inner1, outer1 );
2443     icvGetRectangles( _cameraMatrix2, _distCoeffs2, _R2, _P2, imageSize, inner2, outer2 );
2444
2445     {
2446     newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imageSize;
2447     double cx1_0 = cc_new[0].x;
2448     double cy1_0 = cc_new[0].y;
2449     double cx2_0 = cc_new[1].x;
2450     double cy2_0 = cc_new[1].y;
2451     double cx1 = newImgSize.width*cx1_0/imageSize.width;
2452     double cy1 = newImgSize.height*cy1_0/imageSize.height;
2453     double cx2 = newImgSize.width*cx2_0/imageSize.width;
2454     double cy2 = newImgSize.height*cy2_0/imageSize.height;
2455     double s = 1.;
2456
2457     if( alpha >= 0 )
2458     {
2459         double s0 = std::max(std::max(std::max((double)cx1/(cx1_0 - inner1.x), (double)cy1/(cy1_0 - inner1.y)),
2460                             (double)(newImgSize.width - cx1)/(inner1.x + inner1.width - cx1_0)),
2461                         (double)(newImgSize.height - cy1)/(inner1.y + inner1.height - cy1_0));
2462         s0 = std::max(std::max(std::max(std::max((double)cx2/(cx2_0 - inner2.x), (double)cy2/(cy2_0 - inner2.y)),
2463                          (double)(newImgSize.width - cx2)/(inner2.x + inner2.width - cx2_0)),
2464                      (double)(newImgSize.height - cy2)/(inner2.y + inner2.height - cy2_0)),
2465                  s0);
2466
2467         double s1 = std::min(std::min(std::min((double)cx1/(cx1_0 - outer1.x), (double)cy1/(cy1_0 - outer1.y)),
2468                             (double)(newImgSize.width - cx1)/(outer1.x + outer1.width - cx1_0)),
2469                         (double)(newImgSize.height - cy1)/(outer1.y + outer1.height - cy1_0));
2470         s1 = std::min(std::min(std::min(std::min((double)cx2/(cx2_0 - outer2.x), (double)cy2/(cy2_0 - outer2.y)),
2471                          (double)(newImgSize.width - cx2)/(outer2.x + outer2.width - cx2_0)),
2472                      (double)(newImgSize.height - cy2)/(outer2.y + outer2.height - cy2_0)),
2473                  s1);
2474
2475         s = s0*(1 - alpha) + s1*alpha;
2476     }
2477
2478     fc_new *= s;
2479     cc_new[0] = cvPoint2D64f(cx1, cy1);
2480     cc_new[1] = cvPoint2D64f(cx2, cy2);
2481
2482     cvmSet(_P1, 0, 0, fc_new);
2483     cvmSet(_P1, 1, 1, fc_new);
2484     cvmSet(_P1, 0, 2, cx1);
2485     cvmSet(_P1, 1, 2, cy1);
2486
2487     cvmSet(_P2, 0, 0, fc_new);
2488     cvmSet(_P2, 1, 1, fc_new);
2489     cvmSet(_P2, 0, 2, cx2);
2490     cvmSet(_P2, 1, 2, cy2);
2491     cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3));
2492
2493     if(roi1)
2494     {
2495         *roi1 = cv::Rect(cvCeil((inner1.x - cx1_0)*s + cx1),
2496                      cvCeil((inner1.y - cy1_0)*s + cy1),
2497                      cvFloor(inner1.width*s), cvFloor(inner1.height*s))
2498             & cv::Rect(0, 0, newImgSize.width, newImgSize.height);
2499     }
2500
2501     if(roi2)
2502     {
2503         *roi2 = cv::Rect(cvCeil((inner2.x - cx2_0)*s + cx2),
2504                      cvCeil((inner2.y - cy2_0)*s + cy2),
2505                      cvFloor(inner2.width*s), cvFloor(inner2.height*s))
2506             & cv::Rect(0, 0, newImgSize.width, newImgSize.height);
2507     }
2508     }
2509
2510     if( matQ )
2511     {
2512         double q[] =
2513         {
2514             1, 0, 0, -cc_new[0].x,
2515             0, 1, 0, -cc_new[0].y,
2516             0, 0, 0, fc_new,
2517             0, 0, 1./_t[idx],
2518             (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/_t[idx]
2519         };
2520         CvMat Q = cvMat(4, 4, CV_64F, q);
2521         cvConvert( &Q, matQ );
2522     }
2523 }
2524
2525
2526 void cvGetOptimalNewCameraMatrix( const CvMat* cameraMatrix, const CvMat* distCoeffs,
2527                                   CvSize imgSize, double alpha,
2528                                   CvMat* newCameraMatrix, CvSize newImgSize,
2529                                   CvRect* validPixROI, int centerPrincipalPoint )
2530 {
2531     cv::Rect_<float> inner, outer;
2532     newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imgSize;
2533
2534     double M[3][3];
2535     CvMat matM = cvMat(3, 3, CV_64F, M);
2536     cvConvert(cameraMatrix, &matM);
2537
2538     if( centerPrincipalPoint )
2539     {
2540         double cx0 = M[0][2];
2541         double cy0 = M[1][2];
2542         double cx = (newImgSize.width-1)*0.5;
2543         double cy = (newImgSize.height-1)*0.5;
2544
2545         icvGetRectangles( cameraMatrix, distCoeffs, 0, cameraMatrix, imgSize, inner, outer );
2546         double s0 = std::max(std::max(std::max((double)cx/(cx0 - inner.x), (double)cy/(cy0 - inner.y)),
2547                                       (double)cx/(inner.x + inner.width - cx0)),
2548                              (double)cy/(inner.y + inner.height - cy0));
2549         double s1 = std::min(std::min(std::min((double)cx/(cx0 - outer.x), (double)cy/(cy0 - outer.y)),
2550                                       (double)cx/(outer.x + outer.width - cx0)),
2551                              (double)cy/(outer.y + outer.height - cy0));
2552         double s = s0*(1 - alpha) + s1*alpha;
2553
2554         M[0][0] *= s;
2555         M[1][1] *= s;
2556         M[0][2] = cx;
2557         M[1][2] = cy;
2558
2559         if( validPixROI )
2560         {
2561             inner = cv::Rect_<float>((float)((inner.x - cx0)*s + cx),
2562                                      (float)((inner.y - cy0)*s + cy),
2563                                      (float)(inner.width*s),
2564                                      (float)(inner.height*s));
2565             cv::Rect r(cvCeil(inner.x), cvCeil(inner.y), cvFloor(inner.width), cvFloor(inner.height));
2566             r &= cv::Rect(0, 0, newImgSize.width, newImgSize.height);
2567             *validPixROI = r;
2568         }
2569     }
2570     else
2571     {
2572         // Get inscribed and circumscribed rectangles in normalized
2573         // (independent of camera matrix) coordinates
2574         icvGetRectangles( cameraMatrix, distCoeffs, 0, 0, imgSize, inner, outer );
2575
2576         // Projection mapping inner rectangle to viewport
2577         double fx0 = (newImgSize.width  - 1) / inner.width;
2578         double fy0 = (newImgSize.height - 1) / inner.height;
2579         double cx0 = -fx0 * inner.x;
2580         double cy0 = -fy0 * inner.y;
2581
2582         // Projection mapping outer rectangle to viewport
2583         double fx1 = (newImgSize.width  - 1) / outer.width;
2584         double fy1 = (newImgSize.height - 1) / outer.height;
2585         double cx1 = -fx1 * outer.x;
2586         double cy1 = -fy1 * outer.y;
2587
2588         // Interpolate between the two optimal projections
2589         M[0][0] = fx0*(1 - alpha) + fx1*alpha;
2590         M[1][1] = fy0*(1 - alpha) + fy1*alpha;
2591         M[0][2] = cx0*(1 - alpha) + cx1*alpha;
2592         M[1][2] = cy0*(1 - alpha) + cy1*alpha;
2593
2594         if( validPixROI )
2595         {
2596             icvGetRectangles( cameraMatrix, distCoeffs, 0, newCameraMatrix, imgSize, inner, outer );
2597             cv::Rect r = inner;
2598             r &= cv::Rect(0, 0, newImgSize.width, newImgSize.height);
2599             *validPixROI = r;
2600         }
2601     }
2602
2603     cvConvert(&matM, newCameraMatrix);
2604 }
2605
2606
2607 CV_IMPL int cvStereoRectifyUncalibrated(
2608     const CvMat* _points1, const CvMat* _points2,
2609     const CvMat* F0, CvSize imgSize,
2610     CvMat* _H1, CvMat* _H2, double threshold )
2611 {
2612     Ptr<CvMat> _m1, _m2, _lines1, _lines2;
2613
2614     int i, j, npoints;
2615     double cx, cy;
2616     double u[9], v[9], w[9], f[9], h1[9], h2[9], h0[9], e2[3];
2617     CvMat E2 = cvMat( 3, 1, CV_64F, e2 );
2618     CvMat U = cvMat( 3, 3, CV_64F, u );
2619     CvMat V = cvMat( 3, 3, CV_64F, v );
2620     CvMat W = cvMat( 3, 3, CV_64F, w );
2621     CvMat F = cvMat( 3, 3, CV_64F, f );
2622     CvMat H1 = cvMat( 3, 3, CV_64F, h1 );
2623     CvMat H2 = cvMat( 3, 3, CV_64F, h2 );
2624     CvMat H0 = cvMat( 3, 3, CV_64F, h0 );
2625
2626     CvPoint2D64f* m1;
2627     CvPoint2D64f* m2;
2628     CvPoint3D64f* lines1;
2629     CvPoint3D64f* lines2;
2630
2631     CV_Assert( CV_IS_MAT(_points1) && CV_IS_MAT(_points2) &&
2632         (_points1->rows == 1 || _points1->cols == 1) &&
2633         (_points2->rows == 1 || _points2->cols == 1) &&
2634         CV_ARE_SIZES_EQ(_points1, _points2) );
2635
2636     npoints = _points1->rows * _points1->cols * CV_MAT_CN(_points1->type) / 2;
2637
2638     _m1 = cvCreateMat( _points1->rows, _points1->cols, CV_64FC(CV_MAT_CN(_points1->type)) );
2639     _m2 = cvCreateMat( _points2->rows, _points2->cols, CV_64FC(CV_MAT_CN(_points2->type)) );
2640     _lines1 = cvCreateMat( 1, npoints, CV_64FC3 );
2641     _lines2 = cvCreateMat( 1, npoints, CV_64FC3 );
2642
2643     cvConvert( F0, &F );
2644
2645     cvSVD( (CvMat*)&F, &W, &U, &V, CV_SVD_U_T + CV_SVD_V_T );
2646     W.data.db[8] = 0.;
2647     cvGEMM( &U, &W, 1, 0, 0, &W, CV_GEMM_A_T );
2648     cvMatMul( &W, &V, &F );
2649
2650     cx = cvRound( (imgSize.width-1)*0.5 );
2651     cy = cvRound( (imgSize.height-1)*0.5 );
2652
2653     cvZero( _H1 );
2654     cvZero( _H2 );
2655
2656     cvConvert( _points1, _m1 );
2657     cvConvert( _points2, _m2 );
2658     cvReshape( _m1, _m1, 2, 1 );
2659     cvReshape( _m2, _m2, 2, 1 );
2660
2661     m1 = (CvPoint2D64f*)_m1->data.ptr;
2662     m2 = (CvPoint2D64f*)_m2->data.ptr;
2663     lines1 = (CvPoint3D64f*)_lines1->data.ptr;
2664     lines2 = (CvPoint3D64f*)_lines2->data.ptr;
2665
2666     if( threshold > 0 )
2667     {
2668         cvComputeCorrespondEpilines( _m1, 1, &F, _lines1 );
2669         cvComputeCorrespondEpilines( _m2, 2, &F, _lines2 );
2670
2671         // measure distance from points to the corresponding epilines, mark outliers
2672         for( i = j = 0; i < npoints; i++ )
2673         {
2674             if( fabs(m1[i].x*lines2[i].x +
2675                      m1[i].y*lines2[i].y +
2676                      lines2[i].z) <= threshold &&
2677                 fabs(m2[i].x*lines1[i].x +
2678                      m2[i].y*lines1[i].y +
2679                      lines1[i].z) <= threshold )
2680             {
2681                 if( j > i )
2682                 {
2683                     m1[j] = m1[i];
2684                     m2[j] = m2[i];
2685                 }
2686                 j++;
2687             }
2688         }
2689
2690         npoints = j;
2691         if( npoints == 0 )
2692             return 0;
2693     }
2694
2695     _m1->cols = _m2->cols = npoints;
2696     memcpy( E2.data.db, U.data.db + 6, sizeof(e2));
2697     cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 );
2698
2699     double t[] =
2700     {
2701         1, 0, -cx,
2702         0, 1, -cy,
2703         0, 0, 1
2704     };
2705     CvMat T = cvMat(3, 3, CV_64F, t);
2706     cvMatMul( &T, &E2, &E2 );
2707
2708     int mirror = e2[0] < 0;
2709     double d = MAX(sqrt(e2[0]*e2[0] + e2[1]*e2[1]),DBL_EPSILON);
2710     double alpha = e2[0]/d;
2711     double beta = e2[1]/d;
2712     double r[] =
2713     {
2714         alpha, beta, 0,
2715         -beta, alpha, 0,
2716         0, 0, 1
2717     };
2718     CvMat R = cvMat(3, 3, CV_64F, r);
2719     cvMatMul( &R, &T, &T );
2720     cvMatMul( &R, &E2, &E2 );
2721     double invf = fabs(e2[2]) < 1e-6*fabs(e2[0]) ? 0 : -e2[2]/e2[0];
2722     double k[] =
2723     {
2724         1, 0, 0,
2725         0, 1, 0,
2726         invf, 0, 1
2727     };
2728     CvMat K = cvMat(3, 3, CV_64F, k);
2729     cvMatMul( &K, &T, &H2 );
2730     cvMatMul( &K, &E2, &E2 );
2731
2732     double it[] =
2733     {
2734         1, 0, cx,
2735         0, 1, cy,
2736         0, 0, 1
2737     };
2738     CvMat iT = cvMat( 3, 3, CV_64F, it );
2739     cvMatMul( &iT, &H2, &H2 );
2740
2741     memcpy( E2.data.db, U.data.db + 6, sizeof(e2));
2742     cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 );
2743
2744     double e2_x[] =
2745     {
2746         0, -e2[2], e2[1],
2747        e2[2], 0, -e2[0],
2748        -e2[1], e2[0], 0
2749     };
2750     double e2_111[] =
2751     {
2752         e2[0], e2[0], e2[0],
2753         e2[1], e2[1], e2[1],
2754         e2[2], e2[2], e2[2],
2755     };
2756     CvMat E2_x = cvMat(3, 3, CV_64F, e2_x);
2757     CvMat E2_111 = cvMat(3, 3, CV_64F, e2_111);
2758     cvMatMulAdd(&E2_x, &F, &E2_111, &H0 );
2759     cvMatMul(&H2, &H0, &H0);
2760     CvMat E1=cvMat(3, 1, CV_64F, V.data.db+6);
2761     cvMatMul(&H0, &E1, &E1);
2762
2763     cvPerspectiveTransform( _m1, _m1, &H0 );
2764     cvPerspectiveTransform( _m2, _m2, &H2 );
2765     CvMat A = cvMat( 1, npoints, CV_64FC3, lines1 ), BxBy, B;
2766     double a[9], atb[3], x[3];
2767     CvMat AtA = cvMat( 3, 3, CV_64F, a );
2768     CvMat AtB = cvMat( 3, 1, CV_64F, atb );
2769     CvMat X = cvMat( 3, 1, CV_64F, x );
2770     cvConvertPointsHomogeneous( _m1, &A );
2771     cvReshape( &A, &A, 1, npoints );
2772     cvReshape( _m2, &BxBy, 1, npoints );
2773     cvGetCol( &BxBy, &B, 0 );
2774     cvGEMM( &A, &A, 1, 0, 0, &AtA, CV_GEMM_A_T );
2775     cvGEMM( &A, &B, 1, 0, 0, &AtB, CV_GEMM_A_T );
2776     cvSolve( &AtA, &AtB, &X, CV_SVD_SYM );
2777
2778     double ha[] =
2779     {
2780         x[0], x[1], x[2],
2781         0, 1, 0,
2782         0, 0, 1
2783     };
2784     CvMat Ha = cvMat(3, 3, CV_64F, ha);
2785     cvMatMul( &Ha, &H0, &H1 );
2786     cvPerspectiveTransform( _m1, _m1, &Ha );
2787
2788     if( mirror )
2789     {
2790         double mm[] = { -1, 0, cx*2, 0, -1, cy*2, 0, 0, 1 };
2791         CvMat MM = cvMat(3, 3, CV_64F, mm);
2792         cvMatMul( &MM, &H1, &H1 );
2793         cvMatMul( &MM, &H2, &H2 );
2794     }
2795
2796     cvConvert( &H1, _H1 );
2797     cvConvert( &H2, _H2 );
2798
2799     return 1;
2800 }
2801
2802
2803 void cv::reprojectImageTo3D( InputArray _disparity,
2804                              OutputArray __3dImage, InputArray _Qmat,
2805                              bool handleMissingValues, int dtype )
2806 {
2807     Mat disparity = _disparity.getMat(), Q = _Qmat.getMat();
2808     int stype = disparity.type();
2809
2810     CV_Assert( stype == CV_8UC1 || stype == CV_16SC1 ||
2811                stype == CV_32SC1 || stype == CV_32FC1 );
2812     CV_Assert( Q.size() == Size(4,4) );
2813
2814     if( dtype < 0 )
2815         dtype = CV_32FC3;
2816     else
2817     {
2818         dtype = CV_MAKETYPE(CV_MAT_DEPTH(dtype), 3);
2819         CV_Assert( dtype == CV_16SC3 || dtype == CV_32SC3 || dtype == CV_32FC3 );
2820     }
2821
2822     __3dImage.create(disparity.size(), CV_MAKETYPE(dtype, 3));
2823     Mat _3dImage = __3dImage.getMat();
2824
2825     const double bigZ = 10000.;
2826     double q[4][4];
2827     Mat _Q(4, 4, CV_64F, q);
2828     Q.convertTo(_Q, CV_64F);
2829
2830     int x, cols = disparity.cols;
2831     CV_Assert( cols >= 0 );
2832
2833     vector<float> _sbuf(cols+1), _dbuf(cols*3+1);
2834     float* sbuf = &_sbuf[0], *dbuf = &_dbuf[0];
2835     double minDisparity = FLT_MAX;
2836
2837     // NOTE: here we quietly assume that at least one pixel in the disparity map is not defined.
2838     // and we set the corresponding Z's to some fixed big value.
2839     if( handleMissingValues )
2840         cv::minMaxIdx( disparity, &minDisparity, 0, 0, 0 );
2841
2842     for( int y = 0; y < disparity.rows; y++ )
2843     {
2844         float *sptr = sbuf, *dptr = dbuf;
2845         double qx = q[0][1]*y + q[0][3], qy = q[1][1]*y + q[1][3];
2846         double qz = q[2][1]*y + q[2][3], qw = q[3][1]*y + q[3][3];
2847
2848         if( stype == CV_8UC1 )
2849         {
2850             const uchar* sptr0 = disparity.ptr<uchar>(y);
2851             for( x = 0; x < cols; x++ )
2852                 sptr[x] = (float)sptr0[x];
2853         }
2854         else if( stype == CV_16SC1 )
2855         {
2856             const short* sptr0 = disparity.ptr<short>(y);
2857             for( x = 0; x < cols; x++ )
2858                 sptr[x] = (float)sptr0[x];
2859         }
2860         else if( stype == CV_32SC1 )
2861         {
2862             const int* sptr0 = disparity.ptr<int>(y);
2863             for( x = 0; x < cols; x++ )
2864                 sptr[x] = (float)sptr0[x];
2865         }
2866         else
2867             sptr = (float*)disparity.ptr<float>(y);
2868
2869         if( dtype == CV_32FC3 )
2870             dptr = _3dImage.ptr<float>(y);
2871
2872         for( x = 0; x < cols; x++, qx += q[0][0], qy += q[1][0], qz += q[2][0], qw += q[3][0] )
2873         {
2874             double d = sptr[x];
2875             double iW = 1./(qw + q[3][2]*d);
2876             double X = (qx + q[0][2]*d)*iW;
2877             double Y = (qy + q[1][2]*d)*iW;
2878             double Z = (qz + q[2][2]*d)*iW;
2879             if( fabs(d-minDisparity) <= FLT_EPSILON )
2880                 Z = bigZ;
2881
2882             dptr[x*3] = (float)X;
2883             dptr[x*3+1] = (float)Y;
2884             dptr[x*3+2] = (float)Z;
2885         }
2886
2887         if( dtype == CV_16SC3 )
2888         {
2889             short* dptr0 = _3dImage.ptr<short>(y);
2890             for( x = 0; x < cols*3; x++ )
2891             {
2892                 int ival = cvRound(dptr[x]);
2893                 dptr0[x] = CV_CAST_16S(ival);
2894             }
2895         }
2896         else if( dtype == CV_32SC3 )
2897         {
2898             int* dptr0 = _3dImage.ptr<int>(y);
2899             for( x = 0; x < cols*3; x++ )
2900             {
2901                 int ival = cvRound(dptr[x]);
2902                 dptr0[x] = ival;
2903             }
2904         }
2905     }
2906 }
2907
2908
2909 void cvReprojectImageTo3D( const CvArr* disparityImage,
2910                            CvArr* _3dImage, const CvMat* matQ,
2911                            int handleMissingValues )
2912 {
2913     cv::Mat disp = cv::cvarrToMat(disparityImage);
2914     cv::Mat _3dimg = cv::cvarrToMat(_3dImage);
2915     cv::Mat mq = cv::cvarrToMat(matQ);
2916     CV_Assert( disp.size() == _3dimg.size() );
2917     int dtype = _3dimg.type();
2918     CV_Assert( dtype == CV_16SC3 || dtype == CV_32SC3 || dtype == CV_32FC3 );
2919
2920     cv::reprojectImageTo3D(disp, _3dimg, mq, handleMissingValues != 0, dtype );
2921 }
2922
2923
2924 CV_IMPL void
2925 cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ,
2926                CvMat *matrixQx, CvMat *matrixQy, CvMat *matrixQz,
2927                CvPoint3D64f *eulerAngles)
2928 {
2929     double matM[3][3], matR[3][3], matQ[3][3];
2930     CvMat M = cvMat(3, 3, CV_64F, matM);
2931     CvMat R = cvMat(3, 3, CV_64F, matR);
2932     CvMat Q = cvMat(3, 3, CV_64F, matQ);
2933     double z, c, s;
2934
2935     /* Validate parameters. */
2936     CV_Assert( CV_IS_MAT(matrixM) && CV_IS_MAT(matrixR) && CV_IS_MAT(matrixQ) &&
2937         matrixM->cols == 3 && matrixM->rows == 3 &&
2938         CV_ARE_SIZES_EQ(matrixM, matrixR) && CV_ARE_SIZES_EQ(matrixM, matrixQ));
2939
2940     cvConvert(matrixM, &M);
2941
2942     /* Find Givens rotation Q_x for x axis (left multiplication). */
2943     /*
2944          ( 1  0  0 )
2945     Qx = ( 0  c  s ), c = m33/sqrt(m32^2 + m33^2), s = m32/sqrt(m32^2 + m33^2)
2946          ( 0 -s  c )
2947     */
2948     s = matM[2][1];
2949     c = matM[2][2];
2950     z = 1./sqrt(c * c + s * s + DBL_EPSILON);
2951     c *= z;
2952     s *= z;
2953
2954     double _Qx[3][3] = { {1, 0, 0}, {0, c, s}, {0, -s, c} };
2955     CvMat Qx = cvMat(3, 3, CV_64F, _Qx);
2956
2957     cvMatMul(&M, &Qx, &R);
2958     assert(fabs(matR[2][1]) < FLT_EPSILON);
2959     matR[2][1] = 0;
2960
2961     /* Find Givens rotation for y axis. */
2962     /*
2963          ( c  0 -s )
2964     Qy = ( 0  1  0 ), c = m33/sqrt(m31^2 + m33^2), s = -m31/sqrt(m31^2 + m33^2)
2965          ( s  0  c )
2966     */
2967     s = -matR[2][0];
2968     c = matR[2][2];
2969     z = 1./sqrt(c * c + s * s + DBL_EPSILON);
2970     c *= z;
2971     s *= z;
2972
2973     double _Qy[3][3] = { {c, 0, -s}, {0, 1, 0}, {s, 0, c} };
2974     CvMat Qy = cvMat(3, 3, CV_64F, _Qy);
2975     cvMatMul(&R, &Qy, &M);
2976
2977     assert(fabs(matM[2][0]) < FLT_EPSILON);
2978     matM[2][0] = 0;
2979
2980     /* Find Givens rotation for z axis. */
2981     /*
2982          ( c  s  0 )
2983     Qz = (-s  c  0 ), c = m22/sqrt(m21^2 + m22^2), s = m21/sqrt(m21^2 + m22^2)
2984          ( 0  0  1 )
2985     */
2986
2987     s = matM[1][0];
2988     c = matM[1][1];
2989     z = 1./sqrt(c * c + s * s + DBL_EPSILON);
2990     c *= z;
2991     s *= z;
2992
2993     double _Qz[3][3] = { {c, s, 0}, {-s, c, 0}, {0, 0, 1} };
2994     CvMat Qz = cvMat(3, 3, CV_64F, _Qz);
2995
2996     cvMatMul(&M, &Qz, &R);
2997     assert(fabs(matR[1][0]) < FLT_EPSILON);
2998     matR[1][0] = 0;
2999
3000     // Solve the decomposition ambiguity.
3001     // Diagonal entries of R, except the last one, shall be positive.
3002     // Further rotate R by 180 degree if necessary
3003     if( matR[0][0] < 0 )
3004     {
3005         if( matR[1][1] < 0 )
3006         {
3007             // rotate around z for 180 degree, i.e. a rotation matrix of
3008             // [-1,  0,  0],
3009             // [ 0, -1,  0],
3010             // [ 0,  0,  1]
3011             matR[0][0] *= -1;
3012             matR[0][1] *= -1;
3013             matR[1][1] *= -1;
3014
3015             _Qz[0][0] *= -1;
3016             _Qz[0][1] *= -1;
3017             _Qz[1][0] *= -1;
3018             _Qz[1][1] *= -1;
3019         }
3020         else
3021         {
3022             // rotate around y for 180 degree, i.e. a rotation matrix of
3023             // [-1,  0,  0],
3024             // [ 0,  1,  0],
3025             // [ 0,  0, -1]
3026             matR[0][0] *= -1;
3027             matR[0][2] *= -1;
3028             matR[1][2] *= -1;
3029             matR[2][2] *= -1;
3030
3031             cvTranspose( &Qz, &Qz );
3032
3033             _Qy[0][0] *= -1;
3034             _Qy[0][2] *= -1;
3035             _Qy[2][0] *= -1;
3036             _Qy[2][2] *= -1;
3037         }
3038     }
3039     else if( matR[1][1] < 0 )
3040     {
3041         // ??? for some reason, we never get here ???
3042
3043         // rotate around x for 180 degree, i.e. a rotation matrix of
3044         // [ 1,  0,  0],
3045         // [ 0, -1,  0],
3046         // [ 0,  0, -1]
3047         matR[0][1] *= -1;
3048         matR[0][2] *= -1;
3049         matR[1][1] *= -1;
3050         matR[1][2] *= -1;
3051         matR[2][2] *= -1;
3052
3053         cvTranspose( &Qz, &Qz );
3054         cvTranspose( &Qy, &Qy );
3055
3056         _Qx[1][1] *= -1;
3057         _Qx[1][2] *= -1;
3058         _Qx[2][1] *= -1;
3059         _Qx[2][2] *= -1;
3060     }
3061
3062     // calculate the euler angle
3063     if( eulerAngles )
3064     {
3065         eulerAngles->x = acos(_Qx[1][1]) * (_Qx[1][2] >= 0 ? 1 : -1) * (180.0 / CV_PI);
3066         eulerAngles->y = acos(_Qy[0][0]) * (_Qy[2][0] >= 0 ? 1 : -1) * (180.0 / CV_PI);
3067         eulerAngles->z = acos(_Qz[0][0]) * (_Qz[0][1] >= 0 ? 1 : -1) * (180.0 / CV_PI);
3068     }
3069
3070     /* Calulate orthogonal matrix. */
3071     /*
3072     Q = QzT * QyT * QxT
3073     */
3074     cvGEMM( &Qz, &Qy, 1, 0, 0, &M, CV_GEMM_A_T + CV_GEMM_B_T );
3075     cvGEMM( &M, &Qx, 1, 0, 0, &Q, CV_GEMM_B_T );
3076
3077     /* Save R and Q matrices. */
3078     cvConvert( &R, matrixR );
3079     cvConvert( &Q, matrixQ );
3080
3081     if( matrixQx )
3082         cvConvert(&Qx, matrixQx);
3083     if( matrixQy )
3084         cvConvert(&Qy, matrixQy);
3085     if( matrixQz )
3086         cvConvert(&Qz, matrixQz);
3087 }
3088
3089
3090 CV_IMPL void
3091 cvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr,
3092                              CvMat *rotMatr, CvMat *posVect,
3093                              CvMat *rotMatrX, CvMat *rotMatrY,
3094                              CvMat *rotMatrZ, CvPoint3D64f *eulerAngles)
3095 {
3096     double tmpProjMatrData[16], tmpMatrixDData[16], tmpMatrixVData[16];
3097     CvMat tmpProjMatr = cvMat(4, 4, CV_64F, tmpProjMatrData);
3098     CvMat tmpMatrixD = cvMat(4, 4, CV_64F, tmpMatrixDData);
3099     CvMat tmpMatrixV = cvMat(4, 4, CV_64F, tmpMatrixVData);
3100     CvMat tmpMatrixM;
3101
3102     /* Validate parameters. */
3103     if(projMatr == 0 || calibMatr == 0 || rotMatr == 0 || posVect == 0)
3104         CV_Error(CV_StsNullPtr, "Some of parameters is a NULL pointer!");
3105
3106     if(!CV_IS_MAT(projMatr) || !CV_IS_MAT(calibMatr) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(posVect))
3107         CV_Error(CV_StsUnsupportedFormat, "Input parameters must be a matrices!");
3108
3109     if(projMatr->cols != 4 || projMatr->rows != 3)
3110         CV_Error(CV_StsUnmatchedSizes, "Size of projection matrix must be 3x4!");
3111
3112     if(calibMatr->cols != 3 || calibMatr->rows != 3 || rotMatr->cols != 3 || rotMatr->rows != 3)
3113         CV_Error(CV_StsUnmatchedSizes, "Size of calibration and rotation matrices must be 3x3!");
3114
3115     if(posVect->cols != 1 || posVect->rows != 4)
3116         CV_Error(CV_StsUnmatchedSizes, "Size of position vector must be 4x1!");
3117
3118     /* Compute position vector. */
3119     cvSetZero(&tmpProjMatr); // Add zero row to make matrix square.
3120     int i, k;
3121     for(i = 0; i < 3; i++)
3122         for(k = 0; k < 4; k++)
3123             cvmSet(&tmpProjMatr, i, k, cvmGet(projMatr, i, k));
3124
3125     cvSVD(&tmpProjMatr, &tmpMatrixD, NULL, &tmpMatrixV, CV_SVD_MODIFY_A + CV_SVD_V_T);
3126
3127     /* Save position vector. */
3128     for(i = 0; i < 4; i++)
3129         cvmSet(posVect, i, 0, cvmGet(&tmpMatrixV, 3, i)); // Solution is last row of V.
3130
3131     /* Compute calibration and rotation matrices via RQ decomposition. */
3132     cvGetCols(projMatr, &tmpMatrixM, 0, 3); // M is first square matrix of P.
3133
3134     CV_Assert(cvDet(&tmpMatrixM) != 0.0); // So far only finite cameras could be decomposed, so M has to be nonsingular [det(M) != 0].
3135
3136     cvRQDecomp3x3(&tmpMatrixM, calibMatr, rotMatr, rotMatrX, rotMatrY, rotMatrZ, eulerAngles);
3137 }
3138
3139
3140
3141 namespace cv
3142 {
3143
3144 static void collectCalibrationData( InputArrayOfArrays objectPoints,
3145                                     InputArrayOfArrays imagePoints1,
3146                                     InputArrayOfArrays imagePoints2,
3147                                     Mat& objPtMat, Mat& imgPtMat1, Mat* imgPtMat2,
3148                                     Mat& npoints )
3149 {
3150     int nimages = (int)objectPoints.total();
3151     int i, j = 0, ni = 0, total = 0;
3152     CV_Assert(nimages > 0 && nimages == (int)imagePoints1.total() &&
3153         (!imgPtMat2 || nimages == (int)imagePoints2.total()));
3154
3155     for( i = 0; i < nimages; i++ )
3156     {
3157         ni = objectPoints.getMat(i).checkVector(3, CV_32F);
3158         CV_Assert( ni >= 0 );
3159         total += ni;
3160     }
3161
3162     npoints.create(1, (int)nimages, CV_32S);
3163     objPtMat.create(1, (int)total, CV_32FC3);
3164     imgPtMat1.create(1, (int)total, CV_32FC2);
3165     Point2f* imgPtData2 = 0;
3166
3167     if( imgPtMat2 )
3168     {
3169         imgPtMat2->create(1, (int)total, CV_32FC2);
3170         imgPtData2 = imgPtMat2->ptr<Point2f>();
3171     }
3172
3173     Point3f* objPtData = objPtMat.ptr<Point3f>();
3174     Point2f* imgPtData1 = imgPtMat1.ptr<Point2f>();
3175
3176     for( i = 0; i < nimages; i++, j += ni )
3177     {
3178         Mat objpt = objectPoints.getMat(i);
3179         Mat imgpt1 = imagePoints1.getMat(i);
3180         ni = objpt.checkVector(3, CV_32F);
3181         int ni1 = imgpt1.checkVector(2, CV_32F);
3182         CV_Assert( ni > 0 && ni == ni1 );
3183         npoints.at<int>(i) = ni;
3184         memcpy( objPtData + j, objpt.data, ni*sizeof(objPtData[0]) );
3185         memcpy( imgPtData1 + j, imgpt1.data, ni*sizeof(imgPtData1[0]) );
3186
3187         if( imgPtData2 )
3188         {
3189             Mat imgpt2 = imagePoints2.getMat(i);
3190             int ni2 = imgpt2.checkVector(2, CV_32F);
3191             CV_Assert( ni == ni2 );
3192             memcpy( imgPtData2 + j, imgpt2.data, ni*sizeof(imgPtData2[0]) );
3193         }
3194     }
3195 }
3196
3197
3198 static Mat prepareCameraMatrix(Mat& cameraMatrix0, int rtype)
3199 {
3200     Mat cameraMatrix = Mat::eye(3, 3, rtype);
3201     if( cameraMatrix0.size() == cameraMatrix.size() )
3202         cameraMatrix0.convertTo(cameraMatrix, rtype);
3203     return cameraMatrix;
3204 }
3205
3206 static Mat prepareDistCoeffs(Mat& distCoeffs0, int rtype)
3207 {
3208     Mat distCoeffs = Mat::zeros(distCoeffs0.cols == 1 ? Size(1, 8) : Size(8, 1), rtype);
3209     if( distCoeffs0.size() == Size(1, 4) ||
3210        distCoeffs0.size() == Size(1, 5) ||
3211        distCoeffs0.size() == Size(1, 8) ||
3212        distCoeffs0.size() == Size(4, 1) ||
3213        distCoeffs0.size() == Size(5, 1) ||
3214        distCoeffs0.size() == Size(8, 1) )
3215     {
3216         Mat dstCoeffs(distCoeffs, Rect(0, 0, distCoeffs0.cols, distCoeffs0.rows));
3217         distCoeffs0.convertTo(dstCoeffs, rtype);
3218     }
3219     return distCoeffs;
3220 }
3221
3222 } // namespace cv
3223
3224
3225 void cv::Rodrigues(InputArray _src, OutputArray _dst, OutputArray _jacobian)
3226 {
3227     Mat src = _src.getMat();
3228     bool v2m = src.cols == 1 || src.rows == 1;
3229     _dst.create(3, v2m ? 3 : 1, src.depth());
3230     Mat dst = _dst.getMat();
3231     CvMat _csrc = src, _cdst = dst, _cjacobian;
3232     if( _jacobian.needed() )
3233     {
3234         _jacobian.create(v2m ? Size(9, 3) : Size(3, 9), src.depth());
3235         _cjacobian = _jacobian.getMat();
3236     }
3237     bool ok = cvRodrigues2(&_csrc, &_cdst, _jacobian.needed() ? &_cjacobian : 0) > 0;
3238     if( !ok )
3239         dst = Scalar(0);
3240 }
3241
3242 void cv::matMulDeriv( InputArray _Amat, InputArray _Bmat,
3243                       OutputArray _dABdA, OutputArray _dABdB )
3244 {
3245     Mat A = _Amat.getMat(), B = _Bmat.getMat();
3246     _dABdA.create(A.rows*B.cols, A.rows*A.cols, A.type());
3247     _dABdB.create(A.rows*B.cols, B.rows*B.cols, A.type());
3248     CvMat matA = A, matB = B, c_dABdA = _dABdA.getMat(), c_dABdB = _dABdB.getMat();
3249     cvCalcMatMulDeriv(&matA, &matB, &c_dABdA, &c_dABdB);
3250 }
3251
3252
3253 void cv::composeRT( InputArray _rvec1, InputArray _tvec1,
3254                     InputArray _rvec2, InputArray _tvec2,
3255                     OutputArray _rvec3, OutputArray _tvec3,
3256                     OutputArray _dr3dr1, OutputArray _dr3dt1,
3257                     OutputArray _dr3dr2, OutputArray _dr3dt2,
3258                     OutputArray _dt3dr1, OutputArray _dt3dt1,
3259                     OutputArray _dt3dr2, OutputArray _dt3dt2 )
3260 {
3261     Mat rvec1 = _rvec1.getMat(), tvec1 = _tvec1.getMat();
3262     Mat rvec2 = _rvec2.getMat(), tvec2 = _tvec2.getMat();
3263     int rtype = rvec1.type();
3264     _rvec3.create(rvec1.size(), rtype);
3265     _tvec3.create(tvec1.size(), rtype);
3266     Mat rvec3 = _rvec3.getMat(), tvec3 = _tvec3.getMat();
3267
3268     CvMat c_rvec1 = rvec1, c_tvec1 = tvec1, c_rvec2 = rvec2,
3269           c_tvec2 = tvec2, c_rvec3 = rvec3, c_tvec3 = tvec3;
3270     CvMat c_dr3dr1, c_dr3dt1, c_dr3dr2, c_dr3dt2, c_dt3dr1, c_dt3dt1, c_dt3dr2, c_dt3dt2;
3271     CvMat *p_dr3dr1=0, *p_dr3dt1=0, *p_dr3dr2=0, *p_dr3dt2=0, *p_dt3dr1=0, *p_dt3dt1=0, *p_dt3dr2=0, *p_dt3dt2=0;
3272
3273     if( _dr3dr1.needed() )
3274     {
3275         _dr3dr1.create(3, 3, rtype);
3276         p_dr3dr1 = &(c_dr3dr1 = _dr3dr1.getMat());
3277     }
3278
3279     if( _dr3dt1.needed() )
3280     {
3281         _dr3dt1.create(3, 3, rtype);
3282         p_dr3dt1 = &(c_dr3dt1 = _dr3dt1.getMat());
3283     }
3284
3285     if( _dr3dr2.needed() )
3286     {
3287         _dr3dr2.create(3, 3, rtype);
3288         p_dr3dr2 = &(c_dr3dr2 = _dr3dr2.getMat());
3289     }
3290
3291     if( _dr3dt2.needed() )
3292     {
3293         _dr3dt2.create(3, 3, rtype);
3294         p_dr3dt2 = &(c_dr3dt2 = _dr3dt2.getMat());
3295     }
3296
3297     if( _dt3dr1.needed() )
3298     {
3299         _dt3dr1.create(3, 3, rtype);
3300         p_dt3dr1 = &(c_dt3dr1 = _dt3dr1.getMat());
3301     }
3302
3303     if( _dt3dt1.needed() )
3304     {
3305         _dt3dt1.create(3, 3, rtype);
3306         p_dt3dt1 = &(c_dt3dt1 = _dt3dt1.getMat());
3307     }
3308
3309     if( _dt3dr2.needed() )
3310     {
3311         _dt3dr2.create(3, 3, rtype);
3312         p_dt3dr2 = &(c_dt3dr2 = _dt3dr2.getMat());
3313     }
3314
3315     if( _dt3dt2.needed() )
3316     {
3317         _dt3dt2.create(3, 3, rtype);
3318         p_dt3dt2 = &(c_dt3dt2 = _dt3dt2.getMat());
3319     }
3320
3321     cvComposeRT(&c_rvec1, &c_tvec1, &c_rvec2, &c_tvec2, &c_rvec3, &c_tvec3,
3322                 p_dr3dr1, p_dr3dt1, p_dr3dr2, p_dr3dt2,
3323                 p_dt3dr1, p_dt3dt1, p_dt3dr2, p_dt3dt2);
3324 }
3325
3326
3327 void cv::projectPoints( InputArray _opoints,
3328                         InputArray _rvec,
3329                         InputArray _tvec,
3330                         InputArray _cameraMatrix,
3331                         InputArray _distCoeffs,
3332                         OutputArray _ipoints,
3333                         OutputArray _jacobian,
3334                         double aspectRatio )
3335 {
3336     Mat opoints = _opoints.getMat();
3337     int npoints = opoints.checkVector(3), depth = opoints.depth();
3338     CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));
3339
3340     CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
3341     CvMat *pdpdrot=0, *pdpdt=0, *pdpdf=0, *pdpdc=0, *pdpddist=0;
3342
3343     _ipoints.create(npoints, 1, CV_MAKETYPE(depth, 2), -1, true);
3344     CvMat c_imagePoints = _ipoints.getMat();
3345     CvMat c_objectPoints = opoints;
3346     Mat cameraMatrix = _cameraMatrix.getMat();
3347     Mat distCoeffs = _distCoeffs.getMat();
3348     Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
3349     CvMat c_cameraMatrix = cameraMatrix;
3350     CvMat c_rvec = rvec, c_tvec = tvec;
3351     CvMat c_distCoeffs = distCoeffs;
3352     int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;
3353
3354     if( _jacobian.needed() )
3355     {
3356         _jacobian.create(npoints*2, 3+3+2+2+ndistCoeffs, CV_64F);
3357         Mat jacobian = _jacobian.getMat();
3358         pdpdrot = &(dpdrot = jacobian.colRange(0, 3));
3359         pdpdt = &(dpdt = jacobian.colRange(3, 6));
3360         pdpdf = &(dpdf = jacobian.colRange(6, 8));
3361         pdpdc = &(dpdc = jacobian.colRange(8, 10));
3362         pdpddist = &(dpddist = jacobian.colRange(10, 10+ndistCoeffs));
3363     }
3364
3365     cvProjectPoints2( &c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
3366                       &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio );
3367 }
3368
3369 cv::Mat cv::initCameraMatrix2D( InputArrayOfArrays objectPoints,
3370                                 InputArrayOfArrays imagePoints,
3371                                 Size imageSize, double aspectRatio )
3372 {
3373     Mat objPt, imgPt, npoints, cameraMatrix(3, 3, CV_64F);
3374     collectCalibrationData( objectPoints, imagePoints, noArray(),
3375                             objPt, imgPt, 0, npoints );
3376     CvMat _objPt = objPt, _imgPt = imgPt, _npoints = npoints, _cameraMatrix = cameraMatrix;
3377     cvInitIntrinsicParams2D( &_objPt, &_imgPt, &_npoints,
3378                              imageSize, &_cameraMatrix, aspectRatio );
3379     return cameraMatrix;
3380 }
3381
3382
3383 double cv::calibrateCamera( InputArrayOfArrays _objectPoints,
3384                             InputArrayOfArrays _imagePoints,
3385                             Size imageSize, InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs,
3386                             OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags )
3387 {
3388     int rtype = CV_64F;
3389     Mat cameraMatrix = _cameraMatrix.getMat();
3390     cameraMatrix = prepareCameraMatrix(cameraMatrix, rtype);
3391     Mat distCoeffs = _distCoeffs.getMat();
3392     distCoeffs = prepareDistCoeffs(distCoeffs, rtype);
3393     if( !(flags & CALIB_RATIONAL_MODEL) )
3394         distCoeffs = distCoeffs.rows == 1 ? distCoeffs.colRange(0, 5) : distCoeffs.rowRange(0, 5);
3395
3396     int    i;
3397     size_t nimages = _objectPoints.total();
3398     CV_Assert( nimages > 0 );
3399     Mat objPt, imgPt, npoints, rvecM((int)nimages, 3, CV_64FC1), tvecM((int)nimages, 3, CV_64FC1);
3400     collectCalibrationData( _objectPoints, _imagePoints, noArray(),
3401                             objPt, imgPt, 0, npoints );
3402     CvMat c_objPt = objPt, c_imgPt = imgPt, c_npoints = npoints;
3403     CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
3404     CvMat c_rvecM = rvecM, c_tvecM = tvecM;
3405
3406     double reprojErr = cvCalibrateCamera2(&c_objPt, &c_imgPt, &c_npoints, imageSize,
3407                                           &c_cameraMatrix, &c_distCoeffs, &c_rvecM,
3408                                           &c_tvecM, flags );
3409
3410     bool rvecs_needed = _rvecs.needed(), tvecs_needed = _tvecs.needed();
3411
3412     if( rvecs_needed )
3413         _rvecs.create((int)nimages, 1, CV_64FC3);
3414     if( tvecs_needed )
3415         _tvecs.create((int)nimages, 1, CV_64FC3);
3416
3417     for( i = 0; i < (int)nimages; i++ )
3418     {
3419         if( rvecs_needed )
3420         {
3421             _rvecs.create(3, 1, CV_64F, i, true);
3422             Mat rv = _rvecs.getMat(i);
3423             memcpy(rv.data, rvecM.ptr<double>(i), 3*sizeof(double));
3424         }
3425         if( tvecs_needed )
3426         {
3427             _tvecs.create(3, 1, CV_64F, i, true);
3428             Mat tv = _tvecs.getMat(i);
3429             memcpy(tv.data, tvecM.ptr<double>(i), 3*sizeof(double));
3430         }
3431     }
3432     cameraMatrix.copyTo(_cameraMatrix);
3433     distCoeffs.copyTo(_distCoeffs);
3434
3435     return reprojErr;
3436 }
3437
3438
3439 void cv::calibrationMatrixValues( InputArray _cameraMatrix, Size imageSize,
3440                                   double apertureWidth, double apertureHeight,
3441                                   double& fovx, double& fovy, double& focalLength,
3442                                   Point2d& principalPoint, double& aspectRatio )
3443 {
3444     Mat cameraMatrix = _cameraMatrix.getMat();
3445     CvMat c_cameraMatrix = cameraMatrix;
3446     cvCalibrationMatrixValues( &c_cameraMatrix, imageSize, apertureWidth, apertureHeight,
3447         &fovx, &fovy, &focalLength, (CvPoint2D64f*)&principalPoint, &aspectRatio );
3448 }
3449
3450 double cv::stereoCalibrate( InputArrayOfArrays _objectPoints,
3451                           InputArrayOfArrays _imagePoints1,
3452                           InputArrayOfArrays _imagePoints2,
3453                           InputOutputArray _cameraMatrix1, InputOutputArray _distCoeffs1,
3454                           InputOutputArray _cameraMatrix2, InputOutputArray _distCoeffs2,
3455                           Size imageSize, OutputArray _Rmat, OutputArray _Tmat,
3456                           OutputArray _Emat, OutputArray _Fmat, TermCriteria criteria,
3457                           int flags )
3458 {
3459     int rtype = CV_64F;
3460     Mat cameraMatrix1 = _cameraMatrix1.getMat();
3461     Mat cameraMatrix2 = _cameraMatrix2.getMat();
3462     Mat distCoeffs1 = _distCoeffs1.getMat();
3463     Mat distCoeffs2 = _distCoeffs2.getMat();
3464     cameraMatrix1 = prepareCameraMatrix(cameraMatrix1, rtype);
3465     cameraMatrix2 = prepareCameraMatrix(cameraMatrix2, rtype);
3466     distCoeffs1 = prepareDistCoeffs(distCoeffs1, rtype);
3467     distCoeffs2 = prepareDistCoeffs(distCoeffs2, rtype);
3468
3469     if( !(flags & CALIB_RATIONAL_MODEL) )
3470     {
3471         distCoeffs1 = distCoeffs1.rows == 1 ? distCoeffs1.colRange(0, 5) : distCoeffs1.rowRange(0, 5);
3472         distCoeffs2 = distCoeffs2.rows == 1 ? distCoeffs2.colRange(0, 5) : distCoeffs2.rowRange(0, 5);
3473     }
3474
3475     _Rmat.create(3, 3, rtype);
3476     _Tmat.create(3, 1, rtype);
3477
3478     Mat objPt, imgPt, imgPt2, npoints;
3479
3480     collectCalibrationData( _objectPoints, _imagePoints1, _imagePoints2,
3481                             objPt, imgPt, &imgPt2, npoints );
3482     CvMat c_objPt = objPt, c_imgPt = imgPt, c_imgPt2 = imgPt2, c_npoints = npoints;
3483     CvMat c_cameraMatrix1 = cameraMatrix1, c_distCoeffs1 = distCoeffs1;
3484     CvMat c_cameraMatrix2 = cameraMatrix2, c_distCoeffs2 = distCoeffs2;
3485     CvMat c_matR = _Rmat.getMat(), c_matT = _Tmat.getMat(), c_matE, c_matF, *p_matE = 0, *p_matF = 0;
3486
3487     if( _Emat.needed() )
3488     {
3489         _Emat.create(3, 3, rtype);
3490         p_matE = &(c_matE = _Emat.getMat());
3491     }
3492     if( _Fmat.needed() )
3493     {
3494         _Fmat.create(3, 3, rtype);
3495         p_matF = &(c_matF = _Fmat.getMat());
3496     }
3497
3498     double err = cvStereoCalibrate(&c_objPt, &c_imgPt, &c_imgPt2, &c_npoints, &c_cameraMatrix1,
3499         &c_distCoeffs1, &c_cameraMatrix2, &c_distCoeffs2, imageSize,
3500         &c_matR, &c_matT, p_matE, p_matF, criteria, flags );
3501
3502     cameraMatrix1.copyTo(_cameraMatrix1);
3503     cameraMatrix2.copyTo(_cameraMatrix2);
3504     distCoeffs1.copyTo(_distCoeffs1);
3505     distCoeffs2.copyTo(_distCoeffs2);
3506
3507     return err;
3508 }
3509
3510
3511 void cv::stereoRectify( InputArray _cameraMatrix1, InputArray _distCoeffs1,
3512                         InputArray _cameraMatrix2, InputArray _distCoeffs2,
3513                         Size imageSize, InputArray _Rmat, InputArray _Tmat,
3514                         OutputArray _Rmat1, OutputArray _Rmat2,
3515                         OutputArray _Pmat1, OutputArray _Pmat2,
3516                         OutputArray _Qmat, int flags,
3517                         double alpha, Size newImageSize,
3518                         Rect* validPixROI1, Rect* validPixROI2 )
3519 {
3520     Mat cameraMatrix1 = _cameraMatrix1.getMat(), cameraMatrix2 = _cameraMatrix2.getMat();
3521     Mat distCoeffs1 = _distCoeffs1.getMat(), distCoeffs2 = _distCoeffs2.getMat();
3522     Mat Rmat = _Rmat.getMat(), Tmat = _Tmat.getMat();
3523     CvMat c_cameraMatrix1 = cameraMatrix1;
3524     CvMat c_cameraMatrix2 = cameraMatrix2;
3525     CvMat c_distCoeffs1 = distCoeffs1;
3526     CvMat c_distCoeffs2 = distCoeffs2;
3527     CvMat c_R = Rmat, c_T = Tmat;
3528
3529     int rtype = CV_64F;
3530     _Rmat1.create(3, 3, rtype);
3531     _Rmat2.create(3, 3, rtype);
3532     _Pmat1.create(3, 4, rtype);
3533     _Pmat2.create(3, 4, rtype);
3534     CvMat c_R1 = _Rmat1.getMat(), c_R2 = _Rmat2.getMat(), c_P1 = _Pmat1.getMat(), c_P2 = _Pmat2.getMat();
3535     CvMat c_Q, *p_Q = 0;
3536
3537     if( _Qmat.needed() )
3538     {
3539         _Qmat.create(4, 4, rtype);
3540         p_Q = &(c_Q = _Qmat.getMat());
3541     }
3542
3543     cvStereoRectify( &c_cameraMatrix1, &c_cameraMatrix2, &c_distCoeffs1, &c_distCoeffs2,
3544         imageSize, &c_R, &c_T, &c_R1, &c_R2, &c_P1, &c_P2, p_Q, flags, alpha,
3545         newImageSize, (CvRect*)validPixROI1, (CvRect*)validPixROI2);
3546 }
3547
3548 bool cv::stereoRectifyUncalibrated( InputArray _points1, InputArray _points2,
3549                                     InputArray _Fmat, Size imgSize,
3550                                     OutputArray _Hmat1, OutputArray _Hmat2, double threshold )
3551 {
3552     int rtype = CV_64F;
3553     _Hmat1.create(3, 3, rtype);
3554     _Hmat2.create(3, 3, rtype);
3555     Mat F = _Fmat.getMat();
3556     Mat points1 = _points1.getMat(), points2 = _points2.getMat();
3557     CvMat c_pt1 = points1, c_pt2 = points2;
3558     CvMat c_F, *p_F=0, c_H1 = _Hmat1.getMat(), c_H2 = _Hmat2.getMat();
3559     if( F.size() == Size(3, 3) )
3560         p_F = &(c_F = F);
3561     return cvStereoRectifyUncalibrated(&c_pt1, &c_pt2, p_F, imgSize, &c_H1, &c_H2, threshold) > 0;
3562 }
3563
3564 cv::Mat cv::getOptimalNewCameraMatrix( InputArray _cameraMatrix,
3565                                        InputArray _distCoeffs,
3566                                        Size imgSize, double alpha, Size newImgSize,
3567                                        Rect* validPixROI, bool centerPrincipalPoint )
3568 {
3569     Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
3570     CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
3571
3572     Mat newCameraMatrix(3, 3, CV_MAT_TYPE(c_cameraMatrix.type));
3573     CvMat c_newCameraMatrix = newCameraMatrix;
3574
3575     cvGetOptimalNewCameraMatrix(&c_cameraMatrix, &c_distCoeffs, imgSize,
3576                                 alpha, &c_newCameraMatrix,
3577                                 newImgSize, (CvRect*)validPixROI, (int)centerPrincipalPoint);
3578     return newCameraMatrix;
3579 }
3580
3581
3582 cv::Vec3d cv::RQDecomp3x3( InputArray _Mmat,
3583                            OutputArray _Rmat,
3584                            OutputArray _Qmat,
3585                            OutputArray _Qx,
3586                            OutputArray _Qy,
3587                            OutputArray _Qz )
3588 {
3589     Mat M = _Mmat.getMat();
3590     _Rmat.create(3, 3, M.type());
3591     _Qmat.create(3, 3, M.type());
3592     Vec3d eulerAngles;
3593
3594     CvMat matM = M, matR = _Rmat.getMat(), matQ = _Qmat.getMat(), Qx, Qy, Qz, *pQx=0, *pQy=0, *pQz=0;
3595     if( _Qx.needed() )
3596     {
3597         _Qx.create(3, 3, M.type());
3598         pQx = &(Qx = _Qx.getMat());
3599     }
3600     if( _Qy.needed() )
3601     {
3602         _Qy.create(3, 3, M.type());
3603         pQy = &(Qy = _Qy.getMat());
3604     }
3605     if( _Qz.needed() )
3606     {
3607         _Qz.create(3, 3, M.type());
3608         pQz = &(Qz = _Qz.getMat());
3609     }
3610     cvRQDecomp3x3(&matM, &matR, &matQ, pQx, pQy, pQz, (CvPoint3D64f*)&eulerAngles[0]);
3611     return eulerAngles;
3612 }
3613
3614
3615 void cv::decomposeProjectionMatrix( InputArray _projMatrix, OutputArray _cameraMatrix,
3616                                     OutputArray _rotMatrix, OutputArray _transVect,
3617                                     OutputArray _rotMatrixX, OutputArray _rotMatrixY,
3618                                     OutputArray _rotMatrixZ, OutputArray _eulerAngles )
3619 {
3620     Mat projMatrix = _projMatrix.getMat();
3621     int type = projMatrix.type();
3622     _cameraMatrix.create(3, 3, type);
3623     _rotMatrix.create(3, 3, type);
3624     _transVect.create(4, 1, type);
3625     CvMat c_projMatrix = projMatrix, c_cameraMatrix = _cameraMatrix.getMat();
3626     CvMat c_rotMatrix = _rotMatrix.getMat(), c_transVect = _transVect.getMat();
3627     CvMat c_rotMatrixX, *p_rotMatrixX = 0;
3628     CvMat c_rotMatrixY, *p_rotMatrixY = 0;
3629     CvMat c_rotMatrixZ, *p_rotMatrixZ = 0;
3630     CvPoint3D64f *p_eulerAngles = 0;
3631
3632     if( _rotMatrixX.needed() )
3633     {
3634         _rotMatrixX.create(3, 3, type);
3635         p_rotMatrixX = &(c_rotMatrixX = _rotMatrixX.getMat());
3636     }
3637     if( _rotMatrixY.needed() )
3638     {
3639         _rotMatrixY.create(3, 3, type);
3640         p_rotMatrixY = &(c_rotMatrixY = _rotMatrixY.getMat());
3641     }
3642     if( _rotMatrixZ.needed() )
3643     {
3644         _rotMatrixZ.create(3, 3, type);
3645         p_rotMatrixZ = &(c_rotMatrixZ = _rotMatrixZ.getMat());
3646     }
3647     if( _eulerAngles.needed() )
3648     {
3649         _eulerAngles.create(3, 1, CV_64F, -1, true);
3650         p_eulerAngles = (CvPoint3D64f*)_eulerAngles.getMat().data;
3651     }
3652
3653     cvDecomposeProjectionMatrix(&c_projMatrix, &c_cameraMatrix, &c_rotMatrix,
3654                                 &c_transVect, p_rotMatrixX, p_rotMatrixY,
3655                                 p_rotMatrixZ, p_eulerAngles);
3656 }
3657
3658
3659 namespace cv
3660 {
3661
3662 static void adjust3rdMatrix(InputArrayOfArrays _imgpt1_0,
3663                             InputArrayOfArrays _imgpt3_0,
3664                             const Mat& cameraMatrix1, const Mat& distCoeffs1,
3665                             const Mat& cameraMatrix3, const Mat& distCoeffs3,
3666                             const Mat& R1, const Mat& R3, const Mat& P1, Mat& P3 )
3667 {
3668     size_t n1 = _imgpt1_0.total(), n3 = _imgpt3_0.total();
3669     vector<Point2f> imgpt1, imgpt3;
3670
3671     for( int i = 0; i < (int)std::min(n1, n3); i++ )
3672     {
3673         Mat pt1 = _imgpt1_0.getMat(i), pt3 = _imgpt3_0.getMat(i);
3674         int ni1 = pt1.checkVector(2, CV_32F), ni3 = pt3.checkVector(2, CV_32F);
3675         CV_Assert( ni1 > 0 && ni1 == ni3 );
3676         const Point2f* pt1data = pt1.ptr<Point2f>();
3677         const Point2f* pt3data = pt3.ptr<Point2f>();
3678         std::copy(pt1data, pt1data + ni1, std::back_inserter(imgpt1));
3679         std::copy(pt3data, pt3data + ni3, std::back_inserter(imgpt3));
3680     }
3681
3682     undistortPoints(imgpt1, imgpt1, cameraMatrix1, distCoeffs1, R1, P1);
3683     undistortPoints(imgpt3, imgpt3, cameraMatrix3, distCoeffs3, R3, P3);
3684
3685     double y1_ = 0, y2_ = 0, y1y1_ = 0, y1y2_ = 0;
3686     size_t n = imgpt1.size();
3687
3688     for( size_t i = 0; i < n; i++ )
3689     {
3690         double y1 = imgpt3[i].y, y2 = imgpt1[i].y;
3691
3692         y1_ += y1; y2_ += y2;
3693         y1y1_ += y1*y1; y1y2_ += y1*y2;
3694     }
3695
3696     y1_ /= n;
3697     y2_ /= n;
3698     y1y1_ /= n;
3699     y1y2_ /= n;
3700
3701     double a = (y1y2_ - y1_*y2_)/(y1y1_ - y1_*y1_);
3702     double b = y2_ - a*y1_;
3703
3704     P3.at<double>(0,0) *= a;
3705     P3.at<double>(1,1) *= a;
3706     P3.at<double>(0,2) = P3.at<double>(0,2)*a;
3707     P3.at<double>(1,2) = P3.at<double>(1,2)*a + b;
3708     P3.at<double>(0,3) *= a;
3709     P3.at<double>(1,3) *= a;
3710 }
3711
3712 }
3713
3714 float cv::rectify3Collinear( InputArray _cameraMatrix1, InputArray _distCoeffs1,
3715                    InputArray _cameraMatrix2, InputArray _distCoeffs2,
3716                    InputArray _cameraMatrix3, InputArray _distCoeffs3,
3717                    InputArrayOfArrays _imgpt1,
3718                    InputArrayOfArrays _imgpt3,
3719                    Size imageSize, InputArray _Rmat12, InputArray _Tmat12,
3720                    InputArray _Rmat13, InputArray _Tmat13,
3721                    OutputArray _Rmat1, OutputArray _Rmat2, OutputArray _Rmat3,
3722                    OutputArray _Pmat1, OutputArray _Pmat2, OutputArray _Pmat3,
3723                    OutputArray _Qmat,
3724                    double alpha, Size /*newImgSize*/,
3725                    Rect* roi1, Rect* roi2, int flags )
3726 {
3727     // first, rectify the 1-2 stereo pair
3728     stereoRectify( _cameraMatrix1, _distCoeffs1, _cameraMatrix2, _distCoeffs2,
3729                    imageSize, _Rmat12, _Tmat12, _Rmat1, _Rmat2, _Pmat1, _Pmat2, _Qmat,
3730                    flags, alpha, imageSize, roi1, roi2 );
3731
3732     Mat R12 = _Rmat12.getMat(), R13 = _Rmat13.getMat(), T12 = _Tmat12.getMat(), T13 = _Tmat13.getMat();
3733
3734     _Rmat3.create(3, 3, CV_64F);
3735     _Pmat3.create(3, 4, CV_64F);
3736
3737     Mat P1 = _Pmat1.getMat(), P2 = _Pmat2.getMat();
3738     Mat R3 = _Rmat3.getMat(), P3 = _Pmat3.getMat();
3739
3740     // recompute rectification transforms for cameras 1 & 2.
3741     Mat om, r_r, r_r13;
3742
3743     if( R13.size() != Size(3,3) )
3744         Rodrigues(R13, r_r13);
3745     else
3746         R13.copyTo(r_r13);
3747
3748     if( R12.size() == Size(3,3) )
3749         Rodrigues(R12, om);
3750     else
3751         R12.copyTo(om);
3752
3753     om *= -0.5;
3754     Rodrigues(om, r_r); // rotate cameras to same orientation by averaging
3755     Mat_<double> t12 = r_r * T12;
3756
3757     int idx = fabs(t12(0,0)) > fabs(t12(1,0)) ? 0 : 1;
3758     double c = t12(idx,0), nt = norm(t12, CV_L2);
3759     Mat_<double> uu = Mat_<double>::zeros(3,1);
3760     uu(idx, 0) = c > 0 ? 1 : -1;
3761
3762     // calculate global Z rotation
3763     Mat_<double> ww = t12.cross(uu), wR;
3764     double nw = norm(ww, CV_L2);
3765     ww *= acos(fabs(c)/nt)/nw;
3766     Rodrigues(ww, wR);
3767
3768     // now rotate camera 3 to make its optical axis parallel to cameras 1 and 2.
3769     R3 = wR*r_r.t()*r_r13.t();
3770     Mat_<double> t13 = R3 * T13;
3771
3772     P2.copyTo(P3);
3773     Mat t = P3.col(3);
3774     t13.copyTo(t);
3775     P3.at<double>(0,3) *= P3.at<double>(0,0);
3776     P3.at<double>(1,3) *= P3.at<double>(1,1);
3777
3778     if( !_imgpt1.empty() && _imgpt3.empty() )
3779         adjust3rdMatrix(_imgpt1, _imgpt3, _cameraMatrix1.getMat(), _distCoeffs1.getMat(),
3780                         _cameraMatrix3.getMat(), _distCoeffs3.getMat(), _Rmat1.getMat(), R3, P1, P3);
3781
3782     return (float)((P3.at<double>(idx,3)/P3.at<double>(idx,idx))/
3783                    (P2.at<double>(idx,3)/P2.at<double>(idx,idx)));
3784 }
3785
3786
3787 /* End of file. */