test.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <mrpt/graphs/CNetworkOfPoses.h>
#include <mrpt/graphs/dijkstra.h>
#include <mrpt/gui/CDisplayWindowPlots.h>
#include <mrpt/random.h>
#include <mrpt/system/CTicTac.h>
#include <iostream>
#include <map>

using namespace mrpt;
using namespace mrpt::graphs;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::gui;
using namespace mrpt::random;
using namespace mrpt::system;
using namespace std;

// The type of my Dijkstra problem:
typedef CDijkstra<CNetworkOfPoses2D>
    CMyDijkstra;  // See other options in mrpt::poses::CNetworkOfPoses<>

// Before MRPT 0.9.5 it was:
// typedef CDijkstra<CNetworkOfPoses2D::edge_t> CMyDijkstra;

// adds a new edge to the graph. The edge is annotated with the relative
// position of the two nodes
void addEdge(
    TNodeID from, TNodeID to, const std::map<TNodeID, CPose2D>& real_poses,
    CNetworkOfPoses2D& graph_links)
{
    CPose2D p = real_poses.find(to)->second - real_poses.find(from)->second;
    graph_links.insertEdge(from, to, p);
}

// weight is the distance between two nodes.
double myDijkstraWeight(
    const CMyDijkstra::graph_t& g, const TNodeID from, const TNodeID to,
    const CMyDijkstra::edge_t& edge)
{
    //  return 1;                   // Topological distance
    return edge.norm();  // Metric distance
}

// ------------------------------------------------------
//              TestDijkstra
// ------------------------------------------------------
void TestDijkstra()
{
    CTicTac tictac;
    CNetworkOfPoses2D graph_links;
    CNetworkOfPoses2D::global_poses_t optimal_poses, optimal_poses_dijkstra;
    std::map<TNodeID, CPose2D> real_poses;

    getRandomGenerator().randomize(10);

    // ----------------------------
    // Create a random graph:
    // ----------------------------
    const size_t N_VERTEX = 20;
    const double DIST_THRES = 10;
    const double NODES_XY_MAX = 15;

    vector<double> xs, ys;

    for (size_t j = 0; j < N_VERTEX; j++)
    {
        CPose2D p(
            getRandomGenerator().drawUniform(-NODES_XY_MAX, NODES_XY_MAX),
            getRandomGenerator().drawUniform(-NODES_XY_MAX, NODES_XY_MAX),
            getRandomGenerator().drawUniform(-M_PI, M_PI));
        real_poses[j] = p;

        // for the figure:
        xs.push_back(p.x());
        ys.push_back(p.y());
    }

    // Add some edges
    for (size_t i = 0; i < N_VERTEX; i++)
    {
        for (size_t j = 0; j < N_VERTEX; j++)
        {
            if (i == j) continue;
            if (real_poses[i].distanceTo(real_poses[j]) < DIST_THRES)
                addEdge(i, j, real_poses, graph_links);
        }
    }

    // ----------------------------
    //  Dijkstra
    // ----------------------------
    tictac.Tic();
    const size_t SOURCE_NODE = 0;

    CMyDijkstra myDijkstra(graph_links, SOURCE_NODE, &myDijkstraWeight);

    cout << "Dijkstra took " << tictac.Tac() * 1e3 << " ms for "
         << graph_links.edges.size() << " edges." << endl;

    // Demo of getting the tree representation of
    //  the graph & visit its nodes:
    // ---------------------------------------------------------
    CMyDijkstra::tree_graph_t graphAsTree;
    myDijkstra.getTreeGraph(graphAsTree);

    // Text representation of the tree:
    cout << "TREE:\n" << graphAsTree.getAsTextDescription() << endl;

    struct CMyVisitor : public CMyDijkstra::tree_graph_t::Visitor
    {
        void OnVisitNode(
            const TNodeID parent,
            const CMyDijkstra::tree_graph_t::TEdgeInfo& edge_to_child,
            const size_t depth_level) override
        {
            cout << string(depth_level * 3, ' ');
            cout << edge_to_child.id << endl;
        }
    };

    CMyVisitor myVisitor;

    cout << "Depth-first traverse of graph:\n";
    cout << SOURCE_NODE << endl;
    graphAsTree.visitDepthFirst(SOURCE_NODE, myVisitor);

    cout << endl << "Breadth-first traverse of graph:\n";
    cout << SOURCE_NODE << endl;
    graphAsTree.visitBreadthFirst(SOURCE_NODE, myVisitor);

    // ----------------------------
    // Display results graphically:
    // ----------------------------
    CDisplayWindowPlots win("Dijkstra example");

    win.hold_on();
    win.axis_equal();

    for (TNodeID i = 0; i < N_VERTEX && win.isOpen(); i++)
    {
        if (i == SOURCE_NODE) continue;

        CMyDijkstra::edge_list_t path;

        myDijkstra.getShortestPathTo(i, path);

        cout << "to " << i << " -> #steps= " << path.size() << endl;

        win.setWindowTitle(format(
            "Dijkstra path %u->%u", static_cast<unsigned int>(SOURCE_NODE),
            static_cast<unsigned int>(i)));

        win.clf();

        // plot all edges:
        for (CNetworkOfPoses2D::iterator e = graph_links.begin();
             e != graph_links.end(); ++e)
        {
            const CPose2D& p1 = real_poses[e->first.first];
            const CPose2D& p2 = real_poses[e->first.second];

            vector<double> X(2);
            vector<double> Y(2);
            X[0] = p1.x();
            Y[0] = p1.y();
            X[1] = p2.x();
            Y[1] = p2.y();
            win.plot(X, Y, "k1");
        }

        // Draw the shortest path:
        for (CMyDijkstra::edge_list_t::const_iterator a = path.begin();
             a != path.end(); ++a)
        {
            const CPose2D& p1 = real_poses[a->first];
            const CPose2D& p2 = real_poses[a->second];

            vector<double> X(2);
            vector<double> Y(2);
            X[0] = p1.x();
            Y[0] = p1.y();
            X[1] = p2.x();
            Y[1] = p2.y();
            win.plot(X, Y, "g3");
        }

        // Draw All nodes:
        win.plot(xs, ys, ".b7");
        win.axis_fit(true);

        cout << "Press any key to show next shortest path, close window to "
                "end...\n";
        win.waitForKey();
    }

    win.clear();
}

int main()
{
    try
    {
        TestDijkstra();
        return 0;
    }
    catch (exception& e)
    {
        cout << "MRPT exception caught: " << e.what() << endl;
        return -1;
    }
    catch (...)
    {
        printf("Another exception!!");
        return -1;
    }
}