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