boost dijkstra_shortest_paths: can't extract (or find?) the path (path contains a cycle)

Question

I have the following undirected graph classes:

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, Vertex, Edge> Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor VertexDesc;
typedef boost::graph_traits<Graph>::edge_descriptor EdgeDesc;
typedef boost::graph_traits<Graph>::vertex_iterator VertexIter;
typedef boost::graph_traits<Graph>::edge_iterator EdgeIter;

struct GraphContainer
{
    std::map<OcTreeNode*, VertexDesc> revmap;
    Graph graph;
};

after populating the graph with vertices and weighted edges, I try to run Dijkstra and extract one path from start to goal.

Note: the graph is a graph of 3D points in space, and the edge weight is given by:

g->graph[e].weight = 1.0 + pow(coord.distance(coord1), 2) + 4 * pow(coord.z() - coord1.z(), 2);

so it is always positive.

Here is the code to get the solution:

GraphContainer *g = ...;

boost::print_graph(g->graph);

octomap::point3d startPos = ...;
VertexDesc start = closestVertexDescriptor(g, startPos);
std::cout << "start: " << start << " " << startPos << std::endl;

octomap::point3d goalPos = ...;
VertexDesc goal = closestVertexDescriptor(g, goalPos);
std::cout << "goal: " << goal << " " << goalPos << std::endl;

auto idmap = boost::get(boost::vertex_index, g->graph);
std::vector<VertexDesc> predecessors(boost::num_vertices(g->graph));
std::vector<int> distances(boost::num_vertices(g->graph));
dijkstra_shortest_paths(g->graph, goal,
    boost::weight_map(boost::get(&Edge::weight, g->graph))
    .distance_map(boost::make_iterator_property_map(distances.begin(), idmap))
    .predecessor_map(boost::make_iterator_property_map(predecessors.begin(), idmap))
);

// extract path
std::vector<VertexDesc> path;
VertexDesc current = goal;
while(current != start)
{
    std::cout << "extract path: " << current << " " << g->graph[current].coord << " <- " << predecessors[current] << std::endl;
    path.push_back(current);
    if(current == predecessors[current])
    {
        std::cout << "extract path: detected cycle!" << std::endl;
        break;
    }
    current = predecessors[current];
}
path.push_back(start);

outputs: (graph printout is too big, get graph.txt at: https://drive.google.com/file/d/1nbTyCo_-tMfpH4bblurv-IYcumjjuKFK/view?usp=sharing)

start: 8088 (0.725003 1.675 1.21072e-05)
goal: 10302 (2.925 4.025 3.77501)
extract path: 10302 (2.95 4.05 3.75) <- 10302
extract path: detected cycle!

with code from https://www.ics.uci.edu/~eppstein/161/python/dijkstra.py plus the following snippet:

G = {}
def add_edge(v0,v1,w=1):
    global G
    for v in (v0,v1):
        if v not in G: G[v] = {}
    G[v0][v1] = w
    G[v1][v0] = w
with open('graph.txt','rt') as f:
    for line in f:
        v0, vs = line.rstrip().split(' <--> ')
        for v1 in vs.split():
            add_edge(v0,v1)
start = '8088'
goal = '10302'
print(' '.join(shortestPath(G, start, goal)))

I verified that a solution exists (using unitary weights, as boost::print_graph does not print weights, but it should not be relevant):

8088 7803 7783 7759 5811 5809 5799 5796 5712 5702 5674 5666 2168 2009 1985 1967 1934 1928 1906 1900 1633 1626 1597 1617 1187 1303 1299 1319 1283 1370 1420 1448 1463 4942 4963 4969 4972 5083 5089 5100 5107 5421 5430 5443 5445 5480 5500 5528 5994 5998 6000 6001 6016 6194 6196 6198 6199 6200 6202 6204 6206 6208 6210 6212 6214 8368 8370 9790 9792 10010 10021 10026 10053 10094 10098 10110 10122 10190 10202 10302

so why with boost the solution is not found and/or the path contains a cycle?

score 7 · Accepted Answer · answered Jan 21 '18 at 21:13

dijkstra_shortest_paths initializes the distance and predecessor maps. What you're seeing is likely the effect that a vertex has no predecessor, hence its predecessor is itself. That terminates your loop and (falsely) reports a cycle.

It's easy to reproduce by creating a graph with no edges at all.

Moreover, I noticed you accidentally passed the goal to dijkstra_shortest_paths where you should have passed start:

dijkstra_shortest_paths(gc.graph, start,
    boost::weight_map(boost::get(&Edge::weight, gc.graph))
    .distance_map(boost::make_iterator_property_map(distances.begin(), idmap))
    .predecessor_map(boost::make_iterator_property_map(predecessors.begin(), idmap))
);

Now, finally, if you want the weight of the edges to be that distance function, consider using a distance function:

BONUS DEMO

Changes for illustration:

using Boost Geometry rtree instead of octomap (unknown to me). It has nearest point queries and the distance function
Using a dynamic weight_map() function
Encapsulating closest(), find_path(), print(), and adding save_dot to allow visualization
Using "random" generated graph of 10 vertices with random points inside {-5,-5,-5}x{+5,+5,+5}, with 10:50 (1 in 6) possible edges added. The random seed has been fixed to achieve the demo below (uncomment random_device to allow random data)

int main() {
    GraphContainer gc = GraphContainer::generate_random();

    gc.print();

    auto start = gc.closest({1,1,1}),
         goal  = gc.closest({10,10,10});

    auto coordmap = get(&Vertex::coord, gc.graph());

    std::cout << "start: " << start << " " << coordmap[start] << "\n";
    std::cout << "goal:  " << goal  << " " << coordmap[goal]  << "\n";

    PathType const path = gc.find_path(start, goal);

    std::cout << "Path: ";
    for (auto vd : path)
        std::cout << vd << " (" << coordmap[vd] << ") ";

    if (path.front() != start)
        std::cout << "PATH INCOMPLETE\n";

    std::cout << "\n";

    gc.save_dot("graph.dot", path);
}

Notice how the code is clean and structured. Simple helper functions are simple:

#include <boost/graph/graph_utility.hpp>

void GraphContainer::print() const {
    print_graph(_graph);
}

Finding the nearest vertex:

VertexDesc closest(octomap::point3d const& where) const {
    octomap::point3d closest[1];
    size_t n = bgi::query(_tree, bgi::nearest(where, 1), closest);
    assert(1 == n);
    return revmap.at(*closest); // throws if missing revmap entry
}

Finding a path directly from coordinates:

PathType find_path(octomap::point3d const& startPos, octomap::point3d const& goalPos) const {
    return find_path(closest(startPos), closest(goalPos));
}

The weights get calculated on the fly using the boost::geometry::distance implementation:

struct Weight {
    Graph const& g;

    double operator()(EdgeDesc const& ed) const {
        auto s = g[source(ed, g)].coord,
             t = g[target(ed, g)].coord;

        return bg::distance(s, t);
    }
};

boost::function_property_map<Weight, EdgeDesc, double> weight_map() const {
    return boost::make_function_property_map<EdgeDesc>(Weight{_graph});
}

In the end we save the graph in GraphViz format to be rendered with e.g. dot:

#include <boost/graph/graphviz.hpp>
void GraphContainer::save_dot(std::string const& fname, PathType const& mark) const {
    boost::dynamic_properties dp;
    auto idmap = boost::get(boost::vertex_index, _graph);

    dp.property("node_id", idmap);
    dp.property("label", weight_map());

    auto emphasis = [&mark](VertexDesc vd) { return std::count(mark.begin(), mark.end(), vd); };
    dp.property("color", boost::make_transform_value_property_map([=](VertexDesc vd) {
            return emphasis(vd)? "red" : "black";
        }, idmap));
    dp.property("color", boost::make_function_property_map<EdgeDesc>([=](EdgeDesc ed) {
            return emphasis(source(ed, _graph)) && emphasis(target(ed, _graph))? "red" : "black";
        }));

    std::ofstream ofs(fname);
    write_graphviz_dp(ofs, _graph, dp);
}

It marks the nodes/edges involved in the path in red:

Full Listing

Live On Coliru

#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map/transform_value_property_map.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

namespace octomap {
    struct point3d { double x,y,z; };

    static inline std::ostream& operator<<(std::ostream& os, point3d const& pt) {
        return os << "{" << pt.x << "," << pt.y << "," << pt.z << "}";
    }
}

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/index/rtree.hpp>

namespace bg = boost::geometry;

BOOST_GEOMETRY_REGISTER_POINT_3D(octomap::point3d, double, bg::cs::cartesian, x, y, z)

#include <random>
static std::mt19937 s_prng { 2 }; // { std::random_device{}() };
namespace bgi = bg::index;

namespace MockupTree {
    using octomap::point3d;
    using Tree = bgi::rtree<point3d, bgi::rstar<32> >;

    Tree generate_random() {
        std::uniform_real_distribution<double> dist(-5, 5);
        auto gen = [&] { return point3d { dist(s_prng), dist(s_prng), dist(s_prng) }; };

        Tree tree;
        for (auto i = 0; i < 10; ++i)
            tree.insert(gen());

        return tree;
    }
}

using Tree = MockupTree::Tree;
using OcTreeNode = const Tree::value_type;

struct Vertex {
    octomap::point3d coord;
    OcTreeNode *node;
};

typedef boost::adjacency_list<boost::listS, boost::vecS, boost::undirectedS, Vertex> Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor VertexDesc;
typedef boost::graph_traits<Graph>::edge_descriptor EdgeDesc;
typedef boost::graph_traits<Graph>::vertex_iterator VertexIter;
typedef boost::graph_traits<Graph>::edge_iterator EdgeIter;

using PathType = std::vector<VertexDesc>;

class GraphContainer {
  private:
    GraphContainer() = default;

  public:
    static GraphContainer generate_random() {
        GraphContainer result;
        result._tree = MockupTree::generate_random();
        auto& g = result._graph;

        // add all the vertices (also in revmap)
        for (auto& pt : result._tree) {
            VertexDesc vd = add_vertex(Vertex{ pt, nullptr }, g);
            auto insertion = result.revmap.emplace(pt, vd);
            assert(insertion.second); // no duplicate points please
        }


        // add random part of edges
        std::discrete_distribution<> p({50, 10}); // 1 out of 6

        for (auto from : boost::make_iterator_range(vertices(g)))
            for (auto to : boost::make_iterator_range(vertices(g))) {
                if (from != to && p(s_prng)) {
                    auto e = boost::edge(from, to, g);
                    if (!e.second) // edge didn't exist yet
                        add_edge(from, to, g);
                }
            }

        return result;
    }

    Graph const& graph() const { return _graph; }

    VertexDesc closest(octomap::point3d const& where) const {
        octomap::point3d closest[1];
        size_t n = bgi::query(_tree, bgi::nearest(where, 1), closest);
        assert(1 == n);
        return revmap.at(*closest); // throws if missing revmap entry
    }

    PathType find_path(octomap::point3d const& startPos, octomap::point3d const& goalPos) const {
        return find_path(closest(startPos), closest(goalPos));
    }

    PathType find_path(VertexDesc start, VertexDesc goal) const {

        auto idmap = boost::get(boost::vertex_index, _graph);

        std::vector<VertexDesc> predecessors(boost::num_vertices(_graph), _graph.null_vertex());
        std::vector<int> distances(boost::num_vertices(_graph));

        dijkstra_shortest_paths(_graph, start,
            boost::weight_map(weight_map())
            .distance_map(boost::make_iterator_property_map(distances.begin(), idmap))
            .predecessor_map(boost::make_iterator_property_map(predecessors.begin(), idmap))
        );

        // extract path
        VertexDesc current = goal;
        PathType path { current };

        do {
            auto const pred = predecessors.at(current);

            //std::cout << "extract path: " << current << " " << _graph[current].coord << " <- " << pred << std::endl;

            if(current == pred)
                break; 

            current = pred;

            path.push_back(current);
        } while(current != start);

        std::reverse(path.begin(), path.end());

        return path;
    }

    struct Weight {
        Graph const& g;

        double operator()(EdgeDesc const& ed) const {
            auto s = g[source(ed, g)].coord,
                 t = g[target(ed, g)].coord;

            return bg::distance(s, t);
        }
    };

    boost::function_property_map<Weight, EdgeDesc, double> weight_map() const {
        return boost::make_function_property_map<EdgeDesc>(Weight{_graph});
    }

    void print() const;
    void save_dot(std::string const& fname, PathType const& emphasize = {}) const;

  private:
    Tree _tree;
    Graph _graph;

    //std::map<OcTreeNode*, VertexDesc> revmap;
    struct PointCompare {
        bool operator()(octomap::point3d const& a, octomap::point3d const& b) const {
            return std::tie(a.x, a.y, a.z) < std::tie(b.x, b.y, b.z);
        }
    };

    std::map<octomap::point3d, VertexDesc, PointCompare> revmap;
};

#include <boost/graph/graph_utility.hpp>

void GraphContainer::print() const {
    print_graph(_graph);
}

#include <boost/graph/graphviz.hpp>
void GraphContainer::save_dot(std::string const& fname, PathType const& mark) const {
    boost::dynamic_properties dp;
    auto idmap = boost::get(boost::vertex_index, _graph);

    dp.property("node_id", idmap);
    dp.property("label", weight_map());

    auto emphasis = [&mark](VertexDesc vd) { return std::count(mark.begin(), mark.end(), vd); };
    dp.property("color", boost::make_transform_value_property_map([=](VertexDesc vd) {
            return emphasis(vd)? "red" : "black";
        }, idmap));
    dp.property("color", boost::make_function_property_map<EdgeDesc>([=](EdgeDesc ed) {
            return emphasis(source(ed, _graph)) && emphasis(target(ed, _graph))? "red" : "black";
        }));

    std::ofstream ofs(fname);
    write_graphviz_dp(ofs, _graph, dp);
}

int main() {
    GraphContainer gc = GraphContainer::generate_random();

    gc.print();

    auto start = gc.closest({1,1,1}),
         goal  = gc.closest({10,10,10});

    auto coordmap = get(&Vertex::coord, gc.graph());

    std::cout << "start: " << start << " " << coordmap[start] << "\n";
    std::cout << "goal:  " << goal  << " " << coordmap[goal]  << "\n";

    PathType const path = gc.find_path(start, goal);

    std::cout << "Path: ";
    for (auto vd : path)
        std::cout << vd << " (" << coordmap[vd] << ") ";

    if (path.front() != start)
        std::cout << "PATH INCOMPLETE\n";

    std::cout << "\n";

    gc.save_dot("graph.dot", path);
}

Console Output:

0 <--> 7 1 3 6 9 
1 <--> 0 4 3 9 
2 <--> 4 7 9 
3 <--> 0 1 7 
4 <--> 1 2 5 9 
5 <--> 4 6 9 
6 <--> 5 0 7 9 
7 <--> 0 3 6 2 
8 <--> 9 
9 <--> 4 6 8 0 1 2 5 
start: 4 {-0.0143883,0.86797,2.19754}
goal:  3 {1.32738,3.18227,1.83026}
Path: 4 ({-0.0143883,0.86797,2.19754}) 1 ({-0.152509,-1.79464,-3.45573}) 3 ({1.32738,3.18227,1.83026})

Graphviz output in graph.dot:

graph G {
0 [color=black];
1 [color=red];
2 [color=black];
3 [color=red];
4 [color=red];
5 [color=black];
6 [color=black];
7 [color=black];
8 [color=black];
9 [color=black];
0--7  [color=black, label=11.2344];
1--0  [color=black, label=10.4521];
1--4  [color=red, label=6.25045];
2--4  [color=black, label=5.59547];
3--0  [color=black, label=5.32263];
3--1  [color=red, label=7.40954];
3--7  [color=black, label=9.29024];
4--5  [color=black, label=3.96107];
4--9  [color=black, label=6.14037];
5--6  [color=black, label=4.45081];
6--0  [color=black, label=6.51876];
6--7  [color=black, label=8.683];
6--9  [color=black, label=7.2063];
7--2  [color=black, label=5.10893];
8--9  [color=black, label=2.78728];
9--0  [color=black, label=10.3583];
9--1  [color=black, label=3.8217];
9--2  [color=black, label=5.60891];
9--5  [color=black, label=5.63377];
}

you are my hero! I would upvote this ten times if possible. fixing the vertex argument of dijkstra would make it work. however I put the goal as dijkstra argument because I am interested in computing all the paths from all starting positions to the goal, so in my case I had to reverse the start/goal in the path extraction. — fferri, Jan 22 '18 at 08:16
one more question: the distances vector contains distance defined in *number of hops*. can boost provide a distance as *cumulative weight*? I can compute that by navigating the predecessors map and accumulating weights, but maybe there exists something built in into the library? — fferri, Jan 22 '18 at 09:36
docs: ["The shortest path weight from the source vertex s to each vertex in the graph g is recorded in this property map" (and "weight_map: \[...\] The value type for this map must be the same as the value type of the distance map.")](http://www.boost.org/doc/libs/1_66_0/libs/graph/doc/dijkstra_shortest_paths.html). If you're getting hops, you're doing something wrong. — sehe, Jan 22 '18 at 13:21
the values in `std::vector distances` look like number of hops (grows by one at every hop in the path). also, the weight is floating point, not integer; how can this hold the total weight? — fferri, Jan 22 '18 at 16:34
Change it to `std::vector` like the docs I quoted require? Also, check that your weight map is correct. If necessary, post a minimal, self-contained question on the site if you get stuck (here's my code printing the correct cumulative distances: **[Live On Coliru](http://coliru.stacked-crooked.com/a/f5993c2e1a76278c)**) — sehe, Jan 22 '18 at 17:07
Hello, I am writing an algorithm to output the path. I need a program to verify that my output is correct, but I don't want to calculate it manually, so I want to use your code above to output the result and compare it with my own. Can you tell me what the three values of point3d mean? — Kargath, Aug 20 '21 at 08:30
@4756C Not really. It's from the question code and I just assumed them to be 3d cartesian coordinates (hence I used Boost Geomretry to do the closest point query and distance calculation). — sehe, Aug 20 '21 at 10:52
@sehe Thanks, I have completed it through this website https://graphonline.ru/en/. by the way， Does boost have a function that can output the complete path? — Kargath, Aug 20 '21 at 14:53
@4756C No, but my code shows how to do it as well as create a graphviz DOT file based on the information. — sehe, Aug 20 '21 at 15:09

boost dijkstra_shortest_paths: can't extract (or find?) the path (path contains a cycle)

1 Answers1

BONUS DEMO

Full Listing

Linked