void KalmanSensorCore::update_x(CvMat *x_pred, CvMat *x) {
// x = x_pred + K * (z - H*x_pred)
cvMatMul(H, x_pred, z_pred);
cvScaleAdd(z_pred, cvScalar(-1), z, z_residual);
cvMatMul(K, z_residual, x_gain);
cvScaleAdd(x_pred, cvScalar(1), x_gain, x);
}
void TargetObject::updateKalmanStateWithProb(float xxxx, float yyyy, float zzzz, float prob){
kalmanEstimateState(x, y, z, hue, weight);
CvScalar prob_scal = cvScalarAll(prob);
cvScaleAdd( kalman->state_post, prob_scal, 0, kalman->state_post);
cvScaleAdd( kalman->error_cov_post, prob_scal, 0,kalman->error_cov_post);
}
void Kalman::predict_P() {
// P_pred = F*P*trans(F) + Q
cvTranspose(F, F_trans);
cvMatMul(P, F_trans, P_pred);
cvMatMul(F, P_pred, P_pred);
cvScaleAdd(P_pred, cvScalar(1), Q, P_pred);
}
void KalmanSensor::update_P(CvMat *P_pred, CvMat *P) {
//P = (I - K*H) * P_pred
cvMatMul(K, H, P_tmp);
cvSetIdentity(P);
cvScaleAdd(P_tmp, cvScalar(-1), P, P);
cvMatMul(P, P_pred, P);
}
void KalmanSensor::update_K(CvMat *P_pred) {
// K = P * trans(H) * inv(H*P*trans(H) + R)
cvTranspose(H, H_trans);
cvMatMul(P_pred, H_trans, K);
cvMatMul(H, K, R_tmp);
cvScaleAdd(R_tmp, cvScalar(1), R, R_tmp);
cvInvert(R_tmp, R_tmp);
cvMatMul(H_trans, R_tmp, K);
cvMatMul(P_pred, K, K);
}
double cvCGSolve( CvMatOps AOps, void* userdata, CvMat* B, CvMat* X, CvTermCriteria term_crit )
{
cvZero( X );
CvMat* R = cvCloneMat( B );
double delta = cvDotProduct( R, R );
CvMat* D = cvCloneMat( R );
double delta0 = delta;
double bestres = 1.;
CvMat* BX = cvCloneMat( X );
CvMat* Q = cvCreateMat( X->rows, X->cols, CV_MAT_TYPE(X->type) );
for ( int i = 0; i < term_crit.max_iter; i++ )
{
AOps( D, Q, userdata );
double alpha = delta / cvDotProduct( D, Q );
cvScaleAdd( D, cvScalar(alpha), X, X );
if ( (i + 1) % 50 == 0 )
{
AOps( X, R, userdata );
cvSub( B, R, R );
} else {
cvScaleAdd( Q, cvScalar(-alpha), R, R );
}
double deltaold = delta;
delta = cvDotProduct( R, R );
double beta = delta / deltaold;
cvScaleAdd( D, cvScalar(beta), R, D );
double res = delta / delta0;
if ( res < bestres )
{
cvCopy( X, BX );
bestres = res;
}
if ( delta < delta0 * term_crit.epsilon )
break;
}
cvCopy( BX, X );
cvReleaseMat( &R );
cvReleaseMat( &D );
cvReleaseMat( &BX );
cvReleaseMat( &Q );
return sqrt( bestres );
}
//============================================================================
void AAM_IC::CalcModifiedSD(CvMat* SD, const CvMat* dTx, const CvMat* dTy,
const CvMat* Jx, const CvMat* Jy)
{
int i, j;
//create steepest descent images
double* _x = dTx->data.db;
double* _y = dTy->data.db;
double temp;
for(i = 0; i < __shape.nModes()+4; i++)
{
for(j = 0; j < __paw.nPix(); j++)
{
temp = _x[3*j ]*cvmGet(Jx,j,i) +_y[3*j ]*cvmGet(Jy,j,i);
cvmSet(SD,i,3*j,temp);
temp = _x[3*j+1]*cvmGet(Jx,j,i) +_y[3*j+1]*cvmGet(Jy,j,i);
cvmSet(SD,i,3*j+1,temp);
temp = _x[3*j+2]*cvmGet(Jx,j,i) +_y[3*j+2]*cvmGet(Jy,j,i);
cvmSet(SD,i,3*j+2,temp);
}
}
//project out appearance variation (and linear lighting parameters)
const CvMat* B = __texture.GetBases();
CvMat* V = cvCreateMat(4+__shape.nModes(), __texture.nModes(), CV_64FC1);
CvMat SDMat, BMat;
cvGEMM(SD, B, 1., NULL, 1., V, CV_GEMM_B_T);
// Equation (63),(64)
for(i = 0; i < __shape.nModes()+4; i++)
{
for(j = 0; j < __texture.nModes(); j++)
{
cvGetRow(SD, &SDMat, i);
cvGetRow(B, &BMat, j);
cvScaleAdd(&BMat, cvScalar(-cvmGet(V,i,j)), &SDMat, &SDMat);
}
}
cvReleaseMat(&V);
}
/* 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;
}
//============================================================================
void AAM_Basic::Fit(const IplImage* image, AAM_Shape& Shape,
int max_iter /* = 30 */,bool showprocess /* = false */)
{
//intial some stuff
double t = gettime;
double e1, e2;
const int np = 5;
double k_values[np] = {1, 0.5, 0.25, 0.125, 0.0625};
int k;
IplImage* Drawimg = 0;
Shape.Point2Mat(__s);
InitParams(image);
CvMat subcq;
cvGetCols(__current_c_q, &subcq, 0, 4); cvCopy(__q, &subcq);
cvGetCols(__current_c_q, &subcq, 4, 4+__cam.nModes()); cvCopy(__c, &subcq);
//calculate error
e1 = EstResidual(image, __current_c_q, __s, __t_m, __t_s, __delta_t);
//do a number of iteration until convergence
for(int iter = 0; iter <max_iter; iter++)
{
if(showprocess)
{
if(Drawimg == 0) Drawimg = cvCloneImage(image);
else cvCopy(image, Drawimg);
__cam.CalcShape(__s, __current_c_q);
Shape.Mat2Point(__s);
Draw(Drawimg, Shape, 2);
#ifdef TARGET_WIN32
mkdir("result");
#else
mkdir("result", S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
#endif
char filename[100];
sprintf(filename, "result/ter%d.bmp", iter);
cvSaveImage(filename, Drawimg);
}
// predict parameter update
cvGEMM(__delta_t, __G, 1, NULL, 0, __delta_c_q, CV_GEMM_B_T);
//force first iteration
if(iter == 0)
{
cvAdd(__current_c_q, __delta_c_q, __current_c_q);
CvMat c; cvGetCols(__current_c_q, &c, 4, 4+__cam.nModes());
//constrain parameters
__cam.Clamp(&c);
e1 = EstResidual(image, __current_c_q, __s, __t_m, __t_s, __delta_t);
}
//find largest step size which reduces texture EstResidual
else
{
for(k = 0; k < np; k++)
{
cvScaleAdd(__delta_c_q, cvScalar(k_values[k]), __current_c_q, __update_c_q);
//constrain parameters
CvMat c; cvGetCols(__update_c_q, &c, 4, 4+__cam.nModes());
__cam.Clamp(&c);
e2 = EstResidual(image, __update_c_q, __s, __t_m, __t_s, __delta_t);
if(e2 <= e1) break;
}
}
//check for convergence
if(iter > 0)
{
if(k == np)
{
e1 = e2;
cvCopy(__update_c_q, __current_c_q);
}
else if(fabs(e2-e1)<0.001*e1) break;
else if (cvNorm(__delta_c_q)<0.001) break;
else
{
cvCopy(__update_c_q, __current_c_q);
e1 = e2;
}
}
}
cvReleaseImage(&Drawimg);
__cam.CalcShape(__s, __current_c_q);
Shape.Mat2Point(__s);
t = gettime - t;
printf("AAM-Basic Fitting time cost: %.3f millisec\n", t);
}
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 );
}
//============================================================================
int AAM_Basic::Fit(const IplImage* image, AAM_Shape& Shape,
int max_iter /* = 30 */,bool showprocess /* = false */)
{
//intial some stuff
double t = curtime;
double e1, e2, e3;
double k_v[6] = {-1,-1.15,-0.7,-0.5,-0.2,-0.0625};
Shape.Point2Mat(__current_s);
InitParams(image, __current_s, __current_c);
__cam.__shape.CalcParams(__current_s, __p, __current_q);
cvZero(__current_c);
IplImage* Drawimg =
cvCreateImage(cvGetSize(image), image->depth, image->nChannels);
//mkdir("result");
//char filename[100];
//calculate error
e3 = EstResidual(image, __current_c, __current_s, __delta_t);
if(e3 == -1) return 0;
int iter;
//do a number of iteration until convergence
for( iter = 0; iter <max_iter; iter++)
{
// predict pose and parameter update
// __delta_t rosszul számolódik. Kiiratás ld. AAM_Sahpe::Mat2Point()
//cvGEMM(__delta_t, __Rq, 1, NULL, 0, __delta_q, CV_GEMM_B_T);
cvGEMM(__delta_t, __Rc, 1, NULL, 0, __delta_c, CV_GEMM_B_T);
// if the prediction above didn't improve th fit,
// try amplify and later damp the prediction
for(int k = 0; k < 6; k++)
{
cvScaleAdd(__delta_q, cvScalar(k_v[k]), __current_q, __update_q);
cvScaleAdd(__delta_c, cvScalar(k_v[k]), __current_c, __update_c);
__cam.Clamp(__update_c);//constrain parameters
e2 = EstResidual(image, __update_c, __current_s, __delta_t);
if(k==0) e1 = e2;
else if(e2 != -1 && e2 < e1)break;
}
//check for convergence
if((iter>max_iter/3&&fabs(e2-e3)<0.01*e3) || e2<0.001 )
{
break;
}
else if (cvNorm(__delta_c)<0.001 && cvNorm(__delta_q)<0.001)
{
break;
}
else
{
cvCopy(__update_q, __current_q);
cvCopy(__update_c, __current_c);
e3 = e2;
}
}
__cam.CalcShape(__current_s, __current_c, __current_q);
Shape.Mat2Point(__current_s);
t = curtime - t;
if( AAM_DEBUG_MODE ) printf("AAM-Basic Fitting time cost: %.3f\n", t);
return iter;
}
void TargetObject::updateJPDAstate(CvMat* trans_mat){
/// set the transition matrix
cvCopy(trans_mat, kalman->transition_matrix,0);
///printMatrix(kalman->transition_matrix, "transition matrix");
//cvCopy(control_mat, kalman->control_matrix,0);
/// as in Shashtry paper
//std::cout << " shastry ok 1 " << std::endl;
/// compute the event probs total sum
float allProbSum = 0;
//std::cout << "Nr of events = " << event_probs->size() << std::endl;
for (int i = 0; i < event_probs->size(); i ++){
//std::cout << "event Nr = " << i << " prob = " << event_probs->at(i) << std::endl;
allProbSum = allProbSum + event_probs->at(i);
}
if (allProbSum > 0){
//std::cout << " ok 2 " << std::endl;combined_innov
///printMatrix(kalman->measurement_noise_cov, "mes-noice-cov");
///printMatrix(kalman->error_cov_pre, "error-cov-pre");
/// compute the covariance of combined innovation
//std::cout << "targobj pos 1" << std::endl;
//printMatrix(kalman->error_cov_pre, "error cov pre");
cvMatMul(kalman->measurement_matrix,kalman->error_cov_pre, tempCov1);
//std::cout << "targobj pos 2" << std::endl;
cvTranspose(kalman->measurement_matrix, tempCov2);
//std::cout << "targobj pos 3" << std::endl;
cvMatMul(tempCov1, tempCov2, tempCov3);
//std::cout << "targobj pos 4" << std::endl;
cvAdd(kalman->measurement_noise_cov, tempCov3, combInnovCov);
//std::cout << "targobj pos 5" << std::endl;
///printMatrix(combInnovCov, "comb innov cov");
/// compute the combined Kalman Gain
cvTranspose(kalman->measurement_matrix, tempCov2);
///printMatrix(tempCov2, "tempCov2 1");
///printMatrix(kalman->error_cov_post, "kalman->error_cov_post");
//std::cout << "targobj pos 5.5" << std::endl;
cvMatMul(kalman->error_cov_post, tempCov2, tempCov2);
///printMatrix(tempCov2, "tempCov2 2");
//std::cout << "targobj pos 6" << std::endl;
cvInvert(combInnovCov, tempCov3, CV_LU);
///printMatrix(tempCov3, "tempCov3");
//std::cout << "targobj pos 7" << std::endl;
cvMatMul(tempCov2, tempCov3, kalman->gain);
//std::cout << "targobj pos 8" << std::endl;
///printMatrix(kalman->gain, "comb kalman gain");
///---------------- upate the state estimate-------------------------
///printMatrix(combined_innov, "targ: update state: combined_innov");
cvMatMulAdd( kalman->gain, combined_innov, 0, tempo2 );
//std::cout << " ok 4.1 " << std::endl;
cvAdd( tempo2, kalman->state_post, tempo2, 0 );
//std::cout << " ok 4.2 " << std::endl;
cvCopy(tempo2, kalman->state_post, 0);
///printMatrix(kalman->state_post, "kalman->state_post");
//std::cout << " ok 5 " << std::endl;
/// --------------- compute the state estimation covariance ---------
cvTranspose(kalman->gain,tempCov1 );
//std::cout << " ok 5.1 " << std::endl;
cvMatMul(kalman->gain, combInnovCov, tempCov2);
//std::cout << " ok 5.2 " << std::endl;
cvMatMul(tempCov2, tempCov1, tempCov4);
//std::cout << " ok 5.3 " << std::endl;
cvSet(tempCov5,cvScalarAll(0),0);
//std::cout << " ok 5.4 " << std::endl;
CvScalar prob_scal = cvScalarAll(allProbSum);
//std::cout << " targobj: ok 5.5 " << std::endl;
cvScaleAdd( tempCov4, prob_scal, tempCov5, tempCov5);
//std::cout << " targobj: ok 5.6 " << std::endl;
cvSub(kalman->error_cov_post, tempCov5, tempCov4, 0);
// so until now tempCov4 has the result
//std::cout << " ok 6 " << std::endl;
// now compute the middle bracket
cvSet(tempCov2,cvScalarAll(0),0);
cvSet(tempCov3,cvScalarAll(0),0); // just temp
cvSet(tempCov6,cvScalarAll(0),0);
//std::cout << " ok 6.1..events_prob size = " << event_probs->size() << std::endl;
for (int i = 0; i < event_probs->size(); i ++){
//std::cout << " ok 6.n2 " << std::endl;
CvScalar tmpSca = cvScalarAll(event_probs->at(i));
//std::cout << " ok 6.n3 " << std::endl;
cvTranspose(all_innovs->at(i), tempoTrans1);
//std::cout << " ok 6.n4 " << std::endl;
cvMatMul(all_innovs->at(i), tempoTrans1, tempCov3);
//std::cout << " ok 6.n5 " << std::endl;
cvScaleAdd( tempCov3, tmpSca, tempCov6, tempCov6);
//std::cout << " ok 6.n6 " << std::endl;
//cvCopy(tempCov1, tempCov2);
//std::cout << " ok 6.n7 " << std::endl;
}
/// tempCov6 has the result (which is the sum in the middle bracket)
//std::cout << " ok 6.2 " << std::endl;
cvTranspose(combined_innov, tempoTrans1);
//std::cout << " ok 6.3 " << std::endl;
cvMatMul(combined_innov, tempoTrans1, tempCov3);
//std::cout << " ok 6.4 " << std::endl;
cvSub(tempCov6, tempCov3, tempCov6);
//std::cout << " ok 7 " << std::endl;
// the last sum in the equation in paper
cvTranspose(kalman->gain, tempCov1);
cvMatMul(kalman->gain, tempCov6, tempCov2);
cvMatMul(tempCov2, tempCov1, tempCov5);
//std::cout << " ok 8 " << std::endl;
// now add with tempCov4
cvAdd( tempCov5, tempCov4, kalman->error_cov_post, 0 );
//std::cout << " ok 9 " << std::endl;
//printMatrix(kalman->gain, "gain");
//printMatrix(combInnovCov, "combInnovCov");
//printMatrix(kalman->error_cov_post, "estimate cov ");
//printMatrix(kalman->state_post, "kalman->state_post");
/// set x,y,z
CvMat* Me = kalman->state_post;
int stepMe = Me->step/sizeof(float);
float *dataMe = Me->data.fl;
/// save old state
xOld = x;
yOld = y;
zOld = z;
vxOld = vx;
vyOld = vy;
vzOld = vz;
hueOld = hue;
weightOld = weight;
x = (dataMe+0*stepMe)[0];
y = (dataMe+1*stepMe)[0];
z = (dataMe+2*stepMe)[0];
vx = (dataMe+3*stepMe)[0];
vy = (dataMe+4*stepMe)[0];
vz = (dataMe+5*stepMe)[0];
hue = (dataMe+9*stepMe)[0];
weight = (dataMe+10*stepMe)[0];
if (isnan(x)){
x = xOld;
}
if (isnan(y)){
y = yOld;
}
if (isnan(z)){
z = zOld;
}
if (isnan(vx)){
vx = vxOld;
}
if (isnan(vy)){
vy = vyOld;
}
if (isnan(vz)){
vz = vzOld;
}
if (isnan(hue)){
std::cout << "hue is nan in updateJPDAstate" << std::endl;
hue = hueOld;
}
if (isnan(weight)){
std::cout << "weight is nan in updateJPDAstate" << std::endl;
weight = weightOld;
}
/// ------------------------------------------------------------------
timer.setActiveTime();
/// error_cov_post is increased manually (24 July 2007 ZM)
//cvAdd( kalman->error_cov_post, extra_error , kalman->error_cov_post);
//std::cout << "x,y,vx,vy = " << x << ", " << y << ", " << vx << ", " << vy << std::endl;
}
}
// stores this event for the target
void TargetObject::storeJPDAevent(float xx, float yy, float zz, float huee, float weightt, float prob){
//std::cout << "in store JPDA event: xx, yy, prob = " << xx << ", " << yy << ", " << huee << ", " << weightt << ", " << prob << std::endl;
high_dim_data newData;
newData.x = xx;
newData.y = yy;
newData.z = zz;
if (!hueDataAvailable){
newData.hue = hue;
}
else{
if ((abs(currentHueValue - hue) < 4.5) || (hue <= 0.0)){
newData.hue = currentHueValue;
// reduce attentional prioriy
attentionalPriority = attentionalPriority / 2.0; //0. +
// ((float)rand()/(float)RAND_MAX);
if (attentionalPriority < 1.0){attentionalPriority = ((float)rand()/(float)RAND_MAX); }
}
else{
std::cout << "------------------------> hue diff = " << abs(currentHueValue - hue) << ", hue = " << hue << std::endl;
newData.hue = hue + (currentHueValue - hue)/2.0;
hueDataAvailable = false;
}
}
newData.weight = weightt;
all_measurements->push_back(newData);
event_probs->push_back(prob);
/// compute the innovation
CvMat* tempo3 = cvCreateMat( MES_SIZE, 1, CV_32FC1 );
cvmSet( tempo1, 0, 0, xx);
cvmSet( tempo1, 1, 0, yy);
cvmSet( tempo1, 2, 0, zz);
cvmSet( tempo1, 3, 0, huee);
cvmSet( tempo1, 4, 0, weightt);
cvmSet( measurement_pred, 0, 0, cvmGet(kalman->state_pre,0,0));
cvmSet( measurement_pred, 1, 0, cvmGet(kalman->state_pre,1,0));
cvmSet( measurement_pred, 2, 0, cvmGet(kalman->state_pre,6,0));
cvmSet( measurement_pred, 3, 0, cvmGet(kalman->state_pre,7,0));
cvSub(tempo1, measurement_pred, tempo3, 0);
///printMatrix(tempo1, "target tempo 1: measurement");
///printMatrix(kalman->state_pre, "kalman->state_pre");
/// now scale this with the prob
///cvConvertScale( tempo3, tempo3, prob, 0 );
///printMatrix(tempo3, "temppo 3 innovation");
/// insert this innovation into all_innovs
all_innovs->push_back(tempo3);
/// update the combined innovation
//std::cout << "------------------------> prob = " << prob << std::endl;
CvScalar prob_scal = cvScalarAll(prob);
// std::cout << "targobj: before cvScaleAdd" << std::endl;
// std::cout << "tempo3: " << tempo3->rows << "," << tempo3->cols << std::endl;
//
// std::cout << "combined_innov: " << combined_innov->rows << "," << combined_innov->cols << std::endl;
// std::cout << "tempo1: " << tempo1-> rows << "," << tempo1->cols << std::endl;
cvScaleAdd( tempo3, prob_scal, combined_innov, tempo1);
//std::cout << "targobj: after cvScaleAdd" << std::endl;
cvCopy(tempo1, combined_innov, 0);
///printMatrix(combined_innov, "combined_innov...1");
}