/*---------------------------------------------------------------
perform_TLC
Topological Loop Closure: Performs all the required operations
to close a loop between two areas which have been determined
to be the same.
---------------------------------------------------------------*/
void CHMTSLAM::perform_TLC(
CLocalMetricHypothesis &LMH,
const CHMHMapNode::TNodeID Ai, // areaInLMH,
const CHMHMapNode::TNodeID Ae, // areaLoopClosure,
const mrpt::poses::CPose3DPDFGaussian &pose_i_wrt_e
)
{
MRPT_START
ASSERT_(Ai!=Ae)
synch::CCriticalSectionLocker locker ( &LMH.m_robotPosesGraph.lock );
printf_debug("[perform_TLC] TLC of areas: %u <-> %u \n",(unsigned)Ai, (unsigned)Ae );
// * Verify a1 \in LMH & a2 \notin LMH
// ----------------------------------------------------------------------
ASSERT_( LMH.m_neighbors.count(Ai) == 1);
ASSERT_( LMH.m_neighbors.count(Ae) == 0);
// * Get the old pose reference of the area which will desapear:
// ----------------------------------------------------------------------
TPoseID poseID_Ai_origin;
m_map.getNodeByID(Ai)->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID,poseID_Ai_origin, LMH.m_ID, true);
// TPoseID poseID_new_origin;
// m_map.getNodeByID(Ae)->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID,poseID_new_origin, LMH.m_ID, true);
// * Make a list Li of pose IDs in Ai, which need a change in coordinates:
// ----------------------------------------------------------------------------------------------
//TPoseIDSet Li;
//for (map<TPoseID,CHMHMapNode::TNodeID>::const_iterator it=LMH.m_nodeIDmemberships.begin();it!=LMH.m_nodeIDmemberships.end();++it)
// if(it->second==Ai)
// Li.insert(it->first);
// * Change all poses in "Ai" from "p" to " 0 (-) pose1wrt2 (+) (p (-) oldRef )"
// ----------------------------------------------------------------------------------------------
// For all the particles
//for ( CLocalMetricHypothesis::CParticleList::const_iterator it = LMH.m_particles.begin(); it!=LMH.m_particles.end(); ++it)
//{
// // Touch only the affected poseIDs:
// for (TPoseIDSet::const_iterator n=Li.begin();n!=Li.end();++n)
// {
// map<TPoseID,CPose3D>::iterator itPos = it->d->robotPoses.find(*n);
// ASSERT_(itPos!=it->d->robotPoses.end());
// itPos->second = Apose_ei + (itPos->second - oldRef);
// }
//}
// Change coords in incr. partitioning as well:
//CSimpleMap *SFseq = LMH.m_robotPosesGraph.partitioner.getSequenceOfFrames();
//for (std::map<uint32_t,TPoseID>::const_iterator it=LMH.m_robotPosesGraph.idx2pose.begin();it!=LMH.m_robotPosesGraph.idx2pose.end();++it)
//{
// // Only if the pose has been affected:
// if (Li.count(it->second)==0) continue;
// CPose3DPDFPtr pdf;
// CSensoryFramePtr sf;
// SFseq->get( it->first, pdf, sf);
// // Copy from particles:
// CPose3DPDFParticlesPtr pdfParts = CPose3DPDFParticlesPtr(pdf);
// LMH.getPoseParticles( it->second, *pdfParts);
//}
// * Mark all poses in LMH \in Ai as being of "Ae":
// ----------------------------------------------------------------------------------------------
for (map<TPoseID,CHMHMapNode::TNodeID>::iterator it=LMH.m_nodeIDmemberships.begin();it!=LMH.m_nodeIDmemberships.end();++it)
if(it->second==Ai)
it->second=Ae;
// * Replace "Ai" by "Ae" in the list of neighbors areas:
// ----------------------------------------------------------------------------------------------
LMH.m_neighbors.erase(LMH.m_neighbors.find(Ai));
LMH.m_neighbors.insert(Ae);
// * Reflect the change of coordinates in all existing arcs from/to "Ai", which are not part of the LMH (list of neighbors)
// "p" to " 0 (-) pose1wrt2 (+) (p (-) oldRef )"...
// NOTE: Arcs to "Ai" must be replaced by arcs to "Ae"
// ----------------------------------------------------------------------------------------------
{
TArcList lstArcs;
m_map.getNodeByID(Ai)->getArcs(lstArcs,LMH.m_ID);
for(TArcList::iterator arc=lstArcs.begin();arc!=lstArcs.end();++arc)
{
// TODO ...
}
}
// * The node for "area Ai" must be deleted in the HMAP, and all its arcs.
// ----------------------------------------------------------------------------------------------
{
TArcList lstArcs;
m_map.getNodeByID(Ai)->getArcs(lstArcs);
for(TArcList::iterator arc=lstArcs.begin();arc!=lstArcs.end();++arc)
arc->clear(); // The "delete" will automatically remove the entry in "m_map". Other smrtpnts will be cleared too.
m_map.getNodeByID(Ai).clear(); // The "delete" will automatically remove the entry in "m_map"
}
// * Merge Ae into the LMH: This includes the change of coordinates to those used now in "Ai" and the rest of neighbors!
// ----------------------------------------------------------------------------------------------
CPose3DPDFParticles pdfAiRef;
LMH.getPoseParticles(poseID_Ai_origin,pdfAiRef);
CPose3D AiRef;
pdfAiRef.getMean(AiRef);
const CPose3D &Apose_ie = pose_i_wrt_e.mean;
//const CPose3D Apose_ei = -Apose_ie;
const CPose3D AeRefInLMH = AiRef + Apose_ie;
CRobotPosesGraphPtr AePG = m_map.getNodeByID(Ae)->m_annotations.getAs<CRobotPosesGraph>(NODE_ANNOTATION_POSES_GRAPH, LMH.m_ID);
// For each pose in Ae:
for (CRobotPosesGraph::const_iterator it=AePG->begin();it!=AePG->end();++it)
{
const TPoseID poseId = it->first;
const TPoseInfo &pi = it->second;
// Insert the observations into the metric maps:
ASSERT_(LMH.m_particles.size() == pi.pdf.size() );
for (size_t i=0;i<pi.pdf.size();i++)
{
// Transport coordinates:
CPose3D &p = *pi.pdf.m_particles[i].d;
LMH.m_particles[i].d->robotPoses[poseId] = AeRefInLMH + p;
//pi.sf.insertObservationsInto( &LMH.m_particles[i].d->metricMaps, pi.pdf.m_particles[i].d );
}
// Add node membership:
LMH.m_nodeIDmemberships[poseId] = Ae;
// Now, insert the SF
LMH.m_SFs[poseId] = pi.sf;
// Also add to the list of poses pending to be evaluated by the AA:
LMH.m_posesPendingAddPartitioner.push_back(poseId);
}
// * The current pose of the robot should be in the new coordinate system:
// ----------------------------------------------------------------------------------------------
// This is already done since the current pose is just another poseID and it's threaded like the others.
// * Rebuild metric maps for consistency:
// ----------------------------------------------------------------------------------------------
LMH.rebuildMetricMaps();
// * Update all the data in the node "Ae":
// ----------------------------------------------------------------------------------------------
LMH.updateAreaFromLMH(Ae);
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 );
}
