/*---------------------------------------------------------------
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
}
/*---------------------------------------------------------------
CHMTSLAM_LOG
Implements a 2D local SLAM method based on scan matching
between near observations and an EKF. A part of HMT-SLAM
\param LMH The local metric hypothesis which must be updated by this SLAM algorithm.
\param act The action to process (or NULL).
\param sf The observations to process (or NULL).
--------------------------------------------------------------- */
void CHMTSLAM::generateLogFiles(unsigned int nIteration)
{
MRPT_START
// Speed up the storage of images (in opengl::CTexturedPlane's):
//CImage::DISABLE_ZIP_COMPRESSION = true;
static CTicTac tictac;
tictac.Tic();
THypothesisID bestHypoID;
CLocalMetricHypothesis *bestLMH = NULL;
{
CCriticalSectionLocker locker( &m_LMHs_cs );
printf_debug("[LOG] Number of LMHs: %u\n", (unsigned int)m_LMHs.size() );
// Generate 3D view of local areas:
{
string filLocalAreas = format("%s/LSLAM_3D/mostLikelyLMH_LSLAM_%05u.3Dscene", m_options.LOG_OUTPUT_DIR.c_str(), nIteration );
COpenGLScenePtr sceneLSLAM = COpenGLScene::Create();
// Look for the most likely LMH:
aligned_containers<THypothesisID, CLocalMetricHypothesis>::map_t::iterator it;
for ( it = m_LMHs.begin();it!=m_LMHs.end();it++)
{
if (!bestLMH)
{
bestLMH = & it->second;
}
else if ( it->second.m_log_w > bestLMH->m_log_w)
{
bestLMH = & it->second;
}
}
ASSERT_(bestLMH!=NULL)
bestHypoID = bestLMH->m_ID;
{
CCriticalSectionLocker lockerLMH( &bestLMH->m_lock );
{
// Generate the metric maps 3D view...
opengl::CSetOfObjectsPtr maps3D = opengl::CSetOfObjects::Create();
maps3D->setName("metric-maps");
bestLMH->getMostLikelyParticle()->d->metricMaps.getAs3DObject( maps3D );
sceneLSLAM->insert( maps3D );
// ...and the robot poses, areas, etc:
opengl::CSetOfObjectsPtr LSLAM_3D = opengl::CSetOfObjects::Create();
LSLAM_3D->setName("LSLAM_3D");
bestLMH->getAs3DScene( LSLAM_3D );
sceneLSLAM->insert( LSLAM_3D );
sceneLSLAM->enableFollowCamera(true);
printf_debug("[LOG] Saving %s\n", filLocalAreas.c_str());
CFileGZOutputStream(filLocalAreas) << *sceneLSLAM;
}
// Save the SSO matrix:
#if 0
{
CCriticalSectionLocker locker( &bestLMH->m_robotPosesGraph.lock );
string filSSO = format("%s/ASSO/mostLikelyLMH_ASSO_%05u.3Dscene", m_options.LOG_OUTPUT_DIR.c_str(), nIteration );
COpenGLScene sceneSSO;
opengl::CSetOfObjectsPtr sso3D = opengl::CSetOfObjects::Create();
bestLMH->m_robotPosesGraph.partitioner.getAs3DScene( sso3D, &bestLMH->m_robotPosesGraph.idx2pose );
sceneSSO.insert(sso3D);
CFileGZOutputStream(filSSO) << sceneSSO;
if (1)
{
CMatrix A;
bestLMH->m_robotPosesGraph.partitioner.getAdjacencyMatrix( A );
if (A.getColCount()>0)
{
A.adjustRange();
A.saveToTextFile( format("%s/ASSO/mostLikelyLMH_ASSO_%05u.txt", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
CImage(A,true).saveToFile( format("%s/ASSO/mostLikelyLMH_ASSO_%05u.png", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
}
}
} // end lock partitioner's CS
#endif
} // end LMH's lock
}
} // end of lock on LMHs_cs
#if 1
{
// Save the whole HMT-SLAM state to a dump file
static int CNT = 0;
if ((CNT++ % 20) == 0)
{
string hmtmap_file( format("%s/HMTSLAM_state/state_%05u.hmtslam", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
printf_debug("[LOG] Saving %s\n", hmtmap_file.c_str());
CFileGZOutputStream(hmtmap_file) << *this;
}
}
#endif
#if 1
{
// Update the poses-graph in the HMT-map from the LMH to draw it:
static int CNT = 0;
if ((CNT++ % 5) == 0)
{
CCriticalSectionLocker lockerLMH( &bestLMH->m_lock );
for (TNodeIDSet::const_iterator n = bestLMH->m_neighbors.begin();n!=bestLMH->m_neighbors.end();++n)
bestLMH->updateAreaFromLMH( *n );
// Save global map for most likely hypothesis:
COpenGLScene sceneGlobalHMTMAP;
{
CCriticalSectionLocker locker( &m_map_cs );
printf_debug("[LOG] HMT-map: %u nodes/ %u arcs\n", (unsigned int)m_map.nodeCount(), (unsigned int)m_map.arcCount() );
m_map.getAs3DScene(
sceneGlobalHMTMAP, // Scene
m_map.getFirstNode()->getID(), // Reference node
bestHypoID, // Hypothesis to get
3 // iterations
);
}
string hmtmap_file( format("%s/HMAP_3D/mostLikelyHMT_MAP_%05u.3Dscene", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
printf_debug("[LOG] Saving %s\n", hmtmap_file.c_str());
CFileGZOutputStream(hmtmap_file) << sceneGlobalHMTMAP;
}
}
#endif
// Save the memory usage:
unsigned long memUsage = mrpt::system::getMemoryUsage();
FILE *f=os::fopen( format("%s/log_MemoryUsage.txt",m_options.LOG_OUTPUT_DIR.c_str()).c_str() ,"at");
if (f)
{
os::fprintf(f,"%u\t%f\n",nIteration,memUsage/(1024.0*1024.0));
os::fclose(f);
}
double t_log = tictac.Tac();
printf_debug("[LOG] Time for logging: %f ms\n", 1000*t_log);
MRPT_END
}