/*---------------------------------------------------------------
changeCoordinateOrigin
---------------------------------------------------------------*/
void CLocalMetricHypothesis::changeCoordinateOrigin( const TPoseID &newOrigin )
{
CPose3DPDFParticles originPDF( m_particles.size() );
CParticleList::const_iterator it;
CPose3DPDFParticles::CParticleList::iterator itOrgPDF;
for ( it = m_particles.begin(), itOrgPDF=originPDF.m_particles.begin(); it!=m_particles.end(); it++, itOrgPDF++ )
{
TMapPoseID2Pose3D::iterator refPoseIt = it->d->robotPoses.find( newOrigin );
ASSERT_( refPoseIt != it->d->robotPoses.end() )
const CPose3D &refPose = refPoseIt->second;
// Save in pdf to compute mean:
*itOrgPDF->d = refPose;
itOrgPDF->log_w = it->log_w;
TMapPoseID2Pose3D::iterator End = it->d->robotPoses.end();
// Change all other poses first:
for (TMapPoseID2Pose3D::iterator itP=it->d->robotPoses.begin();itP!=End;++itP)
if (itP!=refPoseIt)
itP->second = itP->second - refPose;
// Now set new origin to 0:
refPoseIt->second.setFromValues(0,0,0);
}
// Rebuild metric maps for consistency:
rebuildMetricMaps();
// Change coords in incr. partitioning as well:
{
synch::CCriticalSectionLocker locker ( &m_robotPosesGraph.lock );
CSimpleMap *SFseq = m_robotPosesGraph.partitioner.getSequenceOfFrames();
for (std::map<uint32_t,TPoseID>::const_iterator it=m_robotPosesGraph.idx2pose.begin();it!=m_robotPosesGraph.idx2pose.end();++it)
{
CPose3DPDFPtr pdf;
CSensoryFramePtr sf;
SFseq->get( it->first, pdf, sf);
// Copy from particles:
ASSERT_( pdf->GetRuntimeClass() == CLASS_ID(CPose3DPDFParticles) );
CPose3DPDFParticlesPtr pdfParts = CPose3DPDFParticlesPtr(pdf);
getPoseParticles( it->second, *pdfParts);
}
}
}
