MRPT  2.0.5
CHMTSLAM_TBI.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hmtslam-precomp.h" // Precomp header
11 
12 #include <mrpt/io/CFileStream.h>
13 #include <mrpt/random.h>
14 #include <mrpt/system/CTicTac.h>
15 #include <mrpt/system/os.h>
16 
17 #include <memory>
18 
19 using namespace mrpt::slam;
20 using namespace mrpt::hmtslam;
21 using namespace mrpt::random;
22 using namespace mrpt::poses;
23 using namespace mrpt::system;
24 using namespace std;
25 
26 /*---------------------------------------------------------------
27 
28  CHMTSLAM_TBI
29 
30  Topological Bayesian Inference (TBI) process within HMT-SLAM
31 
32  ---------------------------------------------------------------*/
33 void CHMTSLAM::thread_TBI()
34 {
35  CHMTSLAM* obj = this;
36  CTicTac tictac;
37 
38  // Seems that must be called in each thread??
39  if (obj->m_options.random_seed)
41  else
43 
44  try
45  {
46  // Start thread:
47  // -------------------------
48  obj->logFmt(
50  "[thread_TBI] Thread started (ID=0x%08lX)\n",
51  std::this_thread::get_id());
52 
53  // --------------------------------------------
54  // The main loop
55  // Executes until termination is signaled
56  // --------------------------------------------
57  while (!obj->m_terminateThreads)
58  {
59  std::this_thread::sleep_for(100ms);
60  }; // end while execute thread
61 
62  // Finish thread:
63  // -------------------------
64  obj->logFmt(mrpt::system::LVL_DEBUG, "[thread_TBI] Thread finished");
65  obj->m_terminationFlag_TBI = true;
66  }
67  catch (const std::exception& e)
68  {
69  obj->m_terminationFlag_TBI = true;
70 
71  // Release semaphores:
72 
73  obj->logFmt(mrpt::system::LVL_ERROR, "%s", e.what());
74 
75  // DEBUG: Terminate application:
76  obj->m_terminateThreads = true;
77  }
78  catch (...)
79  {
80  obj->m_terminationFlag_TBI = true;
81 
82  obj->logFmt(
84  "\n---------------------- EXCEPTION CAUGHT! ---------------------\n"
85  " In CHierarchicalMappingFramework::thread_TBI. Unexpected runtime "
86  "error!!\n");
87 
88  // Release semaphores:
89 
90  // DEBUG: Terminate application:
91  obj->m_terminateThreads = true;
92  }
93 }
94 
95 /*---------------------------------------------------------------
96 
97  TBI_main_method
98 
99  Topological Bayesian Inference (TBI) process within HMT-SLAM
100 
101  ---------------------------------------------------------------*/
102 CHMTSLAM::TMessageLSLAMfromTBI::Ptr CHMTSLAM::TBI_main_method(
103  CLocalMetricHypothesis* LMH, const CHMHMapNode::TNodeID& areaID)
104 {
105  MRPT_START
106 
107  auto* obj = (CHMTSLAM*)LMH->m_parent.get();
108 
109  const THypothesisID LMH_ID = LMH->m_ID;
110 
111  // Lock the map:
112  std::lock_guard<std::mutex>(obj->m_map_cs);
113 
114  TMessageLSLAMfromTBI::Ptr msg = std::make_shared<TMessageLSLAMfromTBI>();
115 
116  // Fill out easy data:
117  msg->hypothesisID = LMH_ID;
118  msg->cur_area = areaID;
119 
120  // get a pointer to the current area:
121  const CHMHMapNode::Ptr currentArea = obj->m_map.getNodeByID(areaID);
122  ASSERT_(currentArea);
123 
124  obj->logFmt(
125  mrpt::system::LVL_DEBUG, "[TBI] Request for area id=%i\n", (int)areaID);
126 
127  // --------------------------------------------------------
128  // 1) Use bounding-boxes to get a first list of candidates
129  // The candidates are saved in "msg->loopClosureData"
130  // -------------------------------------------------------
131  // But first: if the areas are within the LMH, then we have to update the
132  // maps in the HMAP!
133  if (LMH->m_neighbors.find(areaID) != LMH->m_neighbors.end())
134  {
135  // Update:
136  LMH->updateAreaFromLMH(areaID);
137  }
138 
139  for (auto a = obj->m_map.begin(); a != obj->m_map.end(); ++a)
140  {
141  // Only for other areas!
142  if (a->first == areaID) continue;
143 
144  // Test hypothesis: LMH_ID
145  if (a->second->m_hypotheses.has(LMH_ID))
146  {
147  // Not neighbors:
148  if (a->second->isNeighbor(areaID, LMH_ID)) continue;
149 
150  // OK, check:
151  // But first: if the areas are within the LMH, then we have to
152  // update the maps in the HMAP!
153  if (LMH->m_neighbors.find(a->first) != LMH->m_neighbors.end())
154  {
155  // Update:
156  LMH->updateAreaFromLMH(a->first);
157  }
158 
159  // Compute it:
160  double match = obj->m_map.computeOverlapProbabilityBetweenNodes(
161  areaID, // From
162  a->first, // To
163  LMH_ID);
164 
165  obj->logFmt(
166  mrpt::system::LVL_DEBUG, "[TBI] %i-%i -> overlap prob=%f\n",
167  (int)areaID, (int)a->first, match);
168 
169  if (match > 0.9)
170  {
171  // Initialize the new entry in "msg->loopClosureData" for the
172  // areas:
173  // "areaID" <-> "a->first"
175  msg->loopClosureData[a->first];
176 
177  tbi_info.log_lik = 0;
178  tbi_info.delta_new_cur.clear();
179  }
180  }
181  } // end for each node in the graph.
182 
183  // ----------------------------------------------------
184  // 2) Use the TBI engines
185  // ----------------------------------------------------
186  std::set<CHMHMapNode::TNodeID> lstNodesToErase;
187  {
188  std::lock_guard<std::mutex> lock(obj->m_topLCdets_cs);
189 
190  for (auto it = obj->m_topLCdets.begin(); it != obj->m_topLCdets.end();
191  ++it)
192  {
193  for (auto& candidate : msg->loopClosureData)
194  {
195  // If the current log_lik of this area is reaaaally low, we
196  // could skip the computation with other LC detectors...
197  // ----------------------------------------------------------------------------------------------------------------
198  // TODO: ...
199 
200  // Proceed:
201  // ----------------------------------------------------------------------------------------------------------------
202  const CHMHMapNode::Ptr refArea =
203  obj->m_map.getNodeByID(candidate.first);
204  double this_log_lik;
205 
206  // get the output from this LC detector:
207  CPose3DPDF::Ptr pdf = (*it)->computeTopologicalObservationModel(
208  LMH->m_ID, currentArea, refArea, this_log_lik);
209 
210  // Add to the output:
211  candidate.second.log_lik += this_log_lik;
212 
213  // This is because not all LC detector MUST return a pose PDF
214  // (i.e. image-based detectors)
215  if (pdf)
216  {
218  CPose3DPDFSOG::Ptr SOG =
219  std::dynamic_pointer_cast<CPose3DPDFSOG>(pdf);
220 
221  // Mix (append) the modes, if any:
222  if (SOG->size() > 0)
223  candidate.second.delta_new_cur.appendFrom(*SOG);
224  else
225  lstNodesToErase.insert(candidate.first);
226  }
227  } // end for each candidate area
228  } // end for each LC detector
229 
230  } // end of m_topLCdets_cs lock
231 
232  // Delete candidates which had no PDF when they should.
233  for (unsigned long it : lstNodesToErase) msg->loopClosureData.erase(it);
234 
235  obj->logFmt(
236  mrpt::system::LVL_DEBUG, "[TBI_main] Done. %u candidates found.\n",
237  (unsigned int)msg->loopClosureData.size());
238 
239  return msg;
240  MRPT_END
241 }
A namespace of pseudo-random numbers generators of diferent distributions.
int random_seed
0 means randomize, use any other value to have repetitive experiments.
Definition: CHMTSLAM.h:496
#define MRPT_START
Definition: exceptions.h:241
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.
Definition: CHMHMapNode.h:44
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 ...
Definition: CPose3DPDFSOG.h:32
A high-performance stopwatch, with typical resolution of nanoseconds.
std::atomic_bool m_terminationFlag_TBI
Definition: CHMTSLAM.h:341
STL namespace.
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...
Definition: CHMTSLAM.h:147
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:67
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
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.
Definition: CHMTSLAM.h:337
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double log_lik
Log likelihood for this loop-closure.
Definition: CHMTSLAM.h:140
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 MRPT_END
Definition: exceptions.h:245
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)...



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: ecc95703f Thu Jul 2 07:56:41 2020 +0200 at jue jul 2 08:00:14 CEST 2020