46 void CHMTSLAM::thread_LSLAM()
50 unsigned int nIter = 0;
53 if (
obj->m_options.random_seed)
64 "[thread_LSLAM] Thread started (ID=0x%08lX)\n",
65 std::this_thread::get_id());
71 while (!
obj->m_terminateThreads)
73 if (
obj->m_options.random_seed)
81 recMsg =
obj->m_LSLAM_queue.get();
84 obj->LSLAM_process_message(*recMsg);
91 if (!
obj->isInputQueueEmpty())
93 if (
obj->m_options.random_seed)
98 obj->getNextObjectFromInputQueue();
105 if (nextObject->GetRuntimeClass() ==
107 actions = std::dynamic_pointer_cast<CActionCollection>(
112 std::dynamic_pointer_cast<CSensoryFrame>(nextObject);
115 "Element in the queue is neither CActionCollection nor " 124 std::lock_guard<std::mutex> LMHs_cs_lock(
obj->m_LMHs_cs);
126 for (
auto it =
obj->m_LMHs.begin(); it !=
obj->m_LMHs.end();
129 std::lock_guard<std::mutex> LMH_individual_lock(
130 it->second.threadLocks.m_lock);
135 obj->m_LSLAM_method->processOneLMH(
137 actions, observations);
142 if (it->second.m_posesPendingAddPartitioner.size() >
149 unsigned nPosesToInsert =
150 it->second.m_posesPendingAddPartitioner.size();
152 CHMTSLAM::areaAbstraction(
154 it->second.m_posesPendingAddPartitioner);
158 "[AreaAbstraction] Took %.03fms to insert %u " 160 1000 * tictac2.
Tac(), nPosesToInsert);
163 it->second.m_posesPendingAddPartitioner.clear();
165 if (
obj->m_options.random_seed)
167 obj->m_options.random_seed);
174 obj->LSLAM_process_message_from_AA(*msgFromAA);
180 if (!it->second.m_areasPendingTBI.empty())
183 it->second.m_areasPendingTBI.begin();
184 areaID != it->second.m_areasPendingTBI.end();
189 if (
obj->m_options.random_seed)
191 obj->m_options.random_seed);
194 CHMTSLAM::TBI_main_method(
195 &it->second, *areaID);
199 "[TBI] Took %.03fms " 201 1000 * tictac2.
Tac());
207 obj->LSLAM_process_message_from_TBI(
212 it->second.m_areasPendingTBI.clear();
226 if (
obj->m_options.LOG_OUTPUT_DIR.size() &&
227 (nIter %
obj->m_options.LOG_FREQUENCY) == 0)
228 obj->generateLogFiles(nIter);
236 std::this_thread::sleep_for(5ms);
246 obj->m_terminationFlag_LSLAM =
true;
248 catch (std::exception& e)
250 obj->m_terminationFlag_LSLAM =
true;
257 obj->m_terminateThreads =
true;
261 obj->m_terminationFlag_LSLAM =
true;
265 "\n---------------------- EXCEPTION CAUGHT! " 266 "---------------------\n");
269 " In CHierarchicalMappingFramework::thread_LSLAM. Unexpected " 270 "runtime error!!\n");
275 obj->m_terminateThreads =
true;
282 void CHMTSLAM::LSLAM_process_message(
const CMessage& msg)
320 "[LSLAM_proc_msg_AA] Beginning of Msg from AA processing... " 325 ASSERT_(itLMH != m_LMHs.end());
334 itPose != it->end(); ++itPose)
335 if (LMH->m_SFs.find(*itPose) == LMH->m_SFs.end())
337 "PoseID %i in AA's partition but not in LMH.\n",
342 LMH->m_nodeIDmemberships.begin();
343 itA != LMH->m_nodeIDmemberships.end(); ++itA)
345 if (LMH->m_currentRobotPose != itA->first)
354 itPose != it->end(); ++itPose)
355 if (itA->first == *itPose)
362 "LMH's pose %i not found in AA's partitions.",
369 TNodeIDSet neighbors_before(LMH->m_neighbors);
374 std::lock_guard<std::mutex> lock(m_map_cs);
377 LMH->m_nodeIDmemberships.find(LMH->m_currentRobotPose);
378 ASSERT_(itCur != LMH->m_nodeIDmemberships.end());
380 if (!m_map.getNodeByID(itCur->second)
381 ->m_annotations.getElemental(
394 map<unsigned int, map<CHMHMapNode::TNodeID, unsigned int>> votes;
397 static int DEBUG_STEP = 0;
405 DEBUG_STEP = DEBUG_STEP + 0;
409 std::lock_guard<std::mutex> lock(m_map_cs);
410 std::vector<std::string>
s;
415 "%s/HMAP_txt/HMAP_%05i_before.txt",
416 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP));
420 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_before.txt\n", DEBUG_STEP);
427 itPose != it->end(); ++itPose)
430 LMH->m_nodeIDmemberships.find(*itPose);
431 ASSERT_(itP != LMH->m_nodeIDmemberships.end());
433 votes[i][itP->second]++;
437 vector<CHMHMapNode::TNodeID> partIdx2Areas(
440 map<CHMHMapNode::TNodeID, pair<size_t, unsigned int>>
446 for (
size_t k = 0; k < myMsg.
partitions.size(); k++)
448 if (votes[k].
size() == 1)
451 if (votes[k].
begin()->second >
452 mostVotedFrom[votes[k].begin()->first].second)
454 mostVotedFrom[votes[k].begin()->first].first = k;
455 mostVotedFrom[votes[k].begin()->first].second =
456 votes[k].begin()->second;
465 mostVotedFrom.begin();
466 v != mostVotedFrom.end(); ++
v)
467 v->second.second = std::numeric_limits<unsigned int>::max();
471 for (
size_t k = 0; k < myMsg.
partitions.size(); k++)
475 it != votes[k].end(); ++it)
480 map<CHMHMapNode::TNodeID, pair<size_t, unsigned int>>
::iterator 482 mostVotesIt = mostVotedFrom.find(it->first);
483 if (mostVotesIt == mostVotedFrom.end())
486 mostVotedFrom[it->first].first = k;
487 mostVotedFrom[it->first].second = it->second;
492 if (it->second > mostVotesIt->second.second)
494 mostVotesIt->second.first = k;
495 mostVotesIt->second.second = it->second;
503 mostVotedFrom.begin();
504 it != mostVotedFrom.end(); ++it)
505 partIdx2Areas[it->second.first] = it->first;
508 for (i = 0; i < partIdx2Areas.size(); i++)
513 std::lock_guard<std::mutex> lock(m_map_cs);
516 mrpt::make_aligned_shared<CHMHMapNode>(&m_map);
519 newArea->m_hypotheses.insert(LMH->m_ID);
520 newArea->m_nodeType =
"Area";
521 newArea->m_label = generateUniqueAreaLabel();
525 newArea->m_annotations.setMemoryReference(
529 mrpt::make_aligned_shared<CRobotPosesGraph>();
530 newArea->m_annotations.setMemoryReference(
534 partIdx2Areas[i] = newArea->getID();
540 for (
size_t i = 0; i < partIdx2Areas.size(); i++)
543 " Partition #%i -> AREA_ID %i ('%s')\n", (
int)i,
544 (int)partIdx2Areas[i],
545 m_map.getNodeByID(partIdx2Areas[i])->m_label.c_str());
552 LMH->m_neighbors.clear();
553 for (i = 0; i < partIdx2Areas.size(); i++)
558 LMH->m_neighbors.insert(nodeId);
564 LMH->m_nodeIDmemberships[*it] =
572 LMH->getMeans(lstPoses);
575 const CPose3D* curPoseMean = &lstPoses[LMH->m_currentRobotPose];
578 it != lstPoses.end(); ++it)
581 LMH->m_currentRobotPose)
586 closestPose = it->first;
595 LMH->m_nodeIDmemberships[LMH->m_currentRobotPose];
598 LMH->m_nodeIDmemberships[LMH->m_currentRobotPose] =
599 LMH->m_nodeIDmemberships[closestPose];
603 LMH->m_nodeIDmemberships[LMH->m_currentRobotPose];
605 if (curAreaID != oldAreaID)
608 "[LSLAM_proc_msg_AA] Current area has changed: %i -> %i\n",
609 (
int)oldAreaID, (
int)curAreaID);
615 pBef != neighbors_before.end(); ++pBef)
617 if (LMH->m_neighbors.find(*pBef) == LMH->m_neighbors.end())
623 "[LSLAM_proc_msg_AA] Old neighbors: ");
625 it != neighbors_before.end(); ++it)
632 "[LSLAM_proc_msg_AA] Cur. neighbors: ");
634 it != LMH->m_neighbors.end(); ++it)
640 std::lock_guard<std::mutex> lock(m_map_cs);
650 "[LSLAM_proc_msg_AA] Area %i has been removed from the " 651 "neighbors & no longer exists in the HMAP.\n",
658 "[LSLAM_proc_msg_AA] Deleting area %i\n",
670 using TListNodesArcs = map<CHMHMapNode::Ptr, CHMHMapArc::Ptr>;
671 TListNodesArcs lstWithinLMH;
678 if ((*a)->getNodeFrom() == *pBef)
680 nodeB = m_map.getNodeByID((*a)->getNodeTo());
684 nodeB = m_map.getNodeByID((*a)->getNodeFrom());
687 bool inNeib = LMH->m_neighbors.find(nodeB->getID()) !=
688 LMH->m_neighbors.end();
689 bool inBefNeib = neighbors_before.find(nodeB->getID()) !=
690 neighbors_before.end();
692 if (inNeib && inBefNeib)
693 lstWithinLMH[nodeB] = *
a;
706 if (arc->getNodeFrom() == *pBef)
708 nodeB = m_map.getNodeByID((*a)->getNodeTo());
713 nodeB = m_map.getNodeByID((*a)->getNodeFrom());
717 bool inNeib = LMH->m_neighbors.find(nodeB->getID()) !=
718 LMH->m_neighbors.end();
719 bool inBefNeib = neighbors_before.find(nodeB->getID()) !=
720 neighbors_before.end();
722 if (inNeib && inBefNeib)
737 na != lstWithinLMH.end(); ++na)
772 arc->m_annotations.getElemental(
774 refPoseAt_b, LMH->m_ID,
true);
775 arc->m_annotations.getElemental(
777 refPoseAt_a, LMH->m_ID,
true);
781 pdf->inverse(Delta_b_a);
784 arc->m_annotations.getElemental(
786 refPoseAt_b, LMH->m_ID,
true);
787 arc->m_annotations.getElemental(
789 refPoseAt_a, LMH->m_ID,
true);
793 nodeB->m_annotations.getElemental(
795 node_refPoseAt_b, LMH->m_ID,
true);
796 ASSERT_(node_refPoseAt_b == refPoseAt_b);
799 node->m_annotations.getElemental(
801 node_refPoseAt_a, LMH->m_ID,
true);
802 ASSERT_(node_refPoseAt_a == refPoseAt_a);
810 arc_c_a->m_annotations
815 if (arc_c_a->getNodeTo() == node_c->getID())
820 arc_c_a->m_annotations.getElemental(
822 refPoseAt_a, LMH->m_ID,
true);
823 arc_c_a->m_annotations.getElemental(
825 refPoseAt_c, LMH->m_ID,
true);
829 pdf->inverse(Delta_a_c);
832 arc_c_a->m_annotations.getElemental(
834 refPoseAt_a, LMH->m_ID,
true);
835 arc_c_a->m_annotations.getElemental(
837 refPoseAt_c, LMH->m_ID,
true);
841 node_c->m_annotations.getElemental(
843 node_refPoseAt_c, LMH->m_ID,
true);
844 ASSERT_(node_refPoseAt_c == refPoseAt_c);
847 node->m_annotations.getElemental(
849 node_refPoseAt_a, LMH->m_ID,
true);
850 ASSERT_(node_refPoseAt_a == refPoseAt_a);
858 Delta_b_c.
cov(0, 0) = Delta_b_c.
cov(1, 1) =
863 "b_a: " << Delta_b_a.
mean << endl
864 <<
"a_c: " << Delta_a_c.
mean << endl
865 <<
"b_a + a_c: " << Delta_b_c.
mean 873 bool arcDeltaIsInverted;
875 m_map.findArcOfTypeBetweenNodes(
879 "RelativePose", arcDeltaIsInverted);
884 newArc = mrpt::make_aligned_shared<CHMHMapArc>(
890 newArc->m_arcType =
"RelativePose";
891 arcDeltaIsInverted =
false;
894 if (!arcDeltaIsInverted)
896 newArc->m_annotations.set(
902 "[LSLAM_proc_msg_AA] Setting arc " 903 << nodeB->getID() <<
" -> " 904 << node_c->getID() <<
" : " 905 << Delta_b_c.
mean <<
" cov = " 906 << Delta_b_c.
cov.inMatlabFormat() << endl);
907 newArc->m_annotations.setElemental(
909 refPoseAt_b, LMH->m_ID);
910 newArc->m_annotations.setElemental(
912 refPoseAt_c, LMH->m_ID);
917 Delta_b_c.
inverse(Delta_b_c_inv);
920 "[LSLAM_proc_msg_AA] Setting arc " 921 << nodeB->getID() <<
" <- " 922 << node_c->getID() <<
" : " 923 << Delta_b_c_inv.
mean <<
" cov = " 924 << Delta_b_c_inv.
cov.inMatlabFormat()
926 newArc->m_annotations.set(
931 newArc->m_annotations.setElemental(
933 refPoseAt_c, LMH->m_ID);
934 newArc->m_annotations.setElemental(
936 refPoseAt_b, LMH->m_ID);
941 arc->m_annotations.removeAll(LMH->m_ID);
943 if (!arc->m_annotations.size())
974 if (curAreaID != oldAreaID)
976 LMH->m_areasPendingTBI.insert(curAreaID);
979 "[LSLAM_proc_msg_AA] Current area changed: enqueing area %i for " 985 static size_t cntAddTBI = 0;
989 LMH->m_areasPendingTBI.insert(curAreaID);
992 "[LSLAM_proc_msg_AA] Current area %i enqued for TBI (routine " 1015 if (partIdx2Areas.size() > 1)
1017 std::lock_guard<std::mutex> lock(m_map_cs);
1020 set<CHMHMapNode::TNodeID>
1024 map<CHMHMapNode::TNodeID, pair<CHMHMapNode::TNodeID, float>>
1025 lstClosestDoubtfulNeigbors;
1027 for (
size_t idx_area_a = 0; idx_area_a < partIdx2Areas.size();
1046 if (!area_a->m_annotations.getElemental(
1050 poseID_trg = myMsg.
partitions[idx_area_a][0];
1052 area_a->m_annotations.setElemental(
1056 "[LSLAM_proc_msg_AA] Changing reference poseID of area " 1057 "'%i' to pose '%i'\n",
1058 (
int)area_a_ID, (
int)poseID_trg);
1074 TPoseID poseID_trg_old = poseID_trg;
1077 !found &&
p != myMsg.
partitions[idx_area_a].end(); ++
p)
1078 if (poseID_trg == *
p)
1088 poseID_trg = myMsg.
partitions[idx_area_a][0];
1089 area_a->m_annotations.setElemental(
1094 "[LSLAM_proc_msg_AA] Changing reference poseID of area " 1095 "'%i' to pose '%i'\n",
1096 (
int)area_a_ID, (
int)poseID_trg);
1106 area_a->getArcs(arcs);
1108 a != arcs.end(); ++
a)
1122 if (nodeFrom == area_a_ID)
1125 if (LMH->m_neighbors.find(nodeTo) ==
1126 LMH->m_neighbors.end())
1130 LMH->getRelativePose(
1131 poseID_trg, poseID_trg_old, Anew_old_parts);
1138 theArc->m_annotations
1143 newDelta = Anew_old + *oldDelta;
1146 newDelta.
cov(0, 0) = newDelta.
cov(1, 1) =
1151 "[LSLAM_proc_msg_AA] Updating arc " 1152 << nodeFrom <<
" -> " << nodeTo
1153 <<
" OLD: " << oldDelta->mean <<
" cov = " 1154 << oldDelta->cov.inMatlabFormat() << endl);
1156 "[LSLAM_proc_msg_AA] Updating arc " 1157 << nodeFrom <<
" -> " << nodeTo
1158 <<
" NEW: " << newDelta.
mean <<
" cov = " 1159 << newDelta.
cov.inMatlabFormat() << endl);
1161 theArc->m_annotations.set(
1166 theArc->m_annotations.setElemental(
1174 if (LMH->m_neighbors.find(nodeFrom) ==
1175 LMH->m_neighbors.end())
1179 LMH->getRelativePose(
1180 poseID_trg_old, poseID_trg, Aold_new_parts);
1186 theArc->m_annotations
1192 newDelta = *oldDelta + Aold_new;
1196 newDelta.cov(0, 0) = newDelta.cov(1, 1) =
1201 "[LSLAM_proc_msg_AA] Updating arc " 1202 << nodeFrom <<
" <- " << nodeTo
1203 <<
" OLD: " << oldDelta->mean <<
" cov = " 1204 << oldDelta->cov.inMatlabFormat() << endl);
1206 "[LSLAM_proc_msg_AA] Updating arc " 1207 << nodeFrom <<
" <- " << nodeTo
1208 <<
" NEW: " << newDelta.mean <<
" cov = " 1209 << newDelta.cov.inMatlabFormat() << endl);
1211 theArc->m_annotations.set(
1216 theArc->m_annotations.setElemental(
1228 for (
size_t idx_area_b = 0; idx_area_b < myMsg.
partitions.size();
1231 if (idx_area_a == idx_area_b)
1235 double closestDistPoseSrc = 0;
1241 itP0 != myMsg.
partitions[idx_area_a].end(); itP0++)
1243 const CPose3D& pose_trg = lstPoses[*itP0];
1247 itP != myMsg.
partitions[idx_area_b].end(); ++itP)
1249 const CPose3D& otherPose = lstPoses[*itP];
1251 if (
dst < closestDistPoseSrc ||
1254 poseID_closests = *itP;
1255 closestDistPoseSrc =
dst;
1265 if (closestDistPoseSrc <
1266 5 * m_options.SLAM_MIN_DIST_BETWEEN_OBS)
1272 if (!area_b->m_annotations.getElemental(
1277 area_b->m_annotations.setElemental(
1280 poseID_src = poseID_closests;
1283 "[LSLAM_proc_msg_AA] Changing reference poseID of " 1284 "area '%i' to pose '%i' (creat. annot)\n",
1285 (
int)area_b_ID, (
int)poseID_closests);
1290 if (lstInternalArcsToCreate.end() ==
1291 lstInternalArcsToCreate.
find(
1293 lstInternalArcsToCreate.end() ==
1294 lstInternalArcsToCreate.
find(
1297 lstInternalArcsToCreate.
insert(
1299 areasWithLink.insert(area_a_ID);
1300 areasWithLink.insert(area_b_ID);
1305 if (lstClosestDoubtfulNeigbors.find(area_b_ID) ==
1306 lstClosestDoubtfulNeigbors.end() ||
1307 closestDistPoseSrc <
1308 lstClosestDoubtfulNeigbors[area_b_ID].second)
1310 lstClosestDoubtfulNeigbors[area_b_ID].first = area_a_ID;
1311 lstClosestDoubtfulNeigbors[area_b_ID].second =
1326 for (
size_t idx_area = 0; idx_area < myMsg.
partitions.size();
1330 if (areasWithLink.find(area_ID) == areasWithLink.end())
1333 if (lstClosestDoubtfulNeigbors.find(area_ID) !=
1334 lstClosestDoubtfulNeigbors.end())
1338 area_ID, lstClosestDoubtfulNeigbors[area_ID].
first));
1341 areasWithLink.insert(area_ID);
1342 areasWithLink.insert(
1343 lstClosestDoubtfulNeigbors[area_ID].
first);
1348 "Area %i seems unconnected??", (
int)area_ID);
1359 "[LSLAM_proc_msg_AA] lstInternalArcsToCreate contains %i " 1361 (
int)lstInternalArcsToCreate.size());
1363 lstInternalArcsToCreate.begin();
1364 arcCreat != lstInternalArcsToCreate.end(); ++arcCreat)
1371 (
int)closestAreaID, (
int)newAreaID);
1384 std::lock_guard<std::mutex> lock(m_map_cs);
1388 lstInternalArcsToCreate.begin();
1389 arcCreat != lstInternalArcsToCreate.end(); ++arcCreat)
1394 m_map.getNodeByID(area_a_ID)->m_annotations.getElemental(
1399 m_map.getNodeByID(area_b_ID)->m_annotations.getElemental(
1404 LMH->getRelativePose(
1405 area_a_poseID_src, area_b_poseID_trg, relPoseParts);
1409 relPoseGauss.
copyFrom(relPoseParts);
1411 relPoseGauss.
cov.zeros();
1412 relPoseGauss.
cov(0, 0) = relPoseGauss.
cov(1, 1) =
square(0.04);
1417 "[LSLAM_proc_msg_AA] Creating arc %i[ref:%i] -> %i[ref:%i] = " 1418 "(%.03f,%.03f,%.03fdeg)\n",
1419 (
int)area_a_ID, (
int)area_a_poseID_src, (
int)area_b_ID,
1420 (
int)area_b_poseID_trg, relPoseGauss.
mean.
x(),
1425 bool arcDeltaIsInverted;
1427 area_a_ID, area_b_ID, LMH->m_ID,
"RelativePose",
1428 arcDeltaIsInverted);
1433 newArc = mrpt::make_aligned_shared<CHMHMapArc>(
1439 newArc->m_arcType =
"RelativePose";
1440 arcDeltaIsInverted =
false;
1444 if (!arcDeltaIsInverted)
1447 "[LSLAM_proc_msg_AA] Updating int. arc " 1448 << area_a_ID <<
" -> " << area_b_ID <<
" : " 1449 << relPoseGauss.
mean 1450 <<
" cov = " << relPoseGauss.
cov.inMatlabFormat() << endl);
1451 newArc->m_annotations.set(
1456 newArc->m_annotations.setElemental(
1459 newArc->m_annotations.setElemental(
1466 relPoseGauss.
inverse(relPoseInv);
1469 "[LSLAM_proc_msg_AA] Updating int. arc " 1470 << area_a_ID <<
" <- " << area_b_ID <<
" : " 1472 <<
" cov = " << relPoseInv.
cov.inMatlabFormat() << endl);
1473 newArc->m_annotations.set(
1478 newArc->m_annotations.setElemental(
1481 newArc->m_annotations.setElemental(
1496 std::lock_guard<std::mutex> lock(m_map_cs);
1499 pNei != LMH->m_neighbors.end(); ++pNei)
1507 nodeFrom->getArcs(lstArcs,
"RelativePose", LMH->m_ID);
1513 a != lstArcs.end(); ++
a)
1516 (*a)->getNodeFrom() == nodeFromID ? (*a)->getNodeTo()
1517 : (*a)->getNodeFrom();
1519 if (LMH->m_neighbors.find(nodeToID) != LMH->m_neighbors.end())
1524 if (lstInternalArcsToCreate.end() ==
1525 lstInternalArcsToCreate.
find(
1527 lstInternalArcsToCreate.end() ==
1528 lstInternalArcsToCreate.
find(
1532 arc->m_annotations.remove(
1536 "[LSLAM_proc_msg_AA] Deleting annotation of arc: " 1538 (
long unsigned)nodeFromID, (
long unsigned)nodeToID);
1541 if (!arc->m_annotations.getAnyHypothesis(
1546 "[LSLAM_proc_msg_AA] Deleting empty arc: " 1548 (
long unsigned)nodeFromID,
1549 (
long unsigned)nodeToID);
1561 std::lock_guard<std::mutex> lock(m_map_cs);
1562 std::vector<std::string>
s;
1563 m_map.dumpAsText(
s);
1568 "%s/HMAP_txt/HMAP_%05i_mid.txt", m_options.LOG_OUTPUT_DIR.c_str(),
1573 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_mid.txt\n", DEBUG_STEP);
1586 pNei1 != LMH->m_neighbors.end();)
1588 if (*pNei1 != curAreaID)
1592 std::lock_guard<std::mutex> lock(m_map_cs);
1593 m_map.findArcsOfTypeBetweenNodes(
1594 *pNei1, curAreaID, LMH->m_ID,
"RelativePose", lstArcs);
1596 if (lstArcs.empty())
1600 "[LSLAM_proc_msg_AA] Getting area '%u' out of LMH\n",
1601 static_cast<unsigned>(*pNei1));
1609 double ESS_bef = LMH->ESS();
1610 LMH->removeAreaFromLMH(
id);
1611 double ESS_aft = LMH->ESS();
1614 "[LSLAM_proc_msg_AA] ESS: %f -> %f\n", ESS_bef, ESS_aft);
1637 std::lock_guard<std::mutex> lock(m_map_cs);
1640 LMH->m_nodeIDmemberships[LMH->m_currentRobotPose];
1641 currentArea = m_map.getNodeByID(curAreaID);
1643 TPoseID refPoseCurArea_accordingAnnot;
1644 currentArea->m_annotations.getElemental(
1649 currentArea->getArcs(arcsToCurArea,
"RelativePose", LMH->m_ID);
1651 a != arcsToCurArea.end(); ++
a)
1655 arc->getNodeFrom() == curAreaID ? arc->getNodeTo()
1656 : arc->getNodeFrom();
1659 if (LMH->m_neighbors.find(otherAreaID) == LMH->m_neighbors.end())
1663 "[LSLAM_proc_msg_AA] Bringing in LMH area %i\n",
1681 pdf->getMean(Delta_c2a);
1684 TPoseID refPoseIDAtOtherArea, refPoseIDAtCurArea;
1686 if (arc->getNodeTo() == curAreaID)
1691 "[LSLAM_proc_msg_AA] Arc is inverted: " 1692 "(%.03f,%.03f,%.03fdeg) -> ",
1693 Delta_c2a.x(), Delta_c2a.y(),
RAD2DEG(Delta_c2a.yaw()));
1695 Delta_c2a =
CPose3D(0, 0, 0) - Delta_c2a;
1699 Delta_c2a.x(), Delta_c2a.y(),
RAD2DEG(Delta_c2a.yaw()));
1701 arc->m_annotations.getElemental(
1704 arc->m_annotations.getElemental(
1711 arc->m_annotations.getElemental(
1714 arc->m_annotations.getElemental(
1721 "[LSLAM_proc_msg_AA] Bringing in: refPoseCur=%i " 1722 "refPoseOther=%i -> Delta_c2a:(%.03f,%.03f,%.03fdeg)\n",
1723 (
int)refPoseIDAtCurArea, (
int)refPoseIDAtOtherArea,
1724 Delta_c2a.x(), Delta_c2a.y(),
RAD2DEG(Delta_c2a.yaw()));
1727 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG) 1729 TPoseID refPoseOtherArea_accordingAnnot;
1730 area->m_annotations.getElemental(
1732 refPoseOtherArea_accordingAnnot, LMH->m_ID,
true);
1734 refPoseIDAtOtherArea ==
1735 refPoseOtherArea_accordingAnnot);
1738 refPoseIDAtCurArea == refPoseCurArea_accordingAnnot);
1747 lstNewPoseIDs.reserve(pg->size());
1754 lstNewPoseIDs.push_back(poseID);
1759 LMH->m_particles.size());
1765 itTrg = LMH->m_particles.begin();
1766 itTrg != LMH->m_particles.end(); itSrc++, itTrg++)
1770 itTrg->d->robotPoses[poseID] =
1771 itTrg->d->robotPoses[refPoseIDAtCurArea] +
1772 Delta_c2a +
CPose3D(itSrc->d);
1776 LMH->m_nodeIDmemberships[poseID] = otherAreaID;
1779 LMH->m_SFs[poseID] = poseInfo.
sf;
1782 LMH->m_neighbors.
insert(otherAreaID);
1789 LMH->m_posesPendingAddPartitioner.insert(
1790 LMH->m_posesPendingAddPartitioner.end(),
1791 lstNewPoseIDs.begin(), lstNewPoseIDs.end());
1795 areasDelayedMetricMapsInsertion.insert(otherAreaID);
1803 std::vector<std::string>
s;
1808 "%s/HMAP_txt/HMAP_%05i_LMH_mid.txt",
1809 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP));
1813 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_LMH_mid.txt\n", DEBUG_STEP);
1820 mrpt::make_aligned_shared<opengl::CSetOfObjects>();
1821 maps3D->setName(
"metric-maps");
1822 LMH->getMostLikelyParticle()->d->metricMaps.getAs3DObject(maps3D);
1823 sceneLSLAM.
insert(maps3D);
1827 mrpt::make_aligned_shared<opengl::CSetOfObjects>();
1828 LSLAM_3D->setName(
"LSLAM_3D");
1829 LMH->getAs3DScene(LSLAM_3D);
1830 sceneLSLAM.
insert(LSLAM_3D);
1834 string filLocalAreas =
format(
1835 "%s/HMAP_txt/HMAP_%05i_LMH_mid.3Dscene",
1836 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP);
1849 if (!currentArea->m_annotations.getElemental(
1853 if (new_poseID_origin != poseID_origin)
1857 LMH->changeCoordinateOrigin(new_poseID_origin);
1860 "[LSLAM_proc_msg_AA] LMH->changeCoordinateOrigin %lu->%lu took %f " 1862 poseID_origin, new_poseID_origin, tictac.
Tac() * 1000);
1864 else if (areasDelayedMetricMapsInsertion.size())
1871 areasDelayedMetricMapsInsertion.begin();
1872 areaID != areasDelayedMetricMapsInsertion.end(); ++areaID)
1876 LMH->m_nodeIDmemberships.begin();
1877 pn != LMH->m_nodeIDmemberships.end(); ++pn)
1879 if (pn->second == *areaID)
1882 const TPoseID& poseToAdd = pn->first;
1884 LMH->m_SFs.find(poseToAdd)->second;
1888 partIt = LMH->m_particles.begin();
1889 partIt != LMH->m_particles.end(); ++partIt)
1892 partIt->d->robotPoses.find(poseToAdd);
1893 ASSERT_(pose3D != partIt->d->robotPoses.end());
1895 &partIt->d->metricMaps, &pose3D->second);
1903 "[LSLAM_proc_msg_AA] areasDelayedMetricMapsInsertion took %f ms\n",
1904 tictac.
Tac() * 1000);
1909 std::lock_guard<std::mutex> lock(m_map_cs);
1910 std::vector<std::string>
s;
1911 m_map.dumpAsText(
s);
1915 "%s/HMAP_txt/HMAP_%05i_after.txt", m_options.LOG_OUTPUT_DIR.c_str(),
1920 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_after.txt\n", DEBUG_STEP);
1924 std::vector<std::string>
s;
1929 "%s/HMAP_txt/HMAP_%05i_LMH_after.txt",
1930 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP));
1934 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_LMH_after.txt\n", DEBUG_STEP);
1939 "[LSLAM_proc_msg_AA] Msg from AA took %f ms ]\n",
1940 tictac.Tac() * 1000);
1956 "[LSLAM_proc_msg_TBI] Beginning of Msg from TBI processing... " 1961 std::map<CHMHMapNode::TNodeID, CHMHMapNode::TNodeID> alreadyClosedLoops;
1970 "[LSLAM_proc_msg_TBI] Processing TLC of areas: %u <-> %u... \n",
1971 (
unsigned)myMsg.
cur_area, (
unsigned)candidate->first);
1975 if (alreadyClosedLoops.find(myMsg.
cur_area) != alreadyClosedLoops.end())
1977 currentArea = alreadyClosedLoops[myMsg.
cur_area];
1978 cout <<
"[LSLAM_proc_msg_TBI] Using " << myMsg.
cur_area <<
" -> " 1979 << currentArea <<
" due to area being already merged." 1986 m_map.computeCoordinatesTransformationBetweenNodes(
1989 candidate->first, pdfPartsHMap, myMsg.
hypothesisID, 100, 0.10f,
1994 pdfDeltaMap.
copyFrom(pdfPartsHMap);
2005 cout <<
"[LSLAM_proc_msg_TBI] HMap_delta=" << pdfDeltaMap.
mean 2006 <<
" std_x=" << sqrt(pdfDeltaMap.
cov(0, 0))
2007 <<
" std_y=" << sqrt(pdfDeltaMap.
cov(1, 1)) << endl;
2015 ASSERT_(!candidate->second.delta_new_cur.empty());
2016 const double chi2_thres =
2019 map<double, CPose3DPDFGaussian>
2024 candidate->second.delta_new_cur.begin();
2025 itSOG != candidate->second.delta_new_cur.end(); ++itSOG)
2029 cout <<
"[LSLAM_proc_msg_TBI] TLC_delta=" << pdfDelta.
mean 2030 <<
" std_x=" << sqrt(pdfDelta.
cov(0, 0))
2031 <<
" std_y=" << sqrt(pdfDelta.
cov(1, 1))
2032 <<
" std_phi=" <<
RAD2DEG(sqrt(pdfDelta.
cov(3, 3))) << endl;
2036 const double mahaDist2 =
2038 cout <<
"[LSLAM_proc_msg_TBI] maha_dist = " << mahaDist2 << endl;
2040 if (mahaDist2 < chi2_thres)
2042 const double log_lik = itSOG->log_w - 0.5 * mahaDist2;
2043 lstModesAndCompats[log_lik] = itSOG->val;
2044 cout <<
"[LSLAM_proc_msg_TBI] Added to list of candidates: " 2045 "log(overall_lik)= " 2051 if (!lstModesAndCompats.empty())
2054 lstModesAndCompats.rbegin()->second;
2067 "[LSLAM_proc_msg_TBI] Accepting TLC of areas: %u <-> %u with " 2068 "an overall log(lik)=%f \n",
2069 (
unsigned)currentArea, (
unsigned)candidate->first,
2070 lstModesAndCompats.rbegin()->first);
2080 "[LSLAM_proc_msg_TBI] TLC of areas %u <-> %u - DONE in %.03f " 2082 (
unsigned)currentArea, (
unsigned)candidate->first,
2083 1e3 * tictac.
Tac());
2086 alreadyClosedLoops[myMsg.
cur_area] = candidate->first;
2094 "[LSLAM_proc_msg_TBI] Msg from TBI took %f ms ]\n",
2095 tictac.Tac() * 1000);
A namespace of pseudo-random numbers generators of diferent distributions.
double Tac() noexcept
Stops the stopwatch.
double x() const
Common members of all points & poses classes.
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...
CPose3D mean
The mean value.
double RAD2DEG(const double x)
Radians to degrees.
std::list< T >::iterator find(const T &i)
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
std::map< CHMHMapNode::TNodeID, TBI_info > loopClosureData
The meat is here: only feasible loop closures from "cur_area" are included here, with associated data...
#define THROW_EXCEPTION(msg)
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
double mahalanobisDistanceTo(const CPose3DPDFGaussian &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
CParticleList m_particles
The array of particles.
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
double DEG2RAD(const double x)
Degrees to radians.
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
mrpt::poses::CPose3DPDFParticles pdf
The robot pose PDF.
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
A high-performance stopwatch, with typical resolution of nanoseconds.
EIGEN_STRONG_INLINE iterator begin()
double yaw() const
Get the YAW angle (in radians)
THypothesisID hypothesisID
The hypothesis ID (LMH) for these results.
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.
GLsizei GLsizei GLuint * obj
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Declares a class for storing a collection of robot actions.
std::set< CHMHMapNode::TNodeID > TNodeIDSet
std::vector< TPoseID > TPoseIDList
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream, std::istream, std::ostream, std::stringstream
T square(const T x)
Inline function for the square of a number.
double distanceEuclidean6D(const CPose3D &o) const
The euclidean distance between two poses taken as two 6-length vectors (angles in radians)...
CHMHMapNode::TNodeID cur_area
The area for who the loop-closure has been computed.
#define ASSERT_(f)
Defines an assertion mechanism.
This base provides a set of functions for maths stuff.
This class implements a STL container with features of both, a std::set and a std::list.
mrpt::aligned_std_map< TPoseID, mrpt::poses::CPose3D > TMapPoseID2Pose3D
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
A set of hypothesis IDs, used for arcs and nodes in multi-hypothesis hybrid maps. ...
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...
void enableFollowCamera(bool enabled)
If disabled (default), the SceneViewer application will ignore the camera of the "main" viewport and ...
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
std::vector< TPoseIDList > partitions
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class stores a set of r...
THypothesisID hypothesisID
The hypothesis ID (LMH) for these results.
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
This CStream derived class allow using a file as a write-only, binary stream.
#define NODE_ANNOTATION_METRIC_MAPS
GLsizei const GLchar ** string
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
TModesList::const_iterator const_iterator
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...
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
A class that contain generic messages, that can be sent and received from a "CClientTCPSocket" object...
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...
#define ARC_ANNOTATION_DELTA_SRC_POSEID
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt::obs::CSensoryFrame sf
The observations.
The namespace for 3D scene representation and rendering.
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
#define NODE_ANNOTATION_REF_POSEID
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Information kept for each robot pose used in CRobotPosesGraph.
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
void Tic() noexcept
Starts the stopwatch.
This class stores any customizable set of metric maps.
This class is a "CSerializable" wrapper for "CMatrixFloat".
#define NODE_ANNOTATION_POSES_GRAPH
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
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).
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)...
#define ARC_ANNOTATION_DELTA_TRG_POSEID
GLubyte GLubyte GLubyte a
const Scalar * const_iterator
A class for storing a sequence of arcs (a path).
#define ARC_ANNOTATION_DELTA
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.