/*---------------------------------------------------------------
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;
}
}
