33 void CHMTSLAM::thread_TBI()
39 if (
obj->m_options.random_seed)
50 "[thread_TBI] Thread started (ID=0x%08lX)\n",
51 std::this_thread::get_id());
57 while (!
obj->m_terminateThreads)
59 std::this_thread::sleep_for(100ms);
68 obj->m_terminationFlag_TBI =
true;
70 catch (
const std::exception& e)
72 obj->m_terminationFlag_TBI =
true;
79 obj->m_terminateThreads =
true;
83 obj->m_terminationFlag_TBI =
true;
87 "\n---------------------- EXCEPTION CAUGHT! ---------------------\n" 88 " In CHierarchicalMappingFramework::thread_TBI. Unexpected runtime " 94 obj->m_terminateThreads =
true;
115 std::lock_guard<std::mutex>(
obj->m_map_cs);
120 msg->hypothesisID = LMH_ID;
121 msg->cur_area = areaID;
142 for (
auto a =
obj->m_map.begin();
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);
193 for (
auto it =
obj->m_topLCdets.begin(); it !=
obj->m_topLCdets.end();
196 for (
auto& candidate : msg->loopClosureData)
206 obj->m_map.getNodeByID(candidate.first);
211 LMH->
m_ID, currentArea, refArea, this_log_lik);
214 candidate.second.log_lik += this_log_lik;
226 candidate.second.delta_new_cur.
appendFrom(*SOG);
228 lstNodesToErase.insert(candidate.first);
236 for (
unsigned long it : lstNodesToErase) msg->loopClosureData.erase(it);
240 (
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.
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
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.
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
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself)...