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