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()))
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(
307 void CHierarchicalMapMHPartition::saveAreasDiagramWithEllipsedForMATLAB(
309 const THypothesisID& hypothesisID,
float uncertaintyExagerationFactor,
310 bool drawArcs,
unsigned int numberOfIterationsForOptimalGlobalPoses)
const 486 void CHierarchicalMapMHPartition::saveGlobalMapForMATLAB(
734 void CHierarchicalMapMHPartition::findPathBetweenNodes(
759 map<CHMHMapNode::TNodeID, TDistance> d;
760 map<CHMHMapNode::TNodeID, TPrevious> previous;
761 map<CHMHMapNode::TNodeID, CHMHMapArc::Ptr> previous_arcs;
762 map<CHMHMapNode::TNodeID, bool> visited;
764 unsigned visitedCount = 0;
767 m_nodes.find(
source) != m_nodes.end(),
768 format(
"Source node %u not found in the H-Map", (
unsigned int)
source));
770 m_nodes.find(target) != m_nodes.end(),
771 format(
"Target node %u not found in the H-Map", (
unsigned int)target));
773 ASSERT_(m_nodes.find(
source)->second->m_hypotheses.has(hypothesisID));
774 ASSERT_(m_nodes.find(target)->second->m_hypotheses.has(hypothesisID));
784 TNodeList::const_iterator u;
791 unsigned min_d = std::numeric_limits<unsigned>::max();
795 for (
auto i = m_nodes.begin(); i != m_nodes.end(); ++i)
797 if (i->second->m_hypotheses.has(hypothesisID))
799 if (d[i->first].dist < min_d && !visited[i->first])
802 min_d = d[u->first].dist;
809 visited[u->first] =
true;
815 nodeU->getArcs(arcs, hypothesisID);
816 for (
auto i = arcs.begin(); i != arcs.end(); ++i)
821 if ((*i)->getNodeFrom() != nodeU->getID())
822 vID = (*i)->getNodeFrom();
824 vID = (*i)->getNodeTo();
828 if ((*i)->getNodeFrom() != nodeU->getID())
831 vID = (*i)->getNodeTo();
834 if ((min_d + 1) < d[vID].dist)
836 d[vID].dist = min_d + 1;
837 previous[vID].id = u->first;
838 previous_arcs[vID] = *i;
841 }
while (u->first != target && visitedCount < m_nodes.size());
846 if (u->first == target)
852 ret.push_front(previous_arcs[nod]);
853 nod = previous[nod].id;
863 void CHierarchicalMapMHPartition::computeCoordinatesTransformationBetweenNodes(
866 unsigned int particlesCount,
float additionalNoiseXYratio,
867 float additionalNoisePhiRad)
const 872 TArcList::const_iterator arcsIt;
873 const TPose3D nullPose(0, 0, 0, 0, 0, 0);
877 using TPose3DList = std::vector<CPose3D>;
878 std::vector<TPose3DList> listSamples;
879 std::vector<TPose3DList>::iterator lstIt;
880 TPose3DList dummyList;
881 TPose3DList::iterator poseIt;
884 findPathBetweenNodes(nodeFrom, nodeTo, hypothesisID, arcsPath);
886 pathLength = arcsPath.size();
893 dummyList.resize(particlesCount);
894 listSamples.resize(pathLength, dummyList);
895 for (arcsIt = arcsPath.begin(), lstIt = listSamples.begin();
896 arcsIt != arcsPath.end(); lstIt++, arcsIt++)
899 bool reversedArc = (*arcsIt)->getNodeTo() == lastNode;
901 reversedArc ? (*arcsIt)->getNodeFrom() : (*arcsIt)->getNodeTo();
905 TPoseID curNodeRefPoseID, nextNodeRefPoseID;
906 getNodeByID((*arcsIt)->getNodeFrom())
907 ->m_annotations.getElemental(
910 getNodeByID((*arcsIt)->getNodeTo())
911 ->m_annotations.getElemental(
915 TPoseID srcRefPoseID, trgRefPoseID;
916 (*arcsIt)->m_annotations.getElemental(
918 (*arcsIt)->m_annotations.getElemental(
921 ASSERT_(curNodeRefPoseID == srcRefPoseID);
922 ASSERT_(nextNodeRefPoseID == trgRefPoseID);
930 pdf.
copyFrom(*std::dynamic_pointer_cast<CPose3DPDF>(anotation));
938 vector<CVectorDouble>::const_iterator samplIt;
939 for (poseIt = lstIt->begin(), samplIt =
samples.begin();
940 poseIt != lstIt->end(); poseIt++, samplIt++)
943 poseIt->setFromValues(
945 additionalNoiseXYratio *
948 additionalNoiseXYratio *
952 additionalNoisePhiRad *
954 (*samplIt)[4], (*samplIt)[5]);
965 for (
unsigned int i = 0; i < particlesCount; i++)
969 for (
unsigned int j = 0; j < pathLength; j++)
972 sample = sample + listSamples[j][i];
974 sample = listSamples[j][i];
983 cout <<
"[computeCoordinatesTransformationBetweenNodes] Nodes: " << nodeFrom <<
" - " << nodeTo <<
": " << auxPDF;
992 float CHierarchicalMapMHPartition::computeMatchProbabilityBetweenNodes(
995 const THypothesisID& hypothesisID,
unsigned int monteCarloSamplesPose)
1011 void CHierarchicalMapMHPartition::findArcsBetweenNodes(
1021 TArcList::const_iterator itArc;
1023 node1->getArcs(lstArcs);
1024 for (itArc = lstArcs.begin(); itArc != lstArcs.end(); itArc++)
1026 if ((*itArc)->m_hypotheses.has(hypothesisID))
1027 if ((*itArc)->m_nodeFrom == node2id ||
1028 (*itArc)->m_nodeTo == node2id)
1030 ret.push_back(*itArc);
1040 void CHierarchicalMapMHPartition::findArcsOfTypeBetweenNodes(
1051 TArcList::const_iterator itArc;
1053 node1->getArcs(lstArcs);
1054 for (itArc = lstArcs.begin(); itArc != lstArcs.end(); itArc++)
1056 if ((*itArc)->m_hypotheses.has(hypothesisID))
1057 if ((*itArc)->m_nodeFrom == node2id ||
1058 (*itArc)->m_nodeTo == node2id)
1060 if ((*itArc)->m_arcType == arcType) ret.push_back(*itArc);
1070 bool CHierarchicalMapMHPartition::areNodesNeightbour(
1072 const THypothesisID& hypothesisID,
const char* requiredAnnotation)
const 1079 TArcList::const_iterator itArc;
1081 node1->getArcs(lstArcs);
1082 for (itArc = lstArcs.begin(); itArc != lstArcs.end(); itArc++)
1084 if ((*itArc)->m_hypotheses.has(hypothesisID))
1085 if ((*itArc)->m_nodeFrom == node2id ||
1086 (*itArc)->m_nodeTo == node2id)
1088 if (!requiredAnnotation)
1090 else if ((*itArc)->m_annotations.get(
1091 requiredAnnotation, hypothesisID))
1105 void CHierarchicalMapMHPartition::getAs3DScene(
1108 const unsigned int& numberOfIterationsForOptimalGlobalPoses,
1109 const bool& showRobotPoseIDs)
const 1118 obj->setColor(0.3, 0.3, 0.3);
1122 using TMapID2PosePDF = std::map<CHMHMapNode::TNodeID, CPose3DPDFGaussian>;
1124 TMapID2PosePDF nodesPoses;
1125 TMapID2PosePDF::iterator it;
1127 using TMapID2Pose2D = std::map<CHMHMapNode::TNodeID, CPose2D>;
1129 TMapID2Pose2D nodesMeanPoses;
1130 TMapID2Pose2D::iterator it2;
1133 computeGloballyConsistentNodeCoordinates(
1134 nodesPoses, idReferenceNode, hypothesisID,
1135 numberOfIterationsForOptimalGlobalPoses);
1138 for (it = nodesPoses.begin(); it != nodesPoses.end(); it++)
1147 if (posesGraph && posesGraph->size())
1152 for (
auto it = posesGraph->begin(); it != posesGraph->end(); ++it)
1155 meanSFs *= 1.0f / (posesGraph->size());
1158 meanPose = meanPose +
CPose2D(meanSFs);
1165 nodesMeanPoses[it->first] = meanPose;
1172 for (it = nodesPoses.begin(); it != nodesPoses.end(); it++)
1174 const CPose3D& pose = it->second.mean;
1184 metricMap->getAs3DObject(objTex);
1185 objTex->setPose(pose);
1190 float nodes_height = 10.0f;
1191 float nodes_radius = 1.0f;
1195 for (it = nodesPoses.begin(), it2 = nodesMeanPoses.begin();
1196 it != nodesPoses.end(); it++, it2++)
1199 const CPose3D& pose = it->second.mean;
1205 objSphere->setName(node->m_label);
1206 objSphere->setColor(0, 0, 1);
1207 objSphere->setLocation(
1208 meanPose.
x(), meanPose.
y(), meanPose.z() + nodes_height);
1209 objSphere->setRadius(nodes_radius);
1210 objSphere->setNumberDivsLongitude(16);
1211 objSphere->setNumberDivsLatitude(16);
1213 objSphere->enableRadiusIndependentOfEyeDistance();
1215 outScene.
insert(objSphere);
1220 objText->setString(
format(
"%li", (
long int)node->getID()));
1222 objText->setColor(1, 0, 0);
1223 objText->setLocation(
1224 meanPose.
x(), meanPose.
y(), meanPose.z() + nodes_height);
1226 outScene.
insert(objText);
1237 for (
auto it = posesGraph->begin(); it != posesGraph->end(); ++it)
1240 it->second.pdf.getMean(SF_pose);
1242 CPose3D auxPose(pose + SF_pose);
1246 glObj->setColor(1, 0, 0);
1248 glObj->setPose(auxPose);
1251 glObj->setDiskRadius(0.05f);
1252 glObj->setSlicesCount(20);
1253 glObj->setLoopsCount(10);
1255 if (showRobotPoseIDs)
1257 glObj->setName(
format(
"%i", (
int)it->first));
1258 glObj->enableShowName();
1267 objLine->setColor(1, 0, 0, 0.2);
1268 objLine->setLineWidth(1.5);
1270 objLine->setLineCoords(
1271 meanPose.
x(), meanPose.
y(), nodes_height, auxPose.
x(),
1274 outScene.
insert(objLine);
1281 node->getArcs(arcs, hypothesisID);
1282 for (
auto a = arcs.begin();
a != arcs.end(); ++
a)
1286 if (arc->getNodeFrom() == node->getID())
1288 CPose3D poseTo(nodesMeanPoses.find(arc->getNodeTo())->second);
1294 objLine->setColor(0, 1, 0, 0.5);
1295 objLine->setLineWidth(5);
1297 objLine->setLineCoords(
1298 meanPose.
x(), meanPose.
y(), meanPose.z() + nodes_height,
1299 poseTo.
x(), poseTo.
y(), poseTo.z() + nodes_height);
1301 outScene.
insert(objLine);
1309 void CHierarchicalMapMHPartition::computeGloballyConsistentNodeCoordinates(
1310 std::map<CHMHMapNode::TNodeID, mrpt::poses::CPose3DPDFGaussian>& nodePoses,
1313 const unsigned int& numberOfIterations)
const 1319 if (m_arcs.empty())
return;
1325 for (
const auto& m_arc : m_arcs)
1327 if (!m_arc->m_hypotheses.has(hypothesisID))
continue;
1334 if (!anotation)
continue;
1338 *std::dynamic_pointer_cast<CPose3DPDF>(anotation));
1344 pose_graph.
root = idReferenceNode;
1350 graphslam_params[
"max_iterations"] = numberOfIterations;
1353 pose_graph, out_info,
1358 for (
auto it_node = pose_graph.
nodes.begin();
1359 it_node != pose_graph.
nodes.end(); ++it_node)
1365 new_pose.
mean = it_node->second;
1370 #if __computeGloballyConsistentNodeCoordinates__VERBOSE 1371 for (map<CHMHMapNode::TNodeID, CPose3DPDFGaussian>::const_iterator it =
1373 it != nodePoses.end(); ++it)
1374 cout << it->first <<
": " << it->second.mean << endl;
1384 void CHierarchicalMapMHPartition::dumpAsText(std::vector<std::string>& st)
const 1387 st.emplace_back(
"LIST OF NODES");
1388 st.emplace_back(
"================");
1390 for (
const auto& m_node : m_nodes)
1394 "NODE ID: %i\t LABEL:%s\tARCS: ", (
int)m_node.second->getID(),
1395 m_node.second->m_label.c_str());
1397 m_node.second->getArcs(arcs);
1398 for (
auto a = arcs.begin();
a != arcs.end(); ++
a)
1400 "%i-%i, ", (
int)(*a)->getNodeFrom(), (int)(*a)->getNodeTo());
1404 for (
auto ann = m_node.second->m_annotations.begin();
1405 ann != m_node.second->m_annotations.end(); ++ann)
1408 " [HYPO ID #%02i] Annotation '%s' Class: ", (
int)ann->ID,
1411 s +=
string(ann->value->GetRuntimeClass()->className);
1420 m_node.second->m_annotations.getElemental(
1422 st.push_back(
format(
" VALUE: %i", (
int)refID));
1432 " CRobotPosesGraph has %i poses:",
1433 (
int)posesGraph->size()));
1435 for (
auto p = posesGraph->begin();
p != posesGraph->end(); ++
p)
1440 " Pose %i \t (%.03f,%.03f,%.03fdeg)",
1441 (
int)
p->first, pdfMean.
x(), pdfMean.
y(),
1447 st.emplace_back(
"");
1450 st.emplace_back(
"");
1451 st.emplace_back(
"");
1452 st.emplace_back(
"LIST OF ARCS");
1453 st.emplace_back(
"================");
1455 for (
const auto& m_arc : m_arcs)
1459 "ARC: %i -> %i\n", (
int)m_arc->getNodeFrom(),
1460 (int)m_arc->getNodeTo());
1462 s +=
string(
" Arc type: ") + m_arc->m_arcType;
1466 for (
auto ann = m_arc->m_annotations.begin();
1467 ann != m_arc->m_annotations.end(); ++ann)
1470 " [HYPO ID #%02i] Annotation '%s' Class: ", (
int)ann->ID,
1473 s +=
string(ann->value->GetRuntimeClass()->className);
1482 m_arc->m_annotations.getElemental(
1484 st.push_back(
format(
" VALUE: %i", (
int)refID));
1489 m_arc->m_annotations.getElemental(
1491 st.push_back(
format(
" VALUE: %i", (
int)refID));
1501 *std::dynamic_pointer_cast<CPose3DPDF>(o));
1504 " VALUE: (%f,%f,%f , %fdeg,%fdeg,%fdeg)",
1505 relativePoseAcordToArc.
mean.
x(),
1506 relativePoseAcordToArc.
mean.
y(),
1507 relativePoseAcordToArc.
mean.z(),
1514 st.emplace_back(
"");
1524 bool& isInverted)
const 1529 findArcsOfTypeBetweenNodes(
1530 node1id, node2id, hypothesisID, arcType, lstArcs);
1532 for (
auto a = lstArcs.begin();
a != lstArcs.end(); ++
a)
1534 if ((*a)->getNodeFrom() == node1id)
1555 double CHierarchicalMapMHPartition::computeOverlapProbabilityBetweenNodes(
1557 const THypothesisID& hypothesisID,
const size_t& monteCarloSamples,
1558 const float margin_to_substract)
const 1568 computeCoordinatesTransformationBetweenNodes(
1569 nodeFrom, nodeTo, posePDF, hypothesisID, monteCarloSamples);
1574 float r1_x_min, r1_x_max, r1_y_min, r1_y_max, r2_x_min, r2_x_max, r2_y_min,
1584 r1_x_min = grid->getXMin();
1585 r1_x_max = grid->getXMax();
1586 r1_y_min = grid->getYMin();
1587 r1_y_max = grid->getYMax();
1589 if (r1_x_max - r1_x_min > 2 * margin_to_substract)
1591 r1_x_max -= margin_to_substract;
1592 r1_x_min += margin_to_substract;
1594 if (r1_y_max - r1_y_min > 2 * margin_to_substract)
1596 r1_y_max -= margin_to_substract;
1597 r1_y_min += margin_to_substract;
1607 r2_x_min = grid2->getXMin();
1608 r2_x_max = grid2->getXMax();
1609 r2_y_min = grid2->getYMin();
1610 r2_y_max = grid2->getYMax();
1612 if (r2_x_max - r2_x_min > 2 * margin_to_substract)
1614 r2_x_max -= margin_to_substract;
1615 r2_x_min += margin_to_substract;
1617 if (r2_y_max - r2_y_min > 2 * margin_to_substract)
1619 r2_y_max -= margin_to_substract;
1620 r2_y_min += margin_to_substract;
1624 for (i = 0; i < monteCarloSamples; i++)
1627 r1_x_min, r1_x_max, r1_y_min, r1_y_max, r2_x_min, r2_x_max,
1635 return static_cast<double>(hits) / monteCarloSamples;
A namespace of pseudo-random numbers generators of diferent distributions.
bool RectanglesIntersection(const double &R1_x_min, const double &R1_x_max, const double &R1_y_min, const double &R1_y_max, const double &R2_x_min, const double &R2_x_max, const double &R2_y_min, const double &R2_y_max, const double &R2_pose_x, const double &R2_pose_y, const double &R2_pose_phi)
Returns whether two rotated rectangles intersect.
CPose3D mean
The mean value.
static Ptr Create(Args &&... args)
void optimize_graph_spa_levmarq(GRAPH_T &graph, TResultInfoSpaLevMarq &out_info, const std::set< mrpt::graphs::TNodeID > *in_nodes_to_optimize=nullptr, const mrpt::system::TParametersDouble &extra_params=mrpt::system::TParametersDouble(), FEEDBACK_CALLABLE functor_feedback=FEEDBACK_CALLABLE())
Optimize a graph of pose constraints using the Sparse Pose Adjustment (SPA) sparse representation and...
#define THROW_EXCEPTION(msg)
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
CParticleList m_particles
The array of particles.
double normalizeWeights(double *out_max_log_w=nullptr) override
Normalize the (logarithmic) weights, such as the maximum weight is zero.
mrpt::graphs::TNodeID root
The ID of the node that is the origin of coordinates, used as reference by all coordinates in nodes...
void dijkstra_nodes_estimate(std::optional< std::reference_wrapper< std::map< TNodeID, size_t >>> topological_distances=std::nullopt)
Spanning tree computation of a simple estimation of the global coordinates of each node just from the...
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
This file implements several operations that operate element-wise on individual or pairs of container...
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
double pitch() const
Get the PITCH angle (in radians)
double yaw() const
Get the YAW angle (in radians)
static Ptr Create(Args &&... args)
GLsizei GLsizei GLuint * obj
const TDistance & operator=(const unsigned &D)
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
#define ASSERT_(f)
Defines an assertion mechanism.
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
This base provides a set of functions for maths stuff.
void addComponents(const CPose3D &p)
Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" ...
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class stores a set of r...
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
void clear(bool createMainViewport=true)
Clear the list of objects and viewports in the scene, deleting objects' memory, and leaving just the ...
double x() const
Common members of all points & poses classes.
void drawManySamples(size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const override
Draws a number of samples from the distribution, and saves as a list of 1x6 vectors, where each row contains a (x,y,phi) datum.
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
#define NODE_ANNOTATION_METRIC_MAPS
GLsizei const GLchar ** string
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
A class for storing an occupancy grid map.
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define ARC_ANNOTATION_DELTA_SRC_POSEID
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
TDistance(const unsigned &D)
Output information for mrpt::graphslam::optimize_graph_spa_levmarq()
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
The namespace for 3D scene representation and rendering.
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
GLsizei GLsizei GLchar * source
static Ptr Create(Args &&... args)
#define NODE_ANNOTATION_REF_POSEID
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
void normalizeAngles()
Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be cal...
global_poses_t nodes
The nodes (vertices) of the graph, with their estimated "global" (with respect to root) position...
GLenum GLsizei GLenum format
void resetDeterministic(const mrpt::math::TPose3D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose...
static Ptr Create(Args &&... args)
void insertEdgeAtEnd(TNodeID from_nodeID, TNodeID to_nodeID, const edge_t &edge_value)
Insert an edge (from -> to) with the given edge value (more efficient version to be called if you kno...
This class stores any customizable set of metric maps.
#define NODE_ANNOTATION_POSES_GRAPH
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
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
static Ptr Create(Args &&... args)
A class for storing a sequence of arcs (a path).
static Ptr Create(Args &&... args)
#define ARC_ANNOTATION_DELTA
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
int _strcmpi(const char *str1, const char *str2) noexcept
An OS-independent version of strcmpi.