32 void CHMTSLAM::thread_TBI()
38 if (
obj->m_options.random_seed)
48 mrpt::utils::LVL_DEBUG,
49 "[thread_TBI] Thread started (ID=0x%08lX)\n",
50 std::this_thread::get_id());
56 while (!
obj->m_terminateThreads)
58 std::this_thread::sleep_for(100ms);
63 time_t timCreat, timExit;
69 mrpt::utils::LVL_DEBUG,
70 "[thread_TBI] Thread finished. CPU time used:%.06f secs \n",
72 obj->m_terminationFlag_TBI =
true;
74 catch (std::exception& e)
76 obj->m_terminationFlag_TBI =
true;
80 obj->logFmt(mrpt::utils::LVL_ERROR,
"%s", e.what());
83 obj->m_terminateThreads =
true;
87 obj->m_terminationFlag_TBI =
true;
90 mrpt::utils::LVL_ERROR,
91 "\n---------------------- EXCEPTION CAUGHT! ---------------------\n" 92 " In CHierarchicalMappingFramework::thread_TBI. Unexpected runtime " 98 obj->m_terminateThreads =
true;
119 std::lock_guard<std::mutex>(
obj->m_map_cs);
125 msg->hypothesisID = LMH_ID;
126 msg->cur_area = areaID;
133 mrpt::utils::LVL_DEBUG,
"[TBI] Request for area id=%i\n", (
int)areaID);
148 a !=
obj->m_map.end(); ++
a)
151 if (
a->first == areaID)
continue;
154 if (
a->second->m_hypotheses.has(LMH_ID))
157 if (
a->second->isNeighbor(areaID, LMH_ID))
continue;
169 double match =
obj->m_map.computeOverlapProbabilityBetweenNodes(
175 mrpt::utils::LVL_DEBUG,
"[TBI] %i-%i -> overlap prob=%f\n",
176 (
int)areaID, (
int)
a->first, match);
184 msg->loopClosureData[
a->first];
195 std::set<CHMHMapNode::TNodeID> lstNodesToErase;
197 std::lock_guard<std::mutex> lock(
obj->m_topLCdets_cs);
200 obj->m_topLCdets.begin();
201 it !=
obj->m_topLCdets.end(); ++it)
205 msg->loopClosureData.begin();
206 candidate != msg->loopClosureData.end(); ++candidate)
216 obj->m_map.getNodeByID(candidate->first);
221 LMH->
m_ID, currentArea, refArea, this_log_lik);
224 candidate->second.log_lik += this_log_lik;
236 candidate->second.delta_new_cur.
appendFrom(*SOG);
238 lstNodesToErase.insert(candidate->first);
247 it != lstNodesToErase.end(); ++it)
248 msg->loopClosureData.erase(*it);
251 mrpt::utils::LVL_DEBUG,
"[TBI_main] Done. %u candidates found.\n",
252 (
unsigned int)msg->loopClosureData.size());
A namespace of pseudo-random numbers genrators of diferent distributions.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
void appendFrom(const CPose3DPDFSOG &o)
Append the Gaussian modes from "o" to the current set of modes of "this" density. ...
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
const Scalar * const_iterator
mrpt::poses::CPose3DPDFSOG delta_new_cur
Depending on the loop-closure engine, an guess of the relative pose of "area_new" relative to "cur_ar...
GLsizei GLsizei GLuint * obj
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
TNodeList::iterator iterator
std::shared_ptr< TMessageLSLAMfromTBI > Ptr
This class implements a high-performance stopwatch.
mrpt::utils::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
std::shared_ptr< CPose3DPDF > Ptr
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double log_lik
Log likelihood for this loop-closure.
void clear()
Clear all the gaussian modes.
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
void updateAreaFromLMH(const CHMHMapNode::TNodeID areaID, bool eraseSFsFromLMH=false)
The corresponding node in the HMT map is updated with the robot poses & SFs in the LMH: the poses are ref...
std::shared_ptr< CHMHMapNode > Ptr
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
GLubyte GLubyte GLubyte a
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself)...
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
std::shared_ptr< CPose3DPDFSOG > Ptr