47 void CHMTSLAM::thread_LSLAM()
51 unsigned int nIter = 0;
64 "[thread_LSLAM] Thread started ID=" << std::this_thread::get_id());
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);
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();
179 if (!it->second.m_areasPendingTBI.empty())
182 it->second.m_areasPendingTBI.begin();
183 areaID != it->second.m_areasPendingTBI.end();
193 CHMTSLAM::TBI_main_method(
194 &it->second, *areaID);
198 "[TBI] Took %.03fms " 200 1000 * tictac2.
Tac());
211 it->second.m_areasPendingTBI.clear();
235 std::this_thread::sleep_for(5ms);
244 catch (
const std::exception& e)
269 void CHMTSLAM::LSLAM_process_message([[maybe_unused]]
const CMessage& msg)
306 "[LSLAM_proc_msg_AA] Beginning of Msg from AA processing... " 311 ASSERT_(itLMH != m_LMHs.end());
317 for (
const auto& partition : myMsg.
partitions)
318 for (
auto itPose = partition.begin(); itPose != partition.end();
320 if (LMH->m_SFs.find(*itPose) == LMH->m_SFs.end())
322 "PoseID %i in AA's partition but not in LMH.\n",
326 for (
auto itA = LMH->m_nodeIDmemberships.begin();
327 itA != LMH->m_nodeIDmemberships.end(); ++itA)
329 if (LMH->m_currentRobotPose != itA->first)
336 for (
unsigned long itPose : *it)
337 if (itA->first == itPose)
344 "LMH's pose %i not found in AA's partitions.",
351 TNodeIDSet neighbors_before(LMH->m_neighbors);
356 std::lock_guard<std::mutex> lock(m_map_cs);
358 auto itCur = LMH->m_nodeIDmemberships.find(LMH->m_currentRobotPose);
359 ASSERT_(itCur != LMH->m_nodeIDmemberships.end());
361 if (!m_map.getNodeByID(itCur->second)
362 ->m_annotations.getElemental(
375 map<unsigned int, map<CHMHMapNode::TNodeID, unsigned int>> votes;
378 static int DEBUG_STEP = 0;
386 DEBUG_STEP = DEBUG_STEP + 0;
390 std::lock_guard<std::mutex> lock(m_map_cs);
391 std::vector<std::string> s;
396 "%s/HMAP_txt/HMAP_%05i_before.txt",
397 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP));
401 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_before.txt\n", DEBUG_STEP);
405 vector<TPoseIDList>::const_iterator it;
408 for (
unsigned long itPose : *it)
410 auto itP = LMH->m_nodeIDmemberships.find(itPose);
411 ASSERT_(itP != LMH->m_nodeIDmemberships.end());
413 votes[i][itP->second]++;
418 vector<CHMHMapNode::TNodeID> partIdx2Areas(
421 map<CHMHMapNode::TNodeID, pair<size_t, unsigned int>>
427 for (
size_t k = 0; k < myMsg.
partitions.size(); k++)
429 if (votes[k].
size() == 1)
432 if (votes[k].
begin()->second >
433 mostVotedFrom[votes[k].begin()->first].second)
435 mostVotedFrom[votes[k].begin()->first].first = k;
436 mostVotedFrom[votes[k].begin()->first].second =
437 votes[k].begin()->second;
445 for (
auto& v : mostVotedFrom)
446 v.second.second = std::numeric_limits<unsigned int>::max();
450 for (
size_t k = 0; k < myMsg.
partitions.size(); k++)
452 for (
auto it = votes[k].
begin(); it != votes[k].end(); ++it)
457 map<CHMHMapNode::TNodeID, pair<size_t, unsigned int>>::iterator
459 mostVotesIt = mostVotedFrom.find(it->first);
460 if (mostVotesIt == mostVotedFrom.end())
463 mostVotedFrom[it->first].first = k;
464 mostVotedFrom[it->first].second = it->second;
469 if (it->second > mostVotesIt->second.second)
471 mostVotesIt->second.first = k;
472 mostVotesIt->second.second = it->second;
479 for (
auto& it : mostVotedFrom) partIdx2Areas[it.second.first] = it.first;
482 for (i = 0; i < partIdx2Areas.size(); i++)
487 std::lock_guard<std::mutex> lock(m_map_cs);
492 newArea->m_hypotheses.insert(LMH->m_ID);
493 newArea->m_nodeType =
"Area";
494 newArea->m_label = generateUniqueAreaLabel();
497 CMultiMetricMap::Create(m_options.defaultMapsInitializers);
498 newArea->m_annotations.setMemoryReference(
501 auto emptyPoseGraph = CRobotPosesGraph::Create();
502 newArea->m_annotations.setMemoryReference(
506 partIdx2Areas[i] = newArea->getID();
512 for (
unsigned idx = 0; idx < partIdx2Areas.size(); idx++)
515 << idx <<
" -> AREA_ID " << partIdx2Areas[idx] <<
" ('" 516 << m_map.getNodeByID(partIdx2Areas[idx])->m_label <<
"')\n");
523 LMH->m_neighbors.clear();
524 for (i = 0; i < partIdx2Areas.size(); i++)
529 LMH->m_neighbors.insert(nodeId);
534 LMH->m_nodeIDmemberships[it] =
542 LMH->getMeans(lstPoses);
545 const CPose3D* curPoseMean = &lstPoses[LMH->m_currentRobotPose];
547 for (
auto it = lstPoses.begin(); it != lstPoses.end(); ++it)
550 LMH->m_currentRobotPose)
555 closestPose = it->first;
564 LMH->m_nodeIDmemberships[LMH->m_currentRobotPose];
567 LMH->m_nodeIDmemberships[LMH->m_currentRobotPose] =
568 LMH->m_nodeIDmemberships[closestPose];
572 LMH->m_nodeIDmemberships[LMH->m_currentRobotPose];
574 if (curAreaID != oldAreaID)
577 "[LSLAM_proc_msg_AA] Current area has changed: %i -> %i\n",
578 (
int)oldAreaID, (
int)curAreaID);
583 for (
auto pBef = neighbors_before.begin(); pBef != neighbors_before.end();
586 if (LMH->m_neighbors.find(*pBef) == LMH->m_neighbors.end())
592 "[LSLAM_proc_msg_AA] Old neighbors: ");
593 for (
unsigned long it : neighbors_before)
600 "[LSLAM_proc_msg_AA] Cur. neighbors: ");
601 for (
unsigned long m_neighbor : LMH->m_neighbors)
607 std::lock_guard<std::mutex> lock(m_map_cs);
617 "[LSLAM_proc_msg_AA] Area %i has been removed from the " 618 "neighbors & no longer exists in the HMAP.\n",
625 "[LSLAM_proc_msg_AA] Deleting area %i\n",
637 using TListNodesArcs = map<CHMHMapNode::Ptr, CHMHMapArc::Ptr>;
638 TListNodesArcs lstWithinLMH;
640 for (
auto a = arcs.begin(); a != arcs.end(); ++a)
644 if ((*a)->getNodeFrom() == *pBef)
646 nodeB = m_map.getNodeByID((*a)->getNodeTo());
650 nodeB = m_map.getNodeByID((*a)->getNodeFrom());
653 bool inNeib = LMH->m_neighbors.find(nodeB->getID()) !=
654 LMH->m_neighbors.end();
655 bool inBefNeib = neighbors_before.find(nodeB->getID()) !=
656 neighbors_before.end();
658 if (inNeib && inBefNeib)
659 lstWithinLMH[nodeB] = *a;
664 for (
auto a = arcs.begin(); a != arcs.end(); ++a)
671 if (arc->getNodeFrom() == *pBef)
673 nodeB = m_map.getNodeByID((*a)->getNodeTo());
678 nodeB = m_map.getNodeByID((*a)->getNodeFrom());
682 bool inNeib = LMH->m_neighbors.find(nodeB->getID()) !=
683 LMH->m_neighbors.end();
684 bool inBefNeib = neighbors_before.find(nodeB->getID()) !=
685 neighbors_before.end();
687 if (inNeib && inBefNeib)
701 for (
auto& na : lstWithinLMH)
736 arc->m_annotations.getElemental(
738 refPoseAt_b, LMH->m_ID,
true);
739 arc->m_annotations.getElemental(
741 refPoseAt_a, LMH->m_ID,
true);
745 pdf->inverse(Delta_b_a);
748 arc->m_annotations.getElemental(
750 refPoseAt_b, LMH->m_ID,
true);
751 arc->m_annotations.getElemental(
753 refPoseAt_a, LMH->m_ID,
true);
757 nodeB->m_annotations.getElemental(
759 node_refPoseAt_b, LMH->m_ID,
true);
760 ASSERT_(node_refPoseAt_b == refPoseAt_b);
763 node->m_annotations.getElemental(
765 node_refPoseAt_a, LMH->m_ID,
true);
766 ASSERT_(node_refPoseAt_a == refPoseAt_a);
774 arc_c_a->m_annotations
779 if (arc_c_a->getNodeTo() == node_c->getID())
784 arc_c_a->m_annotations.getElemental(
786 refPoseAt_a, LMH->m_ID,
true);
787 arc_c_a->m_annotations.getElemental(
789 refPoseAt_c, LMH->m_ID,
true);
793 pdf->inverse(Delta_a_c);
796 arc_c_a->m_annotations.getElemental(
798 refPoseAt_a, LMH->m_ID,
true);
799 arc_c_a->m_annotations.getElemental(
801 refPoseAt_c, LMH->m_ID,
true);
805 node_c->m_annotations.getElemental(
807 node_refPoseAt_c, LMH->m_ID,
true);
808 ASSERT_(node_refPoseAt_c == refPoseAt_c);
811 node->m_annotations.getElemental(
813 node_refPoseAt_a, LMH->m_ID,
true);
814 ASSERT_(node_refPoseAt_a == refPoseAt_a);
822 Delta_b_c.
cov(0, 0) = Delta_b_c.
cov(1, 1) =
827 "b_a: " << Delta_b_a.
mean << endl
828 <<
"a_c: " << Delta_a_c.
mean << endl
829 <<
"b_a + a_c: " << Delta_b_c.
mean 837 bool arcDeltaIsInverted;
839 m_map.findArcOfTypeBetweenNodes(
843 "RelativePose", arcDeltaIsInverted);
848 newArc = std::make_shared<CHMHMapArc>(
854 newArc->m_arcType =
"RelativePose";
855 arcDeltaIsInverted =
false;
858 if (!arcDeltaIsInverted)
860 newArc->m_annotations.set(
862 std::make_shared<CPose3DPDFGaussian>(
866 "[LSLAM_proc_msg_AA] Setting arc " 867 << nodeB->getID() <<
" -> " 868 << node_c->getID() <<
" : " 869 << Delta_b_c.
mean <<
" cov = " 871 newArc->m_annotations.setElemental(
873 refPoseAt_b, LMH->m_ID);
874 newArc->m_annotations.setElemental(
876 refPoseAt_c, LMH->m_ID);
881 Delta_b_c.
inverse(Delta_b_c_inv);
884 "[LSLAM_proc_msg_AA] Setting arc " 885 << nodeB->getID() <<
" <- " 886 << node_c->getID() <<
" : " 887 << Delta_b_c_inv.
mean <<
" cov = " 890 newArc->m_annotations.set(
892 std::make_shared<CPose3DPDFGaussian>(
895 newArc->m_annotations.setElemental(
897 refPoseAt_c, LMH->m_ID);
898 newArc->m_annotations.setElemental(
900 refPoseAt_b, LMH->m_ID);
905 arc->m_annotations.removeAll(LMH->m_ID);
907 if (!arc->m_annotations.size())
920 for (
auto& arc : al) arc.reset();
936 if (curAreaID != oldAreaID)
938 LMH->m_areasPendingTBI.insert(curAreaID);
941 "[LSLAM_proc_msg_AA] Current area changed: enqueing area %i for " 947 static size_t cntAddTBI = 0;
951 LMH->m_areasPendingTBI.insert(curAreaID);
954 "[LSLAM_proc_msg_AA] Current area %i enqued for TBI (routine " 977 if (partIdx2Areas.size() > 1)
979 std::lock_guard<std::mutex> lock(m_map_cs);
982 set<CHMHMapNode::TNodeID>
986 map<CHMHMapNode::TNodeID, pair<CHMHMapNode::TNodeID, float>>
987 lstClosestDoubtfulNeigbors;
989 for (
size_t idx_area_a = 0; idx_area_a < partIdx2Areas.size();
1008 if (!area_a->m_annotations.getElemental(
1012 poseID_trg = myMsg.
partitions[idx_area_a][0];
1014 area_a->m_annotations.setElemental(
1018 "[LSLAM_proc_msg_AA] Changing reference poseID of area " 1019 "'%i' to pose '%i'\n",
1020 (
int)area_a_ID, (
int)poseID_trg);
1036 TPoseID poseID_trg_old = poseID_trg;
1037 for (
auto p = myMsg.
partitions[idx_area_a].begin();
1038 !found && p != myMsg.
partitions[idx_area_a].end(); ++p)
1039 if (poseID_trg == *p)
1049 poseID_trg = myMsg.
partitions[idx_area_a][0];
1050 area_a->m_annotations.setElemental(
1055 "[LSLAM_proc_msg_AA] Changing reference poseID of area " 1056 "'%i' to pose '%i'\n",
1057 (
int)area_a_ID, (
int)poseID_trg);
1067 area_a->getArcs(arcs);
1068 for (
auto a = arcs.begin(); a != arcs.end(); ++a)
1082 if (nodeFrom == area_a_ID)
1085 if (LMH->m_neighbors.find(nodeTo) ==
1086 LMH->m_neighbors.end())
1090 LMH->getRelativePose(
1091 poseID_trg, poseID_trg_old, Anew_old_parts);
1098 theArc->m_annotations
1103 newDelta = Anew_old + *oldDelta;
1106 newDelta.
cov(0, 0) = newDelta.
cov(1, 1) =
1111 "[LSLAM_proc_msg_AA] Updating arc " 1112 << nodeFrom <<
" -> " << nodeTo
1113 <<
" OLD: " << oldDelta->mean <<
" cov = " 1114 << oldDelta->cov.inMatlabFormat() << endl);
1116 "[LSLAM_proc_msg_AA] Updating arc " 1117 << nodeFrom <<
" -> " << nodeTo
1118 <<
" NEW: " << newDelta.
mean <<
" cov = " 1121 theArc->m_annotations.set(
1123 std::make_shared<CPose3DPDFGaussian>(
1126 theArc->m_annotations.setElemental(
1134 if (LMH->m_neighbors.find(nodeFrom) ==
1135 LMH->m_neighbors.end())
1139 LMH->getRelativePose(
1140 poseID_trg_old, poseID_trg, Aold_new_parts);
1146 theArc->m_annotations
1152 newDelta = *oldDelta + Aold_new;
1156 newDelta.cov(0, 0) = newDelta.cov(1, 1) =
1158 newDelta.cov(3, 3) =
square(1.0_deg);
1161 "[LSLAM_proc_msg_AA] Updating arc " 1162 << nodeFrom <<
" <- " << nodeTo
1163 <<
" OLD: " << oldDelta->mean <<
" cov = " 1164 << oldDelta->cov.inMatlabFormat() << endl);
1166 "[LSLAM_proc_msg_AA] Updating arc " 1167 << nodeFrom <<
" <- " << nodeTo
1168 <<
" NEW: " << newDelta.mean <<
" cov = " 1169 << newDelta.cov.inMatlabFormat() << endl);
1171 theArc->m_annotations.set(
1173 std::make_shared<CPose3DPDFGaussian>(
1176 theArc->m_annotations.setElemental(
1188 for (
size_t idx_area_b = 0; idx_area_b < myMsg.
partitions.size();
1191 if (idx_area_a == idx_area_b)
1195 double closestDistPoseSrc = 0;
1199 for (
auto itP0 = myMsg.
partitions[idx_area_a].begin();
1200 itP0 != myMsg.
partitions[idx_area_a].end(); itP0++)
1202 const CPose3D& pose_trg = lstPoses[*itP0];
1204 for (
unsigned long itP : myMsg.
partitions[idx_area_b])
1206 const CPose3D& otherPose = lstPoses[itP];
1208 if (dst < closestDistPoseSrc ||
1211 poseID_closests = itP;
1212 closestDistPoseSrc = dst;
1222 if (closestDistPoseSrc <
1223 5 * m_options.SLAM_MIN_DIST_BETWEEN_OBS)
1229 if (!area_b->m_annotations.getElemental(
1234 area_b->m_annotations.setElemental(
1237 poseID_src = poseID_closests;
1240 "[LSLAM_proc_msg_AA] Changing reference poseID of " 1241 "area '%i' to pose '%i' (creat. annot)\n",
1242 (
int)area_b_ID, (
int)poseID_closests);
1247 if (lstInternalArcsToCreate.end() ==
1248 lstInternalArcsToCreate.
find(
1250 lstInternalArcsToCreate.end() ==
1251 lstInternalArcsToCreate.
find(
1254 lstInternalArcsToCreate.
insert(
1256 areasWithLink.insert(area_a_ID);
1257 areasWithLink.insert(area_b_ID);
1262 if (lstClosestDoubtfulNeigbors.find(area_b_ID) ==
1263 lstClosestDoubtfulNeigbors.end() ||
1264 closestDistPoseSrc <
1265 lstClosestDoubtfulNeigbors[area_b_ID].second)
1267 lstClosestDoubtfulNeigbors[area_b_ID].first = area_a_ID;
1268 lstClosestDoubtfulNeigbors[area_b_ID].second =
1283 for (
size_t idx_area = 0; idx_area < myMsg.
partitions.size();
1287 if (areasWithLink.find(area_ID) == areasWithLink.end())
1290 if (lstClosestDoubtfulNeigbors.find(area_ID) !=
1291 lstClosestDoubtfulNeigbors.end())
1295 area_ID, lstClosestDoubtfulNeigbors[area_ID].first));
1298 areasWithLink.insert(area_ID);
1299 areasWithLink.insert(
1300 lstClosestDoubtfulNeigbors[area_ID].first);
1305 "Area %i seems unconnected??", (
int)area_ID);
1316 "[LSLAM_proc_msg_AA] lstInternalArcsToCreate contains %i " 1318 (
int)lstInternalArcsToCreate.size());
1319 for (
auto arcCreat = lstInternalArcsToCreate.begin();
1320 arcCreat != lstInternalArcsToCreate.end(); ++arcCreat)
1327 (
int)closestAreaID, (
int)newAreaID);
1340 std::lock_guard<std::mutex> lock(m_map_cs);
1343 for (
auto arcCreat = lstInternalArcsToCreate.begin();
1344 arcCreat != lstInternalArcsToCreate.end(); ++arcCreat)
1349 m_map.getNodeByID(area_a_ID)->m_annotations.getElemental(
1354 m_map.getNodeByID(area_b_ID)->m_annotations.getElemental(
1359 LMH->getRelativePose(
1360 area_a_poseID_src, area_b_poseID_trg, relPoseParts);
1364 relPoseGauss.
copyFrom(relPoseParts);
1367 relPoseGauss.
cov(0, 0) = relPoseGauss.
cov(1, 1) =
square(0.04);
1368 relPoseGauss.
cov(3, 3) =
square(1.0_deg);
1372 "[LSLAM_proc_msg_AA] Creating arc %i[ref:%i] -> %i[ref:%i] = " 1373 "(%.03f,%.03f,%.03fdeg)\n",
1374 (
int)area_a_ID, (
int)area_a_poseID_src, (
int)area_b_ID,
1375 (
int)area_b_poseID_trg, relPoseGauss.
mean.
x(),
1380 bool arcDeltaIsInverted;
1382 area_a_ID, area_b_ID, LMH->m_ID,
"RelativePose",
1383 arcDeltaIsInverted);
1388 newArc = std::make_shared<CHMHMapArc>(
1394 newArc->m_arcType =
"RelativePose";
1395 arcDeltaIsInverted =
false;
1399 if (!arcDeltaIsInverted)
1402 "[LSLAM_proc_msg_AA] Updating int. arc " 1403 << area_a_ID <<
" -> " << area_b_ID <<
" : " 1404 << relPoseGauss.
mean 1406 newArc->m_annotations.set(
1408 std::make_shared<CPose3DPDFGaussian>(relPoseGauss),
1410 newArc->m_annotations.setElemental(
1413 newArc->m_annotations.setElemental(
1420 relPoseGauss.
inverse(relPoseInv);
1423 "[LSLAM_proc_msg_AA] Updating int. arc " 1424 << area_a_ID <<
" <- " << area_b_ID <<
" : " 1427 newArc->m_annotations.set(
1429 std::make_shared<CPose3DPDFGaussian>(relPoseInv),
1432 newArc->m_annotations.setElemental(
1435 newArc->m_annotations.setElemental(
1450 std::lock_guard<std::mutex> lock(m_map_cs);
1452 for (
auto pNei = LMH->m_neighbors.begin();
1453 pNei != LMH->m_neighbors.end(); ++pNei)
1461 nodeFrom->getArcs(lstArcs,
"RelativePose", LMH->m_ID);
1466 for (
auto a = lstArcs.begin(); a != lstArcs.end(); ++a)
1469 (*a)->getNodeFrom() == nodeFromID ? (*a)->getNodeTo()
1470 : (*a)->getNodeFrom();
1472 if (LMH->m_neighbors.find(nodeToID) != LMH->m_neighbors.end())
1477 if (lstInternalArcsToCreate.end() ==
1478 lstInternalArcsToCreate.
find(
1480 lstInternalArcsToCreate.end() ==
1481 lstInternalArcsToCreate.
find(
1485 arc->m_annotations.remove(
1489 "[LSLAM_proc_msg_AA] Deleting annotation of arc: " 1491 (
long unsigned)nodeFromID, (
long unsigned)nodeToID);
1494 if (!arc->m_annotations.getAnyHypothesis(
1499 "[LSLAM_proc_msg_AA] Deleting empty arc: " 1501 (
long unsigned)nodeFromID,
1502 (
long unsigned)nodeToID);
1514 std::lock_guard<std::mutex> lock(m_map_cs);
1515 std::vector<std::string> s;
1516 m_map.dumpAsText(s);
1521 "%s/HMAP_txt/HMAP_%05i_mid.txt", m_options.LOG_OUTPUT_DIR.c_str(),
1526 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_mid.txt\n", DEBUG_STEP);
1538 for (
auto pNei1 = LMH->m_neighbors.begin();
1539 pNei1 != LMH->m_neighbors.end();)
1541 if (*pNei1 != curAreaID)
1545 std::lock_guard<std::mutex> lock(m_map_cs);
1546 m_map.findArcsOfTypeBetweenNodes(
1547 *pNei1, curAreaID, LMH->m_ID,
"RelativePose", lstArcs);
1549 if (lstArcs.empty())
1553 "[LSLAM_proc_msg_AA] Getting area '%u' out of LMH\n",
1554 static_cast<unsigned>(*pNei1));
1562 double ESS_bef = LMH->ESS();
1563 LMH->removeAreaFromLMH(
id);
1564 double ESS_aft = LMH->ESS();
1567 "[LSLAM_proc_msg_AA] ESS: %f -> %f\n", ESS_bef, ESS_aft);
1590 std::lock_guard<std::mutex> lock(m_map_cs);
1593 LMH->m_nodeIDmemberships[LMH->m_currentRobotPose];
1594 currentArea = m_map.getNodeByID(curAreaID);
1596 TPoseID refPoseCurArea_accordingAnnot;
1597 currentArea->m_annotations.getElemental(
1602 currentArea->getArcs(arcsToCurArea,
"RelativePose", LMH->m_ID);
1603 for (
auto& a : arcsToCurArea)
1607 arc->getNodeFrom() == curAreaID ? arc->getNodeTo()
1608 : arc->getNodeFrom();
1611 if (LMH->m_neighbors.find(otherAreaID) == LMH->m_neighbors.end())
1615 "[LSLAM_proc_msg_AA] Bringing in LMH area %i\n",
1633 pdf->getMean(Delta_c2a);
1636 TPoseID refPoseIDAtOtherArea, refPoseIDAtCurArea;
1638 if (arc->getNodeTo() == curAreaID)
1643 "[LSLAM_proc_msg_AA] Arc is inverted: " 1644 "(%.03f,%.03f,%.03fdeg) -> ",
1645 Delta_c2a.x(), Delta_c2a.y(),
RAD2DEG(Delta_c2a.yaw()));
1647 Delta_c2a =
CPose3D(0, 0, 0) - Delta_c2a;
1651 Delta_c2a.x(), Delta_c2a.y(),
RAD2DEG(Delta_c2a.yaw()));
1653 arc->m_annotations.getElemental(
1656 arc->m_annotations.getElemental(
1663 arc->m_annotations.getElemental(
1666 arc->m_annotations.getElemental(
1673 "[LSLAM_proc_msg_AA] Bringing in: refPoseCur=%i " 1674 "refPoseOther=%i -> Delta_c2a:(%.03f,%.03f,%.03fdeg)\n",
1675 (
int)refPoseIDAtCurArea, (
int)refPoseIDAtOtherArea,
1676 Delta_c2a.x(), Delta_c2a.y(),
RAD2DEG(Delta_c2a.yaw()));
1679 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG) 1681 TPoseID refPoseOtherArea_accordingAnnot;
1682 area->m_annotations.getElemental(
1684 refPoseOtherArea_accordingAnnot, LMH->m_ID,
true);
1686 refPoseIDAtOtherArea ==
1687 refPoseOtherArea_accordingAnnot);
1690 refPoseIDAtCurArea == refPoseCurArea_accordingAnnot);
1699 lstNewPoseIDs.reserve(pg->size());
1702 const TPoseID& poseID = p.first;
1705 lstNewPoseIDs.push_back(poseID);
1710 LMH->m_particles.size());
1712 CPose3DPDFParticles::CParticleList::const_iterator itSrc;
1713 CLocalMetricHypothesis::CParticleList::iterator itTrg;
1716 itTrg = LMH->m_particles.begin();
1717 itTrg != LMH->m_particles.end(); itSrc++, itTrg++)
1721 itTrg->d->robotPoses[poseID] =
1722 itTrg->d->robotPoses[refPoseIDAtCurArea] +
1723 Delta_c2a +
CPose3D(itSrc->d);
1727 LMH->m_nodeIDmemberships[poseID] = otherAreaID;
1730 LMH->m_SFs[poseID] = poseInfo.
sf;
1733 LMH->m_neighbors.
insert(otherAreaID);
1740 LMH->m_posesPendingAddPartitioner.insert(
1741 LMH->m_posesPendingAddPartitioner.end(),
1742 lstNewPoseIDs.begin(), lstNewPoseIDs.end());
1746 areasDelayedMetricMapsInsertion.insert(otherAreaID);
1754 std::vector<std::string> s;
1759 "%s/HMAP_txt/HMAP_%05i_LMH_mid.txt",
1760 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP));
1764 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_LMH_mid.txt\n", DEBUG_STEP);
1771 std::make_shared<opengl::CSetOfObjects>();
1772 maps3D->setName(
"metric-maps");
1773 LMH->getMostLikelyParticle()->d->metricMaps.getAs3DObject(maps3D);
1774 sceneLSLAM.
insert(maps3D);
1778 std::make_shared<opengl::CSetOfObjects>();
1779 LSLAM_3D->setName(
"LSLAM_3D");
1780 LMH->getAs3DScene(LSLAM_3D);
1781 sceneLSLAM.
insert(LSLAM_3D);
1785 string filLocalAreas =
format(
1786 "%s/HMAP_txt/HMAP_%05i_LMH_mid.3Dscene",
1787 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP);
1800 if (!currentArea->m_annotations.getElemental(
1804 if (new_poseID_origin != poseID_origin)
1808 LMH->changeCoordinateOrigin(new_poseID_origin);
1811 "[LSLAM_proc_msg_AA] LMH->changeCoordinateOrigin %lu->%lu took %f " 1813 poseID_origin, new_poseID_origin, tt.
Tac() * 1000);
1815 else if (areasDelayedMetricMapsInsertion.size())
1821 for (
unsigned long areaID : areasDelayedMetricMapsInsertion)
1824 for (
auto pn = LMH->m_nodeIDmemberships.begin();
1825 pn != LMH->m_nodeIDmemberships.end(); ++pn)
1827 if (pn->second == areaID)
1830 const TPoseID& poseToAdd = pn->first;
1832 LMH->m_SFs.find(poseToAdd)->second;
1835 for (
auto partIt = LMH->m_particles.begin();
1836 partIt != LMH->m_particles.end(); ++partIt)
1838 auto pose3D = partIt->d->robotPoses.find(poseToAdd);
1839 ASSERT_(pose3D != partIt->d->robotPoses.end());
1841 &partIt->d->metricMaps, &pose3D->second);
1849 "[LSLAM_proc_msg_AA] areasDelayedMetricMapsInsertion took %f ms\n",
1855 std::lock_guard<std::mutex> lock(m_map_cs);
1856 std::vector<std::string> s;
1857 m_map.dumpAsText(s);
1861 "%s/HMAP_txt/HMAP_%05i_after.txt", m_options.LOG_OUTPUT_DIR.c_str(),
1866 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_after.txt\n", DEBUG_STEP);
1870 std::vector<std::string> s;
1875 "%s/HMAP_txt/HMAP_%05i_LMH_after.txt",
1876 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP));
1880 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_LMH_after.txt\n", DEBUG_STEP);
1885 "[LSLAM_proc_msg_AA] Msg from AA took %f ms ]\n",
1886 tictac.Tac() * 1000);
1902 "[LSLAM_proc_msg_TBI] Beginning of Msg from TBI processing... " 1907 std::map<CHMHMapNode::TNodeID, CHMHMapNode::TNodeID> alreadyClosedLoops;
1914 "[LSLAM_proc_msg_TBI] Processing TLC of areas: %u <-> %u... \n",
1915 (
unsigned)myMsg.
cur_area, (
unsigned)candidate->first);
1919 if (alreadyClosedLoops.find(myMsg.
cur_area) != alreadyClosedLoops.end())
1921 currentArea = alreadyClosedLoops[myMsg.
cur_area];
1922 cout <<
"[LSLAM_proc_msg_TBI] Using " << myMsg.
cur_area <<
" -> " 1923 << currentArea <<
" due to area being already merged." 1930 m_map.computeCoordinatesTransformationBetweenNodes(
1933 candidate->first, pdfPartsHMap, myMsg.
hypothesisID, 100, 0.10f,
1938 pdfDeltaMap.
copyFrom(pdfPartsHMap);
1945 pdfDeltaMap.
cov(3, 3) +=
square(5.0_deg);
1946 pdfDeltaMap.
cov(4, 4) +=
square(5.0_deg);
1947 pdfDeltaMap.
cov(5, 5) +=
square(5.0_deg);
1949 cout <<
"[LSLAM_proc_msg_TBI] HMap_delta=" << pdfDeltaMap.
mean 1950 <<
" std_x=" << sqrt(pdfDeltaMap.
cov(0, 0))
1951 <<
" std_y=" << sqrt(pdfDeltaMap.
cov(1, 1)) << endl;
1959 ASSERT_(!candidate->second.delta_new_cur.empty());
1960 const double chi2_thres =
1963 map<double, CPose3DPDFGaussian>
1967 for (
const auto& itSOG : candidate->second.delta_new_cur)
1971 cout <<
"[LSLAM_proc_msg_TBI] TLC_delta=" << pdfDelta.
mean 1972 <<
" std_x=" << sqrt(pdfDelta.
cov(0, 0))
1973 <<
" std_y=" << sqrt(pdfDelta.
cov(1, 1))
1974 <<
" std_phi=" <<
RAD2DEG(sqrt(pdfDelta.
cov(3, 3))) << endl;
1978 const double mahaDist2 =
1980 cout <<
"[LSLAM_proc_msg_TBI] maha_dist = " << mahaDist2 << endl;
1982 if (mahaDist2 < chi2_thres)
1984 const double log_lik = itSOG.log_w - 0.5 * mahaDist2;
1985 lstModesAndCompats[log_lik] = itSOG.val;
1986 cout <<
"[LSLAM_proc_msg_TBI] Added to list of candidates: " 1987 "log(overall_lik)= " 1993 if (!lstModesAndCompats.empty())
1996 lstModesAndCompats.rbegin()->second;
2009 "[LSLAM_proc_msg_TBI] Accepting TLC of areas: %u <-> %u with " 2010 "an overall log(lik)=%f \n",
2011 (
unsigned)currentArea, (
unsigned)candidate->first,
2012 lstModesAndCompats.rbegin()->first);
2022 "[LSLAM_proc_msg_TBI] TLC of areas %u <-> %u - DONE in %.03f " 2024 (
unsigned)currentArea, (
unsigned)candidate->first,
2025 1e3 * tictac.
Tac());
2028 alreadyClosedLoops[myMsg.
cur_area] = candidate->first;
2036 "[LSLAM_proc_msg_TBI] Msg from TBI took %f ms ]\n",
2037 tictac.Tac() * 1000);
A namespace of pseudo-random numbers generators of diferent distributions.
int random_seed
0 means randomize, use any other value to have repetitive experiments.
double Tac() noexcept
Stops the stopwatch.
CLSLAMAlgorithmBase * m_LSLAM_method
An instance of a local SLAM method, to be applied to each LMH - initialized by "initializeEmptyMap" o...
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.
std::list< T >::iterator find(const T &i)
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
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)
int LOG_FREQUENCY
[LOGGING] One SLAM iteration out of "LOGGING_logFrequency", a log file will be generated.
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.
std::string std::string format(std::string_view fmt, ARGS &&... args)
size_t size(const MATRIXLIKE &m, const int dim)
CParticleList m_particles
The array of particles.
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
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.
void generateLogFiles(unsigned int nIteration)
Called from LSLAM thread when log files must be created.
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.
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.
double distanceEuclidean6D(const CPose3D &o) const
The euclidean distance between two poses taken as two 6-length vectors (angles in radians)...
mrpt::serialization::CSerializable::Ptr getNextObjectFromInputQueue()
Used from the LSLAM thread to retrieve the next object from the queue.
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.
std::atomic_bool m_terminationFlag_LSLAM
Threads termination flags:
#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::string LOG_OUTPUT_DIR
[LOGGING] If it is not an empty string (""), a directory with that name will be created and log files...
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...
std::mutex m_LMHs_cs
Critical section for accessing m_LMHs.
constexpr double DEG2RAD(const double x)
Degrees to radians.
CMessageQueue m_LSLAM_queue
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
std::map< THypothesisID, CLocalMetricHypothesis > m_LMHs
The list of LMHs at each instant.
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.
mrpt::hmtslam::CHMTSLAM::TOptions m_options
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
std::atomic_bool m_terminateThreads
Termination flag for signaling all threads to terminate.
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.
return_t square(const num_t x)
Inline function for the square of a number.
#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)
void LSLAM_process_message_from_TBI(const TMessageLSLAMfromTBI &myMsg)
No critical section locks are assumed at the entrance of this method.
constexpr double RAD2DEG(const double x)
Radians to degrees.
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.
void LSLAM_process_message(const mrpt::serialization::CMessage &msg)
Auxiliary method within thread_LSLAM.
#define NODE_ANNOTATION_POSES_GRAPH
virtual void processOneLMH(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection::Ptr &act, const mrpt::obs::CSensoryFrame::Ptr &sf)=0
Main entry point from HMT-SLAM: process some actions & observations.
#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
bool isInputQueueEmpty()
Returns true if the input queue is empty (Note that the queue must not be empty to the user to enqueu...
A class for storing a sequence of arcs (a path).
#define ARC_ANNOTATION_DELTA
void LSLAM_process_message_from_AA(const TMessageLSLAMfromAA &myMsg)
No critical section locks are assumed at the entrance of this method.
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
T * get()
Retrieve the next message in the queue, or nullptr if there is no message.