/*---------------------------------------------------------------
operator =
---------------------------------------------------------------*/
void CPose3DPDFParticles::copyFrom(const CPose3DPDF &o)
{
MRPT_START
CPose3DPDFParticles::CParticleList::const_iterator itSrc;
CPose3DPDFParticles::CParticleList::iterator itDest;
if (this == &o) return; // It may be used sometimes
if (o.GetRuntimeClass()==CLASS_ID(CPose3DPDFParticles))
{
const CPose3DPDFParticles *pdf = static_cast<const CPose3DPDFParticles*>( &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 CPose3D( *itSrc->d );
itDest->log_w = itSrc->log_w;
}
}
}
else
if (o.GetRuntimeClass()==CLASS_ID(CPose3DPDFGaussian))
{
THROW_EXCEPTION("TO DO!!");
}
MRPT_END
}
void CPose3DPDFParticles::copyFrom(const CPose3DPDF& o)
{
MRPT_START
CPose3DPDFParticles::CParticleList::const_iterator itSrc;
CPose3DPDFParticles::CParticleList::iterator itDest;
if (this == &o) return; // It may be used sometimes
if (o.GetRuntimeClass() == CLASS_ID(CPose3DPDFParticles))
{
const CPose3DPDFParticles* pdf =
dynamic_cast<const CPose3DPDFParticles*>(&o);
ASSERT_(pdf);
m_particles = pdf->m_particles;
}
else if (o.GetRuntimeClass() == CLASS_ID(CPose3DPDFGaussian))
{
THROW_EXCEPTION("TO DO!!");
}
MRPT_END
}
/*---------------------------------------------------------------
inverse
---------------------------------------------------------------*/
void CPose3DPDFParticles::inverse(CPose3DPDF& o) const
{
MRPT_START
ASSERT_(o.GetRuntimeClass() == CLASS_ID(CPose3DPDFParticles));
CPose3DPDFParticles* out = static_cast<CPose3DPDFParticles*>(&o);
// Prepare the output:
out->copyFrom(*this);
CPose3DPDFParticles::CParticleList::iterator it;
CPose3D zero(0, 0, 0);
for (it = out->m_particles.begin(); it != out->m_particles.end(); ++it)
*it->d = zero - *it->d;
MRPT_END
}
/*---------------------------------------------------------------
operator =
---------------------------------------------------------------*/
void CPose3DPDFParticles::copyFrom(const CPose3DPDF &o)
{
MRPT_START
CPose3DPDFParticles::CParticleList::const_iterator itSrc;
CPose3DPDFParticles::CParticleList::iterator itDest;
if (this == &o) return; // It may be used sometimes
if (o.GetRuntimeClass()==CLASS_ID(CPose3DPDFParticles))
{
const CPose3DPDFParticles *pdf = static_cast<const CPose3DPDFParticles*>( &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 CPose3D( *itSrc->d );
itDest->log_w = itSrc->log_w;
}
}
}
else
if (o.GetRuntimeClass()==CLASS_ID(CPose3DPDFGaussian))
{
THROW_EXCEPTION("TO DO!!");
/* CPose3DPDFGaussian *pdf = (CPosePDFGaussian*) &o;
int M = (int)m_particles.size();
std::vector<vector_float> parts;
std::vector<vector_float>::iterator partsIt;
mrpt::random::randomNormalMultiDimensionalMany(pdf->cov,M,parts);
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 CPose3D( ( pdf->mean.x + (*partsIt)[0] ),
( pdf->mean.y + (*partsIt)[1] ),
( pdf->mean.phi + (*partsIt)[2] ) );
itDest->d->normalizePhi();
}
*/
}
MRPT_END
}