コード例 #1
0
ファイル: Kalman.cpp プロジェクト: tzwenn/alvar
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);
}
コード例 #2
0
ファイル: targetobject.cpp プロジェクト: xufango/contrib_bk
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);
		
}
コード例 #3
0
ファイル: Kalman.cpp プロジェクト: tzwenn/alvar
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);
}
コード例 #4
0
ファイル: Kalman.cpp プロジェクト: tzwenn/alvar
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);
}
コード例 #5
0
ファイル: Kalman.cpp プロジェクト: tzwenn/alvar
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);
}
コード例 #6
0
ファイル: cvcgsolve.cpp プロジェクト: caomw/l1cs
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 );
}
コード例 #7
0
ファイル: AAM_IC.cpp プロジェクト: HVisionSensing/aamlibrary
//============================================================================
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);
}
コード例 #8
0
ファイル: mlem.cpp プロジェクト: glo/ee384b
/* 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( &centered_sample );
    cvReleaseMat( &cov_eigen_values );
    cvReleaseMat( &samples );
    cvReleaseMat( &sum_probs );

    return log_likelihood;
}
コード例 #9
0
ファイル: AAM_Basic.cpp プロジェクト: Ahmedn1/ccv-hand
//============================================================================
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);
}
コード例 #10
0
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( &center, &rz );

    /* calculate (I - r_trans)*center */
    cvSetIdentity( &sub );
    cvSub( &sub, &r_trans, &sub );
    cvMatMul( &sub, &center, &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 );
}
コード例 #11
0
//============================================================================
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;
}
コード例 #12
0
ファイル: targetobject.cpp プロジェクト: xufango/contrib_bk
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;
	
	
}
	
	
}	
コード例 #13
0
ファイル: targetobject.cpp プロジェクト: xufango/contrib_bk
// 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");
}