template class mrpt::graphs::CNetworkOfPoses

Overview

A directed graph of pose constraints, with edges being the relative poses between pairs of nodes identified by their numeric IDs (of type mrpt::graphs::TNodeID).

A link or edge between two nodes “i” and “j”, that is, the pose \(p_{ij} *\), holds the relative position of “j” with respect to “i”. These poses are stored in the edges in the format specified by the template argument CPOSE. Users should employ the following derived classes depending on the desired representation of edges:

• mrpt::graphs::CNetworkOfPoses2D : 2D edges as a simple CPose2D (x y phi)

• mrpt::graphs::CNetworkOfPoses3D : 3D edges as a simple mrpt::poses::CPose3D (x y z yaw pitch roll)

• mrpt::graphs::CNetworkOfPoses2DInf : 2D edges as a Gaussian PDF with information matrix (CPosePDFGaussianInf)

• mrpt::graphs::CNetworkOfPoses3DInf : 3D edges as a Gaussian PDF with information matrix (CPose3DPDFGaussianInf)

• mrpt::graphs::CNetworkOfPoses2DCov : 2D edges as a Gaussian PDF with covariance matrix (CPosePDFGaussian). It’s more efficient to use the information matrix version instead!

• mrpt::graphs::CNetworkOfPoses3DCov : 3D edges as a Gaussian PDF with covariance matrix (CPose3DPDFGaussian). It’s more efficient to use the information matrix version instead!

Two main members store all the information in this class:

• edges (in the base class mrpt::graphs::CDirectedGraph::edges): A map from pairs of node ID -> pose constraints.

• nodes : A map from node ID -> estimated pose of that node (actually, read below on the template argument MAPS_IMPLEMENTATION).

Graphs can be loaded and saved to text file in the format used by TORO & HoG-man (more on the format here), using loadFromTextFile and saveToTextFile.

This class is the base for representing networks of poses, which are the main data type of a series of SLAM algorithms implemented in the library mrpt-slam, in the namespace mrpt::graphslam.

The template arguments are:

• CPOSE: The type of the edges, which hold a relative pose (2D/3D, just a value or a Gaussian, etc.)

• MAPS_IMPLEMENTATION: Can be either mrpt::containers::map_traits_stdmap or mrpt::containers::map_traits_map_as_vector. Determines the type of the list of global poses (member nodes).

See also:

mrpt::graphslam

#include <mrpt/graphs/CNetworkOfPoses.h>

template <
    class CPOSE,
    class MAPS_IMPLEMENTATION = mrpt::containers::map_traits_stdmap,
    class NODE_ANNOTATIONS = mrpt::graphs::detail::TNodeAnnotationsEmpty,
    class EDGE_ANNOTATIONS = mrpt::graphs::detail::edge_annotations_empty
    >
class CNetworkOfPoses: public mrpt::graphs::CDirectedGraph
{
public:
    // typedefs

    typedef mrpt::graphs::CDirectedGraph<CPOSE, EDGE_ANNOTATIONS> BASE;
    typedef CNetworkOfPoses<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS> self_t;
    typedef CPOSE constraint_t;
    typedef NODE_ANNOTATIONS node_annotations_t;
    typedef EDGE_ANNOTATIONS edge_annotations_t;
    typedef MAPS_IMPLEMENTATION maps_implementation_t;
    typedef typename CPOSE::type_value constraint_no_pdf_t;
    typedef typename MAPS_IMPLEMENTATION::template map<mrpt::graphs::TNodeID, CPOSE> global_poses_pdf_t;
    typedef typename MAPS_IMPLEMENTATION::template map<mrpt::graphs::TNodeID, global_pose_t> global_poses_t;
    typedef typename edges_map_t::iterator iterator;
    typedef typename edges_map_t::reverse_iterator reverse_iterator;
    typedef typename edges_map_t::const_iterator const_iterator;
    typedef typename edges_map_t::const_reverse_iterator const_reverse_iterator;

    // structs

    struct global_pose_t;

    // fields

    global_poses_t nodes;
    mrpt::graphs::TNodeID root {0};
    bool edges_store_inverse_poses {false};

    // methods

    static constexpr static auto getClassName();
    static void connectGraphPartitions(self_t* sub_graph, const std::set<TNodeID>& groupA, const std::set<TNodeID>& groupB);
    void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr object, const mrpt::containers::yaml& viz_params) const;
    void dijkstra_nodes_estimate(std::optional<std::reference_wrapper<std::map<TNodeID, size_t>>> topological_distances = std::nullopt);
    size_t collapseDuplicatedEdges();
    double chi2() const;
    double getGlobalSquareError(bool ignoreCovariances = true) const;

    void extractSubGraph(
        const std::set<TNodeID>& node_IDs,
        self_t* sub_graph,
        const TNodeID root_node_in = INVALID_NODEID,
        bool auto_expand_set = true
        ) const;

    void mergeGraph(
        const self_t& other,
        const typename std::vector<detail::THypothesis<self_t>>& common_hypots,
        const bool hypots_from_other_to_self = true,
        std::map<TNodeID, TNodeID>* old_to_new_nodeID_mappings_out = nullptr
        );

    double getEdgeSquareError(const typename BASE::edges_map_t::const_iterator& itEdge, bool ignoreCovariances = true) const;
    double getEdgeSquareError(const mrpt::graphs::TNodeID from_id, const mrpt::graphs::TNodeID to_id, bool ignoreCovariances = true) const;
    void clear();
    size_t nodeCount() const;
    void saveToTextFile(const std::string& fileName) const;
    void writeAsText(std::ostream& o) const;
    void loadFromTextFile(const std::string& fileName, bool collapse_dup_edges = true);
    void readAsText(std::istream& i);
    iterator begin();
    const_iterator begin() const;
    iterator rbegin();
    const_iterator rbegin() const;
    iterator end();
    const_iterator end() const;
    iterator rend();
    const_iterator rend() const;
};

Inherited Members

public:
    // typedefs

    typedef TYPE_EDGES edge_underlying_t;
    typedef std::multimap<TPairNodeIDs, edge_t> edges_map_t;
    typedef CDirectedGraph<TYPE_EDGES, EDGE_ANNOTATIONS> self_t;

    // structs

    struct edge_t;

    // fields

    edges_map_t edges;

    // methods

    size_t edgeCount() const;
    void clearEdges();
    void insertEdge(TNodeID from_nodeID, TNodeID to_nodeID, const edge_t& edge_value);
    void insertEdgeAtEnd(TNodeID from_nodeID, TNodeID to_nodeID, const edge_t& edge_value);
    bool edgeExists(TNodeID from_nodeID, TNodeID to_nodeID) const;
    edge_t& getEdge(TNodeID from_nodeID, TNodeID to_nodeID);
    const edge_t& getEdge(TNodeID from_nodeID, TNodeID to_nodeID) const;
    std::pair<iterator, iterator> getEdges(TNodeID from_nodeID, TNodeID to_nodeID);
    std::pair<const_iterator, const_iterator> getEdges(TNodeID from_nodeID, TNodeID to_nodeID) const;
    void eraseEdge(TNodeID from_nodeID, TNodeID to_nodeID);
    void getAllNodes(std::set<TNodeID>& lstNode_IDs) const;
    std::set<TNodeID> getAllNodes() const;
    size_t countDifferentNodesInEdges() const;
    void getNeighborsOf(TNodeID const nodeID, std::set<TNodeID>& neighborIDs) const;
    std::set<TNodeID> getNeighborsOf(TNodeID const nodeID) const;

    template <class MAP_NODEID_SET_NODEIDS>
    void getAdjacencyMatrix(MAP_NODEID_SET_NODEIDS& outAdjacency) const;

    template <class MAP_NODEID_SET_NODEIDS, class SET_NODEIDS>
    void getAdjacencyMatrix(
        MAP_NODEID_SET_NODEIDS& outAdjacency,
        const SET_NODEIDS& onlyForTheseNodes
        ) const;

    bool saveAsDot(std::ostream& o, const TGraphvizExportParams& p = TGraphvizExportParams()) const;
    bool saveAsDot(const std::string& fileName, const TGraphvizExportParams& p = TGraphvizExportParams()) const;

Typedefs

typedef mrpt::graphs::CDirectedGraph<CPOSE, EDGE_ANNOTATIONS> BASE

The base class “CDirectedGraph<CPOSE,EDGE_ANNOTATIONS>”.

typedef CNetworkOfPoses<CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS> self_t

My own type.

typedef CPOSE constraint_t

The type of PDF poses in the contraints (edges) (=CPOSE template argument)

typedef NODE_ANNOTATIONS node_annotations_t

The extra annotations in nodes, apart from a constraint_no_pdf_t.

typedef EDGE_ANNOTATIONS edge_annotations_t

The extra annotations in edges, apart from a constraint_t.

typedef MAPS_IMPLEMENTATION maps_implementation_t

The type of map’s implementation (=MAPS_IMPLEMENTATION template argument)

typedef typename CPOSE::type_value constraint_no_pdf_t

The type of edges or their means if they are PDFs (that is, a simple “edge” value)

typedef typename MAPS_IMPLEMENTATION::template map<mrpt::graphs::TNodeID, CPOSE> global_poses_pdf_t

A map from pose IDs to their global coordinate estimates, with uncertainty.

typedef typename MAPS_IMPLEMENTATION::template map<mrpt::graphs::TNodeID, global_pose_t> global_poses_t

A map from pose IDs to their global coordinate estimates, without uncertainty (the “most-likely value”)

Fields

global_poses_t nodes

The nodes (vertices) of the graph, with their estimated “global” (with respect to root) position, without an associated covariance.

See also:

dijkstra_nodes_estimate

mrpt::graphs::TNodeID root {0}

The ID of the node that is the origin of coordinates, used as reference by all coordinates in nodes.

By default, root is the ID “0”.

bool edges_store_inverse_poses {false}

False (default) if an edge i->j stores the normal relative pose of j as seen from i: \(\Delta_i^j = j \ominus i\) True if an edge i->j stores the inverse relateive pose, that is, i as seen from j: \(\Delta_i^j = i \ominus j\).

Methods

static void connectGraphPartitions(
    self_t* sub_graph,
    const std::set<TNodeID>& groupA,
    const std::set<TNodeID>& groupB
    )

Add an edge between the last node of the group with the lower nodeIDs and the first node of the higher nodeIDs.

Given groups of nodes should only contain consecutive nodeIDs and there should be no overlapping between them

It is assumed that the sets of nodes are already in ascending order (default std::set behavior.

void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr object, const mrpt::containers::yaml& viz_params) const

Return 3D Visual Representation of the edges and nodes in the network of poses.

Method makes the call to the corresponding method of the CVisualizer class instance.

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 information in all edges, sorted in a Dijkstra tree based on the current “root” node.

The “global” coordinates are with respect to the node with the ID specified in root.

This method takes into account the value of edges_store_inverse_poses

See also:

node, root

size_t collapseDuplicatedEdges()

Look for duplicated edges (even in opposite directions) between all pairs of nodes and fuse them.

Upon return, only one edge remains between each pair of nodes with the mean & covariance (or information matrix) corresponding to the Bayesian fusion of all the Gaussians.

Returns:

Overall number of removed edges.

double chi2() const

Returns the total chi-squared error of the graph.

Shortcut for getGlobalSquareError(false).

double getGlobalSquareError(bool ignoreCovariances = true) const

Evaluates the graph total square error (ignoreCovariances=true) or chi2 (ignoreCovariances=false) from all the pose constraints (edges) with respect to the global poses in nodes.

Parameters:

std::exception

On global poses not in nodes

See also:

getEdgeSquareError

void extractSubGraph(
    const std::set<TNodeID>& node_IDs,
    self_t* sub_graph,
    const TNodeID root_node_in = INVALID_NODEID,
    bool auto_expand_set = true
    ) const

Find the edges between the nodes in the node_IDs set and fill given graph pointer accordingly.

Parameters:

node_IDs	Set of nodes, between which, edges should be found and inserted in the given sub_graph pointer
root_node_in	Node ID to be used as the root node of sub_graph. If this is not given, the lowest nodeID is to be used.
CNetworkOfPoses	pointer that is to be filled.
auto_expand_set	If true and in case the node_IDs set contains non-consecutive nodes the returned set is expanded with the in-between nodes. This makes sure that the final graph is always connected. If auto_expand_set is false but there exist non-consecutive nodes, virtual edges are inserted in the parts that the graph is not connected

void mergeGraph(
    const self_t& other,
    const typename std::vector<detail::THypothesis<self_t>>& common_hypots,
    const bool hypots_from_other_to_self = true,
    std::map<TNodeID, TNodeID>* old_to_new_nodeID_mappings_out = nullptr
    )

Integrate given graph into own graph using the list of provided common THypotheses.

Nodes of the other graph are renumbered upon integration in own graph.

Parameters:

other	Graph (of the same type) that is to be integrated with own graph.
Hypotheses	that join own and other graph.
hypots_from_other_to_self	Specify the direction of the THypothesis objects in the common_hypots. If true (default) they are directed from other to own graph (other rightarrow own),
old_to_new_nodeID_mappings_out	Map from the old nodeIDs that are in the given graph to the new nodeIDs that have been inserted (by this method) in own graph.

double getEdgeSquareError(
    const typename BASE::edges_map_t::const_iterator& itEdge,
    bool ignoreCovariances = true
    ) const

Computes the square error of one pose constraints (edge) with respect to the global poses in nodes If ignoreCovariances is false, the squared Mahalanobis distance will be computed instead of the straight square error.

Parameters:

std::exception

On global poses not in nodes

double getEdgeSquareError(
    const mrpt::graphs::TNodeID from_id,
    const mrpt::graphs::TNodeID to_id,
    bool ignoreCovariances = true
    ) const

Computes the square error of one pose constraints (edge) with respect to the global poses in nodes If ignoreCovariances is false, the squared Mahalanobis distance will be computed instead of the straight square error.

Parameters:

std::exception

On edge not existing or global poses not in nodes

void clear()

Empty all edges, nodes and set root to ID 0.

size_t nodeCount() const

Return number of nodes in the list nodes of global coordinates (may be different that all nodes appearing in edges)

See also:

mrpt::graphs::CDirectedGraph::countDifferentNodesInEdges

void saveToTextFile(const std::string& fileName) const

Saves to a text file in the format used by TORO, HoG-man, G2O.

See: https://www.mrpt.org/Graph-SLAM_maps

Parameters:

On

any error

See also:

saveToBinaryFile, loadFromTextFile, writeAsText

void writeAsText(std::ostream& o) const

Writes as text in the format used by TORO, HoG-man, G2O.

See: https://www.mrpt.org/Graph-SLAM_maps

Parameters:

On

any error

See also:

saveToBinaryFile, loadFromTextFile, saveToTextFile, readAsText

void loadFromTextFile(
    const std::string& fileName,
    bool collapse_dup_edges = true
    )

Loads from a text file in the format used by TORO & HoG-man (more on the format here) Recognized line entries are: VERTEX2, VERTEX3, EDGE2, EDGE3, EQUIV.

If an unknown entry is found, a warning is dumped to std::cerr (only once for each unknown keyword). An exception will be raised if trying to load a 3D graph into a 2D class (in the opposite case, missing 3D data will default to zero).

Parameters:

fileName	The file to load.
collapse_dup_edges	If true, collapseDuplicatedEdges will be called automatically after loading (note that this operation may take significant time for very large graphs).
On	any error, as a malformed line or loading a 3D graph in a 2D graph.

See also:

loadFromBinaryFile, saveToTextFile

void readAsText(std::istream& i)

Reads as text in the format used by TORO, HoG-man, G2O.

See: https://www.mrpt.org/Graph-SLAM_maps

Parameters:

On

any error

See also:

saveToBinaryFile, loadFromTextFile, saveToTextFile