Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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/utils/CTicTac.h>
13 #include <mrpt/random.h>
14 #include <mrpt/utils/CFileStream.h>
15 #include <mrpt/system/os.h>
16 
17 using namespace mrpt::slam;
18 using namespace mrpt::hmtslam;
19 using namespace mrpt::utils;
20 using namespace mrpt::random;
21 using namespace mrpt::poses;
22 using namespace mrpt::system;
23 using namespace std;
24 
25 /*---------------------------------------------------------------
26 
27  CHMTSLAM_TBI
28 
29  Topological Bayesian Inference (TBI) process within HMT-SLAM
30 
31  ---------------------------------------------------------------*/
32 void CHMTSLAM::thread_TBI()
33 {
34  CHMTSLAM *obj = this;
35  CTicTac tictac;
36 
37  // Seems that must be called in each thread??
38  if (obj->m_options.random_seed)
41 
42  try
43  {
44  // Start thread:
45  // -------------------------
46  obj->logFmt(mrpt::utils::LVL_DEBUG, "[thread_TBI] Thread started (ID=0x%08lX)\n", mrpt::system::getCurrentThreadId() );
47 
48  // --------------------------------------------
49  // The main loop
50  // Executes until termination is signaled
51  // --------------------------------------------
52  while ( !obj->m_terminateThreads )
53  {
55  }; // end while execute thread
56 
57  // Finish thread:
58  // -------------------------
59  time_t timCreat,timExit; double timCPU=0;
60  try { mrpt::system::getCurrentThreadTimes( timCreat,timExit,timCPU); } catch(...) {};
61  obj->logFmt(mrpt::utils::LVL_DEBUG, "[thread_TBI] Thread finished. CPU time used:%.06f secs \n",timCPU);
62  obj->m_terminationFlag_TBI = true;
63 
64  }
65  catch(std::exception &e)
66  {
67  obj->m_terminationFlag_TBI = true;
68 
69  // Release semaphores:
70 
71  obj->logFmt(mrpt::utils::LVL_ERROR, "%s", e.what() );
72 
73  // DEBUG: Terminate application:
74  obj->m_terminateThreads = true;
75 
76 
77  }
78  catch(...)
79  {
80  obj->m_terminationFlag_TBI = true;
81 
82  obj->logFmt(mrpt::utils::LVL_ERROR,
83  "\n---------------------- EXCEPTION CAUGHT! ---------------------\n"
84  " In CHierarchicalMappingFramework::thread_TBI. Unexpected runtime error!!\n");
85 
86  // Release semaphores:
87 
88  // DEBUG: Terminate application:
89  obj->m_terminateThreads = true;
90 
91 
92  }
93 
94 }
95 
96 
97 /*---------------------------------------------------------------
98 
99  TBI_main_method
100 
101  Topological Bayesian Inference (TBI) process within HMT-SLAM
102 
103  ---------------------------------------------------------------*/
104 CHMTSLAM::TMessageLSLAMfromTBIPtr CHMTSLAM::TBI_main_method(
106  const CHMHMapNode::TNodeID &areaID)
107 {
108  MRPT_START
109 
110  CHMTSLAM *obj = (CHMTSLAM*) LMH->m_parent.get();
111 
112  const THypothesisID LMH_ID = LMH->m_ID;
113 
114  // Lock the map:
115  synch::CCriticalSectionLocker( &obj->m_map_cs );
116 
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 CHMHMapNodePtr currentArea = obj->m_map.getNodeByID( areaID );
125  ASSERT_(currentArea);
126 
127  obj->logFmt(mrpt::utils::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 maps in the HMAP!
134  if (LMH->m_neighbors.find( areaID ) != LMH->m_neighbors.end() )
135  {
136  // Update:
137  LMH->updateAreaFromLMH( areaID );
138  }
139 
140  for (CHierarchicalMapMHPartition::iterator a=obj->m_map.begin();a!=obj->m_map.end();++a)
141  {
142  // Only for other areas!
143  if (a->first==areaID) continue;
144 
145  // Test hypothesis: LMH_ID
146  if (a->second->m_hypotheses.has(LMH_ID))
147  {
148  // Not neighbors:
149  if (a->second->isNeighbor( areaID, LMH_ID) )
150  continue;
151 
152  // OK, check:
153  // But first: if the areas are within the LMH, then we have to update the maps in the HMAP!
154  if (LMH->m_neighbors.find( a->first ) != LMH->m_neighbors.end() )
155  {
156  // Update:
157  LMH->updateAreaFromLMH( a->first );
158  }
159 
160  // Compute it:
161  double match = obj->m_map.computeOverlapProbabilityBetweenNodes(
162  areaID, // From
163  a->first, // To
164  LMH_ID );
165 
166  obj->logFmt(mrpt::utils::LVL_DEBUG, "[TBI] %i-%i -> overlap prob=%f\n",(int)areaID,(int)a->first,match);
167 
168  if (match>0.9)
169  {
170  // Initialize the new entry in "msg->loopClosureData" for the areas:
171  // "areaID" <-> "a->first"
172  TMessageLSLAMfromTBI::TBI_info &tbi_info = msg->loopClosureData[ a->first ];
173 
174  tbi_info.log_lik = 0;
175  tbi_info.delta_new_cur.clear();
176  }
177  }
178  } // end for each node in the graph.
179 
180 
181  // ----------------------------------------------------
182  // 2) Use the TBI engines
183  // ----------------------------------------------------
184  std::set<CHMHMapNode::TNodeID> lstNodesToErase;
185  {
186  synch::CCriticalSection lock( obj->m_topLCdets_cs );
187 
188  for ( deque<CTopLCDetectorBase*>::const_iterator it=obj->m_topLCdets.begin();it!=obj->m_topLCdets.end();++it)
189  {
190  for (map< CHMHMapNode::TNodeID, TMessageLSLAMfromTBI::TBI_info >::iterator candidate = msg->loopClosureData.begin();candidate != msg->loopClosureData.end();++candidate)
191  {
192  // If the current log_lik of this area is reaaaally low, we could skip the computation with other LC detectors...
193  // ----------------------------------------------------------------------------------------------------------------
194  // TODO: ...
195 
196  // Proceed:
197  // ----------------------------------------------------------------------------------------------------------------
198  const CHMHMapNodePtr refArea = obj->m_map.getNodeByID( candidate->first );
199  double this_log_lik;
200 
201  // get the output from this LC detector:
202  CPose3DPDFPtr pdf = (*it)->computeTopologicalObservationModel(
203  LMH->m_ID,
204  currentArea,
205  refArea,
206  this_log_lik );
207 
208  // Add to the output:
209  candidate->second.log_lik += this_log_lik;
210 
211  // This is because not all LC detector MUST return a pose PDF (i.e. image-based detectors)
212  if (pdf.present())
213  {
214  ASSERT_( IS_CLASS(pdf, CPose3DPDFSOG ) );
215  CPose3DPDFSOGPtr SOG = CPose3DPDFSOGPtr( pdf );
216 
217  // Mix (append) the modes, if any:
218  if (SOG->size()>0)
219  candidate->second.delta_new_cur.appendFrom( *SOG );
220  else
221  lstNodesToErase.insert(candidate->first);
222  }
223  } // end for each candidate area
224  } // end for each LC detector
225 
226  } // end of m_topLCdets_cs lock
227 
228  // Delete candidates which had no PDF when they should.
229  for (set<CHMHMapNode::TNodeID>::const_iterator it=lstNodesToErase.begin();it!=lstNodesToErase.end();++it)
230  msg->loopClosureData.erase(*it);
231 
232  obj->logFmt(mrpt::utils::LVL_DEBUG, "[TBI_main] Done. %u candidates found.\n",(unsigned int)msg->loopClosureData.size() );
233 
234  return msg;
235  MRPT_END
236 }
GLboolean GLboolean GLboolean GLboolean a
Definition: glew.h:5406
int random_seed
0 means randomize, use any other value to have repetitive experiments.
Definition: CHMTSLAM.h:442
This class provides simple critical sections functionality.
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
unsigned long BASE_IMPEXP getCurrentThreadId() MRPT_NO_THROWS
Returns the ID of the current thread.
Definition: threads.cpp:211
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
Scalar * iterator
Definition: eigen_plugins.h:23
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:34
const Scalar * const_iterator
Definition: eigen_plugins.h:24
bool m_terminateThreads
Termination flag for signaling all threads to terminate.
Definition: CHMTSLAM.h:313
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:132
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:59
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
void BASE_IMPEXP getCurrentThreadTimes(time_t &creationTime, time_t &exitTime, double &cpuTime)
Returns the creation and exit times of the current thread and its CPU time consumed.
Definition: threads.cpp:339
#define MRPT_END
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
mrpt::utils::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
GLhandleARB obj
Definition: glew.h:3276
mrpt::hmtslam::CHMTSLAM::TOptions m_options
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
#define MRPT_START
double log_lik
Log likelihood for this loop-closure.
Definition: CHMTSLAM.h:127
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::utils::CSerializable) is of t...
Definition: CObject.h:103
#define ASSERT_(f)
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...
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself)...
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:49
stlplus::smart_ptr< TMessageLSLAMfromTBI > TMessageLSLAMfromTBIPtr
Definition: CHMTSLAM.h:141



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018