/*--------------------------------------------------------------- TBI_main_method Topological Bayesian Inference (TBI) process within HMT-SLAM ---------------------------------------------------------------*/ CHMTSLAM::TMessageLSLAMfromTBIPtr CHMTSLAM::TBI_main_method( CLocalMetricHypothesis *LMH, const CHMHMapNode::TNodeID &areaID) { MRPT_START CHMTSLAM *obj = (CHMTSLAM*) LMH->m_parent.get(); const THypothesisID LMH_ID = LMH->m_ID; // Lock the map: synch::CCriticalSectionLocker( &obj->m_map_cs ); TMessageLSLAMfromTBIPtr msg = TMessageLSLAMfromTBIPtr(new TMessageLSLAMfromTBI()); // Fill out easy data: msg->hypothesisID = LMH_ID; msg->cur_area = areaID; // get a pointer to the current area: const CHMHMapNodePtr currentArea = obj->m_map.getNodeByID( areaID ); ASSERT_(currentArea); obj->logFmt(mrpt::utils::LVL_DEBUG, "[TBI] Request for area id=%i\n",(int)areaID); // -------------------------------------------------------- // 1) Use bounding-boxes to get a first list of candidates // The candidates are saved in "msg->loopClosureData" // ------------------------------------------------------- // But first: if the areas are within the LMH, then we have to update the maps in the HMAP! if (LMH->m_neighbors.find( areaID ) != LMH->m_neighbors.end() ) { // Update: LMH->updateAreaFromLMH( areaID ); } for (CHierarchicalMapMHPartition::iterator a=obj->m_map.begin();a!=obj->m_map.end();++a) { // Only for other areas! if (a->first==areaID) continue; // Test hypothesis: LMH_ID if (a->second->m_hypotheses.has(LMH_ID)) { // Not neighbors: if (a->second->isNeighbor( areaID, LMH_ID) ) continue; // OK, check: // But first: if the areas are within the LMH, then we have to update the maps in the HMAP! if (LMH->m_neighbors.find( a->first ) != LMH->m_neighbors.end() ) { // Update: LMH->updateAreaFromLMH( a->first ); } // Compute it: double match = obj->m_map.computeOverlapProbabilityBetweenNodes( areaID, // From a->first, // To LMH_ID ); obj->logFmt(mrpt::utils::LVL_DEBUG, "[TBI] %i-%i -> overlap prob=%f\n",(int)areaID,(int)a->first,match); if (match>0.9) { // Initialize the new entry in "msg->loopClosureData" for the areas: // "areaID" <-> "a->first" TMessageLSLAMfromTBI::TBI_info &tbi_info = msg->loopClosureData[ a->first ]; tbi_info.log_lik = 0; tbi_info.delta_new_cur.clear(); } } } // end for each node in the graph. // ---------------------------------------------------- // 2) Use the TBI engines // ---------------------------------------------------- std::set<CHMHMapNode::TNodeID> lstNodesToErase; { synch::CCriticalSection lock( obj->m_topLCdets_cs ); for ( deque<CTopLCDetectorBase*>::const_iterator it=obj->m_topLCdets.begin();it!=obj->m_topLCdets.end();++it) { for (map< CHMHMapNode::TNodeID, TMessageLSLAMfromTBI::TBI_info >::iterator candidate = msg->loopClosureData.begin();candidate != msg->loopClosureData.end();++candidate) { // If the current log_lik of this area is reaaaally low, we could skip the computation with other LC detectors... // ---------------------------------------------------------------------------------------------------------------- // TODO: ... // Proceed: // ---------------------------------------------------------------------------------------------------------------- const CHMHMapNodePtr refArea = obj->m_map.getNodeByID( candidate->first ); double this_log_lik; // get the output from this LC detector: CPose3DPDFPtr pdf = (*it)->computeTopologicalObservationModel( LMH->m_ID, currentArea, refArea, this_log_lik ); // Add to the output: candidate->second.log_lik += this_log_lik; // This is because not all LC detector MUST return a pose PDF (i.e. image-based detectors) if (pdf.present()) { ASSERT_( IS_CLASS(pdf, CPose3DPDFSOG ) ); CPose3DPDFSOGPtr SOG = CPose3DPDFSOGPtr( pdf ); // Mix (append) the modes, if any: if (SOG->size()>0) candidate->second.delta_new_cur.appendFrom( *SOG ); else lstNodesToErase.insert(candidate->first); } } // end for each candidate area } // end for each LC detector } // end of m_topLCdets_cs lock // Delete candidates which had no PDF when they should. for (set<CHMHMapNode::TNodeID>::const_iterator it=lstNodesToErase.begin();it!=lstNodesToErase.end();++it) msg->loopClosureData.erase(*it); obj->logFmt(mrpt::utils::LVL_DEBUG, "[TBI_main] Done. %u candidates found.\n",(unsigned int)msg->loopClosureData.size() ); return msg; MRPT_END }