41 size_t CHierarchicalMapMHPartition::nodeCount()
const {
return m_nodes.size(); }
45 size_t CHierarchicalMapMHPartition::arcCount()
const {
return m_arcs.size(); }
55 auto it = m_nodes.find(
id);
69 auto it = m_nodes.find(
id);
84 for (
auto it = m_nodes.begin(); it != m_nodes.end(); ++it)
85 if (it->second->m_hypotheses.has(hypothesisID))
86 if (!
os::_strcmpi(it->second->m_label.c_str(), label.c_str()))
98 const std::string& label,
const THypothesisID& hypothesisID)
const
103 for (
const auto& m_node : m_nodes)
104 if (m_node.second->m_hypotheses.has(hypothesisID))
105 if (!
os::_strcmpi(m_node.second->m_label.c_str(), label.c_str()))
106 return m_node.second;
122 return (m_nodes.begin())->second;
128 void CHierarchicalMapMHPartition::saveAreasDiagramForMATLAB(
129 [[maybe_unused]]
const std::string& filName,
305 void CHierarchicalMapMHPartition::saveAreasDiagramWithEllipsedForMATLAB(
306 [[maybe_unused]]
const std::string& filName,
309 [[maybe_unused]]
float uncertaintyExagerationFactor,
310 [[maybe_unused]]
bool drawArcs,
311 [[maybe_unused]]
unsigned int numberOfIterationsForOptimalGlobalPoses)
const
481 void CHierarchicalMapMHPartition::saveGlobalMapForMATLAB(
482 [[maybe_unused]]
const std::string& filName,
706 TDistance() : dist(std::numeric_limits<unsigned>::max()) {}
726 void CHierarchicalMapMHPartition::findPathBetweenNodes(
751 map<CHMHMapNode::TNodeID, TDistance> d;
752 map<CHMHMapNode::TNodeID, TPrevious> previous;
753 map<CHMHMapNode::TNodeID, CHMHMapArc::Ptr> previous_arcs;
754 map<CHMHMapNode::TNodeID, bool> visited;
756 unsigned visitedCount = 0;
759 m_nodes.find(source) != m_nodes.end(),
760 format(
"Source node %u not found in the H-Map", (
unsigned int)source));
762 m_nodes.find(target) != m_nodes.end(),
763 format(
"Target node %u not found in the H-Map", (
unsigned int)target));
765 ASSERT_(m_nodes.find(source)->second->m_hypotheses.has(hypothesisID));
766 ASSERT_(m_nodes.find(target)->second->m_hypotheses.has(hypothesisID));
776 TNodeList::const_iterator u;
783 unsigned min_d = std::numeric_limits<unsigned>::max();
787 for (
auto i = m_nodes.begin(); i != m_nodes.end(); ++i)
789 if (i->second->m_hypotheses.has(hypothesisID))
791 if (d[i->first].dist < min_d && !visited[i->first])
794 min_d = d[u->first].dist;
801 visited[u->first] =
true;
807 nodeU->getArcs(arcs, hypothesisID);
808 for (
auto i = arcs.begin(); i != arcs.end(); ++i)
813 if ((*i)->getNodeFrom() != nodeU->getID())
814 vID = (*i)->getNodeFrom();
816 vID = (*i)->getNodeTo();
820 if ((*i)->getNodeFrom() != nodeU->getID())
823 vID = (*i)->getNodeTo();
826 if ((min_d + 1) < d[vID].dist)
828 d[vID].dist = min_d + 1;
829 previous[vID].id = u->first;
830 previous_arcs[vID] = *i;
833 }
while (u->first != target && visitedCount < m_nodes.size());
838 if (u->first == target)
844 ret.push_front(previous_arcs[nod]);
845 nod = previous[nod].id;
846 }
while (nod != source);
855 void CHierarchicalMapMHPartition::computeCoordinatesTransformationBetweenNodes(
858 unsigned int particlesCount,
float additionalNoiseXYratio,
859 float additionalNoisePhiRad)
const
864 TArcList::const_iterator arcsIt;
865 const TPose3D nullPose(0, 0, 0, 0, 0, 0);
869 using TPose3DList = std::vector<CPose3D>;
870 std::vector<TPose3DList> listSamples;
871 std::vector<TPose3DList>::iterator lstIt;
872 TPose3DList dummyList;
873 TPose3DList::iterator poseIt;
876 findPathBetweenNodes(nodeFrom, nodeTo, hypothesisID, arcsPath);
878 pathLength = arcsPath.size();
885 dummyList.resize(particlesCount);
886 listSamples.resize(pathLength, dummyList);
887 for (arcsIt = arcsPath.begin(), lstIt = listSamples.begin();
888 arcsIt != arcsPath.end(); lstIt++, arcsIt++)
891 bool reversedArc = (*arcsIt)->getNodeTo() == lastNode;
893 reversedArc ? (*arcsIt)->getNodeFrom() : (*arcsIt)->getNodeTo();
897 TPoseID curNodeRefPoseID, nextNodeRefPoseID;
898 getNodeByID((*arcsIt)->getNodeFrom())
899 ->m_annotations.getElemental(
902 getNodeByID((*arcsIt)->getNodeTo())
903 ->m_annotations.getElemental(
907 TPoseID srcRefPoseID, trgRefPoseID;
908 (*arcsIt)->m_annotations.getElemental(
910 (*arcsIt)->m_annotations.getElemental(
913 ASSERT_(curNodeRefPoseID == srcRefPoseID);
914 ASSERT_(nextNodeRefPoseID == trgRefPoseID);
922 pdf.
copyFrom(*std::dynamic_pointer_cast<CPose3DPDF>(anotation));
924 vector<CVectorDouble> samples;
927 ASSERT_(samples.size() == lstIt->size());
930 vector<CVectorDouble>::const_iterator samplIt;
931 for (poseIt = lstIt->begin(), samplIt = samples.begin();
932 poseIt != lstIt->end(); poseIt++, samplIt++)
935 poseIt->setFromValues(
937 additionalNoiseXYratio *
940 additionalNoiseXYratio *
944 additionalNoisePhiRad *
946 (*samplIt)[4], (*samplIt)[5]);
957 for (
unsigned int i = 0; i < particlesCount; i++)
961 for (
unsigned int j = 0; j < pathLength; j++)
964 sample = sample + listSamples[j][i];
966 sample = listSamples[j][i];
975 cout <<
"[computeCoordinatesTransformationBetweenNodes] Nodes: " << nodeFrom <<
" - " << nodeTo <<
": " << auxPDF;
984 float CHierarchicalMapMHPartition::computeMatchProbabilityBetweenNodes(
987 [[maybe_unused]]
float& maxMatchProb,
990 [[maybe_unused]]
unsigned int monteCarloSamplesPose)
1000 void CHierarchicalMapMHPartition::findArcsBetweenNodes(
1010 TArcList::const_iterator itArc;
1012 node1->getArcs(lstArcs);
1013 for (itArc = lstArcs.begin(); itArc != lstArcs.end(); itArc++)
1015 if ((*itArc)->m_hypotheses.has(hypothesisID))
1016 if ((*itArc)->m_nodeFrom == node2id ||
1017 (*itArc)->m_nodeTo == node2id)
1019 ret.push_back(*itArc);
1029 void CHierarchicalMapMHPartition::findArcsOfTypeBetweenNodes(
1031 const THypothesisID& hypothesisID,
const std::string& arcType,
1040 TArcList::const_iterator itArc;
1042 node1->getArcs(lstArcs);
1043 for (itArc = lstArcs.begin(); itArc != lstArcs.end(); itArc++)
1045 if ((*itArc)->m_hypotheses.has(hypothesisID))
1046 if ((*itArc)->m_nodeFrom == node2id ||
1047 (*itArc)->m_nodeTo == node2id)
1049 if ((*itArc)->m_arcType == arcType) ret.push_back(*itArc);
1059 bool CHierarchicalMapMHPartition::areNodesNeightbour(
1061 const THypothesisID& hypothesisID,
const char* requiredAnnotation)
const
1068 TArcList::const_iterator itArc;
1070 node1->getArcs(lstArcs);
1071 for (itArc = lstArcs.begin(); itArc != lstArcs.end(); itArc++)
1073 if ((*itArc)->m_hypotheses.has(hypothesisID))
1074 if ((*itArc)->m_nodeFrom == node2id ||
1075 (*itArc)->m_nodeTo == node2id)
1077 if (!requiredAnnotation)
1079 else if ((*itArc)->m_annotations.get(
1080 requiredAnnotation, hypothesisID))
1094 void CHierarchicalMapMHPartition::getAs3DScene(
1097 const unsigned int& numberOfIterationsForOptimalGlobalPoses,
1098 bool showRobotPoseIDs)
const
1107 obj->setColor(0.3f, 0.3f, 0.3f);
1111 using TMapID2PosePDF = std::map<CHMHMapNode::TNodeID, CPose3DPDFGaussian>;
1113 TMapID2PosePDF nodesPoses;
1114 TMapID2PosePDF::iterator it;
1116 using TMapID2Pose2D = std::map<CHMHMapNode::TNodeID, CPose2D>;
1118 TMapID2Pose2D nodesMeanPoses;
1119 TMapID2Pose2D::iterator it2;
1122 computeGloballyConsistentNodeCoordinates(
1123 nodesPoses, idReferenceNode, hypothesisID,
1124 numberOfIterationsForOptimalGlobalPoses);
1127 for (it = nodesPoses.begin(); it != nodesPoses.end(); it++)
1136 if (posesGraph && posesGraph->size())
1141 for (
auto it = posesGraph->begin(); it != posesGraph->end(); ++it)
1144 meanSFs *= 1.0f / (posesGraph->size());
1147 meanPose = meanPose +
CPose2D(meanSFs);
1154 nodesMeanPoses[it->first] = meanPose;
1161 for (it = nodesPoses.begin(); it != nodesPoses.end(); it++)
1163 const CPose3D& pose = it->second.mean;
1173 metricMap->getAs3DObject(objTex);
1174 objTex->setPose(pose);
1179 float nodes_height = 10.0f;
1180 float nodes_radius = 1.0f;
1184 for (it = nodesPoses.begin(), it2 = nodesMeanPoses.begin();
1185 it != nodesPoses.end(); it++, it2++)
1188 const CPose3D& pose = it->second.mean;
1194 objSphere->setName(node->m_label);
1195 objSphere->setColor(0, 0, 1);
1196 objSphere->setLocation(
1197 meanPose.
x(), meanPose.
y(), meanPose.z() + nodes_height);
1198 objSphere->setRadius(nodes_radius);
1199 objSphere->setNumberDivsLongitude(16);
1200 objSphere->setNumberDivsLatitude(16);
1202 outScene.
insert(objSphere);
1207 objText->setString(
format(
"%li", (
long int)node->getID()));
1209 objText->setColor(1, 0, 0);
1210 objText->setLocation(
1211 meanPose.
x(), meanPose.
y(), meanPose.z() + nodes_height);
1213 outScene.
insert(objText);
1224 for (
auto it = posesGraph->begin(); it != posesGraph->end(); ++it)
1227 it->second.pdf.getMean(SF_pose);
1229 CPose3D auxPose(pose + SF_pose);
1233 glObj->setColor(1, 0, 0);
1235 glObj->setPose(auxPose);
1238 glObj->setDiskRadius(0.05f);
1239 glObj->setSlicesCount(20);
1241 if (showRobotPoseIDs)
1243 glObj->setName(
format(
"%i", (
int)it->first));
1244 glObj->enableShowName();
1253 objLine->setColor(1, 0, 0, 0.2);
1254 objLine->setLineWidth(1.5);
1256 objLine->setLineCoords(
1257 meanPose.
x(), meanPose.
y(), nodes_height, auxPose.
x(),
1260 outScene.
insert(objLine);
1267 node->getArcs(arcs, hypothesisID);
1268 for (
auto a = arcs.begin(); a != arcs.end(); ++a)
1272 if (arc->getNodeFrom() == node->getID())
1274 CPose3D poseTo(nodesMeanPoses.find(arc->getNodeTo())->second);
1280 objLine->setColor(0, 1, 0, 0.5);
1281 objLine->setLineWidth(5);
1283 objLine->setLineCoords(
1284 meanPose.
x(), meanPose.
y(), meanPose.z() + nodes_height,
1285 poseTo.
x(), poseTo.
y(), poseTo.z() + nodes_height);
1287 outScene.
insert(objLine);
1295 void CHierarchicalMapMHPartition::computeGloballyConsistentNodeCoordinates(
1296 std::map<CHMHMapNode::TNodeID, mrpt::poses::CPose3DPDFGaussian>& nodePoses,
1299 const unsigned int& numberOfIterations)
const
1305 if (m_arcs.empty())
return;
1311 for (
const auto& m_arc : m_arcs)
1313 if (!m_arc->m_hypotheses.has(hypothesisID))
continue;
1320 if (!anotation)
continue;
1324 *std::dynamic_pointer_cast<CPose3DPDF>(anotation));
1330 pose_graph.
root = idReferenceNode;
1336 graphslam_params[
"max_iterations"] = numberOfIterations;
1339 pose_graph, out_info,
1344 for (
auto it_node = pose_graph.
nodes.begin();
1345 it_node != pose_graph.
nodes.end(); ++it_node)
1351 new_pose.
mean = it_node->second;
1356 #if __computeGloballyConsistentNodeCoordinates__VERBOSE
1357 for (map<CHMHMapNode::TNodeID, CPose3DPDFGaussian>::const_iterator it =
1359 it != nodePoses.end(); ++it)
1360 cout << it->first <<
": " << it->second.mean << endl;
1370 void CHierarchicalMapMHPartition::dumpAsText(std::vector<std::string>& st)
const
1373 st.emplace_back(
"LIST OF NODES");
1374 st.emplace_back(
"================");
1376 for (
const auto& m_node : m_nodes)
1380 "NODE ID: %i\t LABEL:%s\tARCS: ", (
int)m_node.second->getID(),
1381 m_node.second->m_label.c_str());
1383 m_node.second->getArcs(arcs);
1384 for (
auto a = arcs.begin(); a != arcs.end(); ++a)
1386 "%i-%i, ", (
int)(*a)->getNodeFrom(), (
int)(*a)->getNodeTo());
1390 for (
auto ann = m_node.second->m_annotations.begin();
1391 ann != m_node.second->m_annotations.end(); ++ann)
1394 " [HYPO ID #%02i] Annotation '%s' Class: ", (
int)ann->ID,
1397 s += string(ann->value->GetRuntimeClass()->className);
1406 m_node.second->m_annotations.getElemental(
1408 st.push_back(
format(
" VALUE: %i", (
int)refID));
1418 " CRobotPosesGraph has %i poses:",
1419 (
int)posesGraph->size()));
1421 for (
auto p = posesGraph->begin(); p != posesGraph->end(); ++p)
1426 " Pose %i \t (%.03f,%.03f,%.03fdeg)",
1427 (
int)p->first, pdfMean.
x(), pdfMean.
y(),
1433 st.emplace_back(
"");
1436 st.emplace_back(
"");
1437 st.emplace_back(
"");
1438 st.emplace_back(
"LIST OF ARCS");
1439 st.emplace_back(
"================");
1441 for (
const auto& m_arc : m_arcs)
1445 "ARC: %i -> %i\n", (
int)m_arc->getNodeFrom(),
1446 (
int)m_arc->getNodeTo());
1448 s += string(
" Arc type: ") + m_arc->m_arcType;
1452 for (
auto ann = m_arc->m_annotations.begin();
1453 ann != m_arc->m_annotations.end(); ++ann)
1456 " [HYPO ID #%02i] Annotation '%s' Class: ", (
int)ann->ID,
1459 s += string(ann->value->GetRuntimeClass()->className);
1468 m_arc->m_annotations.getElemental(
1470 st.push_back(
format(
" VALUE: %i", (
int)refID));
1475 m_arc->m_annotations.getElemental(
1477 st.push_back(
format(
" VALUE: %i", (
int)refID));
1487 *std::dynamic_pointer_cast<CPose3DPDF>(o));
1490 " VALUE: (%f,%f,%f , %fdeg,%fdeg,%fdeg)",
1491 relativePoseAcordToArc.
mean.
x(),
1492 relativePoseAcordToArc.
mean.
y(),
1493 relativePoseAcordToArc.
mean.z(),
1500 st.emplace_back(
"");
1509 const THypothesisID& hypothesisID,
const std::string& arcType,
1510 bool& isInverted)
const
1515 findArcsOfTypeBetweenNodes(
1516 node1id, node2id, hypothesisID, arcType, lstArcs);
1518 for (
auto a = lstArcs.begin(); a != lstArcs.end(); ++a)
1520 if ((*a)->getNodeFrom() == node1id)
1541 double CHierarchicalMapMHPartition::computeOverlapProbabilityBetweenNodes(
1543 const THypothesisID& hypothesisID,
size_t monteCarloSamples,
1544 const float margin_to_substract)
const
1554 computeCoordinatesTransformationBetweenNodes(
1555 nodeFrom, nodeTo, posePDF, hypothesisID, monteCarloSamples);
1560 float r1_x_min, r1_x_max, r1_y_min, r1_y_max, r2_x_min, r2_x_max, r2_y_min,
1570 r1_x_min = grid->getXMin();
1571 r1_x_max = grid->getXMax();
1572 r1_y_min = grid->getYMin();
1573 r1_y_max = grid->getYMax();
1575 if (r1_x_max - r1_x_min > 2 * margin_to_substract)
1577 r1_x_max -= margin_to_substract;
1578 r1_x_min += margin_to_substract;
1580 if (r1_y_max - r1_y_min > 2 * margin_to_substract)
1582 r1_y_max -= margin_to_substract;
1583 r1_y_min += margin_to_substract;
1593 r2_x_min = grid2->getXMin();
1594 r2_x_max = grid2->getXMax();
1595 r2_y_min = grid2->getYMin();
1596 r2_y_max = grid2->getYMax();
1598 if (r2_x_max - r2_x_min > 2 * margin_to_substract)
1600 r2_x_max -= margin_to_substract;
1601 r2_x_min += margin_to_substract;
1603 if (r2_y_max - r2_y_min > 2 * margin_to_substract)
1605 r2_y_max -= margin_to_substract;
1606 r2_y_min += margin_to_substract;
1610 for (i = 0; i < monteCarloSamples; i++)
1613 r1_x_min, r1_x_max, r1_y_min, r1_y_max, r2_x_min, r2_x_max,
1621 return static_cast<double>(hits) / monteCarloSamples;