generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added some protection against some singular cases in solvePnP/cvFindExtrinsicCameraParams2

Browse Source
This commit is contained in:
Vadim Pisarevsky 2010-11-29 16:43:03 +00:00
parent 6a6ee6403c
commit 9c0b300f52
1 changed files with 22 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
modules/calib3d/src/calibration.cpp
View File

@ -1223,21 +1223,30 @@ CV_IMPL void cvFindExtrinsicCameraParams2( const CvMat* objectPoints,
cvFindHomography( _Mxy, _mn, &matH ); cvFindHomography( _Mxy, _mn, &matH );
cvGetCol( &matH, &_h1, 0 ); if( cvCheckArr(&matH, CV_CHECK_QUIET) )
_h2 = _h1; _h2.data.db++; {
_h3 = _h2; _h3.data.db++; cvGetCol( &matH, &_h1, 0 );
h1_norm = sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]); _h2 = _h1; _h2.data.db++;
h2_norm = sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]); _h3 = _h2; _h3.data.db++;
h1_norm = sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]);
h2_norm = sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]);
cvScale( &_h1, &_h1, 1./h1_norm ); cvScale( &_h1, &_h1, 1./MAX(h1_norm, DBL_EPSILON) );
cvScale( &_h2, &_h2, 1./h2_norm ); cvScale( &_h2, &_h2, 1./MAX(h2_norm, DBL_EPSILON) );
cvScale( &_h3, &_t, 2./(h1_norm + h2_norm)); cvScale( &_h3, &_t, 2./MAX(h1_norm + h2_norm, DBL_EPSILON));
cvCrossProduct( &_h1, &_h2, &_h3 ); cvCrossProduct( &_h1, &_h2, &_h3 );
cvRodrigues2( &matH, &_r );
cvRodrigues2( &_r, &matH );
cvMatMulAdd( &matH, &T_transform, &_t, &_t );
cvMatMul( &matH, R_transform, &matR );
}
else
{
cvSetIdentity( &matR );
cvZero( &_t );
}
cvRodrigues2( &matH, &_r );
cvRodrigues2( &_r, &matH );
cvMatMulAdd( &matH, &T_transform, &_t, &_t );
cvMatMul( &matH, R_transform, &matR );
cvRodrigues2( &matR, &_r ); cvRodrigues2( &matR, &_r );
} }
else else