/*--------------------------------------------------------------- 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 ); }