MRPT  1.9.9
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-2019, 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)
40  getRandomGenerator().randomize(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  MRPT_TODO("Fix thread times");
65  // try { mrpt::system::getCurrentThreadTimes(
66  // timCreat,timExit,timCPU); } catch(...) {};
67  obj->logFmt(mrpt::system::LVL_DEBUG, "[thread_TBI] Thread finished");
68  obj->m_terminationFlag_TBI = true;
69  }
70  catch (const std::exception& e)
71  {
72  obj->m_terminationFlag_TBI = true;
73 
74  // Release semaphores:
75 
76  obj->logFmt(mrpt::system::LVL_ERROR, "%s", e.what());
77 
78  // DEBUG: Terminate application:
79  obj->m_terminateThreads = true;
80  }
81  catch (...)
82  {
83  obj->m_terminationFlag_TBI = true;
84 
85  obj->logFmt(
87  "\n---------------------- EXCEPTION CAUGHT! ---------------------\n"
88  " In CHierarchicalMappingFramework::thread_TBI. Unexpected runtime "
89  "error!!\n");
90 
91  // Release semaphores:
92 
93  // DEBUG: Terminate application:
94  obj->m_terminateThreads = true;
95  }
96 }
97 
98 /*---------------------------------------------------------------
99 
100  TBI_main_method
101 
102  Topological Bayesian Inference (TBI) process within HMT-SLAM
103 
104  ---------------------------------------------------------------*/
105 CHMTSLAM::TMessageLSLAMfromTBI::Ptr CHMTSLAM::TBI_main_method(
106  CLocalMetricHypothesis* LMH, const CHMHMapNode::TNodeID& areaID)
107 {
108  MRPT_START
109 
110  auto* obj = (CHMTSLAM*)LMH->m_parent.get();
111 
112  const THypothesisID LMH_ID = LMH->m_ID;
113 
114  // Lock the map:
115  std::lock_guard<std::mutex>(obj->m_map_cs);
116 
117  TMessageLSLAMfromTBI::Ptr msg = std::make_shared<TMessageLSLAMfromTBI>();
118 
119  // Fill out easy data:
120  msg->hypothesisID = LMH_ID;
121  msg->cur_area = areaID;
122 
123  // get a pointer to the current area:
124  const CHMHMapNode::Ptr currentArea = obj->m_map.getNodeByID(areaID);
125  ASSERT_(currentArea);
126 
127  obj->logFmt(
128  mrpt::system::LVL_DEBUG, "[TBI] Request for area id=%i\n", (int)areaID);
129 
130  // --------------------------------------------------------
131  // 1) Use bounding-boxes to get a first list of candidates
132  // The candidates are saved in "msg->loopClosureData"
133  // -------------------------------------------------------
134  // But first: if the areas are within the LMH, then we have to update the
135  // maps in the HMAP!
136  if (LMH->m_neighbors.find(areaID) != LMH->m_neighbors.end())
137  {
138  // Update:
139  LMH->updateAreaFromLMH(areaID);
140  }
141 
142  for (auto a = obj->m_map.begin(); a != obj->m_map.end(); ++a)
143  {
144  // Only for other areas!
145  if (a->first == areaID) continue;
146 
147  // Test hypothesis: LMH_ID
148  if (a->second->m_hypotheses.has(LMH_ID))
149  {
150  // Not neighbors:
151  if (a->second->isNeighbor(areaID, LMH_ID)) continue;
152 
153  // OK, check:
154  // But first: if the areas are within the LMH, then we have to
155  // update the maps in the HMAP!
156  if (LMH->m_neighbors.find(a->first) != LMH->m_neighbors.end())
157  {
158  // Update:
159  LMH->updateAreaFromLMH(a->first);
160  }
161 
162  // Compute it:
163  double match = obj->m_map.computeOverlapProbabilityBetweenNodes(
164  areaID, // From
165  a->first, // To
166  LMH_ID);
167 
168  obj->logFmt(
169  mrpt::system::LVL_DEBUG, "[TBI] %i-%i -> overlap prob=%f\n",
170  (int)areaID, (int)a->first, match);
171 
172  if (match > 0.9)
173  {
174  // Initialize the new entry in "msg->loopClosureData" for the
175  // areas:
176  // "areaID" <-> "a->first"
178  msg->loopClosureData[a->first];
179 
180  tbi_info.log_lik = 0;
181  tbi_info.delta_new_cur.clear();
182  }
183  }
184  } // end for each node in the graph.
185 
186  // ----------------------------------------------------
187  // 2) Use the TBI engines
188  // ----------------------------------------------------
189  std::set<CHMHMapNode::TNodeID> lstNodesToErase;
190  {
191  std::lock_guard<std::mutex> lock(obj->m_topLCdets_cs);
192 
193  for (auto it = obj->m_topLCdets.begin(); it != obj->m_topLCdets.end();
194  ++it)
195  {
196  for (auto& candidate : msg->loopClosureData)
197  {
198  // If the current log_lik of this area is reaaaally low, we
199  // could skip the computation with other LC detectors...
200  // ----------------------------------------------------------------------------------------------------------------
201  // TODO: ...
202 
203  // Proceed:
204  // ----------------------------------------------------------------------------------------------------------------
205  const CHMHMapNode::Ptr refArea =
206  obj->m_map.getNodeByID(candidate.first);
207  double this_log_lik;
208 
209  // get the output from this LC detector:
210  CPose3DPDF::Ptr pdf = (*it)->computeTopologicalObservationModel(
211  LMH->m_ID, currentArea, refArea, this_log_lik);
212 
213  // Add to the output:
214  candidate.second.log_lik += this_log_lik;
215 
216  // This is because not all LC detector MUST return a pose PDF
217  // (i.e. image-based detectors)
218  if (pdf)
219  {
221  CPose3DPDFSOG::Ptr SOG =
222  std::dynamic_pointer_cast<CPose3DPDFSOG>(pdf);
223 
224  // Mix (append) the modes, if any:
225  if (SOG->size() > 0)
226  candidate.second.delta_new_cur.appendFrom(*SOG);
227  else
228  lstNodesToErase.insert(candidate.first);
229  }
230  } // end for each candidate area
231  } // end for each LC detector
232 
233  } // end of m_topLCdets_cs lock
234 
235  // Delete candidates which had no PDF when they should.
236  for (unsigned long it : lstNodesToErase) msg->loopClosureData.erase(it);
237 
238  obj->logFmt(
239  mrpt::system::LVL_DEBUG, "[TBI_main] Done. %u candidates found.\n",
240  (unsigned int)msg->loopClosureData.size());
241 
242  return msg;
243  MRPT_END
244 }
A namespace of pseudo-random numbers generators of diferent distributions.
#define MRPT_START
Definition: exceptions.h:241
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.
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.
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
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
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
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.
Definition: CHMTSLAM.h:140
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
#define MRPT_TODO(x)
Definition: common.h:129
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.
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
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 1.9.9 Git: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019