//============================================================================
void AAM_IC::CalcHessian(CvMat* H, const CvMat* SD)
{
CvMat* HH = cvCreateMat(H->rows, H->cols, CV_64FC1);
cvMulTransposed(SD, HH, 0);// Equation (65)
cvInvert(HH, H, CV_SVD);
cvReleaseMat(&HH);
}
void ConformalResizing::Constrian(const ConstrainUnits& unit, CvMat*& M)
{
// Preprocess unit to make Matrix M less singular
double meanX(0), meanY(0);
for (int i = 0; i < unit.n; i++)
{
meanX += unit.pnts[i].x;
meanY += unit.pnts[i].y;
}
meanX /= unit.n;
meanY /= unit.n;
int n = unit.n * 2;
M = cvCreateMat(n, n, CV_64F);
CvMat* A = cvCreateMat(n, 4, CV_64F);
CvMat* Q = cvCreateMat(n, 4, CV_64F);
CvMat* P = cvCreateMat(4, 4, CV_64F);
// Initial A
cvZero(A);
for (int i = 0; i < unit.n; i++)
{
double x = unit.pnts[i].x - meanX;
double y = unit.pnts[i].y - meanY;
CV_MAT_ELEM(*A, double, 2*i, 0) = x;
CV_MAT_ELEM(*A, double, 2*i, 1) = -y;
CV_MAT_ELEM(*A, double, 2*i, 2) = 1;
CV_MAT_ELEM(*A, double, 2*i+1, 0) = y;
CV_MAT_ELEM(*A, double, 2*i+1, 1) = x;
CV_MAT_ELEM(*A, double, 2*i+1, 3) = 1;
}
cvMulTransposed(A, P, 1); // P = (A^T * A)
cvInvert(P, P, CV_SVD_SYM); // P = (A^T * A)^(-1)
cvMatMul(A, P, Q);
cvGEMM(Q, A, 1, NULL, 0, M, CV_GEMM_B_T);
// M = M - I
double* d = M->data.db;
for (int i = 0; i < n; i++, d += n+1)
{
*d -= 1;
}
cvReleaseMat(&A);
cvReleaseMat(&Q);
cvReleaseMat(&P);
}
/* log_weight_div_det[k] = -2*log(weights_k) + log(det(Sigma_k)))
covs[k] = cov_rotate_mats[k] * cov_eigen_values[k] * (cov_rotate_mats[k])'
cov_rotate_mats[k] are orthogonal matrices of eigenvectors and
cov_eigen_values[k] are diagonal matrices (represented by 1D vectors) of eigen values.
The <alpha_ik> is the probability of the vector x_i to belong to the k-th cluster:
<alpha_ik> ~ weights_k * exp{ -0.5[ln(det(Sigma_k)) + (x_i - mu_k)' Sigma_k^(-1) (x_i - mu_k)] }
We calculate these probabilities here by the equivalent formulae:
Denote
S_ik = -0.5(log(det(Sigma_k)) + (x_i - mu_k)' Sigma_k^(-1) (x_i - mu_k)) + log(weights_k),
M_i = max_k S_ik = S_qi, so that the q-th class is the one where maximum reaches. Then
alpha_ik = exp{ S_ik - M_i } / ( 1 + sum_j!=q exp{ S_ji - M_i })
*/
double CvEM::run_em( const CvVectors& train_data )
{
CvMat* centered_sample = 0;
CvMat* covs_item = 0;
CvMat* log_det = 0;
CvMat* log_weights = 0;
CvMat* cov_eigen_values = 0;
CvMat* samples = 0;
CvMat* sum_probs = 0;
log_likelihood = -DBL_MAX;
CV_FUNCNAME( "CvEM::run_em" );
__BEGIN__;
int nsamples = train_data.count, dims = train_data.dims, nclusters = params.nclusters;
double min_variation = FLT_EPSILON;
double min_det_value = MAX( DBL_MIN, pow( min_variation, dims ));
double likelihood_bias = -CV_LOG2PI * (double)nsamples * (double)dims / 2., _log_likelihood = -DBL_MAX;
int start_step = params.start_step;
int i, j, k, n;
int is_general = 0, is_diagonal = 0, is_spherical = 0;
double prev_log_likelihood = -DBL_MAX / 1000., det, d;
CvMat whdr, iwhdr, diag, *w, *iw;
double* w_data;
double* sp_data;
if( nclusters == 1 )
{
double log_weight;
CV_CALL( cvSet( probs, cvScalar(1.)) );
if( params.cov_mat_type == COV_MAT_SPHERICAL )
{
d = cvTrace(*covs).val[0]/dims;
d = MAX( d, FLT_EPSILON );
inv_eigen_values->data.db[0] = 1./d;
log_weight = pow( d, dims*0.5 );
}
else
{
w_data = inv_eigen_values->data.db;
if( params.cov_mat_type == COV_MAT_GENERIC )
cvSVD( *covs, inv_eigen_values, *cov_rotate_mats, 0, CV_SVD_U_T );
else
cvTranspose( cvGetDiag(*covs, &diag), inv_eigen_values );
cvMaxS( inv_eigen_values, FLT_EPSILON, inv_eigen_values );
for( j = 0, det = 1.; j < dims; j++ )
det *= w_data[j];
log_weight = sqrt(det);
cvDiv( 0, inv_eigen_values, inv_eigen_values );
}
log_weight_div_det->data.db[0] = -2*log(weights->data.db[0]/log_weight);
log_likelihood = DBL_MAX/1000.;
EXIT;
}
if( params.cov_mat_type == COV_MAT_GENERIC )
is_general = 1;
else if( params.cov_mat_type == COV_MAT_DIAGONAL )
is_diagonal = 1;
else if( params.cov_mat_type == COV_MAT_SPHERICAL )
is_spherical = 1;
/* In the case of <cov_mat_type> == COV_MAT_DIAGONAL, the k-th row of cov_eigen_values
contains the diagonal elements (variations). In the case of
<cov_mat_type> == COV_MAT_SPHERICAL - the 0-ths elements of the vectors cov_eigen_values[k]
are to be equal to the mean of the variations over all the dimensions. */
CV_CALL( log_det = cvCreateMat( 1, nclusters, CV_64FC1 ));
CV_CALL( log_weights = cvCreateMat( 1, nclusters, CV_64FC1 ));
CV_CALL( covs_item = cvCreateMat( dims, dims, CV_64FC1 ));
CV_CALL( centered_sample = cvCreateMat( 1, dims, CV_64FC1 ));
CV_CALL( cov_eigen_values = cvCreateMat( inv_eigen_values->rows, inv_eigen_values->cols, CV_64FC1 ));
CV_CALL( samples = cvCreateMat( nsamples, dims, CV_64FC1 ));
CV_CALL( sum_probs = cvCreateMat( 1, nclusters, CV_64FC1 ));
sp_data = sum_probs->data.db;
// copy the training data into double-precision matrix
for( i = 0; i < nsamples; i++ )
{
const float* src = train_data.data.fl[i];
double* dst = (double*)(samples->data.ptr + samples->step*i);
for( j = 0; j < dims; j++ )
dst[j] = src[j];
}
if( start_step != START_M_STEP )
{
for( k = 0; k < nclusters; k++ )
{
if( is_general || is_diagonal )
{
w = cvGetRow( cov_eigen_values, &whdr, k );
if( is_general )
cvSVD( covs[k], w, cov_rotate_mats[k], 0, CV_SVD_U_T );
else
cvTranspose( cvGetDiag( covs[k], &diag ), w );
w_data = w->data.db;
for( j = 0, det = 1.; j < dims; j++ )
det *= w_data[j];
if( det < min_det_value )
{
if( start_step == START_AUTO_STEP )
det = min_det_value;
else
EXIT;
}
log_det->data.db[k] = det;
}
else
{
d = cvTrace(covs[k]).val[0]/(double)dims;
if( d < min_variation )
{
if( start_step == START_AUTO_STEP )
d = min_variation;
else
EXIT;
}
cov_eigen_values->data.db[k] = d;
log_det->data.db[k] = d;
}
}
cvLog( log_det, log_det );
if( is_spherical )
cvScale( log_det, log_det, dims );
}
for( n = 0; n < params.term_crit.max_iter; n++ )
{
if( n > 0 || start_step != START_M_STEP )
{
// e-step: compute probs_ik from means_k, covs_k and weights_k.
CV_CALL(cvLog( weights, log_weights ));
// S_ik = -0.5[log(det(Sigma_k)) + (x_i - mu_k)' Sigma_k^(-1) (x_i - mu_k)] + log(weights_k)
for( k = 0; k < nclusters; k++ )
{
CvMat* u = cov_rotate_mats[k];
const double* mean = (double*)(means->data.ptr + means->step*k);
w = cvGetRow( cov_eigen_values, &whdr, k );
iw = cvGetRow( inv_eigen_values, &iwhdr, k );
cvDiv( 0, w, iw );
w_data = (double*)(inv_eigen_values->data.ptr + inv_eigen_values->step*k);
for( i = 0; i < nsamples; i++ )
{
double *csample = centered_sample->data.db, p = log_det->data.db[k];
const double* sample = (double*)(samples->data.ptr + samples->step*i);
double* pp = (double*)(probs->data.ptr + probs->step*i);
for( j = 0; j < dims; j++ )
csample[j] = sample[j] - mean[j];
if( is_general )
cvGEMM( centered_sample, u, 1, 0, 0, centered_sample, CV_GEMM_B_T );
for( j = 0; j < dims; j++ )
p += csample[j]*csample[j]*w_data[is_spherical ? 0 : j];
pp[k] = -0.5*p + log_weights->data.db[k];
// S_ik <- S_ik - max_j S_ij
if( k == nclusters - 1 )
{
double max_val = 0;
for( j = 0; j < nclusters; j++ )
max_val = MAX( max_val, pp[j] );
for( j = 0; j < nclusters; j++ )
pp[j] -= max_val;
}
}
}
CV_CALL(cvExp( probs, probs )); // exp( S_ik )
cvZero( sum_probs );
// alpha_ik = exp( S_ik ) / sum_j exp( S_ij ),
// log_likelihood = sum_i log (sum_j exp(S_ij))
for( i = 0, _log_likelihood = likelihood_bias; i < nsamples; i++ )
{
double* pp = (double*)(probs->data.ptr + probs->step*i), sum = 0;
for( j = 0; j < nclusters; j++ )
sum += pp[j];
sum = 1./MAX( sum, DBL_EPSILON );
for( j = 0; j < nclusters; j++ )
{
double p = pp[j] *= sum;
sp_data[j] += p;
}
_log_likelihood -= log( sum );
}
// check termination criteria
if( fabs( (_log_likelihood - prev_log_likelihood) / prev_log_likelihood ) < params.term_crit.epsilon )
break;
prev_log_likelihood = _log_likelihood;
}
// m-step: update means_k, covs_k and weights_k from probs_ik
cvGEMM( probs, samples, 1, 0, 0, means, CV_GEMM_A_T );
for( k = 0; k < nclusters; k++ )
{
double sum = sp_data[k], inv_sum = 1./sum;
CvMat* cov = covs[k], _mean, _sample;
w = cvGetRow( cov_eigen_values, &whdr, k );
w_data = w->data.db;
cvGetRow( means, &_mean, k );
cvGetRow( samples, &_sample, k );
// update weights_k
weights->data.db[k] = sum;
// update means_k
cvScale( &_mean, &_mean, inv_sum );
// compute covs_k
cvZero( cov );
cvZero( w );
for( i = 0; i < nsamples; i++ )
{
double p = probs->data.db[i*nclusters + k]*inv_sum;
_sample.data.db = (double*)(samples->data.ptr + samples->step*i);
if( is_general )
{
cvMulTransposed( &_sample, covs_item, 1, &_mean );
cvScaleAdd( covs_item, cvRealScalar(p), cov, cov );
}
else
for( j = 0; j < dims; j++ )
{
double val = _sample.data.db[j] - _mean.data.db[j];
w_data[is_spherical ? 0 : j] += p*val*val;
}
}
if( is_spherical )
{
d = w_data[0]/(double)dims;
d = MAX( d, min_variation );
w->data.db[0] = d;
log_det->data.db[k] = d;
}
else
{
if( is_general )
cvSVD( cov, w, cov_rotate_mats[k], 0, CV_SVD_U_T );
cvMaxS( w, min_variation, w );
for( j = 0, det = 1.; j < dims; j++ )
det *= w_data[j];
log_det->data.db[k] = det;
}
}
cvConvertScale( weights, weights, 1./(double)nsamples, 0 );
cvMaxS( weights, DBL_MIN, weights );
cvLog( log_det, log_det );
if( is_spherical )
cvScale( log_det, log_det, dims );
} // end of iteration process
//log_weight_div_det[k] = -2*log(weights_k/det(Sigma_k))^0.5) = -2*log(weights_k) + log(det(Sigma_k)))
if( log_weight_div_det )
{
cvScale( log_weights, log_weight_div_det, -2 );
cvAdd( log_weight_div_det, log_det, log_weight_div_det );
}
/* Now finalize all the covariation matrices:
1) if <cov_mat_type> == COV_MAT_DIAGONAL we used array of <w> as diagonals.
Now w[k] should be copied back to the diagonals of covs[k];
2) if <cov_mat_type> == COV_MAT_SPHERICAL we used the 0-th element of w[k]
as an average variation in each cluster. The value of the 0-th element of w[k]
should be copied to the all of the diagonal elements of covs[k]. */
if( is_spherical )
{
for( k = 0; k < nclusters; k++ )
cvSetIdentity( covs[k], cvScalar(cov_eigen_values->data.db[k]));
}
else if( is_diagonal )
{
for( k = 0; k < nclusters; k++ )
cvTranspose( cvGetRow( cov_eigen_values, &whdr, k ),
cvGetDiag( covs[k], &diag ));
}
cvDiv( 0, cov_eigen_values, inv_eigen_values );
log_likelihood = _log_likelihood;
__END__;
cvReleaseMat( &log_det );
cvReleaseMat( &log_weights );
cvReleaseMat( &covs_item );
cvReleaseMat( ¢ered_sample );
cvReleaseMat( &cov_eigen_values );
cvReleaseMat( &samples );
cvReleaseMat( &sum_probs );
return log_likelihood;
}
// finds perspective transformation H between
// the object plane and image plane,
// so that (sxi,syi,s) ~ H*(Xi,Yi,1)
void icvFindHomography( const CvMat* object_points,
const CvMat* image_points, CvMat* __H )
{
CvMat *_m = 0, *_M = 0;
CvMat *_L2 = 0;
CV_FUNCNAME( "cvFindHomography" );
__BEGIN__;
int h_type;
int i, k, count, count2;
CvPoint2D64f *m, *M;
CvPoint2D64f cm = {0,0}, sm = {0,0};
double inv_Hnorm[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 };
double H[9];
CvMat _inv_Hnorm = cvMat( 3, 3, CV_64FC1, inv_Hnorm );
CvMat _H = cvMat( 3, 3, CV_64FC1, H );
double LtL[9*9], LW[9], LV[9*9];
CvMat* _Lp;
double* L;
CvMat _LtL = cvMat( 9, 9, CV_64FC1, LtL );
CvMat _LW = cvMat( 9, 1, CV_64FC1, LW );
CvMat _LV = cvMat( 9, 9, CV_64FC1, LV );
CvMat _Hrem = cvMat( 3, 3, CV_64FC1, LV + 8*9 );
if( !CV_IS_MAT(image_points) || !CV_IS_MAT(object_points) ||
!CV_IS_MAT(__H) )
CV_ERROR( CV_StsBadArg, "one of arguments is not a valid matrix" );
h_type = CV_MAT_TYPE(__H->type);
if( h_type != CV_32FC1 && h_type != CV_64FC1 )
CV_ERROR( CV_StsUnsupportedFormat,
"Homography matrix must have 32fC1 or 64fC1 type" );
if( __H->rows != 3 || __H->cols != 3 )
CV_ERROR( CV_StsBadSize, "Homography matrix must be 3x3" );
count = MAX(image_points->cols, image_points->rows);
count2 = MAX(object_points->cols, object_points->rows);
if( count != count2 )
CV_ERROR( CV_StsUnmatchedSizes,
"Numbers of image and object points do not match" );
CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( icvConvertPointsHomogenious( image_points, _m ));
m = (CvPoint2D64f*)_m->data.ptr;
CV_CALL( _M = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( icvConvertPointsHomogenious( object_points, _M ));
M = (CvPoint2D64f*)_M->data.ptr;
// calculate the normalization transformation Hnorm.
for( i = 0; i < count; i++ )
cm.x += m[i].x, cm.y += m[i].y;
cm.x /= count; cm.y /= count;
for( i = 0; i < count; i++ )
{
double x = m[i].x - cm.x;
double y = m[i].y - cm.y;
sm.x += fabs(x); sm.y += fabs(y);
}
sm.x /= count; sm.y /= count;
inv_Hnorm[0] = sm.x;
inv_Hnorm[4] = sm.y;
inv_Hnorm[2] = cm.x;
inv_Hnorm[5] = cm.y;
sm.x = 1./sm.x;
sm.y = 1./sm.y;
CV_CALL( _Lp = _L2 = cvCreateMat( 2*count, 9, CV_64FC1 ) );
L = _L2->data.db;
for( i = 0; i < count; i++, L += 18 )
{
double x = -(m[i].x - cm.x)*sm.x, y = -(m[i].y - cm.y)*sm.y;
L[0] = L[9 + 3] = M[i].x;
L[1] = L[9 + 4] = M[i].y;
L[2] = L[9 + 5] = 1;
L[9 + 0] = L[9 + 1] = L[9 + 2] = L[3] = L[4] = L[5] = 0;
L[6] = x*M[i].x;
L[7] = x*M[i].y;
L[8] = x;
L[9 + 6] = y*M[i].x;
L[9 + 7] = y*M[i].y;
L[9 + 8] = y;
}
if( count > 4 )
{
cvMulTransposed( _L2, &_LtL, 1 );
_Lp = &_LtL;
}
_LW.rows = MIN(count*2, 9);
cvSVD( _Lp, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
cvScale( &_Hrem, &_Hrem, 1./_Hrem.data.db[8] );
cvMatMul( &_inv_Hnorm, &_Hrem, &_H );
if( count > 4 )
{
// reuse the available storage for jacobian and other vars
CvMat _J = cvMat( 2*count, 8, CV_64FC1, _L2->data.db );
CvMat _err = cvMat( 2*count, 1, CV_64FC1, _L2->data.db + 2*count*8 );
CvMat _JtJ = cvMat( 8, 8, CV_64FC1, LtL );
CvMat _JtErr = cvMat( 8, 1, CV_64FC1, LtL + 8*8 );
CvMat _JtJW = cvMat( 8, 1, CV_64FC1, LW );
CvMat _JtJV = cvMat( 8, 8, CV_64FC1, LV );
CvMat _Hinnov = cvMat( 8, 1, CV_64FC1, LV + 8*8 );
for( k = 0; k < 10; k++ )
{
double* J = _J.data.db, *err = _err.data.db;
for( i = 0; i < count; i++, J += 16, err += 2 )
{
double di = 1./(H[6]*M[i].x + H[7]*M[i].y + 1.);
double _xi = (H[0]*M[i].x + H[1]*M[i].y + H[2])*di;
double _yi = (H[3]*M[i].x + H[4]*M[i].y + H[5])*di;
err[0] = m[i].x - _xi;
err[1] = m[i].y - _yi;
J[0] = M[i].x*di;
J[1] = M[i].y*di;
J[2] = di;
J[8+3] = M[i].x;
J[8+4] = M[i].y;
J[8+5] = di;
J[6] = -J[0]*_xi;
J[7] = -J[1]*_xi;
J[8+6] = -J[8+3]*_yi;
J[8+7] = -J[8+4]*_yi;
J[3] = J[4] = J[5] = J[8+0] = J[8+1] = J[8+2] = 0.;
}
icvGaussNewton( &_J, &_err, &_Hinnov, &_JtJ, &_JtErr, &_JtJW, &_JtJV );
for( i = 0; i < 8; i++ )
H[i] += _Hinnov.data.db[i];
}
}
cvConvert( &_H, __H );
__END__;
cvReleaseMat( &_m );
cvReleaseMat( &_M );
cvReleaseMat( &_L2 );
}
void icvGaussNewton( const CvMat* J, const CvMat* err, CvMat* delta,
CvMat* JtJ, CvMat* JtErr, CvMat* JtJW, CvMat* JtJV )
{
CvMat* _temp_JtJ = 0;
CvMat* _temp_JtErr = 0;
CvMat* _temp_JtJW = 0;
CvMat* _temp_JtJV = 0;
CV_FUNCNAME( "icvGaussNewton" );
__BEGIN__;
if( !CV_IS_MAT(J) || !CV_IS_MAT(err) || !CV_IS_MAT(delta) )
CV_ERROR( CV_StsBadArg,
"Some of required arguments is not a valid matrix" );
if( !JtJ )
{
CV_CALL( _temp_JtJ = cvCreateMat( J->cols, J->cols, J->type ));
JtJ = _temp_JtJ;
}
else if( !CV_IS_MAT(JtJ) )
CV_ERROR( CV_StsBadArg, "JtJ is not a valid matrix" );
if( !JtErr )
{
CV_CALL( _temp_JtErr = cvCreateMat( J->cols, 1, J->type ));
JtErr = _temp_JtErr;
}
else if( !CV_IS_MAT(JtErr) )
CV_ERROR( CV_StsBadArg, "JtErr is not a valid matrix" );
if( !JtJW )
{
CV_CALL( _temp_JtJW = cvCreateMat( J->cols, 1, J->type ));
JtJW = _temp_JtJW;
}
else if( !CV_IS_MAT(JtJW) )
CV_ERROR( CV_StsBadArg, "JtJW is not a valid matrix" );
if( !JtJV )
{
CV_CALL( _temp_JtJV = cvCreateMat( J->cols, J->cols, J->type ));
JtJV = _temp_JtJV;
}
else if( !CV_IS_MAT(JtJV) )
CV_ERROR( CV_StsBadArg, "JtJV is not a valid matrix" );
cvMulTransposed( J, JtJ, 1 );
cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );
cvSVD( JtJ, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T );
cvSVBkSb( JtJW, JtJV, JtJV, JtErr, delta, CV_SVD_U_T + CV_SVD_V_T );
__END__;
if( _temp_JtJ || _temp_JtErr || _temp_JtJW || _temp_JtJV )
{
cvReleaseMat( &_temp_JtJ );
cvReleaseMat( &_temp_JtErr );
cvReleaseMat( &_temp_JtJW );
cvReleaseMat( &_temp_JtJV );
}
}
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;
}
/* for now this function works bad with singular cases
You can see in the code, that when some troubles with
matrices or some variables occur -
box filled with zero values is returned.
However in general function works fine.
*/
static void
icvFitEllipse_F( CvSeq* points, CvBox2D* box )
{
CvMat* D = 0;
CV_FUNCNAME( "icvFitEllipse_F" );
__BEGIN__;
double S[36], C[36], T[36];
int i, j;
double eigenvalues[6], eigenvectors[36];
double a, b, c, d, e, f;
double x0, y0, idet, scale, offx = 0, offy = 0;
int n = points->total;
CvSeqReader reader;
int is_float = CV_SEQ_ELTYPE(points) == CV_32FC2;
CvMat _S = cvMat(6,6,CV_64F,S), _C = cvMat(6,6,CV_64F,C), _T = cvMat(6,6,CV_64F,T);
CvMat _EIGVECS = cvMat(6,6,CV_64F,eigenvectors), _EIGVALS = cvMat(6,1,CV_64F,eigenvalues);
/* create matrix D of input points */
CV_CALL( D = cvCreateMat( n, 6, CV_64F ));
cvStartReadSeq( points, &reader );
/* shift all points to zero */
for( i = 0; i < n; i++ )
{
if( !is_float )
{
offx += ((CvPoint*)reader.ptr)->x;
offy += ((CvPoint*)reader.ptr)->y;
}
else
{
offx += ((CvPoint2D32f*)reader.ptr)->x;
offy += ((CvPoint2D32f*)reader.ptr)->y;
}
CV_NEXT_SEQ_ELEM( points->elem_size, reader );
}
offx /= n;
offy /= n;
// fill matrix rows as (x*x, x*y, y*y, x, y, 1 )
for( i = 0; i < n; i++ )
{
double x, y;
double* Dptr = D->data.db + i*6;
if( !is_float )
{
x = ((CvPoint*)reader.ptr)->x - offx;
y = ((CvPoint*)reader.ptr)->y - offy;
}
else
{
x = ((CvPoint2D32f*)reader.ptr)->x - offx;
y = ((CvPoint2D32f*)reader.ptr)->y - offy;
}
CV_NEXT_SEQ_ELEM( points->elem_size, reader );
Dptr[0] = x * x;
Dptr[1] = x * y;
Dptr[2] = y * y;
Dptr[3] = x;
Dptr[4] = y;
Dptr[5] = 1.;
}
// S = D^t*D
cvMulTransposed( D, &_S, 1 );
cvSVD( &_S, &_EIGVALS, &_EIGVECS, 0, CV_SVD_MODIFY_A + CV_SVD_U_T );
for( i = 0; i < 6; i++ )
{
double a = eigenvalues[i];
a = a < DBL_EPSILON ? 0 : 1./sqrt(sqrt(a));
for( j = 0; j < 6; j++ )
eigenvectors[i*6 + j] *= a;
}
// C = Q^-1 = transp(INVEIGV) * INVEIGV
cvMulTransposed( &_EIGVECS, &_C, 1 );
cvZero( &_S );
S[2] = 2.;
S[7] = -1.;
S[12] = 2.;
// S = Q^-1*S*Q^-1
cvMatMul( &_C, &_S, &_T );
cvMatMul( &_T, &_C, &_S );
// and find its eigenvalues and vectors too
//cvSVD( &_S, &_EIGVALS, &_EIGVECS, 0, CV_SVD_MODIFY_A + CV_SVD_U_T );
cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 );
for( i = 0; i < 3; i++ )
if( eigenvalues[i] > 0 )
break;
if( i >= 3 /*eigenvalues[0] < DBL_EPSILON*/ )
{
box->center.x = box->center.y =
box->size.width = box->size.height =
box->angle = 0.f;
EXIT;
}
// now find truthful eigenvector
_EIGVECS = cvMat( 6, 1, CV_64F, eigenvectors + 6*i );
_T = cvMat( 6, 1, CV_64F, T );
// Q^-1*eigenvecs[0]
cvMatMul( &_C, &_EIGVECS, &_T );
// extract vector components
a = T[0]; b = T[1]; c = T[2]; d = T[3]; e = T[4]; f = T[5];
///////////////// extract ellipse axes from above values ////////////////
/*
1) find center of ellipse
it satisfy equation
| a b/2 | * | x0 | + | d/2 | = |0 |
| b/2 c | | y0 | | e/2 | |0 |
*/
idet = a * c - b * b * 0.25;
idet = idet > DBL_EPSILON ? 1./idet : 0;
// we must normalize (a b c d e f ) to fit (4ac-b^2=1)
scale = sqrt( 0.25 * idet );
if( scale < DBL_EPSILON )
{
box->center.x = (float)offx;
box->center.y = (float)offy;
box->size.width = box->size.height = box->angle = 0.f;
EXIT;
}
a *= scale;
b *= scale;
c *= scale;
d *= scale;
e *= scale;
f *= scale;
x0 = (-d * c + e * b * 0.5) * 2.;
y0 = (-a * e + d * b * 0.5) * 2.;
// recover center
box->center.x = (float)(x0 + offx);
box->center.y = (float)(y0 + offy);
// offset ellipse to (x0,y0)
// new f == F(x0,y0)
f += a * x0 * x0 + b * x0 * y0 + c * y0 * y0 + d * x0 + e * y0;
if( fabs(f) < DBL_EPSILON )
{
box->size.width = box->size.height = box->angle = 0.f;
EXIT;
}
scale = -1. / f;
// normalize to f = 1
a *= scale;
b *= scale;
c *= scale;
// extract axis of ellipse
// one more eigenvalue operation
S[0] = a;
S[1] = S[2] = b * 0.5;
S[3] = c;
_S = cvMat( 2, 2, CV_64F, S );
_EIGVECS = cvMat( 2, 2, CV_64F, eigenvectors );
_EIGVALS = cvMat( 1, 2, CV_64F, eigenvalues );
cvSVD( &_S, &_EIGVALS, &_EIGVECS, 0, CV_SVD_MODIFY_A + CV_SVD_U_T );
// exteract axis length from eigenvectors
box->size.width = (float)(2./sqrt(eigenvalues[0]));
box->size.height = (float)(2./sqrt(eigenvalues[1]));
// calc angle
box->angle = (float)(180 - atan2(eigenvectors[2], eigenvectors[3])*180/CV_PI);
__END__;
cvReleaseMat( &D );
}
void pca(IplImage *image)
{
int i, j, k;
int width, height, step, channels;
unsigned char *src;
float *u;
unsigned char *b;
unsigned char *g;
height = image->height;
width = image->width;
step = image->widthStep;
channels = image->nChannels;
src = (unsigned char *)image->imageData;
u = (float *)malloc(sizeof(float) * height * channels);
b = (unsigned char *)malloc(sizeof(unsigned char) * width * height * channels);
g = (unsigned char *)malloc(sizeof(unsigned char) * height * channels);
//bg = (unsigned char *)malloc(sizeof(unsigned char) * width * height);
//bb = (unsigned char *)malloc(sizeof(unsigned char) * width * height);
init_floatMat(u, height * channels);
init_charMat(b, height * width * channels);
//init_mat(bg, height * width);
//init_mat(bb, height * widht);
init_charMat(g, height * channels);
//begin by calculating the empirical mean
//u[1..height] = 1/n sum(src[i,j])
for(i = 0; i < height; i++)
{
for(j = 0; j < width; j++)
{
for(k = 0; k < channels; k++)
{
//need to fix floating point arithmetic
u[i*channels + k] += (float)src[i*step+j*channels+k] / (float)width;
}
}
}
printf("empirical means working\n");
//we next calculate the deviation from the mean
//b = src[i,j] - u;
for(i = 0; i < height; i++)
{
for(j = 0; j < width; j++)
{
for(k = 0; k < channels; k++)
{
b[i*step+j*channels+k] = src[i*step+j*channels+k] - (int)u[i*channels+k];
}
}
}
printf("deviation working\n");
//we now need to find the covariance matrix
//b in opencv matrix form
CvMat bMat = cvMat(height, width, CV_8UC3, b);
//CvMat bgMat = cvMat(height, width, CV_8U3, bg);
//CvMat bbMat = cvMat(height, width, CV_8U3, bb);
CvMat *bMatb = cvCreateMat(height, width, CV_8UC1);
CvMat *bMatg = cvCreateMat(height, width, CV_8UC1);
CvMat *bMatr = cvCreateMat(height, width, CV_8UC1);
cvSplit(&bMat, bMatb, bMatg, bMatr, 0);
//covariance matrix
CvMat *cb = cvCreateMat(height, height, CV_32FC1);
CvMat *cg = cvCreateMat(height, height, CV_32FC1);
CvMat *cr = cvCreateMat(height, height, CV_32FC1);
CvMat *c = cvCreateMat(height, height, CV_32FC3);
cvMulTransposed(bMatb, cb, 0, NULL, 1.0/(double)width);
cvMulTransposed(bMatg, cg, 0, NULL, 1.0/(double)width);
cvMulTransposed(bMatr, cr, 0, NULL, 1.0/(double)width);
printf("Covariance Matrix working\n");
//eigenvector and values
CvMat *eMatb = cvCreateMat(height, height, CV_32FC1);
CvMat *lMatb = cvCreateMat(height, 1, CV_32FC1);
CvMat *eMatr = cvCreateMat(height, height, CV_32FC1);
CvMat *lMatr = cvCreateMat(height, 1, CV_32FC1);
CvMat *eMatg = cvCreateMat(height, height, CV_32FC1);
CvMat *lMatg = cvCreateMat(height, 1, CV_32FC1);
printf("Successfully created eigen matrices\n");
//cvEigenVV(cb, eMatb, lMatb, 1e-10, -1, -1);
//cvEigenVV(cg, eMatg, lMatg, 1e-10, -1, -1);
//cvEigenVV(cr, eMatr, lMatr, 1e-10, -1, -1);
cvSVD(cb, lMatb, eMatb, NULL, CV_SVD_U_T & CV_SVD_MODIFY_A);
cvSVD(cg, lMatg, eMatg, NULL, CV_SVD_U_T & CV_SVD_MODIFY_A);
cvSVD(cr, lMatr, eMatr, NULL, CV_SVD_U_T & CV_SVD_MODIFY_A);
printf("Eigentvectors and Eigenvalues passes\n");
unsigned char *lb = lMatb->data.ptr;
unsigned char *eb = eMatb->data.ptr;
unsigned char *lr = lMatr->data.ptr;
unsigned char *er = eMatr->data.ptr;
unsigned char *lg = lMatg->data.ptr;
unsigned char *eg = eMatg->data.ptr;
for(i = 0; i < height; i++)
{
for(j = 0; j < i+1; j++)
{
for(k = 0; k < channels; k++)
{
g[i*channels] += lb[i*channels+k];
}
}
}
printf("Successfully computed cumulative energy\n");
int L = 0;
float currVal = 0.0;
/*for(i = 0; i < height; i++)
{
if(currVal >= 0.9)
{
L = i;
break;
}
currVal = 0.0;
for(k = 0; k < channels; k++)
{
currVal += (float)g[i*channels+k] / (float)g[height - 3 + k];
}
}*/
L = 2;
printf("Successfully computed L with value of %d\n", L);
unsigned char *w;
w = (unsigned char *)malloc(sizeof(unsigned char) * height * L * channels);
for(i = 0; i < height; i++)
{
for(j = 0; j < L; j++)
{
//for(k = 0; k < channels; k++)
//{
w[i*(L*channels)+j*channels] = eb[i*height+j];
w[i*(L*channels)+j*channels+1] = eg[i*height+j];
w[i*(L*channels)+j*channels+2] = er[i*height+j];
//}
}
}
printf("Successfully created basis vectors\n");
unsigned char *s;
s = (unsigned char *)malloc(sizeof(unsigned char) * height * channels);
for(i = 0; i < height; i++)
{
s[i*channels] = sqrt(cb->data.ptr[i*cb->cols+i]);
s[i*channels+1] = sqrt(cg->data.ptr[i*cg->cols+i]);
s[i*channels+2] = sqrt(cr->data.ptr[i*cr->cols+i]);
}
printf("Successfully convertd source data to z-scores\n");
unsigned char *z;
z = (unsigned char *)malloc(sizeof(unsigned char) * height * width * channels);
for(i = 0; i < height; i++)
{
for(j = 0; j < width; j++)
{
for(k = 0; k < channels; k++)
{
z[i*step+j*channels+k] = (float)b[i*step+j*channels+k] / (float)s[i*channels+k];
}
}
}
printf("Successfully calculated Z\n");
//Projecting the z-scores of the data onto the new basis
//CvMat wMatb = cvMat(height, L, CV_32FC1, eb);
//CvMat wMatr = cvMat(height, L, CV_32FC1, er);
//CvMat wMatg = cvMat(height, L, CV_32FC1, eg);
CvMat wMat = cvMat(L, height, CV_32FC3, w);
//cvMerge(&wMatb, &wMatr, &wMatg, 0, wMat);
CvMat *wMatT = cvCreateMat(height, L, CV_32FC3);
cvTranspose(&wMat, wMatT);
//char *dat = wMatT->data.ptr;
/*for(i = 0; i < L; i++)
{
for(j = 0; j < height; j++)
{
for(k = 0; k < channels; k++)
{
printf("%d ", dat[i*L+j*channels+k]);
}
printf("\n");
}
}*/
//CvMat *wMatTb = cvCreateMat(height, height, CV_8UC1);
//CvMat *wMatTg = cvCreateMat(height, height, CV_8UC1);
//CvMat *wMatTr = cvCreateMat(height, height, CV_8UC1);
//cvSplit(wMatT, wMatTb, wMatTg, wMatTr, 0);
printf("Transpose of W\n");
CvMat zMat = cvMat(height, width, CV_32FC3, z);
//CvMat *zMatb = cvCreateMat(height, width, CV_8UC1);
//CvMat *zMatg = cvCreateMat(height, width, CV_8UC1);
//CvMat *zMatr = cvCreateMat(height, width, CV_8UC1);
//cvSplit(&zMat, zMatb, zMatg, zMatr, 0);
printf("created z matrix\n");
CvMat *yMat = cvCreateMat(height, width, CV_32FC3);
//CvMat *yMatb = cvCreateMat(height, width, CV_8UC1);
//CvMat *yMatg = cvCreateMat(height, width, CV_8UC1);
//CvMat *yMatr = cvCreateMat(height, width, CV_8UC1);
printf("computed y matrix\n");
char *wdat = wMatT->data.ptr;
char *zdat = zMat.data.ptr;
char *ydat = (char *)malloc(sizeof(char) * L * width * channels);
init_charMat(ydat, L*width*channels);
int r = 0;
for(i = 0; i < L; i++)
{
for(j = 0; j < width; j++)
{
for(k = 0; k < channels; k++)
{
for(r = 0; r < width; r++)
{
ydat[i*step+j*channels+k] += wdat[i*step+r*channels+k] * zdat[r*step+j*channels+k];
}
}
}
}
//char *fnorm = (char *)malloc(sizeof(char) * width);
/*for(i = 0; i < width * channels; i++)
{
printf("%d\n", ydat[i]);
}*/
/*float adotb = cvDotProduct(wMatT, &zMat);
float bdota = cvDotProduct(&zMat, wMatT);
float div = adotb/bdota;
cvDiv(NULL, &zMat, yMat, div);*/
//cvMul(wMatT, &zMat, yMat, 1.0);
//cvMul(wMatTg, zMatg, yMatg, 1.0);
//cvMul(wMatTr, zMatr, yMatr, 1.0);
printf("Matrix Multiply Successful\n");
//char *output = yMat->data.ptr;
//printf("height: %d width: %d channels: %d", height, width, channels);
for(i = 0; i < height; i++)
{
for(j = 0; j < width; j++)
{
for(k = 0; k < channels; k++)
{
src[i*step+j*channels+k] = src[i*step+j*channels+k] * ydat[(i*channels)+width+k];
}
}
}
printf("Successfully multiplied\n");
//cvMerge(yMatb, yMatg, yMatr, 0, yMat);
}
/* Optimization using Levenberg-Marquardt */
void cvLevenbergMarquardtOptimization(pointer_LMJac JacobianFunction,
pointer_LMFunc function,
/*pointer_Err error_function,*/
CvMat *X0,CvMat *observRes,CvMat *resultX,
int maxIter,double epsilon)
{
/* This is not sparse method */
/* Make optimization using */
/* func - function to compute */
/* uses function to compute jacobian */
/* Allocate memory */
CvMat *vectX = 0;
CvMat *vectNewX = 0;
CvMat *resFunc = 0;
CvMat *resNewFunc = 0;
CvMat *error = 0;
CvMat *errorNew = 0;
CvMat *Jac = 0;
CvMat *delta = 0;
CvMat *matrJtJ = 0;
CvMat *matrJtJN = 0;
CvMat *matrJt = 0;
CvMat *vectB = 0;
CV_FUNCNAME( "cvLevenbegrMarquardtOptimization" );
__BEGIN__;
if( JacobianFunction == 0 || function == 0 || X0 == 0 || observRes == 0 || resultX == 0 )
{
CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
}
if( !CV_IS_MAT(X0) || !CV_IS_MAT(observRes) || !CV_IS_MAT(resultX) )
{
CV_ERROR( CV_StsUnsupportedFormat, "Some of input parameters must be a matrices" );
}
int numVal;
int numFunc;
double valError;
double valNewError;
numVal = X0->rows;
numFunc = observRes->rows;
/* test input data */
if( X0->cols != 1 )
{
CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector X0 must be 1" );
}
if( observRes->cols != 1 )
{
CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector observed rusult must be 1" );
}
if( resultX->cols != 1 || resultX->rows != numVal )
{
CV_ERROR( CV_StsUnmatchedSizes, "Size of result vector X must be equals to X0" );
}
if( maxIter <= 0 )
{
CV_ERROR( CV_StsUnmatchedSizes, "Number of maximum iteration must be > 0" );
}
if( epsilon < 0 )
{
CV_ERROR( CV_StsUnmatchedSizes, "Epsilon must be >= 0" );
}
/* copy x0 to current value of x */
CV_CALL( vectX = cvCreateMat(numVal, 1, CV_64F) );
CV_CALL( vectNewX = cvCreateMat(numVal, 1, CV_64F) );
CV_CALL( resFunc = cvCreateMat(numFunc,1, CV_64F) );
CV_CALL( resNewFunc = cvCreateMat(numFunc,1, CV_64F) );
CV_CALL( error = cvCreateMat(numFunc,1, CV_64F) );
CV_CALL( errorNew = cvCreateMat(numFunc,1, CV_64F) );
CV_CALL( Jac = cvCreateMat(numFunc,numVal, CV_64F) );
CV_CALL( delta = cvCreateMat(numVal, 1, CV_64F) );
CV_CALL( matrJtJ = cvCreateMat(numVal, numVal, CV_64F) );
CV_CALL( matrJtJN = cvCreateMat(numVal, numVal, CV_64F) );
CV_CALL( matrJt = cvCreateMat(numVal, numFunc,CV_64F) );
CV_CALL( vectB = cvCreateMat(numVal, 1, CV_64F) );
cvCopy(X0,vectX);
/* ========== Main optimization loop ============ */
double change;
int currIter;
double alpha;
change = 1;
currIter = 0;
alpha = 0.001;
do {
/* Compute value of function */
function(vectX,resFunc);
/* Print result of function to file */
/* Compute error */
cvSub(observRes,resFunc,error);
//valError = error_function(observRes,resFunc);
/* Need to use new version of computing error (norm) */
valError = cvNorm(observRes,resFunc);
/* Compute Jacobian for given point vectX */
JacobianFunction(vectX,Jac);
/* Define optimal delta for J'*J*delta=J'*error */
/* compute J'J */
cvMulTransposed(Jac,matrJtJ,1);
cvCopy(matrJtJ,matrJtJN);
/* compute J'*error */
cvTranspose(Jac,matrJt);
cvmMul(matrJt,error,vectB);
/* Solve normal equation for given alpha and Jacobian */
do
{
/* Increase diagonal elements by alpha */
for( int i = 0; i < numVal; i++ )
{
double val;
val = cvmGet(matrJtJ,i,i);
cvmSet(matrJtJN,i,i,(1+alpha)*val);
}
/* Solve system to define delta */
cvSolve(matrJtJN,vectB,delta,CV_SVD);
/* We know delta and we can define new value of vector X */
cvAdd(vectX,delta,vectNewX);
/* Compute result of function for new vector X */
function(vectNewX,resNewFunc);
cvSub(observRes,resNewFunc,errorNew);
valNewError = cvNorm(observRes,resNewFunc);
currIter++;
if( valNewError < valError )
{/* accept new value */
valError = valNewError;
/* Compute relative change of required parameter vectorX. change = norm(curr-prev) / norm(curr) ) */
change = cvNorm(vectX, vectNewX, CV_RELATIVE_L2);
alpha /= 10;
cvCopy(vectNewX,vectX);
break;
}
else
{
alpha *= 10;
}
} while ( currIter < maxIter );
/* new value of X and alpha were accepted */
} while ( change > epsilon && currIter < maxIter );
/* result was computed */
cvCopy(vectX,resultX);
__END__;
cvReleaseMat(&vectX);
cvReleaseMat(&vectNewX);
cvReleaseMat(&resFunc);
cvReleaseMat(&resNewFunc);
cvReleaseMat(&error);
cvReleaseMat(&errorNew);
cvReleaseMat(&Jac);
cvReleaseMat(&delta);
cvReleaseMat(&matrJtJ);
cvReleaseMat(&matrJtJN);
cvReleaseMat(&matrJt);
cvReleaseMat(&vectB);
return;
}
