/*--------------------------------------------------------------- getEstimatedCovariance ---------------------------------------------------------------*/ void CPosePDFSOG::getCovarianceAndMean(CMatrixDouble33 &estCov, CPose2D &estMean2D) const { size_t N = m_modes.size(); this->getMean(estMean2D); estCov.zeros(); if (N) { // 1) Get the mean: double sumW = 0; CMatrixDouble31 estMeanMat = CMatrixDouble31(estMean2D); CMatrixDouble33 temp; CMatrixDouble31 estMean_i; for (const_iterator it=m_modes.begin();it!=m_modes.end();++it) { double w; sumW += w = exp((it)->log_w); estMean_i = CMatrixDouble31( (it)->mean ); estMean_i -= estMeanMat; temp.multiply_AAt(estMean_i); temp+= (it)->cov; temp*=w; estCov += temp; } if (sumW!=0) estCov *= (1.0/sumW); } }
/*--------------------------------------------------------------- getCovarianceAndMean ---------------------------------------------------------------*/ void CPointPDFSOG::getCovarianceAndMean(CMatrixDouble33 &estCov, CPoint3D &p) const { size_t N = m_modes.size(); getMean(p); estCov.zeros(); if (N) { // 1) Get the mean: double sumW = 0; CMatrixDouble31 estMean = CMatrixDouble31(p); CListGaussianModes::const_iterator it; CMatrixDouble33 partCov; for (it=m_modes.begin();it!=m_modes.end();++it) { double w; sumW += w = exp(it->log_w); // estCov += w * ( it->val.cov + ((estMean_i-estMean)*(~(estMean_i-estMean))) ); CMatrixDouble31 estMean_i = CMatrixDouble31(it->val.mean); estMean_i -=estMean; partCov.multiply_AAt(estMean_i); partCov+=it->val.cov; partCov*=w; estCov += partCov; } if (sumW!=0) estCov *= (1.0/sumW); } }
/*--------------------------------------------------------------- getEstimatedCovariance ---------------------------------------------------------------*/ void CPosePDFParticles::getCovarianceAndMean(CMatrixDouble33 &cov, CPose2D &mean) const { cov.zeros(); getMean(mean); size_t i,n = m_particles.size(); double var_x=0,var_y=0,var_p=0,var_xy=0,var_xp=0,var_yp=0; double mean_phi = mean.phi(); if (mean_phi<0) mean_phi = M_2PI + mean_phi; double lin_w_sum = 0; for (i=0;i<n;i++) lin_w_sum+= exp( m_particles[i].log_w ); if (lin_w_sum==0) lin_w_sum=1; for (i=0;i<n;i++) { double w = exp( m_particles[i].log_w ) / lin_w_sum; // Manage 1 PI range: double err_x = m_particles[i].d->x() - mean.x(); double err_y = m_particles[i].d->y() - mean.y(); double err_phi = math::wrapToPi( fabs(m_particles[i].d->phi() - mean_phi) ); var_x+= square(err_x)*w; var_y+= square(err_y)*w; var_p+= square(err_phi)*w; var_xy+= err_x*err_y*w; var_xp+= err_x*err_phi*w; var_yp+= err_y*err_phi*w; } if (n<2) { // Not enought information to estimate the variance: } else { // Unbiased estimation of variance: cov(0,0) = var_x; cov(1,1) = var_y; cov(2,2) = var_p; cov(1,0) = cov(0,1) = var_xy; cov(2,0) = cov(0,2) = var_xp; cov(1,2) = cov(2,1) = var_yp; } }