/*---------------------------------------------------------------
copyFrom
---------------------------------------------------------------*/
void CPosePDFGaussian::copyFrom(const CPosePDF &o)
{
if (this == &o) return; // It may be used sometimes
// Convert to gaussian pdf:
o.getMean(mean);
o.getCovariance(cov);
}
void CPosePDFParticles::copyFrom(const CPosePDF &o)
{
MRPT_START
CParticleList::iterator itDest;
CParticleList::const_iterator itSrc;
if (this == &o) return; // It may be used sometimes
if (o.GetRuntimeClass()==CLASS_ID(CPosePDFParticles))
{
const CPosePDFParticles *pdf = dynamic_cast<const CPosePDFParticles*>( &o );
ASSERT_(pdf);
// Both are m_particles:
m_particles = pdf->m_particles;
}
else
if (o.GetRuntimeClass()==CLASS_ID(CPosePDFGaussian))
{
const CPosePDFGaussian *pdf = static_cast<const CPosePDFGaussian*>( &o );
size_t M = m_particles.size();
std::vector<CVectorDouble> parts;
std::vector<CVectorDouble>::iterator partsIt;
randomGenerator.drawGaussianMultivariateMany(parts,M,pdf->cov);
clearParticles();
m_particles.resize(M);
for ( itDest = m_particles.begin(),partsIt=parts.begin();itDest!=m_particles.end();++itDest,++partsIt )
{
itDest->log_w = 0;
itDest->d.reset(new CPose2D(
(pdf->mean.x() + (*partsIt)[0]),
(pdf->mean.y() + (*partsIt)[1]),
(pdf->mean.phi() + (*partsIt)[2]))
);
itDest->d->normalizePhi();
}
}
MRPT_END
}
/*---------------------------------------------------------------
inverse
---------------------------------------------------------------*/
void CPosePDFParticlesExtended::inverse(CPosePDF &o) const
{
ASSERT_( o.GetRuntimeClass() == CLASS_ID(CPosePDFParticlesExtended) );
CPosePDFParticlesExtended *out = (CPosePDFParticlesExtended*) &o;
out->copyFrom( *this );
static CPose2D nullPose(0,0,0);
for (unsigned int i=0;i<out->m_particles.size();i++)
out->m_particles[i].d->pose = nullPose - out->m_particles[i].d->pose;
}
/*---------------------------------------------------------------
operator =
---------------------------------------------------------------*/
void CPosePDFParticles::copyFrom(const CPosePDF &o)
{
MRPT_START
CParticleList::iterator itDest;
CParticleList::const_iterator itSrc;
if (this == &o) return; // It may be used sometimes
if (o.GetRuntimeClass()==CLASS_ID(CPosePDFParticles))
{
const CPosePDFParticles *pdf = static_cast<const CPosePDFParticles*>( &o );
// Both are m_particles:
if (m_particles.size()==pdf->m_particles.size())
{
for ( itSrc=pdf->m_particles.begin(), itDest = m_particles.begin();
itSrc!=pdf->m_particles.end();
itSrc++, itDest++ )
{
(*itDest->d) = (*itSrc->d);
itDest->log_w = itSrc->log_w;
}
}
else
{
for ( itDest = m_particles.begin();itDest!=m_particles.end();itDest++ )
delete itDest->d;
m_particles.resize( pdf->m_particles.size() );
for ( itSrc=pdf->m_particles.begin(), itDest = m_particles.begin();
itSrc!=pdf->m_particles.end();
itSrc++, itDest++ )
{
itDest->d = new CPose2D( *itSrc->d );
itDest->log_w = itSrc->log_w;
}
}
}
else
if (o.GetRuntimeClass()==CLASS_ID(CPosePDFGaussian))
{
const CPosePDFGaussian *pdf = static_cast<const CPosePDFGaussian*>( &o );
size_t M = m_particles.size();
std::vector<vector_double> parts;
std::vector<vector_double>::iterator partsIt;
randomGenerator.drawGaussianMultivariateMany(parts,M,pdf->cov);
clearParticles();
m_particles.resize(M);
for ( itDest = m_particles.begin(),partsIt=parts.begin();itDest!=m_particles.end();itDest++,partsIt++ )
{
itDest->log_w = 0;
itDest->d = new CPose2D( ( pdf->mean.x() + (*partsIt)[0] ),
( pdf->mean.y() + (*partsIt)[1] ),
( pdf->mean.phi() + (*partsIt)[2] ) );
itDest->d->normalizePhi();
}
}
MRPT_END
}
/*---------------------------------------------------------------
operator =
---------------------------------------------------------------*/
void CPosePDFParticlesExtended::copyFrom(const CPosePDF &o)
{
MRPT_START
CParticleList::iterator itSrc,itDest;
if (this == &o) return; // It may be used sometimes
if (o.GetRuntimeClass()==CLASS_ID(CPosePDFParticlesExtended))
{
CPosePDFParticlesExtended *pdf = (CPosePDFParticlesExtended*) &o;
// Both are particles:
if (m_particles.size()==pdf->m_particles.size())
{
for ( itSrc=pdf->m_particles.begin(), itDest = m_particles.begin();
itSrc!=pdf->m_particles.end();
itSrc++, itDest++ )
{
(*itDest->d) = (*itSrc->d);
itDest->log_w = itSrc->log_w;
}
}
else
{
for ( itDest = m_particles.begin();itDest!=m_particles.end();itDest++ )
delete itDest->d;
m_particles.resize( pdf->m_particles.size() );
for ( itSrc=pdf->m_particles.begin(), itDest = m_particles.begin();
itSrc!=pdf->m_particles.end();
itSrc++, itDest++ )
{
itDest->d = new TExtendedCPose2D( *itSrc->d );
itDest->log_w = itSrc->log_w;
}
}
}
else
if (o.GetRuntimeClass()==CLASS_ID(CPosePDFGaussian))
{
CPosePDFGaussian *pdf = (CPosePDFGaussian*) &o;
int M = (int)m_particles.size();
std::vector<vector<double> > parts;
std::vector<vector<double> >::iterator partsIt;
randomGenerator.drawGaussianMultivariateMany(parts,M,pdf->cov);
for ( itDest = m_particles.begin();itDest!=m_particles.end();itDest++ )
delete itDest->d;
m_particles.clear();
m_particles.resize(M);
for ( itDest = m_particles.begin(),partsIt=parts.begin();itDest!=m_particles.end();itDest++,partsIt++ )
{
itDest->log_w = 0;
itDest->d = new TExtendedCPose2D();
itDest->d->pose.x( pdf->mean.x() + (*partsIt)[0] );
itDest->d->pose.x( pdf->mean.y() + (*partsIt)[1] );
itDest->d->pose.phi( pdf->mean.phi() + (*partsIt)[2] );
itDest->d->pose.normalizePhi();
}
}
MRPT_END
}
