Go to the documentation of this file.
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;
219 std::dynamic_pointer_cast<CPose3DPDFSOG>(pdf);
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());
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose .
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
std::shared_ptr< mrpt::hmtslam ::CHMHMapNode > Ptr
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...
A high-performance stopwatch, with typical resolution of nanoseconds.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
int random_seed
0 means randomize, use any other value to have repetitive experiments.
void clear()
Clear all the gaussian modes.
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class.
std::shared_ptr< mrpt::poses ::CPose3DPDFSOG > Ptr
std::shared_ptr< TMessageLSLAMfromTBI > Ptr
#define ASSERT_(f)
Defines an assertion mechanism.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
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...
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself).
mrpt::hmtslam::CHMTSLAM::TOptions m_options
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
double log_lik
Log likelihood for this loop-closure.
std::shared_ptr< CPose3DPDF > Ptr
A namespace of pseudo-random numbers generators of diferent distributions.
mrpt::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
std::atomic_bool m_terminationFlag_TBI
std::atomic_bool m_terminateThreads
Termination flag for signaling all threads to terminate.
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Thu May 21 21:53:32 UTC 2020 | |