31 void CHMTSLAM::thread_TBI()
37 if (
obj->m_options.random_seed)
48 "[thread_TBI] Thread started (ID=0x%08lX)\n",
49 std::this_thread::get_id());
55 while (!
obj->m_terminateThreads)
57 std::this_thread::sleep_for(100ms);
66 obj->m_terminationFlag_TBI =
true;
68 catch (std::exception& e)
70 obj->m_terminationFlag_TBI =
true;
77 obj->m_terminateThreads =
true;
81 obj->m_terminationFlag_TBI =
true;
85 "\n---------------------- EXCEPTION CAUGHT! ---------------------\n" 86 " In CHierarchicalMappingFramework::thread_TBI. Unexpected runtime " 92 obj->m_terminateThreads =
true;
113 std::lock_guard<std::mutex>(
obj->m_map_cs);
119 msg->hypothesisID = LMH_ID;
120 msg->cur_area = areaID;
142 a !=
obj->m_map.end(); ++
a)
145 if (
a->first == areaID)
continue;
148 if (
a->second->m_hypotheses.has(LMH_ID))
151 if (
a->second->isNeighbor(areaID, LMH_ID))
continue;
163 double match =
obj->m_map.computeOverlapProbabilityBetweenNodes(
170 (
int)areaID, (
int)
a->first, match);
178 msg->loopClosureData[
a->first];
189 std::set<CHMHMapNode::TNodeID> lstNodesToErase;
191 std::lock_guard<std::mutex> lock(
obj->m_topLCdets_cs);
194 obj->m_topLCdets.begin();
195 it !=
obj->m_topLCdets.end(); ++it)
199 msg->loopClosureData.begin();
200 candidate != msg->loopClosureData.end(); ++candidate)
210 obj->m_map.getNodeByID(candidate->first);
215 LMH->
m_ID, currentArea, refArea, this_log_lik);
218 candidate->second.log_lik += this_log_lik;
230 candidate->second.delta_new_cur.
appendFrom(*SOG);
232 lstNodesToErase.insert(candidate->first);
241 it != lstNodesToErase.end(); ++it)
242 msg->loopClosureData.erase(*it);
246 (
unsigned int)msg->loopClosureData.size());
A namespace of pseudo-random numbers generators of diferent distributions.
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
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 ...
A high-performance stopwatch, with typical resolution of nanoseconds.
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).
#define ASSERT_(f)
Defines an assertion mechanism.
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.
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
mrpt::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
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::rtti::CObject) is of the give...
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...
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
GLubyte GLubyte GLubyte a
const Scalar * const_iterator
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself)...
TNodeList::iterator iterator