void MyMat::Cross(const MyMat &src1, const MyMat &src2)
{
cvCrossProduct(src1.m_data, src2.m_data, m_data);
}
bool
cvFindExtrinsicCameraParams3( const CvMat* obj_points,
const CvMat* img_points, const CvMat* A,
const CvMat* dist_coeffs,
CvMat* r_vec, CvMat* t_vec )
{
bool fGood = true;
const int max_iter = 20;
CvMat *_M = 0, *_Mxy = 0, *_m = 0, *_mn = 0, *_L = 0, *_J = 0;
CV_FUNCNAME( "cvFindExtrinsicCameraParams3" );
__BEGIN__;
int i, j, count;
double a[9], k[4] = { 0, 0, 0, 0 }, R[9], ifx, ify, cx, cy;
double Mc[3] = {0, 0, 0}, MM[9], U[9], V[9], W[3];
double JtJ[6*6], JtErr[6], JtJW[6], JtJV[6*6], delta[6], param[6];
CvPoint3D64f* M = 0;
CvPoint2D64f *m = 0, *mn = 0;
CvMat _a = cvMat( 3, 3, CV_64F, a );
CvMat _R = cvMat( 3, 3, CV_64F, R );
CvMat _r = cvMat( 3, 1, CV_64F, param );
CvMat _t = cvMat( 3, 1, CV_64F, param + 3 );
CvMat _Mc = cvMat( 1, 3, CV_64F, Mc );
CvMat _MM = cvMat( 3, 3, CV_64F, MM );
CvMat _U = cvMat( 3, 3, CV_64F, U );
CvMat _V = cvMat( 3, 3, CV_64F, V );
CvMat _W = cvMat( 3, 1, CV_64F, W );
CvMat _JtJ = cvMat( 6, 6, CV_64F, JtJ );
CvMat _JtErr = cvMat( 6, 1, CV_64F, JtErr );
CvMat _JtJW = cvMat( 6, 1, CV_64F, JtJW );
CvMat _JtJV = cvMat( 6, 6, CV_64F, JtJV );
CvMat _delta = cvMat( 6, 1, CV_64F, delta );
CvMat _param = cvMat( 6, 1, CV_64F, param );
CvMat _dpdr, _dpdt;
if( !CV_IS_MAT(obj_points) || !CV_IS_MAT(img_points) ||
!CV_IS_MAT(A) || !CV_IS_MAT(r_vec) || !CV_IS_MAT(t_vec) )
CV_ERROR( CV_StsBadArg, "One of required arguments is not a valid matrix" );
count = MAX(obj_points->cols, obj_points->rows);
CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 ));
CV_CALL( _Mxy = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( _mn = cvCreateMat( 1, count, CV_64FC2 ));
M = (CvPoint3D64f*)_M->data.db;
m = (CvPoint2D64f*)_m->data.db;
mn = (CvPoint2D64f*)_mn->data.db;
CV_CALL( cvConvertPointsHomogenious( obj_points, _M ));
CV_CALL( cvConvertPointsHomogenious( img_points, _m ));
CV_CALL( cvConvert( A, &_a ));
if( dist_coeffs )
{
CvMat _k;
if( !CV_IS_MAT(dist_coeffs) ||
CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
CV_MAT_DEPTH(dist_coeffs->type) != CV_32F ||
dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 )
CV_ERROR( CV_StsBadArg,
"Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
_k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(dist_coeffs->type)), k );
CV_CALL( cvConvert( dist_coeffs, &_k ));
}
if( CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F ||
r_vec->rows != 1 && r_vec->cols != 1 ||
r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3 )
CV_ERROR( CV_StsBadArg, "Rotation vector must be 1x3 or 3x1 floating-point vector" );
if( CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F ||
t_vec->rows != 1 && t_vec->cols != 1 ||
t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
CV_ERROR( CV_StsBadArg,
"Translation vector must be 1x3 or 3x1 floating-point vector" );
ifx = 1./a[0]; ify = 1./a[4];
cx = a[2]; cy = a[5];
// normalize image points
// (unapply the intrinsic matrix transformation and distortion)
for( i = 0; i < count; i++ )
{
double x = (m[i].x - cx)*ifx, y = (m[i].y - cy)*ify, x0 = x, y0 = y;
// compensate distortion iteratively
if( dist_coeffs )
for( j = 0; j < 5; j++ )
{
double r2 = x*x + y*y;
double icdist = 1./(1 + k[0]*r2 + k[1]*r2*r2);
double delta_x = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
double delta_y = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
x = (x0 - delta_x)*icdist;
y = (y0 - delta_y)*icdist;
}
mn[i].x = x; mn[i].y = y;
// calc mean(M)
Mc[0] += M[i].x;
Mc[1] += M[i].y;
Mc[2] += M[i].z;
}
Mc[0] /= count;
Mc[1] /= count;
Mc[2] /= count;
cvReshape( _M, _M, 1, count );
cvMulTransposed( _M, &_MM, 1, &_Mc );
cvSVD( &_MM, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T );
// initialize extrinsic parameters
if( W[2]/W[1] < 1e-3 || count < 4 )
{
// a planar structure case (all M's lie in the same plane)
double tt[3], h[9], h1_norm, h2_norm;
CvMat* R_transform = &_V;
CvMat T_transform = cvMat( 3, 1, CV_64F, tt );
CvMat _H = cvMat( 3, 3, CV_64F, h );
CvMat _h1, _h2, _h3;
if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
cvSetIdentity( R_transform );
if( cvDet(R_transform) < 0 )
cvScale( R_transform, R_transform, -1 );
cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );
for( i = 0; i < count; i++ )
{
const double* Rp = R_transform->data.db;
const double* Tp = T_transform.data.db;
const double* src = _M->data.db + i*3;
double* dst = _Mxy->data.db + i*2;
dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0];
dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1];
}
cvFindHomography( _Mxy, _mn, &_H );
cvGetCol( &_H, &_h1, 0 );
_h2 = _h1; _h2.data.db++;
_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( &_h2, &_h2, 1./h2_norm );
cvScale( &_h3, &_t, 2./(h1_norm + h2_norm));
cvCrossProduct( &_h1, &_h2, &_h3 );
cvRodrigues2( &_H, &_r );
cvRodrigues2( &_r, &_H );
cvMatMulAdd( &_H, &T_transform, &_t, &_t );
cvMatMul( &_H, R_transform, &_R );
cvRodrigues2( &_R, &_r );
}
else
{
// non-planar structure. Use DLT method
double* L;
double LL[12*12], LW[12], LV[12*12], sc;
CvMat _LL = cvMat( 12, 12, CV_64F, LL );
CvMat _LW = cvMat( 12, 1, CV_64F, LW );
CvMat _LV = cvMat( 12, 12, CV_64F, LV );
CvMat _RRt, _RR, _tt;
CV_CALL( _L = cvCreateMat( 2*count, 12, CV_64F ));
L = _L->data.db;
for( i = 0; i < count; i++, L += 24 )
{
double x = -mn[i].x, y = -mn[i].y;
L[0] = L[16] = M[i].x;
L[1] = L[17] = M[i].y;
L[2] = L[18] = M[i].z;
L[3] = L[19] = 1.;
L[4] = L[5] = L[6] = L[7] = 0.;
L[12] = L[13] = L[14] = L[15] = 0.;
L[8] = x*M[i].x;
L[9] = x*M[i].y;
L[10] = x*M[i].z;
L[11] = x;
L[20] = y*M[i].x;
L[21] = y*M[i].y;
L[22] = y*M[i].z;
L[23] = y;
}
cvMulTransposed( _L, &_LL, 1 );
cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
_RRt = cvMat( 3, 4, CV_64F, LV + 11*12 );
cvGetCols( &_RRt, &_RR, 0, 3 );
cvGetCol( &_RRt, &_tt, 3 );
if( cvDet(&_RR) < 0 )
cvScale( &_RRt, &_RRt, -1 );
sc = cvNorm(&_RR);
cvSVD( &_RR, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T );
cvScale( &_tt, &_t, cvNorm(&_R)/sc );
cvRodrigues2( &_R, &_r );
cvReleaseMat( &_L );
}
//
// Check if new r and t are good
//
if ( cvGetReal1D( r_vec, 0 ) ||
cvGetReal1D( r_vec, 1 ) ||
cvGetReal1D( r_vec, 2 ) ||
cvGetReal1D( t_vec, 0 ) ||
cvGetReal1D( t_vec, 1 ) ||
cvGetReal1D( t_vec, 2 ) )
{
//
// perfom this only when the old r and t exist.
//
CvMat * R_inv = cvCreateMat( 3, 3, CV_64FC1 );
CvMat * r_old = cvCreateMat( 3, 1, CV_64FC1 );
CvMat * R_old = cvCreateMat( 3, 3, CV_64FC1 );
CvMat * t_old = cvCreateMat( 3, 1, CV_64FC1 );
// get new center
cvInvert( &_R, R_inv );
double new_center[3];
CvMat newCenter = cvMat( 3, 1, CV_64FC1, new_center );
cvMatMul( R_inv, &_t, &newCenter );
cvScale( &newCenter, &newCenter, -1 );
// get old center
cvConvert( r_vec, r_old );
cvConvert( t_vec, t_old );
cvRodrigues2( r_old, R_old );
cvInvert( R_old, R_inv );
double old_center[3];
CvMat oldCenter = cvMat( 3, 1, CV_64FC1, old_center );
cvMatMul( R_inv, t_old, &oldCenter );
cvScale( &oldCenter, &oldCenter, -1 );
// get difference
double diff_center = 0;
for ( int i = 0; i < 3 ; i ++ )
diff_center += pow( new_center[i] - old_center[i], 2 );
diff_center = sqrt( diff_center );
if ( diff_center > 300 )
{
printf("diff_center = %.2f --> set initial r and t as same as the previous\n", diff_center);
cvConvert(r_vec, &_r);
cvConvert(t_vec, &_t);
fGood = false;
}
// else printf("diff_center = %.2f\n", diff_center );
cvReleaseMat( &R_inv );
cvReleaseMat( &r_old );
cvReleaseMat( &R_old );
cvReleaseMat( &t_old );
}
if ( fGood )
{
CV_CALL( _J = cvCreateMat( 2*count, 6, CV_64FC1 ));
cvGetCols( _J, &_dpdr, 0, 3 );
cvGetCols( _J, &_dpdt, 3, 6 );
// refine extrinsic parameters using iterative algorithm
for( i = 0; i < max_iter; i++ )
{
double n1, n2;
cvReshape( _mn, _mn, 2, 1 );
cvProjectPoints2( _M, &_r, &_t, &_a, dist_coeffs,
_mn, &_dpdr, &_dpdt, 0, 0, 0 );
cvSub( _m, _mn, _mn );
cvReshape( _mn, _mn, 1, 2*count );
cvMulTransposed( _J, &_JtJ, 1 );
cvGEMM( _J, _mn, 1, 0, 0, &_JtErr, CV_GEMM_A_T );
cvSVD( &_JtJ, &_JtJW, 0, &_JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T );
if( JtJW[5]/JtJW[0] < 1e-12 )
break;
cvSVBkSb( &_JtJW, &_JtJV, &_JtJV, &_JtErr,
&_delta, CV_SVD_U_T + CV_SVD_V_T );
cvAdd( &_delta, &_param, &_param );
n1 = cvNorm( &_delta );
n2 = cvNorm( &_param );
if( n1/n2 < 1e-10 )
break;
}
_r = cvMat( r_vec->rows, r_vec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), param );
_t = cvMat( t_vec->rows, t_vec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), param + 3 );
cvConvert( &_r, r_vec );
cvConvert( &_t, t_vec );
}
__END__;
cvReleaseMat( &_M );
cvReleaseMat( &_Mxy );
cvReleaseMat( &_m );
cvReleaseMat( &_mn );
cvReleaseMat( &_L );
cvReleaseMat( &_J );
return fGood;
}
CV_IMPL void
cvCalcImageHomography( float* line, CvPoint3D32f* _center,
float* _intrinsic, float* _homography )
{
double norm_xy, norm_xz, xy_sina, xy_cosa, xz_sina, xz_cosa, nx1, plane_dist;
float _ry[3], _rz[3], _r_trans[9];
CvMat rx = cvMat( 1, 3, CV_32F, line );
CvMat ry = cvMat( 1, 3, CV_32F, _ry );
CvMat rz = cvMat( 1, 3, CV_32F, _rz );
CvMat r_trans = cvMat( 3, 3, CV_32F, _r_trans );
CvMat center = cvMat( 3, 1, CV_32F, _center );
float _sub[9];
CvMat sub = cvMat( 3, 3, CV_32F, _sub );
float _t_trans[3];
CvMat t_trans = cvMat( 3, 1, CV_32F, _t_trans );
CvMat intrinsic = cvMat( 3, 3, CV_32F, _intrinsic );
CvMat homography = cvMat( 3, 3, CV_32F, _homography );
if( !line || !_center || !_intrinsic || !_homography )
CV_Error( CV_StsNullPtr, "" );
norm_xy = cvSqrt( line[0] * line[0] + line[1] * line[1] );
xy_cosa = line[0] / norm_xy;
xy_sina = line[1] / norm_xy;
norm_xz = cvSqrt( line[0] * line[0] + line[2] * line[2] );
xz_cosa = line[0] / norm_xz;
xz_sina = line[2] / norm_xz;
nx1 = -xz_sina;
_rz[0] = (float)(xy_cosa * nx1);
_rz[1] = (float)(xy_sina * nx1);
_rz[2] = (float)xz_cosa;
cvScale( &rz, &rz, 1./cvNorm(&rz,0,CV_L2) );
/* new axe y */
cvCrossProduct( &rz, &rx, &ry );
cvScale( &ry, &ry, 1./cvNorm( &ry, 0, CV_L2 ) );
/* transpone rotation matrix */
memcpy( &_r_trans[0], line, 3*sizeof(float));
memcpy( &_r_trans[3], _ry, 3*sizeof(float));
memcpy( &_r_trans[6], _rz, 3*sizeof(float));
/* calculate center distanse from arm plane */
plane_dist = cvDotProduct( ¢er, &rz );
/* calculate (I - r_trans)*center */
cvSetIdentity( &sub );
cvSub( &sub, &r_trans, &sub );
cvMatMul( &sub, ¢er, &t_trans );
cvMatMul( &t_trans, &rz, &sub );
cvScaleAdd( &sub, cvRealScalar(1./plane_dist), &r_trans, &sub ); /* ? */
cvMatMul( &intrinsic, &sub, &r_trans );
cvInvert( &intrinsic, &sub, CV_SVD );
cvMatMul( &r_trans, &sub, &homography );
}
bool CamAugmentation::CreateHomographyRotationTranslationMatrix( int c ){
int i;
// Get pointer to homography:
CvMat* m_homography = v_homography[c]->m_homography;
if( m_homography ){
// Vectors holding columns of H and R:
double a_H1[3];
CvMat m_H1 = cvMat( 3, 1, CV_64FC1, a_H1 );
for( i = 0; i < 3; i++ ) cvmSet( &m_H1, i, 0, cvmGet( m_homography, i, 0 ) );
double a_H2[3];
CvMat m_H2 = cvMat( 3, 1, CV_64FC1, a_H2 );
for( i = 0; i < 3; i++ ) cvmSet( &m_H2, i, 0, cvmGet( m_homography, i, 1 ) );
double a_H3[3];
CvMat m_H3 = cvMat( 3, 1, CV_64FC1, a_H3 );
for( i = 0; i < 3; i++ ) cvmSet( &m_H3, i, 0, cvmGet( m_homography, i, 2 ) );
double a_CinvH1[3];
CvMat m_CinvH1 = cvMat( 3, 1, CV_64FC1, a_CinvH1 );
double a_R1[3];
CvMat m_R1 = cvMat( 3, 1, CV_64FC1, a_R1 );
double a_R2[3];
CvMat m_R2 = cvMat( 3, 1, CV_64FC1, a_R2 );
double a_R3[3];
CvMat m_R3 = cvMat( 3, 1, CV_64FC1, a_R3 );
// The rotation matrix:
double a_R[9];
CvMat m_R = cvMat( 3, 3, CV_64FC1, a_R );
// The translation vector:
double a_T[3];
CvMat m_T = cvMat( 3, 1, CV_64FC1, a_T );
////////////////////////////////////////////////////////
// Create inverse calibration matrix:
CvMat* m_Cinv = InverseCalibrationMatrix( s_optimal.v_camera_c[c] );
// Create norming factor lambda:
cvGEMM( m_Cinv, &m_H1, 1, NULL, 0, &m_CinvH1, 0 );
// Search next orthonormal matrix:
if( cvNorm( &m_CinvH1, NULL, CV_L2, NULL ) != 0 ){
double lambda = 1.00/cvNorm( &m_CinvH1, NULL, CV_L2, NULL );
// Create normalized R1 & R2:
cvGEMM( m_Cinv, &m_H1, lambda, NULL, 0, &m_R1, 0 );
cvGEMM( m_Cinv, &m_H2, lambda, NULL, 0, &m_R2, 0 );
// Get R3 orthonormal to R1 and R2:
cvCrossProduct( &m_R1, &m_R2, &m_R3 );
// Put the rotation column vectors in the rotation matrix:
for( i = 0; i < 3; i++ ){
cvmSet( &m_R, i, 0, cvmGet( &m_R1, i, 0 ) );
cvmSet( &m_R, i, 1, cvmGet( &m_R2, i, 0 ) );
cvmSet( &m_R, i, 2, cvmGet( &m_R3, i, 0 ) );
}
// Calculate Translation Vector T (- because of its definition):
cvGEMM( m_Cinv, &m_H3, -lambda, NULL, 0, &m_T, 0 );
// Transformation of R into - in Frobenius sense - next orthonormal matrix:
double a_W[9]; CvMat m_W = cvMat( 3, 3, CV_64FC1, a_W );
double a_U[9]; CvMat m_U = cvMat( 3, 3, CV_64FC1, a_U );
double a_Vt[9]; CvMat m_Vt = cvMat( 3, 3, CV_64FC1, a_Vt );
cvSVD( &m_R, &m_W, &m_U, &m_Vt, CV_SVD_MODIFY_A | CV_SVD_V_T );
cvMatMul( &m_U, &m_Vt, &m_R );
// Put the rotation matrix and the translation vector together:
double a_view_to_cam[12];
CvMat m_view_to_cam = cvMat( 3, 4, CV_64FC1, a_view_to_cam );
CamCalibration::ComposeRotationTranslationTo3x4Matrix( &m_view_to_cam, &m_R, &m_T );
// Transfer to global reference coordinate system:
double a_cam_inv[12];
CvMat m_cam_inv = cvMat( 3, 4, CV_64FC1, a_cam_inv );
CamCalibration::Mat3x4Inverse( s_optimal.v_camera_r_t[c], &m_cam_inv );
CamCalibration::Mat3x4Mul( &m_cam_inv, &m_view_to_cam, v_homography_r_t );
return true;
}
}
return false;
}
void setviewfromhomography(CvMat* h**o)
{
CvMat* intrinsic = cvCreateMat(3,3, MAT_TYPE);
cvZero(intrinsic);
double fx = 531.398682;
double fy = 531.806702;
double cx = 308.162262;
double cy = 231.762756;
cvmSet(intrinsic, 0,0, fx);
cvmSet(intrinsic, 1,1, fy);
cvmSet(intrinsic, 0,2, cx);
cvmSet(intrinsic, 1,2, cy);
cvmSet(intrinsic, 2,2, 1);
CvMat* inv = cvCreateMat(3,3, MAT_TYPE);
cvInvert(intrinsic, inv);
CvMat* rdt0 = cvCreateMat(3,3, MAT_TYPE);
cvMatMul(inv, h**o, rdt0);
CvMat* rdt = cvCreateMat(3,3, MAT_TYPE);
double dvd = sqrt(cvmGet(rdt0,0, 0)*cvmGet(rdt0,0, 0)+cvmGet(rdt0,1, 0)*cvmGet(rdt0,1, 0)+cvmGet(rdt0,2, 0)*cvmGet(rdt0,2, 0));
if (dvd==0)
return;
double norm = (double) ((double) 1.0/dvd);
for(int i=0;i<3;i++){
for(int j=0;j<3;j++){
cvmSet(rdt, i,j, cvmGet(rdt0,i,j)*norm);
}
}
CvMat *r1 = cvCreateMat(3,1, MAT_TYPE);
CvMat *r2 = cvCreateMat(3,1, MAT_TYPE);
CvMat *r1xr2 = cvCreateMat(3,1, MAT_TYPE);
for (int i=0;i<3;i++)
{
cvmSet(r1, i, 0, cvmGet(rdt,i, 0));
cvmSet(r2, i, 0, cvmGet(rdt,i, 1));
}
cvCrossProduct(r1, r2, r1xr2);
CvMat* R = cvCreateMat(3,3, MAT_TYPE);
CvMat* t = cvCreateMat(3,1, MAT_TYPE);
cvZero(R);
cvZero(t);
for(int y=0;y<2;y++){
for(int x=0;x<3;x++){
cvmSet(R, x,y, cvmGet(rdt,x,y));
}
}
for(int x=0;x<3;x++){
cvmSet(R, x,2,-cvmGet(r1xr2,x,0));
cvmSet(t, 0,x,cvmGet(rdt,x,2));
}
setview(R, t, view);
cvReleaseMat(&R);
cvReleaseMat(&t);
cvReleaseMat(&r1);
cvReleaseMat(&r2);
cvReleaseMat(&rdt);
cvReleaseMat(&r1xr2);
cvReleaseMat(&rdt0);
cvReleaseMat(&inv);
cvReleaseMat(&intrinsic);
}
CvMat* CreateHomographyRotationTranslationMatrix( CvMat* m_homography,CvMat* m_intric )
{
//http://forum.openframeworks.cc/index.php?topic=2313.0
//This produces Relative Rotation
// Suppose you are at a some position P(X,Y,Z), and you are looking off in some direction D. Your position is represented by the translation matrix T, and the direction of your view is represented by the rotation matrix R. The combined information is held in the translation matrix Tr. We saw this at the beginning of the presentation:
int i;
// Vectors holding columns of H and R:
//Homography cvGetCol().
double a_H1[3];
CvMat m_H1 = cvMat( 3, 1, CV_64FC1, a_H1 );
for( i = 0; i < 3; i++ ) cvmSet( &m_H1, i, 0, cvmGet( m_homography, i, 0 ) );
double a_H2[3];
CvMat m_H2 = cvMat( 3, 1, CV_64FC1, a_H2 );
for( i = 0; i < 3; i++ ) cvmSet( &m_H2, i, 0, cvmGet( m_homography, i, 1 ) );
double a_H3[3];
CvMat m_H3 = cvMat( 3, 1, CV_64FC1, a_H3 );
for( i = 0; i < 3; i++ ) cvmSet( &m_H3, i, 0, cvmGet( m_homography, i, 2 ) );
double a_CinvH1[3];
CvMat m_CinvH1 = cvMat( 3, 1, CV_64FC1, a_CinvH1 );
double a_R1[3];
CvMat m_R1 = cvMat( 3, 1, CV_64FC1, a_R1 );
double a_R2[3];
CvMat m_R2 = cvMat( 3, 1, CV_64FC1, a_R2 );
double a_R3[3];
CvMat m_R3 = cvMat( 3, 1, CV_64FC1, a_R3 );
// The rotation matrix:
double a_R[9];
CvMat m_R = cvMat( 3, 3, CV_64FC1, a_R );
// The translation vector:
double a_T[3];
CvMat m_T = cvMat( 3, 1, CV_64FC1, a_T );
////////////////////////////////////////////////////////
// Create inverse calibration matrix:
CvMat* m_Cinv = inverseCalibrationMatrix( m_intric);// .
// Create norming factor lambda:
cvGEMM( m_Cinv, &m_H1, 1, NULL, 0, &m_CinvH1, 0 );
// Search next orthonormal matrix:
if( cvNorm( &m_CinvH1, NULL, CV_L2, NULL ) != 0 )
{
double lambda = 1.00/cvNorm( &m_CinvH1, NULL, CV_L2, NULL );
// Create normalized R1 & R2:
cvGEMM( m_Cinv, &m_H1, lambda, NULL, 0, &m_R1, 0 );
cvGEMM( m_Cinv, &m_H2, lambda, NULL, 0, &m_R2, 0 );
// Get R3 orthonormal to R1 and R2:
cvCrossProduct( &m_R1, &m_R2, &m_R3 );
// Put the rotation column vectors in the rotation matrix:
for( i = 0; i < 3; i++ ){
cvmSet( &m_R, i, 0, cvmGet( &m_R1, i, 0 ) );
cvmSet( &m_R, i, 1, cvmGet( &m_R2, i, 0 ) );
cvmSet( &m_R, i, 2, cvmGet( &m_R3, i, 0 ) );
}
// Calculate Translation Vector T (- because of its definition):
cvGEMM( m_Cinv, &m_H3, -lambda, NULL, 0, &m_T, 0 );
// Transformation of R into - in Frobenius sense - next orthonormal matrix:
double a_W[9]; CvMat m_W = cvMat( 3, 3, CV_64FC1, a_W );
double a_U[9]; CvMat m_U = cvMat( 3, 3, CV_64FC1, a_U );
double a_Vt[9]; CvMat m_Vt = cvMat( 3, 3, CV_64FC1, a_Vt );
cvSVD( &m_R, &m_W, &m_U, &m_Vt, CV_SVD_MODIFY_A | CV_SVD_V_T );
cvMatMul( &m_U, &m_Vt, &m_R );
// Put the rotation matrix and the translation vector together:
CvMat* m_view_to_cam = cvCreateMat( 3, 4, CV_64FC1 );
ComposeRotationTranslationTo3x4Matrix( m_view_to_cam, &m_R, &m_T );
return m_view_to_cam;
}
return 0;
}
