MRPT  1.9.9
CHMTSLAM_TBI.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hmtslam-precomp.h" // Precomp header
11 
12 #include <mrpt/system/CTicTac.h>
13 #include <mrpt/random.h>
14 #include <mrpt/io/CFileStream.h>
15 #include <mrpt/system/os.h>
16 
17 using namespace mrpt::slam;
18 using namespace mrpt::hmtslam;
19 using namespace mrpt::random;
20 using namespace mrpt::poses;
21 using namespace mrpt::system;
22 using namespace std;
23 
24 /*---------------------------------------------------------------
25 
26  CHMTSLAM_TBI
27 
28  Topological Bayesian Inference (TBI) process within HMT-SLAM
29 
30  ---------------------------------------------------------------*/
31 void CHMTSLAM::thread_TBI()
32 {
33  CHMTSLAM* obj = this;
34  CTicTac tictac;
35 
36  // Seems that must be called in each thread??
37  if (obj->m_options.random_seed)
38  getRandomGenerator().randomize(obj->m_options.random_seed);
39  else
41 
42  try
43  {
44  // Start thread:
45  // -------------------------
46  obj->logFmt(
48  "[thread_TBI] Thread started (ID=0x%08lX)\n",
49  std::this_thread::get_id());
50 
51  // --------------------------------------------
52  // The main loop
53  // Executes until termination is signaled
54  // --------------------------------------------
55  while (!obj->m_terminateThreads)
56  {
57  std::this_thread::sleep_for(100ms);
58  }; // end while execute thread
59 
60  // Finish thread:
61  // -------------------------
62  MRPT_TODO("Fix thread times");
63  // try { mrpt::system::getCurrentThreadTimes(
64  // timCreat,timExit,timCPU); } catch(...) {};
65  obj->logFmt(mrpt::system::LVL_DEBUG, "[thread_TBI] Thread finished");
66  obj->m_terminationFlag_TBI = true;
67  }
68  catch (std::exception& e)
69  {
70  obj->m_terminationFlag_TBI = true;
71 
72  // Release semaphores:
73 
74  obj->logFmt(mrpt::system::LVL_ERROR, "%s", e.what());
75 
76  // DEBUG: Terminate application:
77  obj->m_terminateThreads = true;
78  }
79  catch (...)
80  {
81  obj->m_terminationFlag_TBI = true;
82 
83  obj->logFmt(
85  "\n---------------------- EXCEPTION CAUGHT! ---------------------\n"
86  " In CHierarchicalMappingFramework::thread_TBI. Unexpected runtime "
87  "error!!\n");
88 
89  // Release semaphores:
90 
91  // DEBUG: Terminate application:
92  obj->m_terminateThreads = true;
93  }
94 }
95 
96 /*---------------------------------------------------------------
97 
98  TBI_main_method
99 
100  Topological Bayesian Inference (TBI) process within HMT-SLAM
101 
102  ---------------------------------------------------------------*/
103 CHMTSLAM::TMessageLSLAMfromTBI::Ptr CHMTSLAM::TBI_main_method(
104  CLocalMetricHypothesis* LMH, const CHMHMapNode::TNodeID& areaID)
105 {
106  MRPT_START
107 
108  CHMTSLAM* obj = (CHMTSLAM*)LMH->m_parent.get();
109 
110  const THypothesisID LMH_ID = LMH->m_ID;
111 
112  // Lock the map:
113  std::lock_guard<std::mutex>(obj->m_map_cs);
114 
117 
118  // Fill out easy data:
119  msg->hypothesisID = LMH_ID;
120  msg->cur_area = areaID;
121 
122  // get a pointer to the current area:
123  const CHMHMapNode::Ptr currentArea = obj->m_map.getNodeByID(areaID);
124  ASSERT_(currentArea);
125 
126  obj->logFmt(
127  mrpt::system::LVL_DEBUG, "[TBI] Request for area id=%i\n", (int)areaID);
128 
129  // --------------------------------------------------------
130  // 1) Use bounding-boxes to get a first list of candidates
131  // The candidates are saved in "msg->loopClosureData"
132  // -------------------------------------------------------
133  // But first: if the areas are within the LMH, then we have to update the
134  // maps in the HMAP!
135  if (LMH->m_neighbors.find(areaID) != LMH->m_neighbors.end())
136  {
137  // Update:
138  LMH->updateAreaFromLMH(areaID);
139  }
140 
141  for (CHierarchicalMapMHPartition::iterator a = obj->m_map.begin();
142  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 
194  obj->m_topLCdets.begin();
195  it != obj->m_topLCdets.end(); ++it)
196  {
197  for (map<CHMHMapNode::TNodeID,
199  msg->loopClosureData.begin();
200  candidate != msg->loopClosureData.end(); ++candidate)
201  {
202  // If the current log_lik of this area is reaaaally low, we
203  // could skip the computation with other LC detectors...
204  // ----------------------------------------------------------------------------------------------------------------
205  // TODO: ...
206 
207  // Proceed:
208  // ----------------------------------------------------------------------------------------------------------------
209  const CHMHMapNode::Ptr refArea =
210  obj->m_map.getNodeByID(candidate->first);
211  double this_log_lik;
212 
213  // get the output from this LC detector:
214  CPose3DPDF::Ptr pdf = (*it)->computeTopologicalObservationModel(
215  LMH->m_ID, currentArea, refArea, this_log_lik);
216 
217  // Add to the output:
218  candidate->second.log_lik += this_log_lik;
219 
220  // This is because not all LC detector MUST return a pose PDF
221  // (i.e. image-based detectors)
222  if (pdf)
223  {
225  CPose3DPDFSOG::Ptr SOG =
226  std::dynamic_pointer_cast<CPose3DPDFSOG>(pdf);
227 
228  // Mix (append) the modes, if any:
229  if (SOG->size() > 0)
230  candidate->second.delta_new_cur.appendFrom(*SOG);
231  else
232  lstNodesToErase.insert(candidate->first);
233  }
234  } // end for each candidate area
235  } // end for each LC detector
236 
237  } // end of m_topLCdets_cs lock
238 
239  // Delete candidates which had no PDF when they should.
240  for (set<CHMHMapNode::TNodeID>::const_iterator it = lstNodesToErase.begin();
241  it != lstNodesToErase.end(); ++it)
242  msg->loopClosureData.erase(*it);
243 
244  obj->logFmt(
245  mrpt::system::LVL_DEBUG, "[TBI_main] Done. %u candidates found.\n",
246  (unsigned int)msg->loopClosureData.size());
247 
248  return msg;
249  MRPT_END
250 }
A namespace of pseudo-random numbers generators of diferent distributions.
Scalar * iterator
Definition: eigen_plugins.h:26
#define MRPT_START
Definition: exceptions.h:262
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:45
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:148
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:68
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
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:141
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 IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:102
#define MRPT_END
Definition: exceptions.h:266
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:6279
const Scalar * const_iterator
Definition: eigen_plugins.h:27
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020