43 void CHMTSLAM::thread_LSLAM()
47 unsigned int nIter = 0;
50 if (
obj->m_options.random_seed)
60 mrpt::utils::LVL_DEBUG,
61 "[thread_LSLAM] Thread started (ID=0x%08lX)\n",
62 std::this_thread::get_id());
68 while (!
obj->m_terminateThreads)
70 if (
obj->m_options.random_seed)
78 recMsg =
obj->m_LSLAM_queue.get();
81 obj->LSLAM_process_message(*recMsg);
88 if (!
obj->isInputQueueEmpty())
90 if (
obj->m_options.random_seed)
95 obj->getNextObjectFromInputQueue();
102 if (nextObject->GetRuntimeClass() ==
104 actions = std::dynamic_pointer_cast<CActionCollection>(
109 std::dynamic_pointer_cast<CSensoryFrame>(nextObject);
112 "Element in the queue is neither CActionCollection nor " 125 std::lock_guard<std::mutex> LMHs_cs_lock(
obj->m_LMHs_cs);
127 for (it =
obj->m_LMHs.begin(); it !=
obj->m_LMHs.end();
130 std::lock_guard<std::mutex> LMH_individual_lock(
131 it->second.threadLocks.m_lock);
136 obj->m_LSLAM_method->processOneLMH(
138 actions, observations);
143 if (it->second.m_posesPendingAddPartitioner.size() >
150 unsigned nPosesToInsert =
151 it->second.m_posesPendingAddPartitioner.size();
153 CHMTSLAM::areaAbstraction(
155 it->second.m_posesPendingAddPartitioner);
158 mrpt::utils::LVL_DEBUG,
159 "[AreaAbstraction] Took %.03fms to insert %u " 161 1000 * tictac.
Tac(), nPosesToInsert);
164 it->second.m_posesPendingAddPartitioner.clear();
166 if (
obj->m_options.random_seed)
168 obj->m_options.random_seed);
175 obj->LSLAM_process_message_from_AA(*msgFromAA);
181 if (!it->second.m_areasPendingTBI.empty())
184 it->second.m_areasPendingTBI.begin();
185 areaID != it->second.m_areasPendingTBI.end();
190 if (
obj->m_options.random_seed)
192 obj->m_options.random_seed);
195 CHMTSLAM::TBI_main_method(
196 &it->second, *areaID);
199 mrpt::utils::LVL_DEBUG,
200 "[TBI] Took %.03fms " 202 1000 * tictac.
Tac());
208 obj->LSLAM_process_message_from_TBI(
213 it->second.m_areasPendingTBI.clear();
227 if (
obj->m_options.LOG_OUTPUT_DIR.size() &&
228 (nIter %
obj->m_options.LOG_FREQUENCY) == 0)
229 obj->generateLogFiles(nIter);
237 std::this_thread::sleep_for(5ms);
243 time_t timCreat, timExit;
249 mrpt::utils::LVL_DEBUG,
250 "[thread_LSLAM] Thread finished. CPU time used:%.06f secs \n",
252 obj->m_terminationFlag_LSLAM =
true;
254 catch (std::exception& e)
256 obj->m_terminationFlag_LSLAM =
true;
260 if (e.what())
obj->logFmt(mrpt::utils::LVL_DEBUG,
"%s", e.what());
263 obj->m_terminateThreads =
true;
267 obj->m_terminationFlag_LSLAM =
true;
270 mrpt::utils::LVL_DEBUG,
271 "\n---------------------- EXCEPTION CAUGHT! " 272 "---------------------\n");
274 mrpt::utils::LVL_DEBUG,
275 " In CHierarchicalMappingFramework::thread_LSLAM. Unexpected " 276 "runtime error!!\n");
281 obj->m_terminateThreads =
true;
288 void CHMTSLAM::LSLAM_process_message(
const CMessage& msg)
325 mrpt::utils::LVL_INFO,
326 "[LSLAM_proc_msg_AA] Beginning of Msg from AA processing... " 332 ASSERT_(itLMH != m_LMHs.end());
341 itPose != it->end(); ++itPose)
342 if (LMH->m_SFs.find(*itPose) == LMH->m_SFs.end())
344 "PoseID %i in AA's partition but not in LMH.\n",
349 LMH->m_nodeIDmemberships.begin();
350 itA != LMH->m_nodeIDmemberships.end(); ++itA)
352 if (LMH->m_currentRobotPose != itA->first)
361 itPose != it->end(); ++itPose)
362 if (itA->first == *itPose)
369 "LMH's pose %i not found in AA's partitions.",
376 TNodeIDSet neighbors_before(LMH->m_neighbors);
381 std::lock_guard<std::mutex> lock(m_map_cs);
384 LMH->m_nodeIDmemberships.find(LMH->m_currentRobotPose);
385 ASSERT_(itCur != LMH->m_nodeIDmemberships.end());
387 if (!m_map.getNodeByID(itCur->second)
388 ->m_annotations.getElemental(
401 map<unsigned int, map<CHMHMapNode::TNodeID, unsigned int>> votes;
404 static int DEBUG_STEP = 0;
407 mrpt::utils::LVL_INFO,
"[LSLAM_proc_msg_AA] DEBUG_STEP=%i\n",
412 DEBUG_STEP = DEBUG_STEP + 0;
416 std::lock_guard<std::mutex> lock(m_map_cs);
420 "%s/HMAP_txt/HMAP_%05i_before.txt",
421 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP));
423 mrpt::utils::LVL_INFO,
424 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_before.txt\n", DEBUG_STEP);
431 itPose != it->end(); ++itPose)
434 LMH->m_nodeIDmemberships.find(*itPose);
435 ASSERT_(itP != LMH->m_nodeIDmemberships.end());
437 votes[i][itP->second]++;
441 vector<CHMHMapNode::TNodeID> partIdx2Areas(
444 map<CHMHMapNode::TNodeID, pair<size_t, unsigned int>>
450 for (
size_t k = 0; k < myMsg.
partitions.size(); k++)
452 if (votes[k].
size() == 1)
455 if (votes[k].
begin()->second >
456 mostVotedFrom[votes[k].begin()->first].second)
458 mostVotedFrom[votes[k].begin()->first].first = k;
459 mostVotedFrom[votes[k].begin()->first].second =
460 votes[k].begin()->second;
469 mostVotedFrom.begin();
470 v != mostVotedFrom.end(); ++
v)
471 v->second.second = std::numeric_limits<unsigned int>::max();
475 for (
size_t k = 0; k < myMsg.
partitions.size(); k++)
479 it != votes[k].end(); ++it)
484 map<CHMHMapNode::TNodeID, pair<size_t, unsigned int>>
::iterator 486 mostVotesIt = mostVotedFrom.find(it->first);
487 if (mostVotesIt == mostVotedFrom.end())
490 mostVotedFrom[it->first].first = k;
491 mostVotedFrom[it->first].second = it->second;
496 if (it->second > mostVotesIt->second.second)
498 mostVotesIt->second.first = k;
499 mostVotesIt->second.second = it->second;
507 mostVotedFrom.begin();
508 it != mostVotedFrom.end(); ++it)
509 partIdx2Areas[it->second.first] = it->first;
512 for (i = 0; i < partIdx2Areas.size(); i++)
517 std::lock_guard<std::mutex> lock(m_map_cs);
520 mrpt::make_aligned_shared<CHMHMapNode>(&m_map);
523 newArea->m_hypotheses.insert(LMH->m_ID);
524 newArea->m_nodeType.setType(
"Area");
525 newArea->m_label = generateUniqueAreaLabel();
529 newArea->m_annotations.setMemoryReference(
533 mrpt::make_aligned_shared<CRobotPosesGraph>();
534 newArea->m_annotations.setMemoryReference(
538 partIdx2Areas[i] = newArea->getID();
543 logFmt(mrpt::utils::LVL_INFO,
"[LSLAM_proc_msg_AA] partIdx2Areas:\n");
544 for (
size_t i = 0; i < partIdx2Areas.size(); i++)
546 mrpt::utils::LVL_INFO,
547 " Partition #%i -> AREA_ID %i ('%s')\n", (
int)i,
548 (int)partIdx2Areas[i],
549 m_map.getNodeByID(partIdx2Areas[i])->m_label.c_str());
556 LMH->m_neighbors.clear();
557 for (i = 0; i < partIdx2Areas.size(); i++)
562 LMH->m_neighbors.insert(nodeId);
568 LMH->m_nodeIDmemberships[*it] =
576 LMH->getMeans(lstPoses);
579 const CPose3D* curPoseMean = &lstPoses[LMH->m_currentRobotPose];
582 it != lstPoses.end(); ++it)
585 LMH->m_currentRobotPose)
590 closestPose = it->first;
599 LMH->m_nodeIDmemberships[LMH->m_currentRobotPose];
602 LMH->m_nodeIDmemberships[LMH->m_currentRobotPose] =
603 LMH->m_nodeIDmemberships[closestPose];
607 LMH->m_nodeIDmemberships[LMH->m_currentRobotPose];
609 if (curAreaID != oldAreaID)
611 mrpt::utils::LVL_INFO,
612 "[LSLAM_proc_msg_AA] Current area has changed: %i -> %i\n",
613 (
int)oldAreaID, (
int)curAreaID);
619 pBef != neighbors_before.end(); ++pBef)
621 if (LMH->m_neighbors.find(*pBef) == LMH->m_neighbors.end())
626 mrpt::utils::LVL_INFO,
627 "[LSLAM_proc_msg_AA] Old neighbors: ");
629 it != neighbors_before.end(); ++it)
630 logFmt(mrpt::utils::LVL_INFO,
"%i ", (
int)*it);
631 logFmt(mrpt::utils::LVL_INFO,
"\n");
635 mrpt::utils::LVL_INFO,
636 "[LSLAM_proc_msg_AA] Cur. neighbors: ");
638 it != LMH->m_neighbors.end(); ++it)
639 logFmt(mrpt::utils::LVL_INFO,
"%i ", (
int)*it);
640 logFmt(mrpt::utils::LVL_INFO,
"\n");
644 std::lock_guard<std::mutex> lock(m_map_cs);
653 mrpt::utils::LVL_INFO,
654 "[LSLAM_proc_msg_AA] Area %i has been removed from the " 655 "neighbors & no longer exists in the HMAP.\n",
661 mrpt::utils::LVL_INFO,
662 "[LSLAM_proc_msg_AA] Deleting area %i\n",
674 typedef map<CHMHMapNode::Ptr, CHMHMapArc::Ptr> TListNodesArcs;
675 TListNodesArcs lstWithinLMH;
682 if ((*a)->getNodeFrom() == *pBef)
684 nodeB = m_map.getNodeByID((*a)->getNodeTo());
688 nodeB = m_map.getNodeByID((*a)->getNodeFrom());
691 bool inNeib = LMH->m_neighbors.
find(nodeB->getID()) !=
692 LMH->m_neighbors.end();
693 bool inBefNeib = neighbors_before.find(nodeB->getID()) !=
694 neighbors_before.end();
696 if (inNeib && inBefNeib)
697 lstWithinLMH[nodeB] = *
a;
710 if (arc->getNodeFrom() == *pBef)
712 nodeB = m_map.getNodeByID((*a)->getNodeTo());
717 nodeB = m_map.getNodeByID((*a)->getNodeFrom());
721 bool inNeib = LMH->m_neighbors.find(nodeB->getID()) !=
722 LMH->m_neighbors.end();
723 bool inBefNeib = neighbors_before.find(nodeB->getID()) !=
724 neighbors_before.end();
726 if (inNeib && inBefNeib)
741 na != lstWithinLMH.end(); ++na)
776 arc->m_annotations.getElemental(
778 refPoseAt_b, LMH->m_ID,
true);
779 arc->m_annotations.getElemental(
781 refPoseAt_a, LMH->m_ID,
true);
785 pdf->inverse(Delta_b_a);
788 arc->m_annotations.getElemental(
790 refPoseAt_b, LMH->m_ID,
true);
791 arc->m_annotations.getElemental(
793 refPoseAt_a, LMH->m_ID,
true);
797 nodeB->m_annotations.getElemental(
799 node_refPoseAt_b, LMH->m_ID,
true);
800 ASSERT_(node_refPoseAt_b == refPoseAt_b);
803 node->m_annotations.getElemental(
805 node_refPoseAt_a, LMH->m_ID,
true);
806 ASSERT_(node_refPoseAt_a == refPoseAt_a);
814 arc_c_a->m_annotations
819 if (arc_c_a->getNodeTo() == node_c->getID())
824 arc_c_a->m_annotations.getElemental(
826 refPoseAt_a, LMH->m_ID,
true);
827 arc_c_a->m_annotations.getElemental(
829 refPoseAt_c, LMH->m_ID,
true);
833 pdf->inverse(Delta_a_c);
836 arc_c_a->m_annotations.getElemental(
838 refPoseAt_a, LMH->m_ID,
true);
839 arc_c_a->m_annotations.getElemental(
841 refPoseAt_c, LMH->m_ID,
true);
845 node_c->m_annotations.getElemental(
847 node_refPoseAt_c, LMH->m_ID,
true);
848 ASSERT_(node_refPoseAt_c == refPoseAt_c);
851 node->m_annotations.getElemental(
853 node_refPoseAt_a, LMH->m_ID,
true);
854 ASSERT_(node_refPoseAt_a == refPoseAt_a);
862 Delta_b_c.
cov(0, 0) = Delta_b_c.
cov(1, 1) =
867 "b_a: " << Delta_b_a.
mean << endl
868 <<
"a_c: " << Delta_a_c.
mean << endl
869 <<
"b_a + a_c: " << Delta_b_c.
mean 877 bool arcDeltaIsInverted;
879 m_map.findArcOfTypeBetweenNodes(
883 "RelativePose", arcDeltaIsInverted);
888 newArc = mrpt::make_aligned_shared<CHMHMapArc>(
894 newArc->m_arcType.setType(
"RelativePose");
895 arcDeltaIsInverted =
false;
898 if (!arcDeltaIsInverted)
900 newArc->m_annotations.set(
906 "[LSLAM_proc_msg_AA] Setting arc " 907 << nodeB->getID() <<
" -> " 908 << node_c->getID() <<
" : " 909 << Delta_b_c.
mean <<
" cov = " 910 << Delta_b_c.
cov.inMatlabFormat() << endl);
911 newArc->m_annotations.setElemental(
913 refPoseAt_b, LMH->m_ID);
914 newArc->m_annotations.setElemental(
916 refPoseAt_c, LMH->m_ID);
921 Delta_b_c.
inverse(Delta_b_c_inv);
924 "[LSLAM_proc_msg_AA] Setting arc " 925 << nodeB->getID() <<
" <- " 926 << node_c->getID() <<
" : " 927 << Delta_b_c_inv.
mean <<
" cov = " 928 << Delta_b_c_inv.
cov.inMatlabFormat()
930 newArc->m_annotations.set(
935 newArc->m_annotations.setElemental(
937 refPoseAt_c, LMH->m_ID);
938 newArc->m_annotations.setElemental(
940 refPoseAt_b, LMH->m_ID);
945 arc->m_annotations.removeAll(LMH->m_ID);
947 if (!arc->m_annotations.size())
978 if (curAreaID != oldAreaID)
980 LMH->m_areasPendingTBI.
insert(curAreaID);
982 mrpt::utils::LVL_INFO,
983 "[LSLAM_proc_msg_AA] Current area changed: enqueing area %i for " 989 static size_t cntAddTBI = 0;
993 LMH->m_areasPendingTBI.insert(curAreaID);
995 mrpt::utils::LVL_INFO,
996 "[LSLAM_proc_msg_AA] Current area %i enqued for TBI (routine " 1019 if (partIdx2Areas.size() > 1)
1021 std::lock_guard<std::mutex> lock(m_map_cs);
1024 set<CHMHMapNode::TNodeID>
1028 map<CHMHMapNode::TNodeID, pair<CHMHMapNode::TNodeID, float>>
1029 lstClosestDoubtfulNeigbors;
1031 for (
size_t idx_area_a = 0; idx_area_a < partIdx2Areas.size();
1050 if (!area_a->m_annotations.getElemental(
1054 poseID_trg = myMsg.
partitions[idx_area_a][0];
1056 area_a->m_annotations.setElemental(
1059 mrpt::utils::LVL_INFO,
1060 "[LSLAM_proc_msg_AA] Changing reference poseID of area " 1061 "'%i' to pose '%i'\n",
1062 (
int)area_a_ID, (
int)poseID_trg);
1078 TPoseID poseID_trg_old = poseID_trg;
1081 !found &&
p != myMsg.
partitions[idx_area_a].end(); ++
p)
1082 if (poseID_trg == *
p)
1092 poseID_trg = myMsg.
partitions[idx_area_a][0];
1093 area_a->m_annotations.setElemental(
1097 mrpt::utils::LVL_INFO,
1098 "[LSLAM_proc_msg_AA] Changing reference poseID of area " 1099 "'%i' to pose '%i'\n",
1100 (
int)area_a_ID, (
int)poseID_trg);
1110 area_a->getArcs(arcs);
1112 a != arcs.end(); ++
a)
1126 if (nodeFrom == area_a_ID)
1129 if (LMH->m_neighbors.find(nodeTo) ==
1130 LMH->m_neighbors.end())
1134 LMH->getRelativePose(
1135 poseID_trg, poseID_trg_old, Anew_old_parts);
1142 theArc->m_annotations
1147 newDelta = Anew_old + *oldDelta;
1150 newDelta.
cov(0, 0) = newDelta.
cov(1, 1) =
1155 "[LSLAM_proc_msg_AA] Updating arc " 1156 << nodeFrom <<
" -> " << nodeTo
1157 <<
" OLD: " << oldDelta->mean <<
" cov = " 1158 << oldDelta->cov.inMatlabFormat() << endl);
1160 "[LSLAM_proc_msg_AA] Updating arc " 1161 << nodeFrom <<
" -> " << nodeTo
1162 <<
" NEW: " << newDelta.
mean <<
" cov = " 1163 << newDelta.
cov.inMatlabFormat() << endl);
1165 theArc->m_annotations.set(
1170 theArc->m_annotations.setElemental(
1178 if (LMH->m_neighbors.find(nodeFrom) ==
1179 LMH->m_neighbors.end())
1183 LMH->getRelativePose(
1184 poseID_trg_old, poseID_trg, Aold_new_parts);
1190 theArc->m_annotations
1196 newDelta = *oldDelta + Aold_new;
1200 newDelta.cov(0, 0) = newDelta.cov(1, 1) =
1205 "[LSLAM_proc_msg_AA] Updating arc " 1206 << nodeFrom <<
" <- " << nodeTo
1207 <<
" OLD: " << oldDelta->mean <<
" cov = " 1208 << oldDelta->cov.inMatlabFormat() << endl);
1210 "[LSLAM_proc_msg_AA] Updating arc " 1211 << nodeFrom <<
" <- " << nodeTo
1212 <<
" NEW: " << newDelta.mean <<
" cov = " 1213 << newDelta.cov.inMatlabFormat() << endl);
1215 theArc->m_annotations.set(
1220 theArc->m_annotations.setElemental(
1232 for (
size_t idx_area_b = 0; idx_area_b < myMsg.
partitions.size();
1235 if (idx_area_a == idx_area_b)
1239 double closestDistPoseSrc = 0;
1245 itP0 != myMsg.
partitions[idx_area_a].end(); itP0++)
1247 const CPose3D& pose_trg = lstPoses[*itP0];
1251 itP != myMsg.
partitions[idx_area_b].end(); ++itP)
1253 const CPose3D& otherPose = lstPoses[*itP];
1255 if (
dst < closestDistPoseSrc ||
1258 poseID_closests = *itP;
1259 closestDistPoseSrc =
dst;
1269 if (closestDistPoseSrc <
1270 5 * m_options.SLAM_MIN_DIST_BETWEEN_OBS)
1276 if (!area_b->m_annotations.getElemental(
1281 area_b->m_annotations.setElemental(
1284 poseID_src = poseID_closests;
1286 mrpt::utils::LVL_INFO,
1287 "[LSLAM_proc_msg_AA] Changing reference poseID of " 1288 "area '%i' to pose '%i' (creat. annot)\n",
1289 (
int)area_b_ID, (
int)poseID_closests);
1294 if (lstInternalArcsToCreate.end() ==
1295 lstInternalArcsToCreate.
find(
1297 lstInternalArcsToCreate.end() ==
1298 lstInternalArcsToCreate.
find(
1301 lstInternalArcsToCreate.
insert(
1303 areasWithLink.insert(area_a_ID);
1304 areasWithLink.insert(area_b_ID);
1309 if (lstClosestDoubtfulNeigbors.find(area_b_ID) ==
1310 lstClosestDoubtfulNeigbors.end() ||
1311 closestDistPoseSrc <
1312 lstClosestDoubtfulNeigbors[area_b_ID].second)
1314 lstClosestDoubtfulNeigbors[area_b_ID].first = area_a_ID;
1315 lstClosestDoubtfulNeigbors[area_b_ID].second =
1330 for (
size_t idx_area = 0; idx_area < myMsg.
partitions.size();
1334 if (areasWithLink.find(area_ID) == areasWithLink.end())
1337 if (lstClosestDoubtfulNeigbors.find(area_ID) !=
1338 lstClosestDoubtfulNeigbors.end())
1342 area_ID, lstClosestDoubtfulNeigbors[area_ID].
first));
1345 areasWithLink.insert(area_ID);
1346 areasWithLink.insert(
1347 lstClosestDoubtfulNeigbors[area_ID].
first);
1352 "Area %i seems unconnected??", (
int)area_ID);
1362 mrpt::utils::LVL_INFO,
1363 "[LSLAM_proc_msg_AA] lstInternalArcsToCreate contains %i " 1365 (
int)lstInternalArcsToCreate.size());
1367 lstInternalArcsToCreate.begin();
1368 arcCreat != lstInternalArcsToCreate.end(); ++arcCreat)
1374 mrpt::utils::LVL_INFO,
" AREA %i <-> AREA %i\n",
1375 (
int)closestAreaID, (
int)newAreaID);
1388 std::lock_guard<std::mutex> lock(m_map_cs);
1392 lstInternalArcsToCreate.begin();
1393 arcCreat != lstInternalArcsToCreate.end(); ++arcCreat)
1398 m_map.getNodeByID(area_a_ID)->m_annotations.getElemental(
1403 m_map.getNodeByID(area_b_ID)->m_annotations.getElemental(
1408 LMH->getRelativePose(
1409 area_a_poseID_src, area_b_poseID_trg, relPoseParts);
1413 relPoseGauss.
copyFrom(relPoseParts);
1415 relPoseGauss.
cov.zeros();
1416 relPoseGauss.
cov(0, 0) = relPoseGauss.
cov(1, 1) =
square(0.04);
1420 mrpt::utils::LVL_INFO,
1421 "[LSLAM_proc_msg_AA] Creating arc %i[ref:%i] -> %i[ref:%i] = " 1422 "(%.03f,%.03f,%.03fdeg)\n",
1423 (
int)area_a_ID, (
int)area_a_poseID_src, (
int)area_b_ID,
1424 (
int)area_b_poseID_trg, relPoseGauss.
mean.
x(),
1429 bool arcDeltaIsInverted;
1431 area_a_ID, area_b_ID, LMH->m_ID,
"RelativePose",
1432 arcDeltaIsInverted);
1437 newArc = mrpt::make_aligned_shared<CHMHMapArc>(
1443 newArc->m_arcType.setType(
"RelativePose");
1444 arcDeltaIsInverted =
false;
1448 if (!arcDeltaIsInverted)
1451 "[LSLAM_proc_msg_AA] Updating int. arc " 1452 << area_a_ID <<
" -> " << area_b_ID <<
" : " 1453 << relPoseGauss.
mean 1454 <<
" cov = " << relPoseGauss.
cov.inMatlabFormat() << endl);
1455 newArc->m_annotations.set(
1460 newArc->m_annotations.setElemental(
1463 newArc->m_annotations.setElemental(
1470 relPoseGauss.
inverse(relPoseInv);
1473 "[LSLAM_proc_msg_AA] Updating int. arc " 1474 << area_a_ID <<
" <- " << area_b_ID <<
" : " 1476 <<
" cov = " << relPoseInv.
cov.inMatlabFormat() << endl);
1477 newArc->m_annotations.set(
1482 newArc->m_annotations.setElemental(
1485 newArc->m_annotations.setElemental(
1500 std::lock_guard<std::mutex> lock(m_map_cs);
1503 pNei != LMH->m_neighbors.end(); ++pNei)
1511 nodeFrom->getArcs(lstArcs,
"RelativePose", LMH->m_ID);
1517 a != lstArcs.end(); ++
a)
1520 (*a)->getNodeFrom() == nodeFromID ? (*a)->getNodeTo()
1521 : (*a)->getNodeFrom();
1523 if (LMH->m_neighbors.find(nodeToID) != LMH->m_neighbors.end())
1528 if (lstInternalArcsToCreate.end() ==
1529 lstInternalArcsToCreate.
find(
1531 lstInternalArcsToCreate.end() ==
1532 lstInternalArcsToCreate.
find(
1536 arc->m_annotations.remove(
1539 mrpt::utils::LVL_INFO,
1540 "[LSLAM_proc_msg_AA] Deleting annotation of arc: " 1542 (
long unsigned)nodeFromID, (
long unsigned)nodeToID);
1545 if (!arc->m_annotations.getAnyHypothesis(
1549 mrpt::utils::LVL_INFO,
1550 "[LSLAM_proc_msg_AA] Deleting empty arc: " 1552 (
long unsigned)nodeFromID,
1553 (
long unsigned)nodeToID);
1565 std::lock_guard<std::mutex> lock(m_map_cs);
1567 m_map.dumpAsText(
s);
1569 "%s/HMAP_txt/HMAP_%05i_mid.txt", m_options.LOG_OUTPUT_DIR.c_str(),
1572 mrpt::utils::LVL_INFO,
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())
1599 mrpt::utils::LVL_INFO,
1600 "[LSLAM_proc_msg_AA] Getting area '%u' out of LMH\n",
1601 static_cast<unsigned>(*pNei1));
1609 double ESS_bef = LMH->m_poseParticles.ESS();
1610 LMH->removeAreaFromLMH(
id);
1611 double ESS_aft = LMH->m_poseParticles.ESS();
1613 mrpt::utils::LVL_INFO,
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())
1662 mrpt::utils::LVL_INFO,
1663 "[LSLAM_proc_msg_AA] Bringing in LMH area %i\n",
1684 TPoseID refPoseIDAtOtherArea, refPoseIDAtCurArea;
1686 if (arc->getNodeTo() == curAreaID)
1690 mrpt::utils::LVL_INFO,
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;
1698 mrpt::utils::LVL_INFO,
"(%.03f,%.03f,%.03fdeg)\n",
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(
1720 mrpt::utils::LVL_INFO,
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_poseParticles.m_particles.size());
1765 itTrg = LMH->m_poseParticles.m_particles.begin();
1766 itTrg != LMH->m_poseParticles.m_particles.end(); itSrc++, itTrg++)
1770 itTrg->d->robotPoses[poseID] =
1771 itTrg->d->robotPoses[refPoseIDAtCurArea] +
1772 Delta_c2a + *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);
1806 "%s/HMAP_txt/HMAP_%05i_LMH_mid.txt",
1807 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP));
1809 mrpt::utils::LVL_INFO,
1810 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_LMH_mid.txt\n", DEBUG_STEP);
1817 mrpt::make_aligned_shared<opengl::CSetOfObjects>();
1818 maps3D->setName(
"metric-maps");
1819 LMH->m_poseParticles.getMostLikelyParticle()->d->metricMaps.getAs3DObject(maps3D);
1820 sceneLSLAM.
insert(maps3D);
1824 mrpt::make_aligned_shared<opengl::CSetOfObjects>();
1825 LSLAM_3D->setName(
"LSLAM_3D");
1826 LMH->getAs3DScene(LSLAM_3D);
1827 sceneLSLAM.
insert(LSLAM_3D);
1831 string filLocalAreas =
format(
1832 "%s/HMAP_txt/HMAP_%05i_LMH_mid.3Dscene",
1833 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP);
1835 mrpt::utils::LVL_INFO,
"[LOG] Saving %s\n", filLocalAreas.c_str());
1845 if (!currentArea->m_annotations.getElemental(
1849 if (new_poseID_origin != poseID_origin)
1853 LMH->changeCoordinateOrigin(new_poseID_origin);
1855 mrpt::utils::LVL_INFO,
1856 "[LSLAM_proc_msg_AA] LMH->changeCoordinateOrigin %lu->%lu took %f " 1858 poseID_origin, new_poseID_origin, tictac.
Tac() * 1000);
1860 else if (areasDelayedMetricMapsInsertion.size())
1867 areasDelayedMetricMapsInsertion.begin();
1868 areaID != areasDelayedMetricMapsInsertion.end(); ++areaID)
1872 LMH->m_nodeIDmemberships.begin();
1873 pn != LMH->m_nodeIDmemberships.end(); ++pn)
1875 if (pn->second == *areaID)
1878 const TPoseID& poseToAdd = pn->first;
1880 LMH->m_SFs.find(poseToAdd)->second;
1884 partIt = LMH->m_poseParticles.m_particles.begin();
1885 partIt != LMH->m_poseParticles.m_particles.end(); ++partIt)
1888 partIt->d->robotPoses.find(poseToAdd);
1889 ASSERT_(pose3D != partIt->d->robotPoses.end());
1891 &partIt->d->metricMaps, &pose3D->second);
1898 mrpt::utils::LVL_INFO,
1899 "[LSLAM_proc_msg_AA] areasDelayedMetricMapsInsertion took %f ms\n",
1900 tictac.
Tac() * 1000);
1905 std::lock_guard<std::mutex> lock(m_map_cs);
1907 m_map.dumpAsText(
s);
1909 "%s/HMAP_txt/HMAP_%05i_after.txt", m_options.LOG_OUTPUT_DIR.c_str(),
1912 mrpt::utils::LVL_INFO,
1913 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_after.txt\n", DEBUG_STEP);
1920 "%s/HMAP_txt/HMAP_%05i_LMH_after.txt",
1921 m_options.LOG_OUTPUT_DIR.c_str(), DEBUG_STEP));
1923 mrpt::utils::LVL_INFO,
1924 "[LSLAM_proc_msg_AA] Saved HMAP_%05i_LMH_after.txt\n", DEBUG_STEP);
1928 mrpt::utils::LVL_INFO,
1929 "[LSLAM_proc_msg_AA] Msg from AA took %f ms ]\n",
1930 tictac.Tac() * 1000);
1945 mrpt::utils::LVL_INFO,
1946 "[LSLAM_proc_msg_TBI] Beginning of Msg from TBI processing... " 1951 std::map<CHMHMapNode::TNodeID, CHMHMapNode::TNodeID> alreadyClosedLoops;
1959 mrpt::utils::LVL_INFO,
1960 "[LSLAM_proc_msg_TBI] Processing TLC of areas: %u <-> %u... \n",
1961 (
unsigned)myMsg.
cur_area, (
unsigned)candidate->first);
1965 if (alreadyClosedLoops.find(myMsg.
cur_area) != alreadyClosedLoops.end())
1967 currentArea = alreadyClosedLoops[myMsg.
cur_area];
1968 cout <<
"[LSLAM_proc_msg_TBI] Using " << myMsg.
cur_area <<
" -> " 1969 << currentArea <<
" due to area being already merged." 1976 m_map.computeCoordinatesTransformationBetweenNodes(
1979 candidate->first, pdfPartsHMap, myMsg.
hypothesisID, 100, 0.10f,
1984 pdfDeltaMap.
copyFrom(pdfPartsHMap);
1995 cout <<
"[LSLAM_proc_msg_TBI] HMap_delta=" << pdfDeltaMap.
mean 1996 <<
" std_x=" << sqrt(pdfDeltaMap.
cov(0, 0))
1997 <<
" std_y=" << sqrt(pdfDeltaMap.
cov(1, 1)) << endl;
2005 ASSERT_(!candidate->second.delta_new_cur.empty());
2006 const double chi2_thres =
2009 map<double, CPose3DPDFGaussian>
2014 candidate->second.delta_new_cur.begin();
2015 itSOG != candidate->second.delta_new_cur.end(); ++itSOG)
2019 cout <<
"[LSLAM_proc_msg_TBI] TLC_delta=" << pdfDelta.
mean 2020 <<
" std_x=" << sqrt(pdfDelta.
cov(0, 0))
2021 <<
" std_y=" << sqrt(pdfDelta.
cov(1, 1))
2022 <<
" std_phi=" <<
RAD2DEG(sqrt(pdfDelta.
cov(3, 3))) << endl;
2026 const double mahaDist2 =
2028 cout <<
"[LSLAM_proc_msg_TBI] maha_dist = " << mahaDist2 << endl;
2030 if (mahaDist2 < chi2_thres)
2032 const double log_lik = itSOG->log_w - 0.5 * mahaDist2;
2033 lstModesAndCompats[log_lik] = itSOG->val;
2034 cout <<
"[LSLAM_proc_msg_TBI] Added to list of candidates: " 2035 "log(overall_lik)= " 2041 if (!lstModesAndCompats.empty())
2044 lstModesAndCompats.rbegin()->second;
2056 mrpt::utils::LVL_INFO,
2057 "[LSLAM_proc_msg_TBI] Accepting TLC of areas: %u <-> %u with " 2058 "an overall log(lik)=%f \n",
2059 (
unsigned)currentArea, (
unsigned)candidate->first,
2060 lstModesAndCompats.rbegin()->first);
2069 mrpt::utils::LVL_INFO,
2070 "[LSLAM_proc_msg_TBI] TLC of areas %u <-> %u - DONE in %.03f " 2072 (
unsigned)currentArea, (
unsigned)candidate->first,
2073 1e3 * tictac.
Tac());
2076 alreadyClosedLoops[myMsg.
cur_area] = candidate->first;
2083 mrpt::utils::LVL_INFO,
2084 "[LSLAM_proc_msg_TBI] Msg from TBI took %f ms ]\n",
2085 tictac.Tac() * 1000);
std::shared_ptr< CRobotPosesGraph > Ptr
A namespace of pseudo-random numbers genrators of diferent distributions.
double x() const
Common members of all points & poses classes.
This class implements a STL container with features of both, a std::set and a std::list.
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...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
CPose3D mean
The mean value.
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
std::shared_ptr< CHMHMapArc > Ptr
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...
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
double mahalanobisDistanceTo(const CPose3DPDFGaussian &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
std::shared_ptr< CPose3DPDFGaussian > Ptr
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
#define THROW_EXCEPTION(msg)
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
mrpt::poses::CPose3DPDFParticles pdf
The robot pose PDF.
EIGEN_STRONG_INLINE iterator begin()
double yaw() const
Get the YAW angle (in radians)
THypothesisID hypothesisID
The hypothesis ID (LMH) for these results.
for(ctr=DCTSIZE;ctr > 0;ctr--)
const Scalar * const_iterator
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
void Tic()
Starts the stopwatch.
GLsizei GLsizei GLuint * obj
Helper types for STL containers with Eigen memory allocators.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Declares a class for storing a collection of robot actions.
T square(const T x)
Inline function for the square of a number.
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
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...
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.
A class for storing a list of text lines.
CParticleList m_particles
The array of particles.
This base provides a set of functions for maths stuff.
mrpt::aligned_containers< TPoseID, mrpt::poses::CPose3D >::map_t 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. ...
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
This CStream derived class allow using a file as a write-only, binary stream.
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::shared_ptr< TMessageLSLAMfromTBI > Ptr
TModesList::const_iterator const_iterator
std::vector< TPoseIDList > partitions
This class implements a high-performance stopwatch.
std::shared_ptr< CSetOfObjects > Ptr
This namespace contains representation of robot actions and observations.
std::shared_ptr< CMultiMetricMap > Ptr
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
std::shared_ptr< CSensoryFrame > Ptr
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 NODE_ANNOTATION_METRIC_MAPS
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::vector< TPoseID > TPoseIDList
std::shared_ptr< CSerializable > Ptr
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.
std::shared_ptr< CActionCollection > Ptr
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::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
mrpt::obs::CSensoryFrame sf
The observations.
std::set< CHMHMapNode::TNodeID > TNodeIDSet
The namespace for 3D scene representation and rendering.
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
double Tac()
Stops the stopwatch.
#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::shared_ptr< CHMHMapNode > Ptr
This class stores any customizable set of metric maps.
This class is a "CSerializable" wrapper for "CMatrixFloat".
#define NODE_ANNOTATION_POSES_GRAPH
std::list< T >::iterator find(const T &i)
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).
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
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).
std::shared_ptr< TMessageLSLAMfromAA > Ptr
#define ARC_ANNOTATION_DELTA
A class that contain generic messages, that can be sent and received from a "CClientTCPSocket" object...
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.