Main MRPT website > C++ reference for MRPT 1.5.6
CHMTSLAM_LSLAM.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/utils/stl_containers_utils.h>
14 #include <mrpt/random.h>
15 #include <mrpt/math/distributions.h>
16 #include <mrpt/hmtslam/CRobotPosesGraph.h>
17 
18 #include <mrpt/utils/CFileOutputStream.h>
19 
20 #include <mrpt/system/os.h>
21 #include <mrpt/poses/CPose3DPDFParticles.h>
22 
23 #include <limits>
24 
25 using namespace mrpt::slam;
26 using namespace mrpt::hmtslam;
27 using namespace mrpt::utils;
28 using namespace mrpt::synch;
29 using namespace mrpt::obs;
30 using namespace mrpt::maps;
31 using namespace mrpt::opengl;
32 using namespace mrpt::random;
33 using namespace mrpt::poses;
34 using namespace mrpt::math;
35 using namespace std;
36 
37 /*---------------------------------------------------------------
38 
39  CHMTSLAM_LSLAM
40 
41  Local SLAM process within HMT-SLAM
42 
43  ---------------------------------------------------------------*/
44 void CHMTSLAM::thread_LSLAM()
45 {
46  CHMTSLAM *obj = this;
47  CTicTac tictac;
48  unsigned int nIter = 0; // For logging purposes only
49 
50  // Seems that must be called in each thread??
51  if (obj->m_options.random_seed)
52  randomGenerator.randomize( obj->m_options.random_seed );
53  else randomGenerator.randomize();
54 
55  try
56  {
57  // Start thread:
58  // -------------------------
59  obj->logFmt(mrpt::utils::LVL_DEBUG,"[thread_LSLAM] Thread started (ID=0x%08lX)\n", mrpt::system::getCurrentThreadId() );
60 
61  // --------------------------------------------
62  // The main loop
63  // Executes until termination is signaled
64  // --------------------------------------------
65  while ( !obj->m_terminateThreads )
66  {
67  if (obj->m_options.random_seed)
68  randomGenerator.randomize(obj->m_options.random_seed);
69 
70  // Process pending message?
71  {
72  CMessage *recMsg;
73  do
74  {
75  recMsg = obj->m_LSLAM_queue.get();
76  if (recMsg)
77  {
78  obj->LSLAM_process_message( *recMsg );
79  delete recMsg;
80  }
81  } while (recMsg);
82  }
83 
84  // There are pending elements?
85  if (!obj->isInputQueueEmpty() )
86  {
87  if (obj->m_options.random_seed)
88  randomGenerator.randomize(obj->m_options.random_seed);
89 
90  // Get the next object from the queue:
91  CSerializablePtr nextObject = obj->getNextObjectFromInputQueue();
92  ASSERT_(nextObject);
93 
94  // Clasify the new object:
95  CActionCollectionPtr actions;
96  CSensoryFramePtr observations;
97 
98  if (nextObject->GetRuntimeClass() == CLASS_ID(CActionCollection) )
99  actions = CActionCollectionPtr( nextObject );
100  else
101  if (nextObject->GetRuntimeClass() == CLASS_ID(CSensoryFrame) )
102  observations = CSensoryFramePtr( nextObject );
103  else THROW_EXCEPTION("Element in the queue is neither CActionCollection nor CSensoryFrame!!");
104 
105 
106  // Process them, for each LMH:
107  // -----------------------------------------
108  ASSERT_(!obj->m_LMHs.empty());
109 
110  aligned_containers<THypothesisID, CLocalMetricHypothesis>::map_t::iterator it;
111 
112  ASSERT_(obj->m_LSLAM_method);
113 
114  {
115  CCriticalSectionLocker LMHs_cs_locker( & obj->m_LMHs_cs );
116 
117  for (it=obj->m_LMHs.begin();it!=obj->m_LMHs.end();it++)
118  {
119  CCriticalSectionLocker LMH_individual_locker( & it->second.m_lock );
120 
121  // ----------------------------------------------
122  // 1) Process acts & obs by Local SLAM method:
123  // ----------------------------------------------
124  obj->m_LSLAM_method->processOneLMH(
125  &it->second, // The LMH
126  actions,
127  observations
128  );
129 
130  // ----------------------------------------------
131  // 2) Invoke Area Abstraction (AA) method
132  // ----------------------------------------------
133  if (it->second.m_posesPendingAddPartitioner.size()>5) // Option: Do this only one out of N new added poses:
134  {
135  static CTicTac tictac;
136  tictac.Tic();
137 
138  unsigned nPosesToInsert = it->second.m_posesPendingAddPartitioner.size();
139  TMessageLSLAMfromAAPtr msgFromAA = CHMTSLAM::areaAbstraction( &it->second, it->second.m_posesPendingAddPartitioner );
140 
141  obj->logFmt(mrpt::utils::LVL_DEBUG,"[AreaAbstraction] Took %.03fms to insert %u new poses. AA\n", 1000*tictac.Tac(), nPosesToInsert );
142 
143  // Empty the list, it's done for now:
144  it->second.m_posesPendingAddPartitioner.clear();
145 
146 
147  if (obj->m_options.random_seed)
148  randomGenerator.randomize(obj->m_options.random_seed);
149  // -----------------------------------------------------------------------
150  // 3) Process the new grouping, which is a quite complex process:
151  // Create new areas, joining, adding/deleting arcs and nodes, etc...
152  // -----------------------------------------------------------------------
153  obj->LSLAM_process_message_from_AA( *msgFromAA);
154  }
155 
156  // ----------------------------------------------
157  // 4) Invoke TBI method
158  // ----------------------------------------------
159  if (!it->second.m_areasPendingTBI.empty())
160  {
161  for (TNodeIDList::const_iterator areaID=it->second.m_areasPendingTBI.begin();areaID!=it->second.m_areasPendingTBI.end();++areaID)
162  {
163 
164  static CTicTac tictac;
165  tictac.Tic();
166  if (obj->m_options.random_seed)
167  randomGenerator.randomize(obj->m_options.random_seed);
168 
169  TMessageLSLAMfromTBIPtr msgFromTBI = CHMTSLAM::TBI_main_method(
170  &it->second,
171  *areaID );
172 
173  obj->logFmt(mrpt::utils::LVL_DEBUG,"[TBI] Took %.03fms TBI\n", 1000*tictac.Tac() );
174 
175  // -----------------------------------------------------------------------
176  // Process the set of (potentially) several topological hypotheses:
177  // -----------------------------------------------------------------------
178  obj->LSLAM_process_message_from_TBI( *msgFromTBI);
179 
180  } // for each pending area.
181 
182  it->second.m_areasPendingTBI.clear(); // Done here
183 
184  } // end of areas pending TBI
185 
186  } // end for each LMH
187 
188  } // end of LMHs_cs_locker
189 
190 
191  // Free the object.
192  nextObject.clear_unique();
193 
194 
195  // -----------------------------------------------------------
196  // SLAM: Save log files
197  // -----------------------------------------------------------
198  if (obj->m_options.LOG_OUTPUT_DIR.size() && (nIter % obj->m_options.LOG_FREQUENCY)==0)
199  obj->generateLogFiles(nIter);
200 
201  nIter++;
202 
203  } // End if queue isn't empty
204  else
205  {
206  // Wait for new data:
207  mrpt::system::sleep(5);
208  }
209  }; // end while execute thread
210 
211  // Finish thread:
212  // -------------------------
213  time_t timCreat,timExit; double timCPU=0;
214  try { mrpt::system::getCurrentThreadTimes( timCreat,timExit,timCPU); } catch(...) {};
215  obj->logFmt(mrpt::utils::LVL_DEBUG,"[thread_LSLAM] Thread finished. CPU time used:%.06f secs \n",timCPU);
216  obj->m_terminationFlag_LSLAM = true;
217 
218  }
219  catch(std::exception &e)
220  {
221  obj->m_terminationFlag_LSLAM = true;
222 
223  // Release semaphores:
224 
225  if (e.what()) obj->logFmt(mrpt::utils::LVL_DEBUG,"%s", e.what() );
226 
227  // DEBUG: Terminate application:
228  obj->m_terminateThreads = true;
229 
230 
231  }
232  catch(...)
233  {
234  obj->m_terminationFlag_LSLAM = true;
235 
236  obj->logFmt(mrpt::utils::LVL_DEBUG,"\n---------------------- EXCEPTION CAUGHT! ---------------------\n");
237  obj->logFmt(mrpt::utils::LVL_DEBUG," In CHierarchicalMappingFramework::thread_LSLAM. Unexpected runtime error!!\n");
238 
239  // Release semaphores:
240 
241  // DEBUG: Terminate application:
242  obj->m_terminateThreads = true;
243  }
244 }
245 
246 /*---------------------------------------------------------------
247  LSLAM_process_message
248  ---------------------------------------------------------------*/
249 void CHMTSLAM::LSLAM_process_message( const CMessage &msg )
250 {
251  MRPT_UNUSED_PARAM(msg);
252  MRPT_START
253 
254 /* switch(msg.type)
255  {
256 */
257  /* =============================
258  MSG FROM AA
259  ============================= */
260 /* case MSG_SOURCE_AA:
261  {
262  CHMTSLAM::TMessageLSLAMfromAA *MSG = reinterpret_cast<CHMTSLAM::TMessageLSLAMfromAA*> ( msg.getContentAsPointer() );
263  LSLAM_process_message_from_AA( *MSG );
264  delete MSG; // Free memory
265 
266  } break; // end msg from AA
267  default: THROW_EXCEPTION("Invalid msg type");
268  }
269 */
270 
271  MRPT_END
272 }
273 
274 
275 /*---------------------------------------------------------------
276  LSLAM_process_message_from_AA
277  ---------------------------------------------------------------*/
278 void CHMTSLAM::LSLAM_process_message_from_AA( const TMessageLSLAMfromAA &myMsg )
279 {
280  MRPT_START
281 
282  CTicTac tictac;
283  tictac.Tic();
284  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Beginning of Msg from AA processing... [\n" );
285 
286  // Get the corresponding LMH:
287  aligned_containers<THypothesisID, CLocalMetricHypothesis>::map_t::iterator itLMH = m_LMHs.find( myMsg.hypothesisID );
288  ASSERT_( itLMH!=m_LMHs.end() );
289  CLocalMetricHypothesis *LMH = &itLMH->second;
290 
291  // Sanity checks:
292  {
293  // All poses in the AA's partitions must exist in the current LMH
294  for (vector< TPoseIDList >::const_iterator it= myMsg.partitions.begin(); it!=myMsg.partitions.end();++it)
295  for (TPoseIDList::const_iterator itPose = it->begin();itPose!=it->end();++itPose)
296  if (LMH->m_SFs.find( *itPose ) == LMH->m_SFs.end() )
297  THROW_EXCEPTION_FMT("PoseID %i in AA's partition but not in LMH.\n", (int)*itPose );
298 
299 
300  // All poses in the LMH must be in the AA's partitions:
301  for (map<TPoseID,CHMHMapNode::TNodeID>::const_iterator itA=LMH->m_nodeIDmemberships.begin();itA!=LMH->m_nodeIDmemberships.end();++itA)
302  {
303  if ( LMH->m_currentRobotPose != itA->first ) // The current pose is not included in the AA method
304  {
305  bool found = false;
306  for (vector< TPoseIDList >::const_iterator it= myMsg.partitions.begin(); !found && it!=myMsg.partitions.end();++it)
307  for (TPoseIDList::const_iterator itPose = it->begin();itPose!=it->end();++itPose)
308  if ( itA->first == *itPose )
309  {
310  found=true;
311  break;
312  }
313  if (!found) THROW_EXCEPTION_FMT("LMH's pose %i not found in AA's partitions.", (int)itA->first );
314  }
315  }
316  }
317 
318  // Neighbors BEFORE:
319  TNodeIDSet neighbors_before( LMH->m_neighbors );
320 
321  // Get current coords origin:
322  TPoseID poseID_origin;
323  {
324  CCriticalSectionLocker locker( &m_map_cs );
325 
326  map<TPoseID,CHMHMapNode::TNodeID>::const_iterator itCur = LMH->m_nodeIDmemberships.find( LMH->m_currentRobotPose );
327  ASSERT_(itCur != LMH->m_nodeIDmemberships.end() );
328 
329  if (!m_map.getNodeByID( itCur->second)->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID,poseID_origin, LMH->m_ID ) )
330  THROW_EXCEPTION("Current area reference pose not found");
331  }
332 
333  // ---------------------------------------------------------------------
334  // Process the partitioning:
335  // The goal is to obtain a mapping "int --> CHMHMapNode::TNodeID" from
336  // indexes in "myMsg.partitions" to topological areas.
337  // To do this, we establish a voting scheme: each robot pose votes for
338  // its current area ID in the particle data, then the maximum is kept.
339  // ---------------------------------------------------------------------
340  // map<TPoseID,CHMHMapNode::TNodeID>: LMH->m_nodeIDmemberships
341  map< unsigned int, map<CHMHMapNode::TNodeID, unsigned int> > votes;
342  unsigned int i;
343 
344  static int DEBUG_STEP = 0;
345  DEBUG_STEP++;
346  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] DEBUG_STEP=%i\n",DEBUG_STEP);
347  if (DEBUG_STEP==3)
348  {
349  CMatrix A(3,3);
350  DEBUG_STEP = DEBUG_STEP + 0;
351  }
352  if (0)
353  {
354  CCriticalSectionLocker locker( &m_map_cs );
355  utils::CStringList s;
356  m_map.dumpAsText(s);
357  s.saveToFile( format("%s/HMAP_txt/HMAP_%05i_before.txt",m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP ) );
358  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Saved HMAP_%05i_before.txt\n",DEBUG_STEP);
359  }
360 
361  vector< TPoseIDList >::const_iterator it;
362  for ( i=0, it= myMsg.partitions.begin(); it!=myMsg.partitions.end();++it, i++)
363  for (TPoseIDList::const_iterator itPose = it->begin();itPose!=it->end();++itPose)
364  {
365  map<TPoseID,CHMHMapNode::TNodeID>::const_iterator itP = LMH->m_nodeIDmemberships.find( *itPose );
366  ASSERT_(itP != LMH->m_nodeIDmemberships.end() );
367 
368  votes[i][ itP->second ]++;
369  }
370 
371  // The goal: a mapping from partition index -> area IDs:
372  vector<CHMHMapNode::TNodeID> partIdx2Areas( myMsg.partitions.size(), AREAID_INVALID );
373 
374  map<CHMHMapNode::TNodeID, pair<size_t, unsigned int> > mostVotedFrom; // ID -> (index, votes)
375  ASSERT_(votes.size() == myMsg.partitions.size());
376 
377  // 1) For partitions voting for just one area, assign them that area if they are the most voted partitions:
378  for (size_t k=0;k<myMsg.partitions.size();k++)
379  {
380  if (votes[k].size()==1)
381  { // map< unsigned int, map<CHMHMapNode::TNodeID, unsigned int> > votes; recall!
382  if (votes[k].begin()->second>mostVotedFrom[ votes[k].begin()->first ].second)
383  {
384  mostVotedFrom[ votes[k].begin()->first ].first = k;
385  mostVotedFrom[ votes[k].begin()->first ].second = votes[k].begin()->second;
386  }
387  }
388  }
389 
390  // To the winners, assign very high votes so the rest of votes do not interfere in what has been
391  // already decided above:
392  for (map<CHMHMapNode::TNodeID, pair<size_t, unsigned int> >::iterator v =mostVotedFrom.begin();v!=mostVotedFrom.end();++v)
393  v->second.second = std::numeric_limits<unsigned int>::max();
394 
395  // 2) Assign each area ID to the partition that votes it most:
396 
397  for (size_t k=0;k<myMsg.partitions.size();k++)
398  {
399  for (map<CHMHMapNode::TNodeID, unsigned int>::iterator it=votes[k].begin();it!=votes[k].end();++it)
400  {
401  // Recall:
402  // "votes" is index -> ( map: ID -> #votes )
403  // "mostVotedFrom" is ID -> (index, votes)
404  map<CHMHMapNode::TNodeID, pair<size_t, unsigned int> >::iterator mostVotesIt;
405  mostVotesIt = mostVotedFrom.find( it->first );
406  if (mostVotesIt == mostVotedFrom.end())
407  {
408  // First time: add
409  mostVotedFrom[it->first].first = k;
410  mostVotedFrom[it->first].second = it->second;
411  }
412  else
413  {
414  // compare:
415  if ( it->second > mostVotesIt->second.second )
416  {
417  mostVotesIt->second.first = k;
418  mostVotesIt->second.second = it->second;
419  }
420  }
421  }
422  }
423 
424  // Fill out "partIdx2Areas" from "mostVotedFrom":
425  for (map<CHMHMapNode::TNodeID, pair<size_t, unsigned int> >::iterator it=mostVotedFrom.begin();it!=mostVotedFrom.end();++it)
426  partIdx2Areas[ it->second.first ] = it->first;
427 
428 
429  // Create new area IDs for new areas (ie, partIdx2Areas[] still unassigned):
430  for (i=0;i<partIdx2Areas.size();i++)
431  {
432  if (partIdx2Areas[i]== AREAID_INVALID)
433  {
434  // Create new area in the H-MAP:
435  CCriticalSectionLocker locker( &m_map_cs );
436 
437  CHMHMapNodePtr newArea = CHMHMapNode::Create( &m_map );
438 
439  // For now, the area exists in this hypothesis only:
440  newArea->m_hypotheses.insert( LMH->m_ID );
441  newArea->m_nodeType.setType( "Area" );
442  newArea->m_label = generateUniqueAreaLabel();
443 
444  CMultiMetricMapPtr emptyMap = CMultiMetricMapPtr( new CMultiMetricMap(&m_options.defaultMapsInitializers) );
445  newArea->m_annotations.setMemoryReference( NODE_ANNOTATION_METRIC_MAPS, emptyMap, LMH->m_ID );
446 
447  CRobotPosesGraphPtr emptyPoseGraph = CRobotPosesGraph::Create();
448  newArea->m_annotations.setMemoryReference( NODE_ANNOTATION_POSES_GRAPH, emptyPoseGraph, LMH->m_ID );
449 
450  // Set ID in list:
451  partIdx2Areas[i] = newArea->getID();
452  }
453  } // end for i
454 
455 
456  {
457  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] partIdx2Areas:\n");
458  for (size_t i=0;i<partIdx2Areas.size();i++)
459  logFmt(mrpt::utils::LVL_INFO," Partition #%i -> AREA_ID %i ('%s')\n",(int)i,(int)partIdx2Areas[i], m_map.getNodeByID(partIdx2Areas[i])->m_label.c_str() );
460  }
461 
462 
463  // --------------------------------------------------------
464  // Set the new area memberships into the LMH, and rebuild
465  // the list of neighbors:
466  // --------------------------------------------------------
467  LMH->m_neighbors.clear();
468  for (i=0;i<partIdx2Areas.size();i++)
469  {
470  CHMHMapNode::TNodeID nodeId = partIdx2Areas[i];
471 
472  // Add only if unique:
473  LMH->m_neighbors.insert(nodeId);
474  //if (LMH->m_neighbors.find(nodeId)==LMH->m_neighbors.end()) LMH->m_neighbors.push_back(nodeId);
475 
476  for (TPoseIDList::const_iterator it=myMsg.partitions[i].begin();it!=myMsg.partitions[i].end();++it)
477  LMH->m_nodeIDmemberships[ *it ] = nodeId; // Bind robot poses -> area IDs.
478  } // end for i
479 
480 
481  // ------------------------------------------------------------------------
482  // The current robot pose is set as the membership of the closest pose:
483  // ------------------------------------------------------------------------
484  TMapPoseID2Pose3D lstPoses;
485  LMH->getMeans( lstPoses );
486  TPoseID closestPose = POSEID_INVALID;
487  double minDist=0;
488  const CPose3D *curPoseMean = & lstPoses[ LMH->m_currentRobotPose ];
489 
490  for ( TMapPoseID2Pose3D::const_iterator it=lstPoses.begin();it!=lstPoses.end();++it )
491  {
492  if ( it->first != LMH->m_currentRobotPose ) // Only compare to OTHER poses!
493  {
494  double dist = curPoseMean->distanceEuclidean6D( it->second );
495  if (closestPose==POSEID_INVALID || dist<minDist)
496  {
497  closestPose = it->first;
498  minDist = dist;
499  }
500  }
501  }
502  ASSERT_( closestPose != POSEID_INVALID );
503 
504 
505  // Save old one:
506  const CHMHMapNode::TNodeID oldAreaID = LMH->m_nodeIDmemberships[ LMH->m_currentRobotPose ];
507 
508  // set it:
509  LMH->m_nodeIDmemberships[ LMH->m_currentRobotPose ] = LMH->m_nodeIDmemberships[closestPose];
510 
511  // Save old one:
512  const CHMHMapNode::TNodeID curAreaID = LMH->m_nodeIDmemberships[ LMH->m_currentRobotPose ];
513 
514  if (curAreaID!=oldAreaID)
515  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Current area has changed: %i -> %i\n",(int)oldAreaID,(int)curAreaID );
516 
517 
518  // --------------------------------------------------------
519  // Check for areas that have disapeared
520  // --------------------------------------------------------
521  for (TNodeIDSet::const_iterator pBef=neighbors_before.begin();pBef!=neighbors_before.end();++pBef)
522  {
523  if ( LMH->m_neighbors.find(*pBef) == LMH->m_neighbors.end() )
524  {
525 
526 #if 1
527  {
528  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Old neighbors: ");
529  for (TNodeIDSet::const_iterator it=neighbors_before.begin();it!=neighbors_before.end();++it)
530  logFmt(mrpt::utils::LVL_INFO,"%i ",(int)*it);
531  logFmt(mrpt::utils::LVL_INFO,"\n");
532  }
533  {
534  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Cur. neighbors: ");
535  for (TNodeIDSet::const_iterator it=LMH->m_neighbors.begin();it!=LMH->m_neighbors.end();++it)
536  logFmt(mrpt::utils::LVL_INFO,"%i ",(int)*it);
537  logFmt(mrpt::utils::LVL_INFO,"\n");
538  }
539 #endif
540 
541  CCriticalSectionLocker locker( &m_map_cs );
542 
543  // A node has dissappeared:
544  // Delete the node from the HMT map:
545  CHMHMapNodePtr node = m_map.getNodeByID( *pBef );
546 
547  if (!node)
548  {
549  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Area %i has been removed from the neighbors & no longer exists in the HMAP.\n", (int)*pBef);
550  }
551  else
552  {
553  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Deleting area %i\n", (int)node->getID() );
554 
555  // ----------------------------------------------------------------------------
556  // Check if arcs to nodes out of the LMH must be modified coz this deletion
557  // ----------------------------------------------------------------------------
558  TArcList arcs;
559  node->getArcs( arcs );
560 
561  // 1) First, make a list of nodes WITHIN the LMH with arcs to "a":
562  typedef map<CHMHMapNodePtr,CHMHMapArcPtr> TListNodesArcs;
563  TListNodesArcs lstWithinLMH;
564 
565  for (TArcList::const_iterator a=arcs.begin();a!=arcs.end();++a)
566  {
567  CHMHMapNodePtr nodeB;
568 
569  if ( (*a)->getNodeFrom() == *pBef )
570  { // node to delete is: "from"
571  nodeB = m_map.getNodeByID( (*a)->getNodeTo() );
572  }
573  else
574  {// node to delete is: "to"
575  nodeB = m_map.getNodeByID( (*a)->getNodeFrom() );
576  }
577 
578  bool inNeib = LMH->m_neighbors.find( nodeB->getID() ) != LMH->m_neighbors.end();
579  bool inBefNeib = neighbors_before.find( nodeB->getID() ) != neighbors_before.end();
580 
581  if ( inNeib && inBefNeib )
582  lstWithinLMH[nodeB] = *a; // Add to list:
583 
584  } // end for each arc
585 
586  // 2) Now, process:
587  for (TArcList::const_iterator a=arcs.begin();a!=arcs.end();++a)
588  {
589  CHMHMapNodePtr nodeB;
590  bool dirA2B;
591 
592  CHMHMapArcPtr arc = *a;
593 
594  if ( arc->getNodeFrom() == *pBef )
595  { // node to delete is: "from"
596  nodeB = m_map.getNodeByID( (*a)->getNodeTo() );
597  dirA2B = true;
598  }
599  else
600  {// node to delete is: "to"
601  nodeB = m_map.getNodeByID( (*a)->getNodeFrom() );
602  dirA2B = false;
603  }
604 
605  bool inNeib = LMH->m_neighbors.find( nodeB->getID() ) != LMH->m_neighbors.end();
606  bool inBefNeib = neighbors_before.find( nodeB->getID() ) != neighbors_before.end();
607 
608  if ( inNeib && inBefNeib )
609  {
610  // Target was and is in the LMH, nothing extra to do here.
611  }
612  else // The target was into the LMH, but not anymore.
613  {
614  // Target is outside of the LMH:
615  // --------------------------------------------------------------
616  // Since we are deleting this node, we must readjust the
617  // arcs "a"<->"b" containing relative poses so they
618  // refer to valid reference poses.
619  // --------------------------------------------------------------
620  for (TListNodesArcs::iterator na=lstWithinLMH.begin();na!=lstWithinLMH.end();++na)
621  {
622  CHMHMapNodePtr node_c = na->first;
623  const CHMHMapArcPtr arc_c_a = na->second;
624 
625  // Now we have the arc "arc" from "node"<->"nodeB" in the direction "dirA2B", which will be deleted next.
626  // The arc "a<->c", being "node_c" a node within the LMH, is in "arc_c_a".
627  // node_b -> outside LMH
628  // node_c -> within LMH
629  // Then:
630  // A new arc "b->c" will be created with the Delta:
631  // Delta_b_c = [ a (-) b ] (+) [ c (-) a ] = c (-) b
632  // \----v----/ \----v----/
633  // Delta_b_a Delta_a_c
634  //
635 
636  // Get "Delta_b_a":
637  CPose3DPDFGaussian Delta_b_a;
638  TPoseID refPoseAt_b;
639  {
640  CPose3DPDFGaussianPtr pdf = arc->m_annotations.getAs<CPose3DPDFGaussian>( ARC_ANNOTATION_DELTA, LMH->m_ID, false );
641  TPoseID refPoseAt_a;
642  if (!dirA2B)
643  {
644  Delta_b_a.copyFrom( *pdf );
645 
646  // Check valid reference poseIDs:
647  arc->m_annotations.getElemental( ARC_ANNOTATION_DELTA_SRC_POSEID, refPoseAt_b, LMH->m_ID, true );
648  arc->m_annotations.getElemental( ARC_ANNOTATION_DELTA_TRG_POSEID, refPoseAt_a, LMH->m_ID, true );
649  }
650  else
651  {
652  pdf->inverse( Delta_b_a );
653 
654  // Check valid reference poseIDs:
655  arc->m_annotations.getElemental( ARC_ANNOTATION_DELTA_TRG_POSEID, refPoseAt_b, LMH->m_ID, true );
656  arc->m_annotations.getElemental( ARC_ANNOTATION_DELTA_SRC_POSEID, refPoseAt_a, LMH->m_ID, true );
657  }
658 
659  TPoseID node_refPoseAt_b;
660  nodeB->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, node_refPoseAt_b, LMH->m_ID, true);
661  ASSERT_(node_refPoseAt_b==refPoseAt_b);
662 
663  TPoseID node_refPoseAt_a;
664  node->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, node_refPoseAt_a, LMH->m_ID, true);
665  ASSERT_(node_refPoseAt_a==refPoseAt_a);
666  }
667 
668  // Get "Delta_a_c":
669  CPose3DPDFGaussian Delta_a_c;
670  TPoseID refPoseAt_c;
671  {
672  CPose3DPDFGaussianPtr pdf = arc_c_a->m_annotations.getAs<CPose3DPDFGaussian>( ARC_ANNOTATION_DELTA, LMH->m_ID, false );
673  TPoseID refPoseAt_a;
674  if ( arc_c_a->getNodeTo()==node_c->getID() )
675  {
676  Delta_a_c.copyFrom( *pdf );
677 
678  // Check valid reference poseIDs:
679  arc_c_a->m_annotations.getElemental( ARC_ANNOTATION_DELTA_SRC_POSEID, refPoseAt_a, LMH->m_ID, true );
680  arc_c_a->m_annotations.getElemental( ARC_ANNOTATION_DELTA_TRG_POSEID, refPoseAt_c, LMH->m_ID, true );
681  }
682  else
683  {
684  pdf->inverse( Delta_a_c );
685 
686  // Check valid reference poseIDs:
687  arc_c_a->m_annotations.getElemental( ARC_ANNOTATION_DELTA_TRG_POSEID, refPoseAt_a, LMH->m_ID, true );
688  arc_c_a->m_annotations.getElemental( ARC_ANNOTATION_DELTA_SRC_POSEID, refPoseAt_c, LMH->m_ID, true );
689  }
690 
691  TPoseID node_refPoseAt_c;
692  node_c->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, node_refPoseAt_c, LMH->m_ID, true);
693  ASSERT_(node_refPoseAt_c==refPoseAt_c);
694 
695  TPoseID node_refPoseAt_a;
696  node->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, node_refPoseAt_a, LMH->m_ID, true);
697  ASSERT_(node_refPoseAt_a==refPoseAt_a);
698  }
699 
700  // Compose:
701  // Delta_b_c = Delta_b_a (+) Delta_a_c
702  CPose3DPDFGaussian Delta_b_c( Delta_b_a + Delta_a_c );
703  Delta_b_c.cov.zeros(); // *********** DEBUG !!!!!!!!!!!
704  Delta_b_c.cov(0,0)=Delta_b_c.cov(1,1)=square(0.04);
705  Delta_b_c.cov(3,3)=square(DEG2RAD(1));
706 
707  MRPT_LOG_DEBUG_STREAM( "b_a: " << Delta_b_a.mean << endl << "a_c: " << Delta_a_c.mean << endl << "b_a + a_c: " << Delta_b_c.mean << endl);
708 
709  // ------------------------------------------------
710  // Finally, add the new annotation to arc "b->c":
711  // ------------------------------------------------
712  // Did an arc already exist? Look into existing arcs, in both directions:
713  bool arcDeltaIsInverted;
714  CHMHMapArcPtr newArc = m_map.findArcOfTypeBetweenNodes(
715  nodeB->getID(), // Source
716  node_c->getID(), // Target
717  LMH->m_ID, // Hypos
718  "RelativePose",
719  arcDeltaIsInverted );
720 
721  if (!newArc)
722  {
723  // Create a new one:
724  newArc = CHMHMapArc::Create(
725  nodeB, // Source
726  node_c, // Target
727  LMH->m_ID, // Hypos
728  &m_map // The graph
729  );
730  newArc->m_arcType.setType("RelativePose");
731  arcDeltaIsInverted = false;
732  }
733 
734  if (!arcDeltaIsInverted)
735  { // arc: b->c
736  newArc->m_annotations.set(ARC_ANNOTATION_DELTA, CPose3DPDFGaussianPtr( new CPose3DPDFGaussian(Delta_b_c) ),LMH->m_ID );
737  MRPT_LOG_DEBUG_STREAM( "[LSLAM_proc_msg_AA] Setting arc " << nodeB->getID() << " -> " << node_c->getID() << " : " << Delta_b_c.mean << " cov = " << Delta_b_c.cov.inMatlabFormat() << endl);
738  newArc->m_annotations.setElemental(ARC_ANNOTATION_DELTA_SRC_POSEID, refPoseAt_b ,LMH->m_ID );
739  newArc->m_annotations.setElemental(ARC_ANNOTATION_DELTA_TRG_POSEID, refPoseAt_c ,LMH->m_ID );
740  }
741  else
742  { // arc: c->b
743  CPose3DPDFGaussian Delta_b_c_inv;
744  Delta_b_c.inverse( Delta_b_c_inv );
745 
746  MRPT_LOG_DEBUG_STREAM( "[LSLAM_proc_msg_AA] Setting arc " << nodeB->getID() << " <- " << node_c->getID() << " : " << Delta_b_c_inv.mean << " cov = " << Delta_b_c_inv.cov.inMatlabFormat() << endl);
747  newArc->m_annotations.set(ARC_ANNOTATION_DELTA,CPose3DPDFGaussianPtr( new CPose3DPDFGaussian(Delta_b_c_inv)),LMH->m_ID );
748  newArc->m_annotations.setElemental(ARC_ANNOTATION_DELTA_SRC_POSEID, refPoseAt_c ,LMH->m_ID );
749  newArc->m_annotations.setElemental(ARC_ANNOTATION_DELTA_TRG_POSEID, refPoseAt_b ,LMH->m_ID );
750  }
751  } // end for each arc-node
752 
753  // Remove arc data for this hypothesis:
754  arc->m_annotations.removeAll( LMH->m_ID );
755 
756  if (!arc->m_annotations.size())
757  {
758  arc.clear();
759  }
760 
761  } // end of adjust arcs
762 
763  } // end for each arc
764 
765  // Make sure we delete all its arcs as well first:
766  {
767  TArcList arcs;
768  node->getArcs(arcs);
769  for (TArcList::iterator a=arcs.begin();a!=arcs.end();++a)
770  a->clear();
771  }
772 
773  node.clear(); // And finally, delete the node.
774  } // end of "node" still exist in HMAP.
775  }
776 
777  } // end for each before beigbors
778 
779 
780  // ---------------------------------------------------------------------------------------
781  // Add areas to be considered by the TBI to launch potential loop-closure hypotheses.
782  // One option: To add the just newly entered area
783  // ---------------------------------------------------------------------------------------
784  //const CHMHMapNode::TNodeID new_curAreaID = LMH->m_nodeIDmemberships[ LMH->m_currentRobotPose ];
785  if ( curAreaID != oldAreaID )
786  {
787  LMH->m_areasPendingTBI.insert(curAreaID); // Add to TBI list
788  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Current area changed: enqueing area %i for TBI.\n", (int)curAreaID );
789  }
790  else
791  {
792  static size_t cntAddTBI = 0;
793  if (++cntAddTBI>4)
794  {
795  cntAddTBI = 0;
796  LMH->m_areasPendingTBI.insert(curAreaID); // Add to TBI list
797  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Current area %i enqued for TBI (routine check).\n", (int)curAreaID );
798  }
799  }
800 
801  // ---------------------------------------------------------------------------------------
802  // Create arcs between areas and the closest partition's area,
803  // and keep in order reference poses for each area, etc...
804  // This block of code also:
805  // - Update the arcs' deltas between all the pairs of connected areas within the LMH
806  // ---------------------------------------------------------------------------------------
807  // List of all LMH's internal arcs to create or update:
808  // Each entry is a pair of areas to create an arc between, and the "->second" is
809  // the corresponding reference pose IDs of each area.
810  //map<TPairNodeIDs,TPairPoseIDs> lstInternalArcsToCreate;
811  list_searchable<TPairNodeIDs> lstInternalArcsToCreate;
812 
813 // const CHMHMapNode::TNodeID curAreaID = LMH->m_nodeIDmemberships[ LMH->m_currentRobotPose ];
814 
815  if (partIdx2Areas.size()>1)
816  {
817  CCriticalSectionLocker locker( &m_map_cs );
818  //THypothesisIDSet theArcHypos( LMH->m_ID );
819 
820  set<CHMHMapNode::TNodeID> areasWithLink; // All the areas with at least one internal arc.
821 
822  // The closest distance between areas (only when above the threshold)
823  map<CHMHMapNode::TNodeID, pair< CHMHMapNode::TNodeID, float > > lstClosestDoubtfulNeigbors;
824 
825  for (size_t idx_area_a=0;idx_area_a<partIdx2Areas.size();idx_area_a++)
826  {
827  // Get the area for this partition from the graph:
828  // ------------------------------------------------------
829  CHMHMapNode::TNodeID area_a_ID = partIdx2Areas[idx_area_a];
830  CHMHMapNodePtr area_a = m_map.getNodeByID(area_a_ID);
831  ASSERT_(area_a);
832 
833  // Look for the closest area & it's reference pose:
834  // -------------------------------------------------------------
835 
836  ASSERT_(myMsg.partitions[idx_area_a].size()>0);
837  TPoseID poseID_trg;
838 
839  // Add the "trg" pose as reference in its area, or take the current reference, or change it
840  // if the pose id is no longer on the partition:
841 
842  if ( !area_a->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, poseID_trg, LMH->m_ID ) )
843  {
844  // No reference pose annotation yet: add it now:
845  poseID_trg = myMsg.partitions[idx_area_a][0];
846 
847  area_a->m_annotations.setElemental( NODE_ANNOTATION_REF_POSEID, poseID_trg, LMH->m_ID );
848  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Changing reference poseID of area '%i' to pose '%i'\n", (int)area_a_ID,(int)poseID_trg);
849 
850  // Reconsider the arcs of this area again, since the ref. poseID has changed:
851  /*for ( list_searchable<TPairNodeIDs>::iterator it=lstAlreadyUpdated.begin();it!=lstAlreadyUpdated.end(); )
852  {
853  if (it->first == area_a_ID || it->second==area_a_ID)
854  it = lstAlreadyUpdated.erase( it);
855  else it++;
856  }*/
857 
858  }
859  else
860  {
861  // Check if "poseID_trg" is still in the partition:
862  bool found = false;
863  TPoseID poseID_trg_old = poseID_trg;
864  for (TPoseIDList::const_iterator p=myMsg.partitions[idx_area_a].begin();!found && p!=myMsg.partitions[idx_area_a].end();++p)
865  if (poseID_trg==*p)
866  {
867  found = true;
868  break;
869  }
870 
871  if (!found)
872  {
873  // We must overwrite the anotation with a new reference pose:
874  poseID_trg = myMsg.partitions[idx_area_a][0];
875  area_a->m_annotations.setElemental( NODE_ANNOTATION_REF_POSEID, poseID_trg, LMH->m_ID );
876 
877  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Changing reference poseID of area '%i' to pose '%i'\n", (int)area_a_ID,(int)poseID_trg);
878 
879  // ------------------------------------------------------------------------
880  // Look for existing arcs from "area_a"<->other areas outside the LMH to
881  // fix the "Delta" annotations due to the change in reference poseID from
882  // the old "poseID_trg_old" to the new "poseID_trg".
883  // ------------------------------------------------------------------------
884  TArcList arcs;
885  area_a->getArcs( arcs );
886  for (TArcList::const_iterator a=arcs.begin();a!=arcs.end();++a)
887  {
888  CHMHMapArcPtr theArc = *a;
889  CHMHMapNode::TNodeID nodeFrom = theArc->getNodeFrom();
890  CHMHMapNode::TNodeID nodeTo = theArc->getNodeTo();
891 
892  // --------------------------------------------------------------------------------------------
893  // Ok... we are here updating an existing arc "nodeFrom"->"nodeTo", with only one of the
894  // extremes being within the LMH, to account for a change in the reference pose of the area
895  // within the LMH.
896  // The old "poseID_trg_old" --> the new "poseID_trg".
897  // --------------------------------------------------------------------------------------------
898  if (nodeFrom==area_a_ID)
899  {
900  // Is nodeTo out of the LMH?
901  if (LMH->m_neighbors.find( nodeTo )==LMH->m_neighbors.end())
902  { // nodeTo is outside the LMH:
903  // The source area is into the LMH.
904  CPose3DPDFParticles Anew_old_parts;
905  LMH->getRelativePose(poseID_trg, poseID_trg_old, Anew_old_parts);
906 
907  CPose3DPDFGaussian Anew_old;
908  Anew_old.copyFrom( Anew_old_parts );
909 
910  CPose3DPDFGaussian newDelta;
911  CPose3DPDFGaussianPtr oldDelta = theArc->m_annotations.getAs<CPose3DPDFGaussian>(ARC_ANNOTATION_DELTA,LMH->m_ID, false );
912 
913  newDelta = Anew_old + *oldDelta;
914  newDelta.cov.zeros(); // *********** DEBUG !!!!!!!!!!!
915  newDelta.cov(0,0)=newDelta.cov(1,1)=square(0.04);
916  newDelta.cov(3,3)=square(DEG2RAD(1));
917 
918  MRPT_LOG_DEBUG_STREAM( "[LSLAM_proc_msg_AA] Updating arc " << nodeFrom << " -> " << nodeTo << " OLD: " << oldDelta->mean << " cov = " << oldDelta->cov.inMatlabFormat() << endl);
919  MRPT_LOG_DEBUG_STREAM( "[LSLAM_proc_msg_AA] Updating arc " << nodeFrom << " -> " << nodeTo << " NEW: " << newDelta.mean << " cov = " << newDelta.cov.inMatlabFormat() << endl);
920 
921  theArc->m_annotations.set(ARC_ANNOTATION_DELTA, CPose3DPDFGaussianPtr( new CPose3DPDFGaussian(newDelta)),LMH->m_ID );
922  theArc->m_annotations.setElemental(ARC_ANNOTATION_DELTA_SRC_POSEID, poseID_trg ,LMH->m_ID );
923  }
924  }
925  else
926  {
927  // Is nodeFrom out of the LMH?
928  if (LMH->m_neighbors.find( nodeFrom )==LMH->m_neighbors.end())
929  { // nodeFrom is outside the LMH:
930  // The target area is into the LMH:
931  CPose3DPDFParticles Aold_new_parts;
932  LMH->getRelativePose(poseID_trg_old, poseID_trg, Aold_new_parts);
933 
934  CPose3DPDFGaussian Aold_new;
935  Aold_new.copyFrom( Aold_new_parts );
936 
937  CPose3DPDFGaussianPtr oldDelta = theArc->m_annotations.getAs<CPose3DPDFGaussian>(ARC_ANNOTATION_DELTA,LMH->m_ID, false );
938  CPose3DPDFGaussian newDelta;
939 
940  newDelta = *oldDelta + Aold_new;
941 
942  newDelta.cov.zeros(); // *********** DEBUG !!!!!!!!!!!
943  newDelta.cov(0,0)=newDelta.cov(1,1)=square(0.04);
944  newDelta.cov(3,3)=square(DEG2RAD(1));
945 
946  MRPT_LOG_DEBUG_STREAM( "[LSLAM_proc_msg_AA] Updating arc " << nodeFrom << " <- " << nodeTo << " OLD: " << oldDelta->mean << " cov = " << oldDelta->cov.inMatlabFormat() << endl);
947  MRPT_LOG_DEBUG_STREAM( "[LSLAM_proc_msg_AA] Updating arc " << nodeFrom << " <- " << nodeTo << " NEW: " << newDelta.mean << " cov = " << newDelta.cov.inMatlabFormat() << endl);
948 
949  theArc->m_annotations.set(ARC_ANNOTATION_DELTA,CPose3DPDFGaussianPtr(new CPose3DPDFGaussian(newDelta)),LMH->m_ID );
950  theArc->m_annotations.setElemental(ARC_ANNOTATION_DELTA_TRG_POSEID, poseID_trg ,LMH->m_ID );
951  }
952  }
953 
954  } // end for each arc
955  }
956  }
957 
958  // Now, go thru all other areas to check whether they are neighbors of "area_a":
959  for (size_t idx_area_b=0;idx_area_b<myMsg.partitions.size();idx_area_b++)
960  {
961  if (idx_area_a==idx_area_b) continue; // Look for poses in a different area only!
962 
963  TPoseID poseID_closests = POSEID_INVALID;
964  double closestDistPoseSrc=0;
965 
966  // Get the "trg" pose at "area_a": Sweep over all the poses in the "area_a", to find the closests poses to other clusters:
967  for (TPoseIDList::const_iterator itP0 = myMsg.partitions[idx_area_a].begin();itP0 != myMsg.partitions[idx_area_a].end();itP0++)
968  {
969  const CPose3D &pose_trg= lstPoses[*itP0]; // Get its pose
970 
971  for (TPoseIDList::const_iterator itP = myMsg.partitions[idx_area_b].begin();itP != myMsg.partitions[idx_area_b].end();++itP)
972  {
973  const CPose3D &otherPose = lstPoses[*itP];
974  double dst = otherPose.distanceTo( pose_trg );
975  if (dst<closestDistPoseSrc || poseID_closests == POSEID_INVALID)
976  {
977  poseID_closests = *itP;
978  closestDistPoseSrc = dst;
979  //closestAreaID = partIdx2Areas[k];
980  }
981  }
982  } // end for itP0
983 
984  ASSERT_(poseID_closests != POSEID_INVALID);
985 
986  // Should we create an arc between area_a <-> area_b ??
987  CHMHMapNode::TNodeID area_b_ID = partIdx2Areas[idx_area_b];
988  if ( closestDistPoseSrc< 5*m_options.SLAM_MIN_DIST_BETWEEN_OBS )
989  {
990  CHMHMapNodePtr area_b = m_map.getNodeByID( area_b_ID );
991  ASSERT_(area_b);
992 
993  TPoseID poseID_src = POSEID_INVALID;
994  if (!area_b->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, poseID_src, LMH->m_ID ) )
995  {
996  // Add 'poseID_closests': this should happen when the closest area is a new one:
997  area_b->m_annotations.setElemental( NODE_ANNOTATION_REF_POSEID, poseID_closests, LMH->m_ID );
998  poseID_src = poseID_closests;
999  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Changing reference poseID of area '%i' to pose '%i' (creat. annot)\n", (int)area_b_ID,(int)poseID_closests);
1000  }
1001  ASSERT_(poseID_src != POSEID_INVALID);
1002 
1003  // Add to the list of arcs to be computed after this loop:
1004  if ( lstInternalArcsToCreate.end() == lstInternalArcsToCreate.find(TPairNodeIDs( area_b_ID,area_a_ID )) &&
1005  lstInternalArcsToCreate.end() == lstInternalArcsToCreate.find(TPairNodeIDs( area_a_ID, area_b_ID)) )
1006  {
1007  lstInternalArcsToCreate.insert( TPairNodeIDs( area_b_ID,area_a_ID ) );
1008  areasWithLink.insert( area_a_ID );
1009  areasWithLink.insert( area_b_ID );
1010  }
1011  }
1012  else
1013  {
1014  if (lstClosestDoubtfulNeigbors.find(area_b_ID) == lstClosestDoubtfulNeigbors.end() ||
1015  closestDistPoseSrc < lstClosestDoubtfulNeigbors[area_b_ID].second )
1016  {
1017  lstClosestDoubtfulNeigbors[area_b_ID].first = area_a_ID;
1018  lstClosestDoubtfulNeigbors[area_b_ID].second = closestDistPoseSrc ;
1019  }
1020  }
1021 
1022  } // end for idx_area_b
1023 
1024  } // end for each idx_area_a
1025 
1026  // ------------------------------------------------------------------------------------------------------
1027  // If two areas are neighbors but above the distance threshold, no link will be created between them:
1028  // Check this situation by looking for doubtful neighbors for areas without any link:
1029  // ------------------------------------------------------------------------------------------------------
1030  for (size_t idx_area=0;idx_area<myMsg.partitions.size();idx_area++)
1031  {
1032  CHMHMapNode::TNodeID area_ID = partIdx2Areas[idx_area];
1033  if (areasWithLink.find(area_ID) == areasWithLink.end())
1034  {
1035  // OK, this area does not have neighbor.. this cannot be so!
1036  if (lstClosestDoubtfulNeigbors.find(area_ID) != lstClosestDoubtfulNeigbors.end() )
1037  {
1038  // Add link to closest area:
1039  lstInternalArcsToCreate.insert( TPairNodeIDs( area_ID,lstClosestDoubtfulNeigbors[area_ID].first ) );
1040 
1041  // Now they have a link:
1042  areasWithLink.insert( area_ID );
1043  areasWithLink.insert( lstClosestDoubtfulNeigbors[area_ID].first );
1044  }
1045  else
1046  {
1047  THROW_EXCEPTION_FMT("Area %i seems unconnected??", (int)area_ID);
1048  }
1049  }
1050  }
1051 
1052  } // end if # partitions >= 2 && lock on m_map
1053 
1054 #if 1
1055  {
1056  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] lstInternalArcsToCreate contains %i entries:\n",(int)lstInternalArcsToCreate.size());
1057  for ( list_searchable<TPairNodeIDs>::const_iterator arcCreat=lstInternalArcsToCreate.begin();arcCreat!=lstInternalArcsToCreate.end();++arcCreat)
1058  {
1059  // Get the reference pose IDs:
1060  CHMHMapNode::TNodeID closestAreaID = arcCreat->first;
1061  CHMHMapNode::TNodeID newAreaID = arcCreat->second;
1062  logFmt(mrpt::utils::LVL_INFO," AREA %i <-> AREA %i\n", (int)closestAreaID, (int)newAreaID );
1063  }
1064  }
1065 #endif
1066 
1067  // -------------------------------------------------------------------------------------
1068  // Now, create or update all the internal arcs in the list "lstInternalArcsToCreate"
1069  // The relative pose between the two referencePoseId's is computed and stored
1070  // in the corresponding arc in the HMT-map:
1071  // -------------------------------------------------------------------------------------
1072  {
1073  CCriticalSectionLocker locker( &m_map_cs );
1074  THypothesisIDSet theArcHypos( LMH->m_ID );
1075 
1076  for ( list_searchable<TPairNodeIDs>::const_iterator arcCreat=lstInternalArcsToCreate.begin();arcCreat!=lstInternalArcsToCreate.end();++arcCreat)
1077  {
1078  // Get the reference pose IDs:
1079  CHMHMapNode::TNodeID area_a_ID = arcCreat->first;
1080  TPoseID area_a_poseID_src;
1081  m_map.getNodeByID(area_a_ID)->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, area_a_poseID_src, LMH->m_ID, true );
1082 
1083  CHMHMapNode::TNodeID area_b_ID = arcCreat->second;
1084  TPoseID area_b_poseID_trg;
1085  m_map.getNodeByID(area_b_ID)->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, area_b_poseID_trg, LMH->m_ID, true );
1086 
1087  // Get relative pose PDF according to this LMH:
1088  CPose3DPDFParticles relPoseParts;
1089  LMH->getRelativePose(area_a_poseID_src, area_b_poseID_trg, relPoseParts );
1090 
1091  // Pass to gaussian PDF:
1092  CPose3DPDFGaussian relPoseGauss;
1093  relPoseGauss.copyFrom(relPoseParts);
1094 
1095  relPoseGauss.cov.zeros(); // *********** DEBUG !!!!!!!!!!!
1096  relPoseGauss.cov(0,0)=relPoseGauss.cov(1,1)=square(0.04);
1097  relPoseGauss.cov(3,3)=square(DEG2RAD(1));
1098 
1099  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Creating arc %i[ref:%i] -> %i[ref:%i] = (%.03f,%.03f,%.03fdeg)\n",
1100  (int)area_a_ID,(int)area_a_poseID_src,
1101  (int)area_b_ID,(int)area_b_poseID_trg,
1102  relPoseGauss.mean.x(),relPoseGauss.mean.y(),RAD2DEG(relPoseGauss.mean.yaw())
1103  );
1104 
1105  // Did an arc already exist?
1106  // Look into existing arcs, in both directions:
1107  bool arcDeltaIsInverted;
1108  CHMHMapArcPtr newArc = m_map.findArcOfTypeBetweenNodes(
1109  area_a_ID,
1110  area_b_ID,
1111  LMH->m_ID,
1112  "RelativePose",
1113  arcDeltaIsInverted );
1114 
1115  // If not found, create it now:
1116  if (!newArc)
1117  {
1118  newArc = CHMHMapArc::Create(
1119  area_a_ID, // Source
1120  area_b_ID, // Target
1121  theArcHypos, // Hypos
1122  &m_map // The graph
1123  );
1124  newArc->m_arcType.setType("RelativePose");
1125  arcDeltaIsInverted = false;
1126  }
1127 
1128  // Add data to arc:
1129  if (!arcDeltaIsInverted)
1130  {
1131  MRPT_LOG_DEBUG_STREAM( "[LSLAM_proc_msg_AA] Updating int. arc " << area_a_ID << " -> " << area_b_ID << " : " << relPoseGauss.mean << " cov = " << relPoseGauss.cov.inMatlabFormat() << endl);
1132  newArc->m_annotations.set(ARC_ANNOTATION_DELTA, CPose3DPDFGaussianPtr(new CPose3DPDFGaussian(relPoseGauss)),LMH->m_ID );
1133  newArc->m_annotations.setElemental(ARC_ANNOTATION_DELTA_SRC_POSEID, area_a_poseID_src ,LMH->m_ID );
1134  newArc->m_annotations.setElemental(ARC_ANNOTATION_DELTA_TRG_POSEID, area_b_poseID_trg ,LMH->m_ID );
1135  }
1136  else
1137  {
1138  CPose3DPDFGaussian relPoseInv;
1139  relPoseGauss.inverse(relPoseInv);
1140 
1141  MRPT_LOG_DEBUG_STREAM( "[LSLAM_proc_msg_AA] Updating int. arc " << area_a_ID << " <- " << area_b_ID << " : " << relPoseInv.mean << " cov = " << relPoseInv.cov.inMatlabFormat() << endl);
1142  newArc->m_annotations.set(ARC_ANNOTATION_DELTA,CPose3DPDFGaussianPtr( new CPose3DPDFGaussian(relPoseInv)),LMH->m_ID );
1143 
1144  newArc->m_annotations.setElemental(ARC_ANNOTATION_DELTA_SRC_POSEID, area_b_poseID_trg,LMH->m_ID );
1145  newArc->m_annotations.setElemental(ARC_ANNOTATION_DELTA_TRG_POSEID, area_a_poseID_src,LMH->m_ID );
1146  }
1147 
1148  } // end for each arc in lstInternalArcsToCreate
1149 
1150  } // end lock m_map
1151 
1152  // ----------------------------------------------------------------
1153  // Remove arcs between areas that now do not need to have
1154  // an arcs between them: we know by seeing if there is not
1155  // an entry in 'lstAlreadyUpdated' but the arc actually exists:
1156  // ----------------------------------------------------------------
1157  {
1158  CCriticalSectionLocker locker( &m_map_cs );
1159 
1160  for (TNodeIDSet::const_iterator pNei=LMH->m_neighbors.begin();pNei!=LMH->m_neighbors.end();++pNei)
1161  {
1162  const CHMHMapNode::TNodeID nodeFromID = *pNei;
1163 
1164  // Follow all arcs of this node:
1165  CHMHMapNodePtr nodeFrom=m_map.getNodeByID(nodeFromID);
1166  ASSERT_(nodeFrom);
1167  TArcList lstArcs;
1168  nodeFrom->getArcs( lstArcs, "RelativePose", LMH->m_ID );
1169 
1170  // Look for arcs to be removed:
1171  // A) Arcs to areas within the LMH but which are not in "lstAlreadyUpdated"
1172  for (TArcList::const_iterator a=lstArcs.begin();a!=lstArcs.end();++a)
1173  {
1174  const CHMHMapNode::TNodeID nodeToID = (*a)->getNodeFrom()==nodeFromID ? (*a)->getNodeTo():(*a)->getNodeFrom();
1175 
1176  if (LMH->m_neighbors.find( nodeToID )!=LMH->m_neighbors.end())
1177  {
1178  CHMHMapArcPtr arc = *a;
1179 
1180  // Do exist a corresponding entry in "lstAlreadyUpdated"?
1181  if ( lstInternalArcsToCreate.end() == lstInternalArcsToCreate.find(TPairNodeIDs( nodeFromID,nodeToID )) &&
1182  lstInternalArcsToCreate.end() == lstInternalArcsToCreate.find(TPairNodeIDs( nodeToID, nodeFromID)) )
1183  {
1184  // it doesn't! Delete this arc:
1185  arc->m_annotations.remove(ARC_ANNOTATION_DELTA,LMH->m_ID );
1186  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Deleting annotation of arc: %lu-%lu\n", (long unsigned)nodeFromID, (long unsigned) nodeToID);
1187  // Any other ARC_ANNOTATION_DELTA? If not, delete the entire arc:
1188  if (! arc->m_annotations.getAnyHypothesis(ARC_ANNOTATION_DELTA) )
1189  {
1190  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Deleting empty arc: %lu-%lu\n", (long unsigned)nodeFromID, (long unsigned) nodeToID);
1191  arc.clear();
1192  }
1193  }
1194  }
1195  } // end for each arc in lstArcs
1196 
1197  } // end for each neighbor
1198  } // end lock m_map_cs
1199 
1200  if (0)
1201  {
1202  CCriticalSectionLocker locker( &m_map_cs );
1203  utils::CStringList s;
1204  m_map.dumpAsText(s);
1205  s.saveToFile( format("%s/HMAP_txt/HMAP_%05i_mid.txt",m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP ) );
1206  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Saved HMAP_%05i_mid.txt\n",DEBUG_STEP);
1207  }
1208 
1209  // -----------------------------------------------------------------------------
1210  // Remove areas from LMH if they are at a topological distance of 2 or more
1211  // We can quickly check this by identifying areas without a direct arc between
1212  // them and the current area.
1213  // -----------------------------------------------------------------------------
1214 // const CHMHMapNode::TNodeID curAreaID = LMH->m_nodeIDmemberships[ LMH->m_currentRobotPose ];
1215 
1216  for (TNodeIDSet::iterator pNei1=LMH->m_neighbors.begin();pNei1!=LMH->m_neighbors.end(); )
1217  {
1218  if (*pNei1 != curAreaID )
1219  {
1220  TArcList lstArcs;
1221  {
1222  CCriticalSectionLocker locker( &m_map_cs );
1223  m_map.findArcsOfTypeBetweenNodes( *pNei1, curAreaID , LMH->m_ID, "RelativePose", lstArcs);
1224  }
1225  if (lstArcs.empty())
1226  {
1227  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Getting area '%u' out of LMH\n", static_cast<unsigned>(*pNei1) );
1228 
1229  // Remove from list first:
1230  CHMHMapNode::TNodeID id = *pNei1;
1231 
1232  pNei1 = erase_return_next(LMH->m_neighbors,pNei1);
1233 
1234  // Now: this calls internally to "updateAreaFromLMH"
1235  double ESS_bef = LMH->ESS();
1236  LMH->removeAreaFromLMH( id );
1237  double ESS_aft = LMH->ESS();
1238  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] ESS: %f -> %f\n", ESS_bef, ESS_aft );
1239  }
1240  else pNei1++; // Go next:
1241  }
1242  else pNei1++; // Go next:
1243  } // end for pNei1
1244 
1245 
1246 
1247  // This list contains those areas just inserted into the LMH, so their poses have been added
1248  // to the particles, etc... but not their observations into the metric maps: this is delayed
1249  // since in the case we would need to change coordinate origin, it would had been pointless.
1250  TNodeIDSet areasDelayedMetricMapsInsertion;
1251 
1252  // -------------------------------------------------------------
1253  // Recompose LMH by bringing in all areas with an arc to the
1254  // current area:
1255  // -------------------------------------------------------------
1256  CHMHMapNodePtr currentArea;
1257  {
1258  CCriticalSectionLocker locker( &m_map_cs );
1259 
1260  const CHMHMapNode::TNodeID curAreaID = LMH->m_nodeIDmemberships[ LMH->m_currentRobotPose ];
1261  currentArea = m_map.getNodeByID( curAreaID );
1262 
1263  TPoseID refPoseCurArea_accordingAnnot;
1264  currentArea->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, refPoseCurArea_accordingAnnot, LMH->m_ID, true );
1265 
1266  TArcList arcsToCurArea;
1267  currentArea->getArcs(arcsToCurArea,"RelativePose",LMH->m_ID);
1268  for (TArcList::iterator a=arcsToCurArea.begin();a!=arcsToCurArea.end();++a)
1269  {
1270  const CHMHMapArcPtr arc = (*a);
1271  const CHMHMapNode::TNodeID otherAreaID = arc->getNodeFrom()==curAreaID ? arc->getNodeTo():arc->getNodeFrom();
1272 
1273  // If otherArea is out of the LMH, we must bring it in!
1274  if ( LMH->m_neighbors.find(otherAreaID)==LMH->m_neighbors.end() )
1275  {
1276  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Bringing in LMH area %i\n",(int)otherAreaID );
1277 
1278  CHMHMapNodePtr area = m_map.getNodeByID( otherAreaID );
1279  ASSERT_(area);
1280 
1281  CRobotPosesGraphPtr pg = area->m_annotations.getAs<CRobotPosesGraph>( NODE_ANNOTATION_POSES_GRAPH, LMH->m_ID, false );
1282 
1283  // Find the coordinate transformation between areas "currentArea"->"area" = Delta_c2a
1284  CPose3D Delta_c2a; // We are just interested in the mean
1285  {
1286  CPose3DPDFGaussianPtr pdf = arc->m_annotations.getAs<CPose3DPDFGaussian>( ARC_ANNOTATION_DELTA, LMH->m_ID, false );
1287 
1288  pdf->getMean( Delta_c2a );
1289  }
1290 
1291  TPoseID refPoseIDAtOtherArea, refPoseIDAtCurArea;
1292 
1293  if (arc->getNodeTo()==curAreaID)
1294  {
1295  // It is inverted:
1296  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Arc is inverted: (%.03f,%.03f,%.03fdeg) -> ",
1297  Delta_c2a.x(),Delta_c2a.y(),RAD2DEG(Delta_c2a.yaw()) );
1298 
1299  Delta_c2a = CPose3D(0,0,0) - Delta_c2a;
1300 
1301  logFmt(mrpt::utils::LVL_INFO,"(%.03f,%.03f,%.03fdeg)\n",
1302  Delta_c2a.x(),Delta_c2a.y(),RAD2DEG(Delta_c2a.yaw()) );
1303 
1304  arc->m_annotations.getElemental( ARC_ANNOTATION_DELTA_SRC_POSEID, refPoseIDAtOtherArea, LMH->m_ID, true );
1305  arc->m_annotations.getElemental( ARC_ANNOTATION_DELTA_TRG_POSEID, refPoseIDAtCurArea, LMH->m_ID, true );
1306  }
1307  else
1308  {
1309  // It is NOT inverted.
1310  arc->m_annotations.getElemental( ARC_ANNOTATION_DELTA_TRG_POSEID, refPoseIDAtOtherArea, LMH->m_ID, true );
1311  arc->m_annotations.getElemental( ARC_ANNOTATION_DELTA_SRC_POSEID, refPoseIDAtCurArea, LMH->m_ID, true );
1312  }
1313 
1314  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Bringing in: refPoseCur=%i refPoseOther=%i -> Delta_c2a:(%.03f,%.03f,%.03fdeg)\n",
1315  (int)refPoseIDAtCurArea, (int)refPoseIDAtOtherArea,
1316  Delta_c2a.x(),Delta_c2a.y(),RAD2DEG(Delta_c2a.yaw()) );
1317 
1318  // Assure the arc's references are OK:
1319 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
1320  {
1321  TPoseID refPoseOtherArea_accordingAnnot;
1322  area->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, refPoseOtherArea_accordingAnnot, LMH->m_ID, true );
1323  ASSERT_( refPoseIDAtOtherArea == refPoseOtherArea_accordingAnnot );
1324 
1325  ASSERT_( refPoseIDAtCurArea == refPoseCurArea_accordingAnnot );
1326  }
1327 #endif
1328  // Given the above checks: the new particles' poses are simply:
1329  // POSE_i' = refPoseCurrentArea (+) Delta_cur_area (+) POSE_i
1330 
1331  // Create new poses within the particles:
1332  // --------------------------------------------
1333  TPoseIDList lstNewPoseIDs;
1334  lstNewPoseIDs.reserve( pg->size() );
1335  for (CRobotPosesGraph::iterator p=pg->begin();p!=pg->end();++p)
1336  {
1337  const TPoseID &poseID = p->first;
1338  const TPoseInfo &poseInfo = p->second;
1339 
1340  lstNewPoseIDs.push_back(poseID);
1341 
1342  // Add the particles:
1343  ASSERT_( poseInfo.pdf.m_particles.size() == LMH->m_particles.size() );
1344 
1345  CPose3DPDFParticles::CParticleList::const_iterator itSrc;
1346  CLocalMetricHypothesis::CParticleList::iterator itTrg;
1347 
1348  for (itSrc=poseInfo.pdf.m_particles.begin(), itTrg=LMH->m_particles.begin(); itTrg!=LMH->m_particles.end(); itSrc++,itTrg++)
1349  {
1350  // log_w: not modified since diff. areas are independent...
1351  itTrg->d->robotPoses[ poseID ] = itTrg->d->robotPoses[ refPoseIDAtCurArea ] + Delta_c2a + *itSrc->d;
1352  }
1353 
1354  // Update m_nodeIDmemberships
1355  LMH->m_nodeIDmemberships[ poseID ] = otherAreaID;
1356 
1357  // Update m_SFs
1358  LMH->m_SFs[ poseID ] = poseInfo.sf;
1359 
1360  // Add area to neighbors:
1361  LMH->m_neighbors.insert( otherAreaID );
1362 
1363  } // for each pose in the new area (Crobotposesgraph)
1364 
1365  // Update m_robotPosesGraph: This will be done in the next iteration of the LSLAM thread,
1366  // now just add to the list of pending pose IDs:
1367  LMH->m_posesPendingAddPartitioner.insert( LMH->m_posesPendingAddPartitioner.end(),lstNewPoseIDs.begin(),lstNewPoseIDs.end() );
1368 
1369  // Mark this new area as to pending for updating the metric map at the end of this method:
1370  areasDelayedMetricMapsInsertion.insert( otherAreaID );
1371 
1372  } // end if the area is out of LMH
1373  } // end for each arc
1374  } // end of lock on m_map_cs
1375 
1376 
1377  if (0)
1378  {
1379  utils::CStringList s;
1380  LMH->dumpAsText(s);
1381  s.saveToFile( format("%s/HMAP_txt/HMAP_%05i_LMH_mid.txt",m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP ) );
1382  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Saved HMAP_%05i_LMH_mid.txt\n",DEBUG_STEP);
1383  }
1384  if (0)
1385  {
1386  COpenGLScene sceneLSLAM;
1387  // Generate the metric maps 3D view...
1388  opengl::CSetOfObjectsPtr maps3D = opengl::CSetOfObjects::Create();
1389  maps3D->setName( "metric-maps" );
1390  LMH->getMostLikelyParticle()->d->metricMaps.getAs3DObject( maps3D );
1391  sceneLSLAM.insert( maps3D );
1392 
1393  // ...and the robot poses, areas, etc:
1394  opengl::CSetOfObjectsPtr LSLAM_3D = opengl::CSetOfObjects::Create();
1395  LSLAM_3D->setName("LSLAM_3D");
1396  LMH->getAs3DScene( LSLAM_3D );
1397  sceneLSLAM.insert( LSLAM_3D );
1398 
1399  sceneLSLAM.enableFollowCamera(true);
1400 
1401  string filLocalAreas = format("%s/HMAP_txt/HMAP_%05i_LMH_mid.3Dscene",m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP );
1402  logFmt(mrpt::utils::LVL_INFO,"[LOG] Saving %s\n", filLocalAreas.c_str());
1403  CFileOutputStream(filLocalAreas) << sceneLSLAM;
1404  }
1405 
1406  // -------------------------------------------------------------
1407  // Change local coordinate system, as required
1408  // This regenerates the metric maps as well.
1409  // -------------------------------------------------------------
1410  TPoseID new_poseID_origin;
1411 
1412  if (! currentArea->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID,new_poseID_origin, LMH->m_ID ) )
1413  THROW_EXCEPTION("New coordinate origin not found");
1414 
1415  if ( new_poseID_origin != poseID_origin )
1416  { // Change coords AND rebuild metric maps
1417  CTicTac tictac;
1418  tictac.Tic();
1419  LMH->changeCoordinateOrigin( new_poseID_origin );
1420  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] LMH->changeCoordinateOrigin %lu->%lu took %f ms\n", poseID_origin,new_poseID_origin,tictac.Tac()*1000 );
1421  }
1422  else if (areasDelayedMetricMapsInsertion.size())
1423  {
1424  CTicTac tictac;
1425  tictac.Tic();
1426  // We haven't rebuilt the whole metric maps, so just insert the new observations as needed:
1427  for (TNodeIDSet::iterator areaID=areasDelayedMetricMapsInsertion.begin();areaID!=areasDelayedMetricMapsInsertion.end();++areaID)
1428  {
1429  // For each posesID within this areaID:
1430  for(map<TPoseID,CHMHMapNode::TNodeID>::const_iterator pn=LMH->m_nodeIDmemberships.begin();pn!=LMH->m_nodeIDmemberships.end();++pn)
1431  {
1432  if (pn->second==*areaID)
1433  {
1434  // We must add this poseID:
1435  const TPoseID & poseToAdd = pn->first;
1436  const CSensoryFrame &SF = LMH->m_SFs.find(poseToAdd)->second;
1437 
1438  // Process the poses in the list for each particle:
1439  for (CLocalMetricHypothesis::CParticleList::iterator partIt=LMH->m_particles.begin();partIt!=LMH->m_particles.end();++partIt)
1440  {
1441  TMapPoseID2Pose3D::const_iterator pose3D = partIt->d->robotPoses.find( poseToAdd );
1442  ASSERT_(pose3D!=partIt->d->robotPoses.end());
1443  SF.insertObservationsInto( &partIt->d->metricMaps, & pose3D->second );
1444  } // end for each particle
1445  }
1446  } // end for each m_nodeIDmemberships
1447  } // end for each areasDelayedMetricMapsInsertion
1448 
1449  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] areasDelayedMetricMapsInsertion took %f ms\n", tictac.Tac()*1000 );
1450  }
1451 
1452  if (0)
1453  {
1454  CCriticalSectionLocker locker( &m_map_cs );
1455  utils::CStringList s;
1456  m_map.dumpAsText(s);
1457  s.saveToFile( format("%s/HMAP_txt/HMAP_%05i_after.txt",m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP ) );
1458  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Saved HMAP_%05i_after.txt\n",DEBUG_STEP);
1459  }
1460  if (0)
1461  {
1462  utils::CStringList s;
1463  LMH->dumpAsText(s);
1464  s.saveToFile( format("%s/HMAP_txt/HMAP_%05i_LMH_after.txt",m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP ) );
1465  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Saved HMAP_%05i_LMH_after.txt\n",DEBUG_STEP);
1466  }
1467 
1468  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_AA] Msg from AA took %f ms ]\n", tictac.Tac()*1000 );
1469 
1470  MRPT_END
1471 }
1472 
1473 
1474 /*---------------------------------------------------------------
1475  LSLAM_process_message_from_TBI
1476  ---------------------------------------------------------------*/
1477 void CHMTSLAM::LSLAM_process_message_from_TBI( const TMessageLSLAMfromTBI &myMsg )
1478 {
1479  MRPT_START
1480 
1481  CTicTac tictac;
1482  tictac.Tic();
1483  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_TBI] Beginning of Msg from TBI processing... [\n" );
1484 
1485  // In case of multiple areas involved in a TLC, we need a mapping from the old areaIDs to the new ones:
1486  std::map<CHMHMapNode::TNodeID,CHMHMapNode::TNodeID> alreadyClosedLoops;
1487 
1488  for (map< CHMHMapNode::TNodeID, TMessageLSLAMfromTBI::TBI_info >::const_iterator candidate = myMsg.loopClosureData.begin();candidate != myMsg.loopClosureData.end();++candidate)
1489  {
1490  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_TBI] Processing TLC of areas: %u <-> %u... \n",(unsigned)myMsg.cur_area, (unsigned)candidate->first );
1491 
1492  // Check if the area has already been merged:
1493  CHMHMapNode::TNodeID currentArea = myMsg.cur_area;
1494  if (alreadyClosedLoops.find(myMsg.cur_area)!=alreadyClosedLoops.end())
1495  {
1496  currentArea = alreadyClosedLoops[myMsg.cur_area];
1497  cout << "[LSLAM_proc_msg_TBI] Using " << myMsg.cur_area << " -> " << currentArea << " due to area being already merged." << endl;
1498  }
1499 
1500  // Get pose PDF according to HMAP
1501  // -----------------------------------------
1502  CPose3DPDFParticles pdfPartsHMap;
1503  m_map.computeCoordinatesTransformationBetweenNodes(
1504  currentArea, // Pose of "candidate->first" as seen from "currentArea" (The order is critical!!)
1505  candidate->first,
1506  pdfPartsHMap,
1507  myMsg.hypothesisID,
1508  100,
1509  0.10f,DEG2RAD(1.0f) // Extra noise in each "arc"
1510  );
1511 
1512  CPose3DPDFGaussian pdfDeltaMap;
1513  pdfDeltaMap.copyFrom(pdfPartsHMap);
1514 
1515  // Increase the uncertainty to avoid too understimated covariances and make the chi-test fail:
1516  pdfDeltaMap.cov(0,0) += square(1.0);
1517  pdfDeltaMap.cov(1,1) += square(1.0);
1518  pdfDeltaMap.cov(2,2) += square(1.0);
1519  pdfDeltaMap.cov(3,3) += square(DEG2RAD(5));
1520  pdfDeltaMap.cov(4,4) += square(DEG2RAD(5));
1521  pdfDeltaMap.cov(5,5) += square(DEG2RAD(5));
1522 
1523  cout << "[LSLAM_proc_msg_TBI] HMap_delta=" << pdfDeltaMap.mean << " std_x=" << sqrt(pdfDeltaMap.cov(0,0)) <<" std_y=" << sqrt(pdfDeltaMap.cov(1,1)) << endl;
1524 
1525  // Get pose PDF according to TLC detector:
1526  // It's a SOG, so we should make an ordered list with each Gaussian mode
1527  // and its probability/compatibility according to the metric information:
1528  // -------------------------------------------------------------------------
1529  ASSERT_(!candidate->second.delta_new_cur.empty());
1530  const double chi2_thres = mrpt::math::chi2inv(0.999, CPose3DPDFGaussian::state_length );
1531 
1532  map<double,CPose3DPDFGaussian> lstModesAndCompats; // first=log(e^-0.5*maha_dist)+log(likelihood); The list only contains those chi2 compatible
1533 
1534  for (CPose3DPDFSOG::const_iterator itSOG=candidate->second.delta_new_cur.begin();itSOG!=candidate->second.delta_new_cur.end();++itSOG)
1535  {
1536  const CPose3DPDFGaussian &pdfDelta = itSOG->val;
1537 
1538  cout << "[LSLAM_proc_msg_TBI] TLC_delta=" << pdfDelta.mean << " std_x=" << sqrt(pdfDelta.cov(0,0)) <<" std_y=" << sqrt(pdfDelta.cov(1,1)) <<" std_phi=" << RAD2DEG(sqrt(pdfDelta.cov(3,3))) << endl;
1539 
1540  // Perform chi2 test (with Mahalanobis distance):
1541  // ------------------------------------------------
1542  const double mahaDist2 = square( pdfDeltaMap.mahalanobisDistanceTo(pdfDelta) );
1543  cout << "[LSLAM_proc_msg_TBI] maha_dist = " << mahaDist2 << endl;
1544 
1545  if (mahaDist2<chi2_thres)
1546  {
1547  const double log_lik = itSOG->log_w - 0.5*mahaDist2;
1548  lstModesAndCompats[ log_lik ] = itSOG->val;
1549  cout << "[LSLAM_proc_msg_TBI] Added to list of candidates: log(overall_lik)= " << log_lik << endl;
1550  }
1551  } // for each SOG mode
1552 
1553  // Any good TLC candidate?
1554  if (!lstModesAndCompats.empty())
1555  {
1556  const CPose3DPDFGaussian &pdfDelta = lstModesAndCompats.rbegin()->second;
1557 
1558  mrpt::system::pause();
1559 
1560  // --------------------------------------------------------
1561  // Two options here:
1562  // 1) Create a new LMH for each acceptable possibility
1563  // 2) Just keep the most likely one (***** CHOICE, FOR NOW!!! *****)
1564  // --------------------------------------------------------
1565  static CTicTac tictac;
1566  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_TBI] Accepting TLC of areas: %u <-> %u with an overall log(lik)=%f \n",(unsigned)currentArea, (unsigned)candidate->first,lstModesAndCompats.rbegin()->first );
1567 
1568  tictac.Tic();
1569  this->perform_TLC(
1570  m_LMHs[myMsg.hypothesisID],
1571  currentArea, // Area in the LMH
1572  candidate->first, // External area
1573  pdfDelta );
1574  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_TBI] TLC of areas %u <-> %u - DONE in %.03f ms\n",(unsigned)currentArea, (unsigned)candidate->first,1e3*tictac.Tac() );
1575 
1576  // The old area "myMsg.cur_area" is now "candidate->first"
1577  alreadyClosedLoops[myMsg.cur_area] = candidate->first;
1578 
1579  } // end there is any good TLC candidate
1580 
1581 
1582  } // end for each candidate
1583 
1584  logFmt(mrpt::utils::LVL_INFO,"[LSLAM_proc_msg_TBI] Msg from TBI took %f ms ]\n", tictac.Tac()*1000 );
1585 
1586  MRPT_END
1587 }
mrpt::random
A namespace of pseudo-random numbers genrators of diferent distributions.
Definition: RandomGenerators.h:22
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
mrpt::utils::TPairNodeIDs
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
Definition: types_simple.h:46
mrpt::synch::CCriticalSectionLocker
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
Definition: CCriticalSection.h:90
mrpt::utils::list_searchable
This class implements a STL container with features of both, a std::set and a std::list.
Definition: list_searchable.h:27
CFileOutputStream.h
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:73
CRobotPosesGraph.h
mrpt::system::getCurrentThreadId
unsigned long BASE_IMPEXP getCurrentThreadId() MRPT_NO_THROWS
Returns the ID of the current thread.
Definition: threads.cpp:211
mrpt::poses::CPoseOrPoint::distanceTo
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:150
mrpt::poses::CPose3DPDFGaussian::copyFrom
void copyFrom(const CPose3DPDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
Definition: CPose3DPDFGaussian.cpp:281
mrpt::poses::CPose3DPDFGaussian::mahalanobisDistanceTo
double mahalanobisDistanceTo(const CPose3DPDFGaussian &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
Definition: CPose3DPDFGaussian.cpp:602
mrpt::hmtslam
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
Definition: CHierarchicalMapMHPartition.h:27
first
GLint * first
Definition: glext.h:3703
mrpt::system::pause
void BASE_IMPEXP pause(const std::string &msg=std::string("Press any key to continue...")) MRPT_NO_THROWS
Shows the message "Press any key to continue" (or other custom message) to the current standard outpu...
Definition: os.cpp:435
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: maps/CColouredPointsMap.h:154
mrpt::random::randomGenerator
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:23
mrpt::random::CRandomGenerator::randomize
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
Definition: RandomGenerator.cpp:105
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: maps/CColouredPointsMap.h:163
mrpt::hmtslam::TPoseInfo::pdf
mrpt::poses::CPose3DPDFParticles pdf
The robot pose PDF.
Definition: CRobotPosesGraph.h:29
iterator
Scalar * iterator
Definition: eigen_plugins.h:23
begin
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromAA::hypothesisID
THypothesisID hypothesisID
The hypothesis ID (LMH) for these results.
Definition: CHMTSLAM.h:92
std
STL namespace.
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
mrpt::hmtslam::TPoseID
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
Definition: HMT_SLAM_common.h:65
AREAID_INVALID
#define AREAID_INVALID
Definition: HMT_SLAM_common.h:42
s
GLdouble s
Definition: glext.h:3602
mrpt::utils::CTicTac::Tic
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
obj
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
mrpt::aligned_containers
Helper types for STL containers with Eigen memory allocators.
Definition: aligned_containers.h:25
mrpt::hmtslam::CHMTSLAM
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:59
mrpt::obs::CActionCollection
Declares a class for storing a collection of robot actions.
Definition: obs/CActionCollection.h:31
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
mrpt::utils::erase_return_next
std::list< T >::iterator erase_return_next(std::list< T > &cont, typename std::list< T >::iterator &it)
Calls the standard "erase" method of a STL container, but also returns an iterator to the next elemen...
Definition: stl_containers_utils.h:37
mrpt::poses::CPose3D::distanceEuclidean6D
double distanceEuclidean6D(const CPose3D &o) const
The euclidean distance between two poses taken as two 6-length vectors (angles in radians)...
Definition: CPose3D.cpp:410
mrpt::system::getCurrentThreadTimes
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
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::cur_area
CHMHMapNode::TNodeID cur_area
The area for who the loop-closure has been computed.
Definition: CHMTSLAM.h:120
mrpt::utils::CStringList
A class for storing a list of text lines.
Definition: CStringList.h:32
mrpt::bayes::CParticleFilterData::m_particles
CParticleList m_particles
The array of particles.
Definition: CParticleFilterData.h:177
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::opengl::COpenGLScene::insert
void insert(const CRenderizablePtr &newObject, const std::string &viewportName=std::string("main"))
Insert a new object into the scene, in the given viewport (by default, into the "main" viewport)...
Definition: COpenGLScene.cpp:195
dst
GLuint dst
Definition: glext.h:6198
mrpt::hmtslam::THypothesisIDSet
A set of hypothesis IDs, used for arcs and nodes in multi-hypothesis hybrid maps. ...
Definition: HMT_SLAM_common.h:78
MRPT_END
#define MRPT_END
Definition: maps/CColouredPointsMap.h:370
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: maps/CColouredPointsMap.h:307
mrpt::utils::CFileOutputStream
This CStream derived class allow using a file as a write-only, binary stream.
Definition: CFileOutputStream.h:25
POSEID_INVALID
#define POSEID_INVALID
Definition: HMT_SLAM_common.h:40
mrpt::system::sleep
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
mrpt::opengl::COpenGLScene::enableFollowCamera
void enableFollowCamera(bool enabled)
If disabled (default), the SceneViewer application will ignore the camera of the "main" viewport and ...
Definition: COpenGLScene.h:113
mrpt::slam::CMultiMetricMapPtr
mrpt::maps::CMultiMetricMapPtr CMultiMetricMapPtr
Backward compatible typedef.
Definition: slam/CMultiMetricMap.h:14
mrpt::poses::CPose3DPDFSOG::const_iterator
TModesList::const_iterator const_iterator
Definition: CPose3DPDFSOG.h:57
mrpt::utils::CTicTac
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:18
mrpt::obs::CSensoryFrame::insert
void insert(const CObservationPtr &obs)
Inserts a new observation to the list: The pointer to the objects is copied, thus DO NOT delete the p...
Definition: CSensoryFrame.cpp:186
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: obs/CSensoryFrame.h:52
mrpt::format
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
stl_containers_utils.h
mrpt::hmtslam::CRobotPosesGraph
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class stores a set of r...
Definition: CRobotPosesGraph.h:37
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::hypothesisID
THypothesisID hypothesisID
The hypothesis ID (LMH) for these results.
Definition: CHMTSLAM.h:118
mrpt::synch
This namespace provides multitask, synchronization utilities.
Definition: atomic_incr.h:29
DEG2RAD
#define DEG2RAD
Definition: maps/CColouredPointsMap.h:99
mrpt::hmtslam::CLocalMetricHypothesis
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
Definition: CLocalMetricHypothesis.h:64
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromAA::partitions
std::vector< TPoseIDList > partitions
Definition: CHMTSLAM.h:93
NODE_ANNOTATION_METRIC_MAPS
#define NODE_ANNOTATION_METRIC_MAPS
Definition: HMT_SLAM_common.h:21
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
CPose3DPDFParticles.h
mrpt::hmtslam::TPoseIDList
std::vector< TPoseID > TPoseIDList
Definition: HMT_SLAM_common.h:69
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::loopClosureData
std::map< CHMHMapNode::TNodeID, TBI_info > loopClosureData
The meat is here: only feasible loop closures from "cur_area" are included here, with associated data...
Definition: CHMTSLAM.h:137
mrpt::poses::CPose3DPDFGaussian::getMean
void getMean(CPose3D &mean_pose) const MRPT_OVERRIDE
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
Definition: CPose3DPDFGaussian.h:85
mrpt::hmtslam::TMapPoseID2Pose3D
mrpt::aligned_containers< TPoseID, mrpt::poses::CPose3D >::map_t TMapPoseID2Pose3D
Definition: CLocalMetricHypothesis.h:31
CTicTac.h
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:77
CLASS_ID
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:92
MRPT_START
#define MRPT_START
Definition: maps/CColouredPointsMap.h:366
RAD2DEG
#define RAD2DEG
Definition: maps/CColouredPointsMap.h:100
v
const GLdouble * v
Definition: glext.h:3603
mrpt::slam::CMultiMetricMap
mrpt::maps::CMultiMetricMap CMultiMetricMap
Backward compatible typedef.
Definition: slam/CMultiMetricMap.h:13
mrpt::math::chi2inv
double BASE_IMPEXP chi2inv(double P, unsigned int dim=1)
The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1 (the inverse...
Definition: math.cpp:967
ARC_ANNOTATION_DELTA_SRC_POSEID
#define ARC_ANNOTATION_DELTA_SRC_POSEID
Definition: HMT_SLAM_common.h:28
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
mrpt::hmtslam::TPoseInfo::sf
mrpt::obs::CSensoryFrame sf
The observations.
Definition: CRobotPosesGraph.h:28
mrpt::hmtslam::TNodeIDSet
std::set< CHMHMapNode::TNodeID > TNodeIDSet
Definition: CHMHMapNode.h:150
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CAngularObservationMesh.h:23
mrpt::opengl::COpenGLScene
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:49
mrpt::utils::CTicTac::Tac
double Tac()
Stops the stopwatch.
Definition: CTicTac.cpp:92
ASSERT_
#define ASSERT_(f)
Definition: maps/CColouredPointsMap.h:278
NODE_ANNOTATION_REF_POSEID
#define NODE_ANNOTATION_REF_POSEID
Definition: HMT_SLAM_common.h:22
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Definition: CPose3DPDFGaussian.h:37
mrpt::hmtslam::TPoseInfo
Information kept for each robot pose used in CRobotPosesGraph.
Definition: CRobotPosesGraph.h:26
size
GLsizeiptr size
Definition: glext.h:3779
mrpt::math::CMatrix
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:30
NODE_ANNOTATION_POSES_GRAPH
#define NODE_ANNOTATION_POSES_GRAPH
Definition: HMT_SLAM_common.h:23
mrpt::utils::list_searchable::find
std::list< T >::iterator find(const T &i)
Definition: list_searchable.h:32
MRPT_LOG_DEBUG_STREAM
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
ARC_ANNOTATION_DELTA_TRG_POSEID
#define ARC_ANNOTATION_DELTA_TRG_POSEID
Definition: HMT_SLAM_common.h:29
a
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
p
GLfloat GLfloat p
Definition: glext.h:5587
mrpt::hmtslam::TArcList
A class for storing a sequence of arcs (a path).
Definition: HMT_SLAM_common.h:119
mrpt::poses::CPose3DPDFGaussian::inverse
void inverse(CPose3DPDF &o) const MRPT_OVERRIDE
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
Definition: CPose3DPDFGaussian.cpp:450
ARC_ANNOTATION_DELTA
#define ARC_ANNOTATION_DELTA
Definition: HMT_SLAM_common.h:27
mrpt::utils::CMessage
A class that contain generic messages, that can be sent and received from a "CClientTCPSocket" object...
Definition: CMessage.h:29
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromAAPtr
stlplus::smart_ptr< TMessageLSLAMfromAA > TMessageLSLAMfromAAPtr
Definition: CHMTSLAM.h:97
mrpt::obs::CSensoryFrame::insertObservationsInto
bool insertObservationsInto(mrpt::maps::CMetricMap *theMap, const mrpt::poses::CPose3D *robotPose=NULL) const
Insert all the observations in this SF into a metric map or any kind (see mrpt::maps::CMetricMap).
Definition: CSensoryFrame.cpp:317
mrpt::poses::CPose3DPDFParticles
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
Definition: CPose3DPDFParticles.h:33
mrpt::hmtslam::CHMHMapNode::TNodeID
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:49
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBIPtr
stlplus::smart_ptr< TMessageLSLAMfromTBI > TMessageLSLAMfromTBIPtr
Definition: CHMTSLAM.h:141
mrpt::opengl::CSetOfObjects::Create
static CSetOfObjectsPtr Create()
mrpt::utils::list_searchable::insert
void insert(const T &o)
Definition: list_searchable.h:30


Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019