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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019