33 void CHMTSLAM::thread_TBI()
50 "[thread_TBI] Thread started (ID=0x%08lX)\n",
51 std::this_thread::get_id());
59 std::this_thread::sleep_for(100ms);
67 catch (
const std::exception& e)
84 "\n---------------------- EXCEPTION CAUGHT! ---------------------\n" 85 " In CHierarchicalMappingFramework::thread_TBI. Unexpected runtime " 112 std::lock_guard<std::mutex>(obj->m_map_cs);
117 msg->hypothesisID = LMH_ID;
118 msg->cur_area = areaID;
139 for (
auto a = obj->m_map.begin(); a != obj->m_map.end(); ++a)
142 if (a->first == areaID)
continue;
145 if (a->second->m_hypotheses.has(LMH_ID))
148 if (a->second->isNeighbor(areaID, LMH_ID))
continue;
160 double match = obj->m_map.computeOverlapProbabilityBetweenNodes(
167 (
int)areaID, (
int)a->first, match);
175 msg->loopClosureData[a->first];
186 std::set<CHMHMapNode::TNodeID> lstNodesToErase;
188 std::lock_guard<std::mutex> lock(obj->m_topLCdets_cs);
190 for (
auto it = obj->m_topLCdets.begin(); it != obj->m_topLCdets.end();
193 for (
auto& candidate : msg->loopClosureData)
203 obj->m_map.getNodeByID(candidate.first);
208 LMH->
m_ID, currentArea, refArea, this_log_lik);
211 candidate.second.log_lik += this_log_lik;
223 candidate.second.delta_new_cur.
appendFrom(*SOG);
225 lstNodesToErase.insert(candidate.first);
233 for (
unsigned long it : lstNodesToErase) msg->loopClosureData.erase(it);
237 (
unsigned int)msg->loopClosureData.size());
A namespace of pseudo-random numbers generators of diferent distributions.
int random_seed
0 means randomize, use any other value to have repetitive experiments.
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
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.
std::atomic_bool m_terminationFlag_TBI
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...
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...
mrpt::hmtslam::CHMTSLAM::TOptions m_options
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
std::atomic_bool m_terminateThreads
Termination flag for signaling all threads to terminate.
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.
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself)...