/*---------------------------------------------------------------
getRelativePose
---------------------------------------------------------------*/
void CLocalMetricHypothesis::getRelativePose(
const TPoseID &reference,
const TPoseID &pose,
CPose3DPDFParticles &outPDF ) const
{
MRPT_START
// Resize output:
outPDF.resetDeterministic( CPose3D(), m_particles.size() );
CParticleList::const_iterator it;
CPose3DPDFParticles::CParticleList::iterator itP;
for ( it = m_particles.begin(), itP = outPDF.m_particles.begin(); it!=m_particles.end(); it++, itP++ )
{
itP->log_w = it->log_w;
TMapPoseID2Pose3D::const_iterator srcPose = it->d->robotPoses.find( reference );
TMapPoseID2Pose3D::const_iterator trgPose = it->d->robotPoses.find( pose );
ASSERT_( srcPose != it->d->robotPoses.end() )
ASSERT_( trgPose != it->d->robotPoses.end() )
*itP->d = trgPose->second - srcPose->second;
}
MRPT_END
}
/*---------------------------------------------------------------
getPoseParticles
---------------------------------------------------------------*/
void CLocalMetricHypothesis::getPoseParticles( const TPoseID &poseID, CPose3DPDFParticles &outPDF ) const
{
MRPT_START
ASSERT_(!m_particles.empty());
CParticleList::const_iterator it;
outPDF.resetDeterministic( CPose3D(), m_particles.size() );
CPose3DPDFParticles::CParticleList::iterator itP;
for ( it = m_particles.begin(), itP = outPDF.m_particles.begin(); it!=m_particles.end(); it++, itP++ )
{
itP->log_w = it->log_w;
TMapPoseID2Pose3D::const_iterator itPose = it->d->robotPoses.find(poseID);
ASSERT_( itPose!=it->d->robotPoses.end() );
*itP->d = itPose->second;
}
MRPT_END
}
