/*---------------------------------------------------------------
copyFrom 3D
---------------------------------------------------------------*/
void CPosePDFGaussian::copyFrom(const CPose3DPDF &o)
{
// Convert to gaussian pdf:
mean = CPose2D(o.getMeanVal());
CMatrixDouble66 C;
o.getCovariance(C);
// Clip to 3x3:
C(2,0)=C(0,2) = C(0,3);
C(2,1)=C(1,2) = C(1,3);
C(2,2)= C(3,3);
cov = C.block(0,0,3,3);
}
/*---------------------------------------------------------------
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
}
// CSimpleMap
void CSimpleMap_insert(CSimpleMap &self, CPose3DPDF &in_posePDF, CSensoryFrame &in_SF)
{
// create smart pointers
CPose3DPDFPtr in_posePDFPtr = (CPose3DPDFPtr) in_posePDF.duplicateGetSmartPtr();
CSensoryFramePtr in_SFPtr = (CSensoryFramePtr) in_SF.duplicateGetSmartPtr();
// insert smart pointers
self.insert(in_posePDFPtr, in_SFPtr);
}
void CEllipsoid_setFromPosePDF(CEllipsoid& self, CPose3DPDF& posePDF)
{
CPose3D meanPose;
CMatrixDouble66 COV;
posePDF.getCovarianceAndMean(COV, meanPose);
CMatrixDouble33 COV3 = COV.block(0,0,3,3);
self.setLocation(meanPose.x(), meanPose.y(), meanPose.z() + 0.001 );
self.setCovMatrix(COV3, COV3(2,2)==0 ? 2:3 );
}
// CSimpleMap
void CSimpleMap_insert(
CSimpleMap& self, CPose3DPDF& in_posePDF, CSensoryFrame& in_SF)
{
// create smart pointers
CPose3DPDF::Ptr in_posePDFPtr = std::dynamic_pointer_cast<CPose3DPDF>(
in_posePDF.duplicateGetSmartPtr());
CSensoryFrame::Ptr in_SFPtr =
std::dynamic_pointer_cast<CSensoryFrame>(in_SF.duplicateGetSmartPtr());
// insert smart pointers
self.insert(in_posePDFPtr, in_SFPtr);
}
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 CPose3DPDFGaussianInf::copyFrom(const CPose3DPDF &o)
{
if (this == &o) return; // It may be used sometimes
if (IS_CLASS(&o, CPose3DPDFGaussianInf))
{ // It's my same class:
const CPose3DPDFGaussianInf *ptr = static_cast<const CPose3DPDFGaussianInf*>(&o);
mean = ptr->mean;
cov_inv = ptr->cov_inv;
}
else
{
// Convert to gaussian pdf:
CMatrixDouble66 cov(UNINITIALIZED_MATRIX);
o.getCovarianceAndMean(cov,mean);
cov.inv_fast(this->cov_inv);
}
}
/*---------------------------------------------------------------
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
}
