CNetworkOfPoses_impl.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                       http://www.mrpt.org/                                |
   |                                                                           |
   |   Copyright (C) 2005-2011  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Machine Perception and Intelligent    |
   |      Robotics Lab, University of Malaga (Spain).                          |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#ifndef CONSTRAINED_POSE_NETWORK_IMPL_H
#define CONSTRAINED_POSE_NETWORK_IMPL_H

#include <mrpt/graphs/dijkstra.h>
#include <mrpt/utils/CTextFileLinesParser.h>


namespace mrpt
{
        namespace graphs
        {
                namespace detail
                {
                        using namespace std;
                        using namespace mrpt;
                        using namespace mrpt::utils;
                        using namespace mrpt::poses;
                        using namespace mrpt::graphs;

                        template <class POSE_PDF> struct TPosePDFHelper
                        {
                                static inline void copyFrom2D(POSE_PDF &p, const CPosePDFGaussianInf &pdf ) { p.copyFrom( pdf ); }
                                static inline void copyFrom3D(POSE_PDF &p, const CPose3DPDFGaussianInf &pdf ) { p.copyFrom( pdf ); }
                        };
                        template <> struct TPosePDFHelper<CPose2D>
                        {
                                static inline void copyFrom2D(CPose2D &p, const CPosePDFGaussianInf &pdf ) { p = pdf.mean; }
                                static inline void copyFrom3D(CPose2D &p, const CPose3DPDFGaussianInf &pdf ) { p = CPose2D(pdf.mean); }
                        };
                        template <> struct TPosePDFHelper<CPose3D>
                        {
                                static inline void copyFrom2D(CPose3D &p, const CPosePDFGaussianInf &pdf ) { p = pdf.mean; }
                                static inline void copyFrom3D(CPose3D &p, const CPose3DPDFGaussianInf &pdf ) { p = pdf.mean; }
                        };

                        /// a helper struct with static template functions \sa CNetworkOfPoses
                        template <class graph_t>
                        struct graph_ops
                        {
                                static void write_VERTEX_line(const TNodeID id, const CPose2D &p, std::ofstream &f)
                                {
                                        //  VERTEX2 id x y phi
                                        f << "VERTEX2 " << id << format(" %.04f %.04f %.04f\n",p.x(),p.y(),p.phi() );
                                }
                                static void write_VERTEX_line(const TNodeID id, const CPose3D &p, std::ofstream &f)
                                {
                                        //  VERTEX3 id x y z roll pitch yaw
                                        // **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.
                                        f << "VERTEX3 " << id << format(" %.04f %.04f %.04f %.04f %.04f %.04f\n",p.x(),p.y(),p.z(),p.roll(),p.pitch(),p.yaw() );
                                }


                                static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPosePDFGaussianInf & edge, std::ofstream &f)
                                {
                                        //  EDGE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp inf_xp inf_yp
                                        // **CAUTION** TORO docs say "from_id" "to_id" in the opposite order, but it seems from the data that this is the correct expected format.
                                        f << "EDGE2 " << edgeIDs.first << " " << edgeIDs.second << " " <<
                                                //format(" %.06f %.06f %.06f %e %e %e %e %e %e\n",
                                                        edge.mean.x()<<" "<<edge.mean.y()<<" "<<edge.mean.phi()<<" "<<
                                                        edge.cov_inv(0,0)<<" "<<edge.cov_inv(0,1)<<" "<<edge.cov_inv(1,1)<<" "<<
                                                        edge.cov_inv(2,2)<<" "<<edge.cov_inv(0,2)<<" "<<edge.cov_inv(1,2) << endl;
                                }
                                static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPose3DPDFGaussianInf & edge, std::ofstream &f)
                                {
                                        //  EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
                                        // **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.
                                        // **CAUTION** TORO docs say "from_id" "to_id" in the opposite order, but it seems from the data that this is the correct expected format.
                                        f << "EDGE3 " << edgeIDs.first << " " << edgeIDs.second << " " <<
                                                //format(" %.06f %.06f %.06f %.06f %.06f %.06f %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e\n",
                                                        edge.mean.x()<<" "<<edge.mean.y()<<" "<<edge.mean.z()<<" "<<
                                                        edge.mean.roll()<<" "<<edge.mean.pitch()<<" "<<edge.mean.yaw()<<" "<<
                                                        edge.cov_inv(0,0)<<" "<<edge.cov_inv(0,1)<<" "<<edge.cov_inv(0,2)<<" "<<edge.cov_inv(0,5)<<" "<<edge.cov_inv(0,4)<<" "<<edge.cov_inv(0,3)<<" "<<
                                                        edge.cov_inv(1,1)<<" "<<edge.cov_inv(1,2)<<" "<<edge.cov_inv(1,5)<<" "<<edge.cov_inv(1,4)<<" "<<edge.cov_inv(1,3)<<" "<<
                                                        edge.cov_inv(2,2)<<" "<<edge.cov_inv(2,5)<<" "<<edge.cov_inv(2,4)<<" "<<edge.cov_inv(2,3)<<" "<<
                                                        edge.cov_inv(5,5)<<" "<<edge.cov_inv(5,4)<<" "<<edge.cov_inv(5,3)<<" "<<
                                                        edge.cov_inv(4,4)<<" "<<edge.cov_inv(4,3)<<" "<<
                                                        edge.cov_inv(3,3) << endl;
                                }
                                static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPosePDFGaussian & edge, std::ofstream &f)
                                {
                                        CPosePDFGaussianInf p;
                                        p.copyFrom(edge);
                                        write_EDGE_line(edgeIDs,p,f);
                                }
                                static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPose3DPDFGaussian & edge, std::ofstream &f)
                                {
                                        CPose3DPDFGaussianInf p;
                                        p.copyFrom(edge);
                                        write_EDGE_line(edgeIDs,p,f);
                                }
                                static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPose2D & edge, std::ofstream &f)
                                {
                                        CPosePDFGaussianInf p;
                                        p.mean = edge;
                                        p.cov_inv.unit(3,1.0);
                                        write_EDGE_line(edgeIDs,p,f);
                                }
                                static void write_EDGE_line( const TPairNodeIDs &edgeIDs,const CPose3D & edge, std::ofstream &f)
                                {
                                        CPose3DPDFGaussianInf p;
                                        p.mean = edge;
                                        p.cov_inv.unit(6,1.0);
                                        write_EDGE_line(edgeIDs,p,f);
                                }


                                // =================================================================
                                //                     save_graph_of_poses_from_text_file
                                // =================================================================
                                static void save_graph_of_poses_from_text_file(const graph_t *g, const std::string &fil)
                                {
                                        typedef typename graph_t::constraint_t CPOSE;

                                        std::ofstream  f;
                                        f.open(fil.c_str());
                                        if (!f.is_open())
                                                THROW_EXCEPTION_CUSTOM_MSG1("Error opening file '%s' for writing",fil.c_str());

                                        // 1st: Nodes
                                        for (typename graph_t::global_poses_t::const_iterator itNod = g->nodes.begin();itNod!=g->nodes.end();++itNod)
                                                write_VERTEX_line(itNod->first, itNod->second, f);

                                        // 2nd: Edges:
                                        for (typename graph_t::const_iterator it=g->begin();it!=g->end();++it)
                                                if (it->first.first!=it->first.second)  // Ignore self-edges, typically from importing files with EQUIV's
                                                        write_EDGE_line( it->first, it->second, f);

                                } // end save_graph


                                // =================================================================
                                //                     load_graph_of_poses_from_text_file
                                // =================================================================
                                static void load_graph_of_poses_from_text_file(graph_t *g, const std::string &fil)
                                {
                                        typedef typename graph_t::constraint_t CPOSE;

                                        set<string>  alreadyWarnedUnknowns; // for unknown line types, show a warning to cerr just once.

                                        // First, empty the graph:
                                        g->clear();

                                        // Determine if it's a 2D or 3D graph, just to raise an error if loading a 3D graph in a 2D one, since
                                        //  it would be an unintentional loss of information:
                                        const bool graph_is_3D = CPOSE::is_3D();

                                        CTextFileLinesParser   filParser(fil);  // raises an exception on error

                                        // -------------------------------------------
                                        // 1st PASS: Read EQUIV entries only
                                        //  since processing them AFTER loading the data
                                        //  is much much slower.
                                        // -------------------------------------------
                                        map<TNodeID,TNodeID> lstEquivs; // List of EQUIV entries: NODEID -> NEWNODEID. NEWNODEID will be always the lowest ID number.

                                        // Read & process lines each at once until EOF:
                                        istringstream s;
                                        while (filParser.getNextLine(s))
                                        {
                                                const unsigned int lineNum = filParser.getCurrentLineNumber();
                                                const string lin = s.str();

                                                string key;
                                                if ( !(s >> key) || key.empty() )
                                                        THROW_EXCEPTION(format("Line %u: Can't read string for entry type in: '%s'", lineNum, lin.c_str() ) );

                                                if ( strCmpI(key,"EQUIV") )
                                                {
                                                        // Process these ones at the end, for now store in a list:
                                                        TNodeID  id1,id2;
                                                        if (!(s>> id1 >> id2))
                                                                THROW_EXCEPTION(format("Line %u: Can't read id1 & id2 in EQUIV line: '%s'", lineNum, lin.c_str() ) );
                                                        lstEquivs[std::max(id1,id2)] = std::min(id1,id2);
                                                }
                                        } // end 1st pass

                                        // -------------------------------------------
                                        // 2nd PASS: Read all other entries
                                        // -------------------------------------------
                                        filParser.rewind();

                                        // Read & process lines each at once until EOF:
                                        while (filParser.getNextLine(s))
                                        {
                                                const unsigned int lineNum = filParser.getCurrentLineNumber();
                                                const string lin = s.str();

                                                // Recognized strings:
                                                //  VERTEX2 id x y phi
                                                //  EDGE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp inf_xp inf_yp
                                                //  VERTEX3 id x y z roll pitch yaw
                                                //  EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
                                                //  EQUIV id1 id2
                                                string key;
                                                if ( !(s >> key) || key.empty() )
                                                        THROW_EXCEPTION(format("Line %u: Can't read string for entry type in: '%s'", lineNum, lin.c_str() ) );

                                                if ( strCmpI(key,"VERTEX2") )
                                                {
                                                        TNodeID  id;
                                                        TPose2D  p2D;
                                                        if (!(s>> id >> p2D.x >> p2D.y >> p2D.phi))
                                                                THROW_EXCEPTION(format("Line %u: Error parsing VERTEX2 line: '%s'", lineNum, lin.c_str() ) );

                                                        // Make sure the node is new:
                                                        if (g->nodes.find(id)!=g->nodes.end())
                                                                THROW_EXCEPTION(format("Line %u: Error, duplicated verted ID %u in line: '%s'", lineNum, static_cast<unsigned int>(id), lin.c_str() ) );

                                                        // EQUIV? Replace ID by new one.
                                                        {
                                                                const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(id);
                                                                if (itEq!=lstEquivs.end()) id = itEq->second;
                                                        }

                                                        // Add to map: ID -> absolute pose:
                                                        if (g->nodes.find(id)==g->nodes.end())
                                                        {
                                                                typename CNetworkOfPoses<CPOSE>::constraint_t::type_value & newNode = g->nodes[id];
                                                                newNode = CPose2D(p2D); // Auto converted to CPose3D if needed
                                                        }
                                                }
                                                else if ( strCmpI(key,"VERTEX3") )
                                                {
                                                        if (!graph_is_3D)
                                                                THROW_EXCEPTION(format("Line %u: Try to load VERTEX3 into a 2D graph: '%s'", lineNum, lin.c_str() ) );

                                                        //  VERTEX3 id x y z roll pitch yaw
                                                        TNodeID  id;
                                                        TPose3D  p3D;
                                                        // **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.
                                                        if (!(s>> id >> p3D.x >> p3D.y >> p3D.z >> p3D.roll >> p3D.pitch >> p3D.yaw ))
                                                                THROW_EXCEPTION(format("Line %u: Error parsing VERTEX3 line: '%s'", lineNum, lin.c_str() ) );

                                                        // Make sure the node is new:
                                                        if (g->nodes.find(id)!=g->nodes.end())
                                                                THROW_EXCEPTION(format("Line %u: Error, duplicated verted ID %u in line: '%s'", lineNum, static_cast<unsigned int>(id), lin.c_str() ) );

                                                        // EQUIV? Replace ID by new one.
                                                        {
                                                                const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(id);
                                                                if (itEq!=lstEquivs.end()) id = itEq->second;
                                                        }

                                                        // Add to map: ID -> absolute pose:
                                                        if (g->nodes.find(id)==g->nodes.end())
                                                        {
                                                                g->nodes[id] = typename CNetworkOfPoses<CPOSE>::constraint_t::type_value( CPose3D(p3D) ); // Auto converted to CPose2D if needed
                                                        }
                                                }
                                                else if ( strCmpI(key,"EDGE2") )
                                                {
                                                        //  EDGE2 from_id to_id Ax Ay Aphi inf_xx inf_xy inf_yy inf_pp inf_xp inf_yp
                                                        //                                   s00   s01     s11    s22    s02    s12
                                                        //  Read values are:
                                                        //    [ s00 s01 s02 ]
                                                        //    [  -  s11 s12 ]
                                                        //    [  -   -  s22 ]
                                                        //
                                                        TNodeID  to_id, from_id;
                                                        if (!(s>> from_id >> to_id ))
                                                                THROW_EXCEPTION(format("Line %u: Error parsing EDGE2 line: '%s'", lineNum, lin.c_str() ) );

                                                        // EQUIV? Replace ID by new one.
                                                        {
                                                                const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(to_id);
                                                                if (itEq!=lstEquivs.end()) to_id = itEq->second;
                                                        }
                                                        {
                                                                const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(from_id);
                                                                if (itEq!=lstEquivs.end()) from_id = itEq->second;
                                                        }

                                                        if (from_id!=to_id)     // Don't load self-edges! (probably come from an EQUIV)
                                                        {
                                                                TPose2D  Ap_mean;
                                                                CMatrixDouble33 Ap_cov_inv;
                                                                if (!(s>>
                                                                                Ap_mean.x >> Ap_mean.y >> Ap_mean.phi >>
                                                                                Ap_cov_inv(0,0) >> Ap_cov_inv(0,1) >> Ap_cov_inv(1,1) >>
                                                                                Ap_cov_inv(2,2) >> Ap_cov_inv(0,2) >> Ap_cov_inv(1,2) ))
                                                                        THROW_EXCEPTION(format("Line %u: Error parsing EDGE2 line: '%s'", lineNum, lin.c_str() ) );

                                                                // Complete low triangular part of inf matrix:
                                                                Ap_cov_inv(1,0) = Ap_cov_inv(0,1);
                                                                Ap_cov_inv(2,0) = Ap_cov_inv(0,2);
                                                                Ap_cov_inv(2,1) = Ap_cov_inv(1,2);

                                                                // Convert to 2D cov, 3D cov or 3D inv_cov as needed:
                                                                typename CNetworkOfPoses<CPOSE>::edge_t  newEdge;
                                                                TPosePDFHelper<CPOSE>::copyFrom2D(newEdge, CPosePDFGaussianInf( CPose2D(Ap_mean), Ap_cov_inv ) );
                                                                g->insertEdge(from_id, to_id, newEdge);
                                                        }
                                                }
                                                else if ( strCmpI(key,"EDGE3") )
                                                {
                                                        if (!graph_is_3D)
                                                                THROW_EXCEPTION(format("Line %u: Try to load EDGE3 into a 2D graph: '%s'", lineNum, lin.c_str() ) );

                                                        //  EDGE3 from_id to_id Ax Ay Az Aroll Apitch Ayaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
                                                        TNodeID  to_id, from_id;
                                                        if (!(s>> from_id >> to_id ))
                                                                THROW_EXCEPTION(format("Line %u: Error parsing EDGE3 line: '%s'", lineNum, lin.c_str() ) );

                                                        // EQUIV? Replace ID by new one.
                                                        {
                                                                const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(to_id);
                                                                if (itEq!=lstEquivs.end()) to_id = itEq->second;
                                                        }
                                                        {
                                                                const map<TNodeID,TNodeID>::const_iterator itEq = lstEquivs.find(from_id);
                                                                if (itEq!=lstEquivs.end()) from_id = itEq->second;
                                                        }

                                                        if (from_id!=to_id)     // Don't load self-edges! (probably come from an EQUIV)
                                                        {
                                                                TPose3D  Ap_mean;
                                                                CMatrixDouble66 Ap_cov_inv;
                                                                // **CAUTION** In the TORO graph format angles are in the RPY order vs. MRPT's YPR.
                                                                if (!(s>> Ap_mean.x >> Ap_mean.y >> Ap_mean.z >> Ap_mean.roll >> Ap_mean.pitch >> Ap_mean.yaw ))
                                                                        THROW_EXCEPTION(format("Line %u: Error parsing EDGE3 line: '%s'", lineNum, lin.c_str() ) );

                                                                // **CAUTION** Indices are shuffled to the change YAW(3) <-> ROLL(5) in the order of the data.
                                                                if (!(s>>
                                                                                Ap_cov_inv(0,0) >> Ap_cov_inv(0,1) >> Ap_cov_inv(0,2) >> Ap_cov_inv(0,5) >> Ap_cov_inv(0,4) >> Ap_cov_inv(0,3) >>
                                                                                Ap_cov_inv(1,1) >> Ap_cov_inv(1,2) >> Ap_cov_inv(1,5) >> Ap_cov_inv(1,4) >> Ap_cov_inv(1,3) >>
                                                                                Ap_cov_inv(2,2) >> Ap_cov_inv(2,5) >> Ap_cov_inv(2,4) >> Ap_cov_inv(2,3) >>
                                                                                Ap_cov_inv(5,5) >> Ap_cov_inv(5,4) >> Ap_cov_inv(5,3) >>
                                                                                Ap_cov_inv(4,4) >> Ap_cov_inv(4,3) >>
                                                                                Ap_cov_inv(3,3) ))
                                                                {
                                                                        // Cov may be omitted in the file:
                                                                        Ap_cov_inv.unit(6,1.0);

                                                                        if (alreadyWarnedUnknowns.find("MISSING_3D")==alreadyWarnedUnknowns.end())
                                                                        {
                                                                                alreadyWarnedUnknowns.insert("MISSING_3D");
                                                                                cerr << "[CNetworkOfPoses::loadFromTextFile] " << fil << ":" << lineNum << ": Warning: Information matrix missing, assuming unity.\n";
                                                                        }
                                                                }
                                                                else
                                                                {
                                                                        // Complete low triangular part of inf matrix:
                                                                        for (size_t r=1;r<6;r++)
                                                                                for (size_t c=0;c<r;c++)
                                                                                        Ap_cov_inv(r,c) = Ap_cov_inv(c,r);
                                                                }

                                                                // Convert as needed:
                                                                typename CNetworkOfPoses<CPOSE>::edge_t  newEdge;
                                                                TPosePDFHelper<CPOSE>::copyFrom3D(newEdge, CPose3DPDFGaussianInf( CPose3D(Ap_mean), Ap_cov_inv ) );
                                                                g->insertEdge(from_id, to_id, newEdge);
                                                        }
                                                }
                                                else if ( strCmpI(key,"EQUIV") )
                                                {
                                                        // Already read in the 1st pass.
                                                }
                                                else
                                                {       // Unknown entry: Warn the user just once:
                                                        if (alreadyWarnedUnknowns.find(key)==alreadyWarnedUnknowns.end())
                                                        {
                                                                alreadyWarnedUnknowns.insert(key);
                                                                cerr << "[CNetworkOfPoses::loadFromTextFile] " << fil << ":" << lineNum << ": Warning: unknown entry type: " << key << endl;
                                                        }
                                                }
                                        } // end while

                                } // end load_graph


                                // --------------------------------------------------------------------------------
                                //               Implements: 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.
                                // --------------------------------------------------------------------------------
                                static size_t graph_of_poses_collapse_dup_edges(graph_t *g)
                                {
                                        MRPT_START
                                        typedef typename graph_t::edges_map_t::iterator TEdgeIterator;

                                        // Data structure: (id1,id2) -> all edges between them
                                        //  (with id1 < id2)
                                        typedef map<pair<TNodeID,TNodeID>, vector<TEdgeIterator> > TListAllEdges; // For god's sake... when will ALL compilers support auto!! :'-(
                                        TListAllEdges lstAllEdges;

                                        // Clasify all edges to identify duplicated ones:
                                        for (TEdgeIterator itEd=g->edges.begin();itEd!=g->edges.end();++itEd)
                                        {
                                                // Build a pair <id1,id2> with id1 < id2:
                                                const pair<TNodeID,TNodeID> arc_id = make_pair( std::min(itEd->first.first,itEd->first.second),std::max(itEd->first.first,itEd->first.second) );
                                                // get (or create the first time) the list of edges between them:
                                                vector<TEdgeIterator> &lstEdges = lstAllEdges[arc_id];
                                                // And add this one:
                                                lstEdges.push_back(itEd);
                                        }

                                        // Now, remove all but the first edge:
                                        size_t  nRemoved = 0;
                                        for (typename TListAllEdges::const_iterator it=lstAllEdges.begin();it!=lstAllEdges.end();++it)
                                        {
                                                const size_t N = it->second.size();
                                                for (size_t i=1;i<N;i++)  // i=0 is NOT removed
                                                        g->edges.erase( it->second[i] );

                                                if (N>=2) nRemoved+=N-1;
                                        }

                                        return nRemoved;
                                        MRPT_END
                                } // end of graph_of_poses_collapse_dup_edges

                                // --------------------------------------------------------------------------------
                                //               Implements: dijkstra_nodes_estimate
                                //
                                //      Compute 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.
                                //      Note that "global" coordinates are with respect to the node with the ID specified in \a root.
                                // --------------------------------------------------------------------------------
                                static void graph_of_poses_dijkstra_init(graph_t *g)
                                {
                                        MRPT_START

                                        // Do Dijkstra shortest path from "root" to all other nodes:
                                        typedef CDijkstra<graph_t,typename graph_t::maps_implementation_t> dijkstra_t;
                                        typedef typename graph_t::constraint_t  constraint_t;

                                        dijkstra_t dijkstra(*g, g->root);

                                        // Get the tree representation of the graph and traverse it
                                        //  from its root toward the leafs:
                                        typename dijkstra_t::tree_graph_t  treeView;
                                        dijkstra.getTreeGraph(treeView);

                                        // This visitor class performs the real job of
                                        struct VisitorComputePoses : public dijkstra_t::tree_graph_t::Visitor
                                        {
                                                graph_t * m_g; // The original graph

                                                VisitorComputePoses(graph_t *g) : m_g(g) { }
                                                virtual void OnVisitNode( const TNodeID parent_id, const typename dijkstra_t::tree_graph_t::Visitor::tree_t::TEdgeInfo &edge_to_child, const size_t depth_level )
                                                {
                                                        const TNodeID  child_id = edge_to_child.id;

                                                        // Compute the pose of "child_id" as parent_pose (+) edge_delta_pose,
                                                        //  taking into account that that edge may be in reverse order and then have to invert the delta_pose:
                                                        if ( (!edge_to_child.reverse && !m_g->edges_store_inverse_poses) ||
                                                                 ( edge_to_child.reverse &&  m_g->edges_store_inverse_poses)
                                                                )
                                                        {       // pose_child = p_parent (+) p_delta
                                                                m_g->nodes[child_id].composeFrom( m_g->nodes[parent_id],  edge_to_child.data->getPoseMean() );
                                                        }
                                                        else
                                                        {       // pose_child = p_parent (+) [(-)p_delta]
                                                                m_g->nodes[child_id].composeFrom( m_g->nodes[parent_id], - edge_to_child.data->getPoseMean() );
                                                        }
                                                }
                                        };

                                        // Remove all global poses but for the root node, which is the origin:
                                        g->nodes.clear();
                                        g->nodes[g->root] = typename constraint_t::type_value();  // Typ: CPose2D() or CPose3D()

                                        // Run the visit thru all nodes in the tree:
                                        VisitorComputePoses  myVisitor(g);
                                        treeView.visitBreadthFirst(treeView.root, myVisitor);

                                        MRPT_END
                                }


                                // Auxiliary funcs:
                                template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPosePDFGaussianInf &p) {
                                        math::wrapToPiInPlace(err[2]);
                                        return mrpt::math::multiply_HCHt_scalar(err,p.cov_inv); // err^t*cov_inv*err
                                }
                                template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPose3DPDFGaussianInf &p) {
                                        math::wrapToPiInPlace(err[3]);
                                        math::wrapToPiInPlace(err[4]);
                                        math::wrapToPiInPlace(err[5]);
                                        return mrpt::math::multiply_HCHt_scalar(err,p.cov_inv); // err^t*cov_inv*err
                                }
                                template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPosePDFGaussian &p) {
                                        math::wrapToPiInPlace(err[2]);
                                        CMatrixDouble33  COV_INV(UNINITIALIZED_MATRIX);
                                        p.cov.inv(COV_INV);
                                        return mrpt::math::multiply_HCHt_scalar(err,COV_INV); // err^t*cov_inv*err
                                }
                                template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPose3DPDFGaussian &p) {
                                        math::wrapToPiInPlace(err[3]);
                                        math::wrapToPiInPlace(err[4]);
                                        math::wrapToPiInPlace(err[5]);
                                        CMatrixDouble66 COV_INV(UNINITIALIZED_MATRIX);
                                        p.cov.inv(COV_INV);
                                        return mrpt::math::multiply_HCHt_scalar(err,COV_INV); // err^t*cov_inv*err
                                }
                                // These two are for simulating maha2 distances for non-PDF types: fallback to squared-norm:
                                template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPose2D &p) {
                                        math::wrapToPiInPlace(err[2]);
                                        return square(err[0])+square(err[1])+square(err[2]);
                                }
                                template <class VEC> static inline double auxMaha2Dist(VEC &err,const CPose3D &p) {
                                        math::wrapToPiInPlace(err[3]);
                                        math::wrapToPiInPlace(err[4]);
                                        math::wrapToPiInPlace(err[5]);
                                        return square(err[0])+square(err[1])+square(err[2])+square(err[3])+square(err[4])+square(err[5]);
                                }


                                static inline double auxEuclid2Dist(const CPose2D &p1,const CPose2D &p2) {
                                        return
                                                square(p1.x()-p2.x())+
                                                square(p1.y()-p2.y())+
                                                square( mrpt::math::wrapToPi(p1.phi()-p2.phi() ) );
                                }
                                static inline double auxEuclid2Dist(const CPose3D &p1,const CPose3D &p2) {
                                        return
                                                square(p1.x()-p2.x())+
                                                square(p1.y()-p2.y())+
                                                square(p1.z()-p2.z())+
                                                square( mrpt::math::wrapToPi(p1.yaw()-p2.yaw() ) )+
                                                square( mrpt::math::wrapToPi(p1.pitch()-p2.pitch() ) )+
                                                square( mrpt::math::wrapToPi(p1.roll()-p2.roll() ) );
                                }

                                // --------------------------------------------------------------------------------
                                //               Implements: detail::graph_edge_sqerror
                                //
                                //      Compute the square error of a single edge, in comparison to the nodes global poses.
                                // --------------------------------------------------------------------------------
                                static double graph_edge_sqerror(
                                        const graph_t *g,
                                        const typename mrpt::graphs::CDirectedGraph<typename graph_t::constraint_t>::edges_map_t::const_iterator &itEdge,
                                        bool ignoreCovariances )
                                {
                                        MRPT_START

                                        // Get node IDs:
                                        const TNodeID from_id = itEdge->first.first;
                                        const TNodeID to_id   = itEdge->first.second;

                                        // And their global poses as stored in "nodes"
                                        typename graph_t::global_poses_t::const_iterator itPoseFrom = g->nodes.find(from_id);
                                        typename graph_t::global_poses_t::const_iterator itPoseTo   = g->nodes.find(to_id);
                                        ASSERTMSG_(itPoseFrom!=g->nodes.end(), format("Node %u doesn't have a global pose in 'nodes'.", static_cast<unsigned int>(from_id)))
                                        ASSERTMSG_(itPoseTo!=g->nodes.end(), format("Node %u doesn't have a global pose in 'nodes'.", static_cast<unsigned int>(to_id)))

                                        // The global poses:
                                        typedef typename graph_t::constraint_t constraint_t;

                                        const typename constraint_t::type_value &from_mean = itPoseFrom->second;
                                        const typename constraint_t::type_value &to_mean   = itPoseTo->second;

                                        // The delta_pose as stored in the edge:
                                        const constraint_t &edge_delta_pose = itEdge->second;
                                        const typename constraint_t::type_value &edge_delta_pose_mean = edge_delta_pose.getPoseMean();

                                        if (ignoreCovariances)
                                        {       // Square Euclidean distance: Just use the mean values, ignore covs.
                                                // from_plus_delta = from_mean (+) edge_delta_pose_mean
                                                typename constraint_t::type_value from_plus_delta(UNINITIALIZED_POSE);
                                                from_plus_delta.composeFrom(from_mean, edge_delta_pose_mean);

                                                // (auxMaha2Dist will also take into account the 2PI wrapping)
                                                return auxEuclid2Dist(from_plus_delta,to_mean);
                                        }
                                        else
                                        {
                                                // Square Mahalanobis distance
                                                // from_plus_delta = from_mean (+) edge_delta_pose (as a Gaussian)
                                                constraint_t from_plus_delta = edge_delta_pose;
                                                from_plus_delta.changeCoordinatesReference(from_mean);

                                                // "from_plus_delta" is now a 3D or 6D Gaussian, to be compared to "to_mean":
                                                //  We want to compute the squared Mahalanobis distance:
                                                //       err^t * INV_COV * err
                                                //
                                                CArrayDouble<constraint_t::type_value::static_size> err;
                                                for (size_t i=0;i<constraint_t::type_value::static_size;i++)
                                                        err[i] = from_plus_delta.getPoseMean()[i] - to_mean[i];

                                                // (auxMaha2Dist will also take into account the 2PI wrapping)
                                                return auxMaha2Dist(err,from_plus_delta);
                                        }
                                        MRPT_END
                                }

                        }; // end of graph_ops<graph_t>

                }// end NS
        }// end NS
} // end NS

#endif