/*---------------------------------------------------------------
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
}
/*---------------------------------------------------------------
updateAreaFromLMH
// The corresponding node in the HMT map is updated with the
// robot poses & SFs in the LMH.
---------------------------------------------------------------*/
void CLocalMetricHypothesis::updateAreaFromLMH(
const CHMHMapNode::TNodeID areaID,
bool eraseSFsFromLMH )
{
// Build the list with the poses belonging to that area from LMH:
// ----------------------------------------------------------------------
TNodeIDList lstPoseIDs;
for (map<TPoseID,CHMHMapNode::TNodeID>::const_iterator it=m_nodeIDmemberships.begin();it!=m_nodeIDmemberships.end();++it)
if ( it->second==areaID )
lstPoseIDs.insert(it->first);
ASSERT_( !lstPoseIDs.empty() );
CHMHMapNodePtr node;
{
synch::CCriticalSectionLocker ( &m_parent->m_map_cs );
node = m_parent->m_map.getNodeByID( areaID );
ASSERT_(node);
ASSERT_(node->m_hypotheses.has( m_ID ));
} // end of HMT map cs
// The pose to become the origin:
TPoseID poseID_origin;
node->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID,poseID_origin, m_ID, true);
// 1) The set of robot poses and SFs
// In annotation: NODE_ANNOTATION_POSES_GRAPH
// ---------------------------------------------------------------------
CRobotPosesGraphPtr posesGraph;
{
CSerializablePtr annot = node->m_annotations.get(NODE_ANNOTATION_POSES_GRAPH,m_ID);
if (!annot)
{
// Add it now:
posesGraph = CRobotPosesGraph::Create();
node->m_annotations.setMemoryReference(NODE_ANNOTATION_POSES_GRAPH, posesGraph, m_ID);
}
else
{
posesGraph = CRobotPosesGraphPtr(annot);
posesGraph->clear();
}
}
// For each pose in the area:
CPose3DPDFParticles pdfOrigin;
bool pdfOrigin_ok=false;
for (TNodeIDList::const_iterator it=lstPoseIDs.begin();it!=lstPoseIDs.end();++it)
{
TPoseInfo &poseInfo = (*posesGraph)[*it];
getPoseParticles( *it, poseInfo.pdf ); // Save pose particles
// Save the PDF of the origin:
if (*it==poseID_origin)
{
pdfOrigin.copyFrom( poseInfo.pdf );
pdfOrigin_ok = true;
}
if ( *it != m_currentRobotPose ) // The current robot pose has no SF
{
std::map<TPoseID,CSensoryFrame>::iterator itSF = m_SFs.find(*it);
ASSERT_(itSF!=m_SFs.end());
if (eraseSFsFromLMH)
{
poseInfo.sf.moveFrom( itSF->second ); // This leaves m_SFs[*it] without observations, but it is being erased just now:
m_SFs.erase( itSF );
}
else
{
poseInfo.sf = itSF->second; // Copy observations
}
}
}
// Readjust to set the origin pose ID:
ASSERT_(pdfOrigin_ok);
CPose3DPDFParticles pdfOriginInv;
pdfOrigin.inverse(pdfOriginInv);
for (CRobotPosesGraph::iterator it=posesGraph->begin();it!=posesGraph->end();++it)
{
CPose3DPDFParticles::CParticleList::iterator orgIt,pdfIt;
ASSERT_( it->second.pdf.size() == pdfOriginInv.size() );
for ( pdfIt=it->second.pdf.m_particles.begin(), orgIt= pdfOriginInv.m_particles.begin();orgIt!=pdfOriginInv.m_particles.end();orgIt++,pdfIt++)
*pdfIt->d = *orgIt->d + *pdfIt->d;
}
// 2) One single metric map built from the most likelily robot poses
// In annotation: NODE_ANNOTATION_METRIC_MAPS
// ---------------------------------------------------------------------
CMultiMetricMapPtr metricMap = node->m_annotations.getAs<CMultiMetricMap>(NODE_ANNOTATION_METRIC_MAPS,m_ID, false);
metricMap->clear();
posesGraph->insertIntoMetricMap( *metricMap );
}
