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",
336 for (
unsigned long itPose : *it)
337 if (itA->first == itPose)
344 "LMH's pose %i not found in AA's partitions.",
356 std::lock_guard<std::mutex> lock(m_map_cs);
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)
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");
524 for (i = 0; i < partIdx2Areas.size(); i++)
547 for (
auto it = lstPoses.begin(); it != lstPoses.end(); ++it)
555 closestPose = it->first;
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();
592 "[LSLAM_proc_msg_AA] Old neighbors: ");
593 for (
unsigned long it : neighbors_before)
600 "[LSLAM_proc_msg_AA] Cur. 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()) !=
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()) !=
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)
941 "[LSLAM_proc_msg_AA] Current area changed: enqueing area %i for "
947 static size_t cntAddTBI = 0;
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)
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(
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(
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);
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();
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);
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();
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);
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();
1615 "[LSLAM_proc_msg_AA] Bringing in LMH area %i\n",
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);
1712 CPose3DPDFParticles::CParticleList::const_iterator itSrc;
1713 CLocalMetricHypothesis::CParticleList::iterator itTrg;
1717 itTrg != LMH->
m_particles.end(); itSrc++, itTrg++)
1721 itTrg->d->robotPoses[poseID] =
1722 itTrg->d->robotPoses[refPoseIDAtCurArea] +
1723 Delta_c2a +
CPose3D(itSrc->d);
1730 LMH->
m_SFs[poseID] = poseInfo.
sf;
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");
1774 sceneLSLAM.
insert(maps3D);
1778 std::make_shared<opengl::CSetOfObjects>();
1779 LSLAM_3D->setName(
"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)
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)
1827 if (pn->second == areaID)
1830 const TPoseID& poseToAdd = pn->first;
1832 LMH->
m_SFs.find(poseToAdd)->second;
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);