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


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST