/*---------------------------------------------------------------
getEstimatedPose
Returns an estimate of the pose, i.e. a "mean value", computed
as a weighted average over all m_particles.
---------------------------------------------------------------*/
void CPosePDFParticlesExtended::getMean(CPose2D &est) const
{
est = CPose2D(0,0,0);
CPose2D p;
size_t i,n = m_particles.size();
double phi,w,W=0;
double W_phi_R=0,W_phi_L=0;
double phi_R=0,phi_L=0;
if (!n) return;
// First: XY
// -----------------------------------
for (i=0;i<n;i++)
{
p = m_particles[i].d->pose;
w = exp(m_particles[i].log_w);
W += w;
est.x_incr( p.x() * w );
est.y_incr( p.y() * w );
// PHI is special:
phi = p.phi();
if (fabs(phi)>1.5707963267948966192313216916398f)
{
// LEFT HALF: 0,2pi
if (phi<0) phi = (M_2PI + phi);
phi_L += phi * w;
W_phi_L += w;
}
else
{
// RIGHT HALF: -pi,pi
phi_R += phi * w;
W_phi_R += w;
}
}
est*=(1/W);
// Next: PHI
// -----------------------------------
// The mean value from each side:
if (W_phi_L>0) phi_L /= W_phi_L; // [0,2pi]
if (W_phi_R>0) phi_R /= W_phi_R; // [-pi,pi]
// Left side to [-pi,pi] again:
if (phi_L>M_PI) phi_L = phi_L - M_2PI;
// The total mean:
est.phi( ((phi_L * W_phi_L + phi_R * W_phi_R )/(W_phi_L+W_phi_R)) );
}
