47 void CHMTSLAM::thread_LSLAM()
51 unsigned int nIter = 0;
54 if (
obj->m_options.random_seed)
64 "[thread_LSLAM] Thread started ID=" << std::this_thread::get_id());
70 while (!
obj->m_terminateThreads)
72 if (
obj->m_options.random_seed)
80 recMsg =
obj->m_LSLAM_queue.get();
83 obj->LSLAM_process_message(*recMsg);
90 if (!
obj->isInputQueueEmpty())
92 if (
obj->m_options.random_seed)
97 obj->getNextObjectFromInputQueue();
104 if (nextObject->GetRuntimeClass() ==
106 actions = std::dynamic_pointer_cast<CActionCollection>(
111 std::dynamic_pointer_cast<CSensoryFrame>(nextObject);
114 "Element in the queue is neither CActionCollection nor " 123 std::lock_guard<std::mutex> LMHs_cs_lock(
obj->m_LMHs_cs);
125 for (
auto it =
obj->m_LMHs.begin(); it !=
obj->m_LMHs.end();
128 std::lock_guard<std::mutex> LMH_individual_lock(
129 it->second.threadLocks.m_lock);
134 obj->m_LSLAM_method->processOneLMH(
136 actions, observations);
141 if (it->second.m_posesPendingAddPartitioner.size() >
148 unsigned nPosesToInsert =
149 it->second.m_posesPendingAddPartitioner.size();
151 CHMTSLAM::areaAbstraction(
153 it->second.m_posesPendingAddPartitioner);
157 "[AreaAbstraction] Took %.03fms to insert %u " 159 1000 * tictac2.
Tac(), nPosesToInsert);
162 it->second.m_posesPendingAddPartitioner.clear();
164 if (
obj->m_options.random_seed)
166 obj->m_options.random_seed);
173 obj->LSLAM_process_message_from_AA(*msgFromAA);
179 if (!it->second.m_areasPendingTBI.empty())
182 it->second.m_areasPendingTBI.begin();
183 areaID != it->second.m_areasPendingTBI.end();
188 if (
obj->m_options.random_seed)
190 obj->m_options.random_seed);
193 CHMTSLAM::TBI_main_method(
194 &it->second, *areaID);
198 "[TBI] Took %.03fms " 200 1000 * tictac2.
Tac());
206 obj->LSLAM_process_message_from_TBI(
211 it->second.m_areasPendingTBI.clear();
225 if (
obj->m_options.LOG_OUTPUT_DIR.size() &&
226 (nIter %
obj->m_options.LOG_FREQUENCY) == 0)
227 obj->generateLogFiles(nIter);
235 std::this_thread::sleep_for(5ms);
243 obj->m_terminationFlag_LSLAM =
true;
245 catch (
const std::exception& e)
247 obj->m_terminationFlag_LSLAM =
true;
254 obj->m_terminateThreads =
true;
258 obj->m_terminationFlag_LSLAM =
true;
263 obj->m_terminateThreads =
true;
270 void CHMTSLAM::LSLAM_process_message(
const CMessage& msg)
308 "[LSLAM_proc_msg_AA] Beginning of Msg from AA processing... " 313 ASSERT_(itLMH != m_LMHs.end());
319 for (
const auto& partition : myMsg.
partitions)
320 for (
auto itPose = partition.begin(); itPose != partition.end();
322 if (LMH->m_SFs.find(*itPose) == LMH->m_SFs.end())
324 "PoseID %i in AA's partition but not in LMH.\n",
328 for (
auto itA = LMH->m_nodeIDmemberships.begin();
329 itA != LMH->m_nodeIDmemberships.end(); ++itA)
331 if (LMH->m_currentRobotPose != itA->first)
338 for (
unsigned long itPose : *it)
339 if (itA->first == itPose)
346 "LMH's pose %i not found in AA's partitions.",
353 TNodeIDSet neighbors_before(LMH->m_neighbors);
358 std::lock_guard<std::mutex> lock(m_map_cs);
360 auto itCur = LMH->m_nodeIDmemberships.find(LMH->m_currentRobotPose);
361 ASSERT_(itCur != LMH->m_nodeIDmemberships.end());
363 if (!m_map.getNodeByID(itCur->second)
364 ->m_annotations.getElemental(
377 map<unsigned int, map<CHMHMapNode::TNodeID, unsigned int>> votes;
380 static int DEBUG_STEP = 0;
388 DEBUG_STEP = DEBUG_STEP + 0;
392 std::lock_guard<std::mutex> lock(m_map_cs);
393 std::vector<std::string>
s;
398 "%s/HMAP_txt/HMAP_%05i_before.txt",
399 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP));
403 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_before.txt\n", DEBUG_STEP);
407 vector<TPoseIDList>::const_iterator it;
410 for (
unsigned long itPose : *it)
412 auto itP = LMH->m_nodeIDmemberships.find(itPose);
413 ASSERT_(itP != LMH->m_nodeIDmemberships.end());
415 votes[i][itP->second]++;
420 vector<CHMHMapNode::TNodeID> partIdx2Areas(
423 map<CHMHMapNode::TNodeID, pair<size_t, unsigned int>>
429 for (
size_t k = 0; k < myMsg.
partitions.size(); k++)
431 if (votes[k].
size() == 1)
434 if (votes[k].
begin()->second >
435 mostVotedFrom[votes[k].begin()->first].second)
437 mostVotedFrom[votes[k].begin()->first].first = k;
438 mostVotedFrom[votes[k].begin()->first].second =
439 votes[k].begin()->second;
447 for (
auto&
v : mostVotedFrom)
448 v.second.second = std::numeric_limits<unsigned int>::max();
452 for (
size_t k = 0; k < myMsg.
partitions.size(); k++)
454 for (
auto it = votes[k].
begin(); it != votes[k].end(); ++it)
459 map<CHMHMapNode::TNodeID, pair<size_t, unsigned int>>::iterator
461 mostVotesIt = mostVotedFrom.find(it->first);
462 if (mostVotesIt == mostVotedFrom.end())
465 mostVotedFrom[it->first].first = k;
466 mostVotedFrom[it->first].second = it->second;
471 if (it->second > mostVotesIt->second.second)
473 mostVotesIt->second.first = k;
474 mostVotesIt->second.second = it->second;
481 for (
auto& it : mostVotedFrom) partIdx2Areas[it.second.first] = it.first;
484 for (i = 0; i < partIdx2Areas.size(); i++)
489 std::lock_guard<std::mutex> lock(m_map_cs);
494 newArea->m_hypotheses.insert(LMH->m_ID);
495 newArea->m_nodeType =
"Area";
496 newArea->m_label = generateUniqueAreaLabel();
499 CMultiMetricMap::Create(m_options.defaultMapsInitializers);
500 newArea->m_annotations.setMemoryReference(
503 auto emptyPoseGraph = CRobotPosesGraph::Create();
504 newArea->m_annotations.setMemoryReference(
508 partIdx2Areas[i] = newArea->getID();
514 for (
unsigned idx = 0; idx < partIdx2Areas.size(); idx++)
517 << idx <<
" -> AREA_ID " << partIdx2Areas[idx] <<
" ('" 518 << m_map.getNodeByID(partIdx2Areas[idx])->m_label <<
"')\n");
525 LMH->m_neighbors.clear();
526 for (i = 0; i < partIdx2Areas.size(); i++)
531 LMH->m_neighbors.insert(nodeId);
536 LMH->m_nodeIDmemberships[it] =
544 LMH->getMeans(lstPoses);
547 const CPose3D* curPoseMean = &lstPoses[LMH->m_currentRobotPose];
549 for (
auto it = lstPoses.begin(); it != lstPoses.end(); ++it)
552 LMH->m_currentRobotPose)
557 closestPose = it->first;
566 LMH->m_nodeIDmemberships[LMH->m_currentRobotPose];
569 LMH->m_nodeIDmemberships[LMH->m_currentRobotPose] =
570 LMH->m_nodeIDmemberships[closestPose];
574 LMH->m_nodeIDmemberships[LMH->m_currentRobotPose];
576 if (curAreaID != oldAreaID)
579 "[LSLAM_proc_msg_AA] Current area has changed: %i -> %i\n",
580 (
int)oldAreaID, (
int)curAreaID);
585 for (
auto pBef = neighbors_before.begin(); pBef != neighbors_before.end();
588 if (LMH->m_neighbors.find(*pBef) == LMH->m_neighbors.end())
594 "[LSLAM_proc_msg_AA] Old neighbors: ");
595 for (
unsigned long it : neighbors_before)
602 "[LSLAM_proc_msg_AA] Cur. neighbors: ");
603 for (
unsigned long m_neighbor : LMH->m_neighbors)
609 std::lock_guard<std::mutex> lock(m_map_cs);
619 "[LSLAM_proc_msg_AA] Area %i has been removed from the " 620 "neighbors & no longer exists in the HMAP.\n",
627 "[LSLAM_proc_msg_AA] Deleting area %i\n",
639 using TListNodesArcs = map<CHMHMapNode::Ptr, CHMHMapArc::Ptr>;
640 TListNodesArcs lstWithinLMH;
642 for (
auto a = arcs.begin();
a != arcs.end(); ++
a)
646 if ((*a)->getNodeFrom() == *pBef)
648 nodeB = m_map.getNodeByID((*a)->getNodeTo());
652 nodeB = m_map.getNodeByID((*a)->getNodeFrom());
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();
660 if (inNeib && inBefNeib)
661 lstWithinLMH[nodeB] = *
a;
666 for (
auto a = arcs.begin();
a != arcs.end(); ++
a)
673 if (arc->getNodeFrom() == *pBef)
675 nodeB = m_map.getNodeByID((*a)->getNodeTo());
680 nodeB = m_map.getNodeByID((*a)->getNodeFrom());
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();
689 if (inNeib && inBefNeib)
703 for (
auto& na : lstWithinLMH)
738 arc->m_annotations.getElemental(
740 refPoseAt_b, LMH->m_ID,
true);
741 arc->m_annotations.getElemental(
743 refPoseAt_a, LMH->m_ID,
true);
747 pdf->inverse(Delta_b_a);
750 arc->m_annotations.getElemental(
752 refPoseAt_b, LMH->m_ID,
true);
753 arc->m_annotations.getElemental(
755 refPoseAt_a, LMH->m_ID,
true);
759 nodeB->m_annotations.getElemental(
761 node_refPoseAt_b, LMH->m_ID,
true);
762 ASSERT_(node_refPoseAt_b == refPoseAt_b);
765 node->m_annotations.getElemental(
767 node_refPoseAt_a, LMH->m_ID,
true);
768 ASSERT_(node_refPoseAt_a == refPoseAt_a);
776 arc_c_a->m_annotations
781 if (arc_c_a->getNodeTo() == node_c->getID())
786 arc_c_a->m_annotations.getElemental(
788 refPoseAt_a, LMH->m_ID,
true);
789 arc_c_a->m_annotations.getElemental(
791 refPoseAt_c, LMH->m_ID,
true);
795 pdf->inverse(Delta_a_c);
798 arc_c_a->m_annotations.getElemental(
800 refPoseAt_a, LMH->m_ID,
true);
801 arc_c_a->m_annotations.getElemental(
803 refPoseAt_c, LMH->m_ID,
true);
807 node_c->m_annotations.getElemental(
809 node_refPoseAt_c, LMH->m_ID,
true);
810 ASSERT_(node_refPoseAt_c == refPoseAt_c);
813 node->m_annotations.getElemental(
815 node_refPoseAt_a, LMH->m_ID,
true);
816 ASSERT_(node_refPoseAt_a == refPoseAt_a);
824 Delta_b_c.
cov(0, 0) = Delta_b_c.
cov(1, 1) =
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 839 bool arcDeltaIsInverted;
841 m_map.findArcOfTypeBetweenNodes(
845 "RelativePose", arcDeltaIsInverted);
850 newArc = std::make_shared<CHMHMapArc>(
856 newArc->m_arcType =
"RelativePose";
857 arcDeltaIsInverted =
false;
860 if (!arcDeltaIsInverted)
862 newArc->m_annotations.set(
864 std::make_shared<CPose3DPDFGaussian>(
868 "[LSLAM_proc_msg_AA] Setting arc " 869 << nodeB->getID() <<
" -> " 870 << node_c->getID() <<
" : " 871 << Delta_b_c.
mean <<
" cov = " 873 newArc->m_annotations.setElemental(
875 refPoseAt_b, LMH->m_ID);
876 newArc->m_annotations.setElemental(
878 refPoseAt_c, LMH->m_ID);
883 Delta_b_c.
inverse(Delta_b_c_inv);
886 "[LSLAM_proc_msg_AA] Setting arc " 887 << nodeB->getID() <<
" <- " 888 << node_c->getID() <<
" : " 889 << Delta_b_c_inv.
mean <<
" cov = " 892 newArc->m_annotations.set(
894 std::make_shared<CPose3DPDFGaussian>(
897 newArc->m_annotations.setElemental(
899 refPoseAt_c, LMH->m_ID);
900 newArc->m_annotations.setElemental(
902 refPoseAt_b, LMH->m_ID);
907 arc->m_annotations.removeAll(LMH->m_ID);
909 if (!arc->m_annotations.size())
922 for (
auto& arc : al) arc.reset();
938 if (curAreaID != oldAreaID)
940 LMH->m_areasPendingTBI.insert(curAreaID);
943 "[LSLAM_proc_msg_AA] Current area changed: enqueing area %i for " 949 static size_t cntAddTBI = 0;
953 LMH->m_areasPendingTBI.insert(curAreaID);
956 "[LSLAM_proc_msg_AA] Current area %i enqued for TBI (routine " 979 if (partIdx2Areas.size() > 1)
981 std::lock_guard<std::mutex> lock(m_map_cs);
984 set<CHMHMapNode::TNodeID>
988 map<CHMHMapNode::TNodeID, pair<CHMHMapNode::TNodeID, float>>
989 lstClosestDoubtfulNeigbors;
991 for (
size_t idx_area_a = 0; idx_area_a < partIdx2Areas.size();
1010 if (!area_a->m_annotations.getElemental(
1014 poseID_trg = myMsg.
partitions[idx_area_a][0];
1016 area_a->m_annotations.setElemental(
1020 "[LSLAM_proc_msg_AA] Changing reference poseID of area " 1021 "'%i' to pose '%i'\n",
1022 (
int)area_a_ID, (
int)poseID_trg);
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)
1051 poseID_trg = myMsg.
partitions[idx_area_a][0];
1052 area_a->m_annotations.setElemental(
1057 "[LSLAM_proc_msg_AA] Changing reference poseID of area " 1058 "'%i' to pose '%i'\n",
1059 (
int)area_a_ID, (
int)poseID_trg);
1069 area_a->getArcs(arcs);
1070 for (
auto a = arcs.begin();
a != arcs.end(); ++
a)
1084 if (nodeFrom == area_a_ID)
1087 if (LMH->m_neighbors.find(nodeTo) ==
1088 LMH->m_neighbors.end())
1092 LMH->getRelativePose(
1093 poseID_trg, poseID_trg_old, Anew_old_parts);
1100 theArc->m_annotations
1105 newDelta = Anew_old + *oldDelta;
1108 newDelta.
cov(0, 0) = newDelta.
cov(1, 1) =
1113 "[LSLAM_proc_msg_AA] Updating arc " 1114 << nodeFrom <<
" -> " << nodeTo
1115 <<
" OLD: " << oldDelta->mean <<
" cov = " 1116 << oldDelta->cov.inMatlabFormat() << endl);
1118 "[LSLAM_proc_msg_AA] Updating arc " 1119 << nodeFrom <<
" -> " << nodeTo
1120 <<
" NEW: " << newDelta.
mean <<
" cov = " 1123 theArc->m_annotations.set(
1125 std::make_shared<CPose3DPDFGaussian>(
1128 theArc->m_annotations.setElemental(
1136 if (LMH->m_neighbors.find(nodeFrom) ==
1137 LMH->m_neighbors.end())
1141 LMH->getRelativePose(
1142 poseID_trg_old, poseID_trg, Aold_new_parts);
1148 theArc->m_annotations
1154 newDelta = *oldDelta + Aold_new;
1158 newDelta.cov(0, 0) = newDelta.cov(1, 1) =
1163 "[LSLAM_proc_msg_AA] Updating arc " 1164 << nodeFrom <<
" <- " << nodeTo
1165 <<
" OLD: " << oldDelta->mean <<
" cov = " 1166 << oldDelta->cov.inMatlabFormat() << endl);
1168 "[LSLAM_proc_msg_AA] Updating arc " 1169 << nodeFrom <<
" <- " << nodeTo
1170 <<
" NEW: " << newDelta.mean <<
" cov = " 1171 << newDelta.cov.inMatlabFormat() << endl);
1173 theArc->m_annotations.set(
1175 std::make_shared<CPose3DPDFGaussian>(
1178 theArc->m_annotations.setElemental(
1190 for (
size_t idx_area_b = 0; idx_area_b < myMsg.
partitions.size();
1193 if (idx_area_a == idx_area_b)
1197 double closestDistPoseSrc = 0;
1201 for (
auto itP0 = myMsg.
partitions[idx_area_a].begin();
1202 itP0 != myMsg.
partitions[idx_area_a].end(); itP0++)
1204 const CPose3D& pose_trg = lstPoses[*itP0];
1206 for (
unsigned long itP : myMsg.
partitions[idx_area_b])
1208 const CPose3D& otherPose = lstPoses[itP];
1210 if (
dst < closestDistPoseSrc ||
1213 poseID_closests = itP;
1214 closestDistPoseSrc =
dst;
1224 if (closestDistPoseSrc <
1225 5 * m_options.SLAM_MIN_DIST_BETWEEN_OBS)
1231 if (!area_b->m_annotations.getElemental(
1236 area_b->m_annotations.setElemental(
1239 poseID_src = poseID_closests;
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);
1249 if (lstInternalArcsToCreate.end() ==
1250 lstInternalArcsToCreate.
find(
1252 lstInternalArcsToCreate.end() ==
1253 lstInternalArcsToCreate.
find(
1256 lstInternalArcsToCreate.
insert(
1258 areasWithLink.insert(area_a_ID);
1259 areasWithLink.insert(area_b_ID);
1264 if (lstClosestDoubtfulNeigbors.find(area_b_ID) ==
1265 lstClosestDoubtfulNeigbors.end() ||
1266 closestDistPoseSrc <
1267 lstClosestDoubtfulNeigbors[area_b_ID].second)
1269 lstClosestDoubtfulNeigbors[area_b_ID].first = area_a_ID;
1270 lstClosestDoubtfulNeigbors[area_b_ID].second =
1285 for (
size_t idx_area = 0; idx_area < myMsg.
partitions.size();
1289 if (areasWithLink.find(area_ID) == areasWithLink.end())
1292 if (lstClosestDoubtfulNeigbors.find(area_ID) !=
1293 lstClosestDoubtfulNeigbors.end())
1297 area_ID, lstClosestDoubtfulNeigbors[area_ID].
first));
1300 areasWithLink.insert(area_ID);
1301 areasWithLink.insert(
1302 lstClosestDoubtfulNeigbors[area_ID].
first);
1307 "Area %i seems unconnected??", (
int)area_ID);
1318 "[LSLAM_proc_msg_AA] lstInternalArcsToCreate contains %i " 1320 (
int)lstInternalArcsToCreate.size());
1321 for (
auto arcCreat = lstInternalArcsToCreate.begin();
1322 arcCreat != lstInternalArcsToCreate.end(); ++arcCreat)
1329 (
int)closestAreaID, (
int)newAreaID);
1342 std::lock_guard<std::mutex> lock(m_map_cs);
1345 for (
auto arcCreat = lstInternalArcsToCreate.begin();
1346 arcCreat != lstInternalArcsToCreate.end(); ++arcCreat)
1351 m_map.getNodeByID(area_a_ID)->m_annotations.getElemental(
1356 m_map.getNodeByID(area_b_ID)->m_annotations.getElemental(
1361 LMH->getRelativePose(
1362 area_a_poseID_src, area_b_poseID_trg, relPoseParts);
1366 relPoseGauss.
copyFrom(relPoseParts);
1369 relPoseGauss.
cov(0, 0) = relPoseGauss.
cov(1, 1) =
square(0.04);
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(),
1382 bool arcDeltaIsInverted;
1384 area_a_ID, area_b_ID, LMH->m_ID,
"RelativePose",
1385 arcDeltaIsInverted);
1390 newArc = std::make_shared<CHMHMapArc>(
1396 newArc->m_arcType =
"RelativePose";
1397 arcDeltaIsInverted =
false;
1401 if (!arcDeltaIsInverted)
1404 "[LSLAM_proc_msg_AA] Updating int. arc " 1405 << area_a_ID <<
" -> " << area_b_ID <<
" : " 1406 << relPoseGauss.
mean 1408 newArc->m_annotations.set(
1410 std::make_shared<CPose3DPDFGaussian>(relPoseGauss),
1412 newArc->m_annotations.setElemental(
1415 newArc->m_annotations.setElemental(
1422 relPoseGauss.
inverse(relPoseInv);
1425 "[LSLAM_proc_msg_AA] Updating int. arc " 1426 << area_a_ID <<
" <- " << area_b_ID <<
" : " 1429 newArc->m_annotations.set(
1431 std::make_shared<CPose3DPDFGaussian>(relPoseInv),
1434 newArc->m_annotations.setElemental(
1437 newArc->m_annotations.setElemental(
1452 std::lock_guard<std::mutex> lock(m_map_cs);
1454 for (
auto pNei = LMH->m_neighbors.begin();
1455 pNei != LMH->m_neighbors.end(); ++pNei)
1463 nodeFrom->getArcs(lstArcs,
"RelativePose", LMH->m_ID);
1468 for (
auto a = lstArcs.begin();
a != lstArcs.end(); ++
a)
1471 (*a)->getNodeFrom() == nodeFromID ? (*a)->getNodeTo()
1472 : (*a)->getNodeFrom();
1474 if (LMH->m_neighbors.find(nodeToID) != LMH->m_neighbors.end())
1479 if (lstInternalArcsToCreate.end() ==
1480 lstInternalArcsToCreate.
find(
1482 lstInternalArcsToCreate.end() ==
1483 lstInternalArcsToCreate.
find(
1487 arc->m_annotations.remove(
1491 "[LSLAM_proc_msg_AA] Deleting annotation of arc: " 1493 (
long unsigned)nodeFromID, (
long unsigned)nodeToID);
1496 if (!arc->m_annotations.getAnyHypothesis(
1501 "[LSLAM_proc_msg_AA] Deleting empty arc: " 1503 (
long unsigned)nodeFromID,
1504 (
long unsigned)nodeToID);
1516 std::lock_guard<std::mutex> lock(m_map_cs);
1517 std::vector<std::string>
s;
1518 m_map.dumpAsText(
s);
1523 "%s/HMAP_txt/HMAP_%05i_mid.txt", m_options.LOG_OUTPUT_DIR.c_str(),
1528 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_mid.txt\n", DEBUG_STEP);
1540 for (
auto pNei1 = LMH->m_neighbors.begin();
1541 pNei1 != LMH->m_neighbors.end();)
1543 if (*pNei1 != curAreaID)
1547 std::lock_guard<std::mutex> lock(m_map_cs);
1548 m_map.findArcsOfTypeBetweenNodes(
1549 *pNei1, curAreaID, LMH->m_ID,
"RelativePose", lstArcs);
1551 if (lstArcs.empty())
1555 "[LSLAM_proc_msg_AA] Getting area '%u' out of LMH\n",
1556 static_cast<unsigned>(*pNei1));
1564 double ESS_bef = LMH->ESS();
1565 LMH->removeAreaFromLMH(
id);
1566 double ESS_aft = LMH->ESS();
1569 "[LSLAM_proc_msg_AA] ESS: %f -> %f\n", ESS_bef, ESS_aft);
1592 std::lock_guard<std::mutex> lock(m_map_cs);
1595 LMH->m_nodeIDmemberships[LMH->m_currentRobotPose];
1596 currentArea = m_map.getNodeByID(curAreaID);
1598 TPoseID refPoseCurArea_accordingAnnot;
1599 currentArea->m_annotations.getElemental(
1604 currentArea->getArcs(arcsToCurArea,
"RelativePose", LMH->m_ID);
1605 for (
auto&
a : arcsToCurArea)
1609 arc->getNodeFrom() == curAreaID ? arc->getNodeTo()
1610 : arc->getNodeFrom();
1613 if (LMH->m_neighbors.find(otherAreaID) == LMH->m_neighbors.end())
1617 "[LSLAM_proc_msg_AA] Bringing in LMH area %i\n",
1635 pdf->getMean(Delta_c2a);
1638 TPoseID refPoseIDAtOtherArea, refPoseIDAtCurArea;
1640 if (arc->getNodeTo() == curAreaID)
1645 "[LSLAM_proc_msg_AA] Arc is inverted: " 1646 "(%.03f,%.03f,%.03fdeg) -> ",
1647 Delta_c2a.x(), Delta_c2a.y(),
RAD2DEG(Delta_c2a.yaw()));
1649 Delta_c2a =
CPose3D(0, 0, 0) - Delta_c2a;
1653 Delta_c2a.x(), Delta_c2a.y(),
RAD2DEG(Delta_c2a.yaw()));
1655 arc->m_annotations.getElemental(
1658 arc->m_annotations.getElemental(
1665 arc->m_annotations.getElemental(
1668 arc->m_annotations.getElemental(
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()));
1681 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG) 1683 TPoseID refPoseOtherArea_accordingAnnot;
1684 area->m_annotations.getElemental(
1686 refPoseOtherArea_accordingAnnot, LMH->m_ID,
true);
1688 refPoseIDAtOtherArea ==
1689 refPoseOtherArea_accordingAnnot);
1692 refPoseIDAtCurArea == refPoseCurArea_accordingAnnot);
1701 lstNewPoseIDs.reserve(pg->size());
1707 lstNewPoseIDs.push_back(poseID);
1712 LMH->m_particles.size());
1714 CPose3DPDFParticles::CParticleList::const_iterator itSrc;
1715 CLocalMetricHypothesis::CParticleList::iterator itTrg;
1718 itTrg = LMH->m_particles.begin();
1719 itTrg != LMH->m_particles.end(); itSrc++, itTrg++)
1723 itTrg->d->robotPoses[poseID] =
1724 itTrg->d->robotPoses[refPoseIDAtCurArea] +
1725 Delta_c2a +
CPose3D(itSrc->d);
1729 LMH->m_nodeIDmemberships[poseID] = otherAreaID;
1732 LMH->m_SFs[poseID] = poseInfo.
sf;
1735 LMH->m_neighbors.
insert(otherAreaID);
1742 LMH->m_posesPendingAddPartitioner.insert(
1743 LMH->m_posesPendingAddPartitioner.end(),
1744 lstNewPoseIDs.begin(), lstNewPoseIDs.end());
1748 areasDelayedMetricMapsInsertion.insert(otherAreaID);
1756 std::vector<std::string>
s;
1761 "%s/HMAP_txt/HMAP_%05i_LMH_mid.txt",
1762 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP));
1766 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_LMH_mid.txt\n", DEBUG_STEP);
1773 std::make_shared<opengl::CSetOfObjects>();
1774 maps3D->setName(
"metric-maps");
1775 LMH->getMostLikelyParticle()->d->metricMaps.getAs3DObject(maps3D);
1776 sceneLSLAM.
insert(maps3D);
1780 std::make_shared<opengl::CSetOfObjects>();
1781 LSLAM_3D->setName(
"LSLAM_3D");
1782 LMH->getAs3DScene(LSLAM_3D);
1783 sceneLSLAM.
insert(LSLAM_3D);
1787 string filLocalAreas =
format(
1788 "%s/HMAP_txt/HMAP_%05i_LMH_mid.3Dscene",
1789 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP);
1802 if (!currentArea->m_annotations.getElemental(
1806 if (new_poseID_origin != poseID_origin)
1810 LMH->changeCoordinateOrigin(new_poseID_origin);
1813 "[LSLAM_proc_msg_AA] LMH->changeCoordinateOrigin %lu->%lu took %f " 1815 poseID_origin, new_poseID_origin, tt.
Tac() * 1000);
1817 else if (areasDelayedMetricMapsInsertion.size())
1823 for (
unsigned long areaID : areasDelayedMetricMapsInsertion)
1826 for (
auto pn = LMH->m_nodeIDmemberships.begin();
1827 pn != LMH->m_nodeIDmemberships.end(); ++pn)
1829 if (pn->second == areaID)
1832 const TPoseID& poseToAdd = pn->first;
1834 LMH->m_SFs.find(poseToAdd)->second;
1837 for (
auto partIt = LMH->m_particles.begin();
1838 partIt != LMH->m_particles.end(); ++partIt)
1840 auto pose3D = partIt->d->robotPoses.find(poseToAdd);
1841 ASSERT_(pose3D != partIt->d->robotPoses.end());
1843 &partIt->d->metricMaps, &pose3D->second);
1851 "[LSLAM_proc_msg_AA] areasDelayedMetricMapsInsertion took %f ms\n",
1857 std::lock_guard<std::mutex> lock(m_map_cs);
1858 std::vector<std::string>
s;
1859 m_map.dumpAsText(
s);
1863 "%s/HMAP_txt/HMAP_%05i_after.txt", m_options.LOG_OUTPUT_DIR.c_str(),
1868 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_after.txt\n", DEBUG_STEP);
1872 std::vector<std::string>
s;
1877 "%s/HMAP_txt/HMAP_%05i_LMH_after.txt",
1878 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP));
1882 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_LMH_after.txt\n", DEBUG_STEP);
1887 "[LSLAM_proc_msg_AA] Msg from AA took %f ms ]\n",
1888 tictac.Tac() * 1000);
1904 "[LSLAM_proc_msg_TBI] Beginning of Msg from TBI processing... " 1909 std::map<CHMHMapNode::TNodeID, CHMHMapNode::TNodeID> alreadyClosedLoops;
1916 "[LSLAM_proc_msg_TBI] Processing TLC of areas: %u <-> %u... \n",
1917 (
unsigned)myMsg.
cur_area, (
unsigned)candidate->first);
1921 if (alreadyClosedLoops.find(myMsg.
cur_area) != alreadyClosedLoops.end())
1923 currentArea = alreadyClosedLoops[myMsg.
cur_area];
1924 cout <<
"[LSLAM_proc_msg_TBI] Using " << myMsg.
cur_area <<
" -> " 1925 << currentArea <<
" due to area being already merged." 1932 m_map.computeCoordinatesTransformationBetweenNodes(
1935 candidate->first, pdfPartsHMap, myMsg.
hypothesisID, 100, 0.10f,
1940 pdfDeltaMap.
copyFrom(pdfPartsHMap);
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;
1961 ASSERT_(!candidate->second.delta_new_cur.empty());
1962 const double chi2_thres =
1965 map<double, CPose3DPDFGaussian>
1969 for (
const auto& itSOG : candidate->second.delta_new_cur)
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;
1980 const double mahaDist2 =
1982 cout <<
"[LSLAM_proc_msg_TBI] maha_dist = " << mahaDist2 << endl;
1984 if (mahaDist2 < chi2_thres)
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)= " 1995 if (!lstModesAndCompats.empty())
1998 lstModesAndCompats.rbegin()->second;
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);
2024 "[LSLAM_proc_msg_TBI] TLC of areas %u <-> %u - DONE in %.03f " 2026 (
unsigned)currentArea, (
unsigned)candidate->first,
2027 1e3 * tictac.
Tac());
2030 alreadyClosedLoops[myMsg.
cur_area] = candidate->first;
2038 "[LSLAM_proc_msg_TBI] Msg from TBI took %f ms ]\n",
2039 tictac.Tac() * 1000);
A namespace of pseudo-random numbers generators of diferent distributions.
double Tac() noexcept
Stops the stopwatch.
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)
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.
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
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
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.
#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::map< TPoseID, mrpt::poses::CPose3D > TMapPoseID2Pose3D
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.
double x() const
Common members of all points & poses classes.
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...
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.
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
This class is a "CSerializable" wrapper for "CMatrixFloat".
const_iterator begin() const
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).
#define MRPT_LOG_ERROR(_STRING)
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt::obs::CSensoryFrame sf
The observations.
std::string inMatlabFormat(const std::size_t decimal_digits=6) const
Exports the matrix as a string compatible with Matlab/Octave.
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.
#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
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.