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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3a26b90fd Wed Mar 25 20:17:03 2020 +0100 at mié mar 25 23:05:41 CET 2020