Create Edges in Boost Graph using Multi Threading

Question

I am trying to create a boost graph with more than 50K nodes (It will map the configuration space of a robot) and I want to create edges between the node using multi threading as it has become a bottleneck for my program. I store all the vertices' index in a hash map so that they are easy for lookup while adding edges. For each vertex I find 5 nearest neighbors that are to be connected.

Also I have disabled parallel edges in the graph and the graph definition is

 using Graph = boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS, VertexProperties, EdgeProperties>;

For finding the nearest neighbours, I use Local Senstivity Hashing (github_repo).

model* myModel;
myModel = new lshEuclidean();

myModel->fit(datapoints, status);  /// training on all leaf nodes that are not in collision

Also before connecting the edges, I insert all the vertices in the graph and also make a hash map so that it is easy to recover the vertex index for adding an edge. (For quick testing, I convert the vector to a string to store in the hashmap, I know this is inefficient and need to make my own hash function)

BoostGraph::VertexProperties vp1;

BoostGraph graph(5);

std::unordered_map<std::string, int> map;
    
    for(int center = 0; center < finalLeafNodes.size(); center++){

    Vec origin = finalLeafNodes[center]->getOrigin();
        std::vector<double> joint_angle = {origin.at(0)*toRadians, origin.at(1)*toRadians, origin.at(2)*toRadians,
                                            origin.at(3)*toRadians, origin.at(4)*toRadians};

        Eigen::VectorXd joint_angle_center;
        joint_angle_center.resize(5);
        joint_angle_center << joint_angle[0], joint_angle[1], joint_angle[2], joint_angle[3], joint_angle[4];

        vp1.joint_angles = joint_angle;
        BoostGraph::Vertex v_center = graph.AddVertex(vp1);
        int vertex_index_center = graph.getVertexIndex(v_center);

        Vec joint_angle_in_vector_degrees = origin;
        
        std::stringstream output;
        std::copy(joint_angle_in_vector_degrees.begin(), joint_angle_in_vector_degrees.end(), std::ostream_iterator<double>(output, " "));

        map[output.str()] = vertex_index_center;

    } 

Then for each vertex node, I find neighbours in a given radius, sort them to nearest neighbour and take top 3/5 and add an edge by finding those neighbours vertex index through the hashmap mentioned above. I also have a local planner that checks if the path between two points will also be collision free or not. If its collision free, edge is added.

neighbors.sort([&query](Item &a, Item &b) -> bool {compare(a, b, query);});

 auto edge = graph.AddEdge(center_iterator->second, neighbour_iterator->second, BoostGraph::EdgeProperties{(double)recursion_index + 1.});

Also I am now trying on a five degree of freedom robot, so the dimension has also increased.

I have tried multi threading with mutex_lock() but its not giving much of a speedup.

Is there a way to create a shared memory object where I can store the all the edges in multi threading and just loop over it to add the edges in the graph so that I don't have parallel edges.

Answer 1

I want to create edges between the node using multi threading as it has become a bottleneck for my program

Frequently the solution will be to change the choice of datastructure or algorithm. And it is quite likely that the time is actually spent doing other things than actually inserting edges.

In some cases you will even want to have a graph model that is just an edge list.

Here's a implementation of your description (using parts of the code from previous questions). In some sense it is straight-forward. In some sense it might show you some advanced algorithm/datastructure ideas. I think it doesn't have the performance bottleneck you are talking about?

Generating Input Data

Let's read vertices from CSV data. Generating 50k input lines:

Live On Coliru: gen.cpp

./a.out > input.txt; wc -l input.txt;  tail input.txt
50000 input.txt
-0.54953,0.309816,1.49314
-1.38758,1.10754,1.12841
0.468204,-1.38628,1.29798
1.44672,-0.600287,-1.1688
1.28432,-1.40215,0.701882
1.4669,-0.215648,-0.404705
-0.701017,-0.130071,-0.62072
1.3742,-0.639261,1.44033
-1.17127,-1.48499,-1.03837
-1.16458,-1.19539,-0.946286

Parsing Vertices From Input Data

Note I included the optimization I suggested in an earlier question:

using JointAngles = std::array<double, 3>;

This also makes it easier later on to use geometry algorithms.

The parsing is not really related to the question, so posted as-is:

template <typename F> size_t read_vertices(std::string_view input, F callback) {
    using namespace boost::spirit::x3;
    using boost::fusion::at_c;

    Vertex n      = 0;
    auto   action = [&](auto& ctx) {
        auto& vv = _attr(ctx);
        callback(JointAngles{at_c<0>(vv), at_c<1>(vv), at_c<2>(vv)});
        n += 1;
    };
    static auto const line = (double_ >> ',' >> double_ >> ',' >> double_)[action];

    parse(begin(input), end(input), skip(blank)[line % (eol | eoi) > (*eol >> eoi)]);
    return n;
}

Note how it is a whitespace tolerant where possible and supports ±inf/nan.

A Spatial Index

Instead of brute-forcing our way, let's use a Spatial Index from Boost Geometry. What this will allow us to do is find the nearest-k points much cheaper than bruteforce.

Firstly, include the relevant headers:

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/adapted/std_array.hpp>
#include <boost/geometry/index/adaptors/query.hpp>
#include <boost/geometry/index/rtree.hpp>

Now, let's tell Boost Geometry about our point type, and define a Tree type:

BOOST_GEOMETRY_REGISTER_STD_ARRAY_CS(bg::cs::cartesian)

namespace bg  = boost::geometry;
namespace bgi = bg::index;
using Tree = bgi::rtree<std::pair<JointAngles, Vertex>, bgi::rstar<16>>;

We choose R* packing algorithm, which should usually give us best nearest() performance at the cost of higher insertion cost:

Actually Read The Graph

Using the parsing function above, let's build the graph and the spatial index tree at once:

int main() {
    // read and index vertices
    Tree  tree;
    Graph graph;

    std::ifstream ifs("input.txt", std::ios::binary);
    std::string const input(std::istreambuf_iterator<char>(ifs), {});

    graph.m_vertices.reserve(50'000);
    auto const n = read_vertices(input, [&](JointAngles ja) {
        tree.insert({ja, add_vertex(VertexProperties{ja}, graph)});
    });
    std::cout << "Parsed " << n << " vertices, indexed: " << tree.size()
              << " graph: " << num_vertices(graph) << "\n";

That's all. Note how each inserted point in the tree carries the vertex descriptor as meta data, so we can correlate vertices with tree nodes.

This code will print, as expected, for our generated input.txt:

Parsed 50000 vertices, indexed: 50000 graph: 50000

Adding 5-nearest edges

Using a bgi query this is pretty simple. Likely this can be optimized, but let's do the naive thing first, just to see whether the performance is reasonable:

// connect 5-degree nearest vertices
size_t added = 0, dups =0;
for (auto& [vja, v] : tree) {
    for (auto& [uja, u] : tree | queried(bgi::nearest(vja, 6))) {
        if (v == u)
            continue;
        auto w       = bg::distance(vja, uja);
        auto [e, ok] = add_edge(v, u, EdgeProperties{w}, graph);
        //std::cout << (ok ? "Added " : "Duplicate ") << e << " weight " << w << "\n";
        (ok? added:dups)++;
    }
}
std::cout << "Total edges added:" << added << " dups:" << dups << "\n";

Note that we omit self-edges, and rely on setS and undirectedS to detect duplicates - which are obviously expected. This prints, for our test data:

Total edges added:150778 dups:99222

BONUS: A* search

Like in your previous question, let's perform an A* search between arbitrary vertices:

// do A* search
std::vector<Vertex> predecessors(n);
std::vector<double> distances(n);

auto vidx      = get(boost::vertex_index, graph); // redundant with vecS
auto pmap      = make_iterator_property_map(predecessors.data(), vidx);
auto dmap      = make_iterator_property_map(distances.data(), vidx);
auto weightmap = get(&EdgeProperties::weight, graph);

std::mt19937 gen(std::random_device{}());
Vertex       start = random_vertex(graph, gen);
Vertex       goal  = random_vertex(graph, gen);

try {
    // call astar named parameter interface
    auto heuristic = [&, gja = graph[goal].joint_angles](Vertex u) {
        return bg::distance(graph[u].joint_angles, gja);
    };
    astar_search( //
        graph, start, heuristic,
        boost::predecessor_map(pmap) //
            .distance_map(dmap)
            .weight_map(weightmap)
            .visitor(goal_visitor{goal}));

    fmt::print("{} -> {}: No path\n", start, goal);
} catch (goal_visitor::found) {
    std::list<Vertex> path;
    for (auto cursor = goal;;) {
        path.push_front(cursor);
        auto previous = std::exchange(cursor, predecessors.at(cursor));
        if (cursor == previous)
            break;
    }

    fmt::print("{} -> {}: {}\n", start, goal, path);
}

As you can see everything is basically unchanged, except the distance_heuristic class has been replaced by the much simpler lambda:

    auto heuristic = [&, gja = graph[goal].joint_angles](Vertex u) {
        return bg::distance(graph[u].joint_angles, gja);
    };

This effectively does the same as your manual heuristic, except potentially smarter - who knows :).

Possible outputs. Doing 1000 random searches took ~1.8s:

Parsed 50000 vertices, indexed: 50000 graph: 50000 0.161082s
Total edges added:150778 dups:99222 0.190395s
7489 -> 8408: [7489, 23635, 34645, 41337, 1725, 46184, 25161, 33297, 30471, 37500, 4073, 30763, 4488, 30949, 9505, 48543, 33639, 35640, 19525, 34765, 18439, 21830, 4170, 27552, 22621, 6327, 8277, 8082, 15932, 23390, 8408]
6968 -> 49906: [6968, 43210, 9331, 36641, 15088, 45635, 47530, 9136, 18177, 30781, 46243, 21125, 12868, 42416, 46187, 24824, 39841, 39095, 13494, 27104, 34973, 49906]
39242 -> 46236: [39242, 34365, 14041, 30310, 8757, 35459, 41035, 32883, 1552, 24120, 43646, 38812, 17835, 14082, 46568, 37492, 17564, 4934, 28288, 20393, 924, 14615, 15993, 39413, 10407, 46236]
--
31949 -> 38708: [31949, 16473, 18328, 20099, 22828, 42868, 46176, 22766, 49370, 17479, 636, 6173, 36367, 32040, 16961, 48438, 18883, 44611, 19468, 4095, 18156, 33083, 12925, 41017, 17514, 17765, 19710, 25790, 46668, 28202, 12010, 39520, 17796, 45443, 9474, 17370, 5071, 27279, 17083, 3503, 11401, 11209, 32403, 23265, 38708]
9895 -> 41286: [9895, 7793, 34802, 28190, 24889, 578, 49750, 20217, 41057, 2637, 24109, 4262, 38363, 11680, 7513, 39893, 21158, 15747, 33531, 11051, 7893, 31583, 45825, 18988, 38405, 13631, 31016, 45820, 9078, 37368, 28401, 14573, 9294, 6214, 28330, 22949, 10575, 41286]
42176 -> 37875: [42176, 12091, 19799, 41080, 47399, 30041, 41714, 10766, 8904, 41305, 4973, 21270, 18139, 29246, 34739, 35599, 11807, 36557, 48764, 9641, 3619, 11747, 34201, 33629, 20414, 24646, 43402, 36831, 7384, 29363, 24768, 33415, 41325, 17709, 32108, 42284, 28683, 5310, 1506, 14339, 27331, 14861, 7152, 37211, 22754, 7602, 48398, 27378, 39577, 37875]
Total search time: 1.79371s

real    0m2,209s
user    0m2,160s
sys     0m0,044s

Complete Benchmark

Live On Coliru

#include <boost/fusion/adapted/std_array.hpp>
#include <boost/spirit/home/x3.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/random.hpp>
#include <chrono>
#include <fmt/ranges.h>
#include <fstream>
#include <random>
static auto now = &std::chrono::steady_clock::now;
using namespace std::chrono_literals;

using JointAngles = std::array<double, 3>;

struct VertexProperties {
    JointAngles joint_angles{0, 0, 0};
};

struct EdgeProperties {
    double weight = 0;
};

using Graph  = boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS,
                                    VertexProperties, EdgeProperties>;
using Vertex = Graph::vertex_descriptor;

template <typename F> size_t read_vertices(std::string_view input, F callback) {
    using namespace boost::spirit::x3;
    using boost::fusion::at_c;

    Vertex n      = 0;
    auto   action = [&](auto& ctx) {
        auto& vv = _attr(ctx);
        callback(JointAngles{at_c<0>(vv), at_c<1>(vv), at_c<2>(vv)});
        n += 1;
    };
    static auto const line = (double_ >> ',' >> double_ >> ',' >> double_)[action];

    parse(begin(input), end(input), skip(blank)[line % (eol | eoi) > (*eol >> eoi)]);
    return n;
}

// visitor that terminates when we find the goal
struct goal_visitor : boost::default_astar_visitor {
    struct found {}; // exception for termination
    Vertex m_goal;

    goal_visitor(Vertex g) : m_goal(g) {}

    template <class Graph> void examine_vertex(Vertex u, Graph&) {
        if (u == m_goal)
            throw found{};
    }
};

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/adapted/std_array.hpp>
#include <boost/geometry/index/adaptors/query.hpp>
#include <boost/geometry/index/rtree.hpp>
namespace bg  = boost::geometry;
namespace bgi = bg::index;
using bgi::adaptors::queried;
BOOST_GEOMETRY_REGISTER_STD_ARRAY_CS(bg::cs::cartesian)
using Tree = bgi::rtree<std::pair<JointAngles, Vertex>, bgi::rstar<16>>;

int main() {
    auto elapsed = [start = now()]() mutable {
        auto n = now();
        return (n - std::exchange(start, n)) / 1.0s;
    };
    // read and index vertices
    Tree  tree;
    Graph graph;

    std::ifstream ifs("input.txt", std::ios::binary);
    std::string const input(std::istreambuf_iterator<char>(ifs), {});

    graph.m_vertices.reserve(50'000);
    auto const n = read_vertices(input, [&](JointAngles ja) {
        tree.insert({ja, add_vertex(VertexProperties{ja}, graph)});
    });
    std::cout << "Parsed " << n << " vertices, indexed: " << tree.size()
              << " graph: " << num_vertices(graph) << " " << elapsed() << "s\n";

    assert(n == tree.size());
    assert(n == num_vertices(graph));

    // connect 5-degree nearest vertices
    size_t added = 0, dups =0;
    for (auto& [vja, v] : tree) {
        for (auto& [uja, u] : tree | queried(bgi::nearest(vja, 6))) {
            if (v == u)
                continue;
            auto w       = bg::distance(vja, uja);
            auto [e, ok] = add_edge(v, u, EdgeProperties{w}, graph);
            //std::cout << (ok ? "Added " : "Duplicate ") << e << " weight " << w << "\n";
            (ok? added:dups)++;
        }
    }
    std::cout << "Total edges added:" << added << " dups:" << dups << " " << elapsed() << "s\n";

    // do A* search
    std::vector<Vertex> predecessors(n);
    std::vector<double> distances(n);

    for (auto i = 0; i < 1'000; ++i) {
        auto vidx      = get(boost::vertex_index, graph); // redundant with vecS
        auto pmap      = make_iterator_property_map(predecessors.data(), vidx);
        auto dmap      = make_iterator_property_map(distances.data(), vidx);
        auto weightmap = get(&EdgeProperties::weight, graph);

        std::mt19937 gen(std::random_device{}());
        Vertex       start = random_vertex(graph, gen);
        Vertex       goal  = random_vertex(graph, gen);

        try {
            // call astar named parameter interface
            auto heuristic = [&, gja = graph[goal].joint_angles](Vertex u) {
                return bg::distance(graph[u].joint_angles, gja);
            };
            astar_search( //
                    graph, start, heuristic,
                    boost::predecessor_map(pmap) //
                    .distance_map(dmap)
                    .weight_map(weightmap)
                    .visitor(goal_visitor{goal}));

            fmt::print("{} -> {}: No path\n", start, goal);
        } catch (goal_visitor::found) {
            std::list<Vertex> path;
            for (auto cursor = goal;;) {
                path.push_front(cursor);
                auto previous = std::exchange(cursor, predecessors.at(cursor));
                if (cursor == previous)
                    break;
            }

            fmt::print("{} -> {}: {}\n", start, goal, path);
        }
    }

    std::cout << "Total search time: " << elapsed() << "s\n";
}

On Coliru, takes a little longer:

Parsed 50000 vertices, indexed: 50000 graph: 50000 0.252916s
Total edges added:150778 dups:99222 0.38979s
43176 -> 2998: [43176, 8919, 27234, 38221, 8714, 2907, 45819, 32924, 33376, 14539, 9174, 19001, 30909, 3923, 36332, 4521, 43005, 31867, 7326, 46231, 20699, 24026, 44641, 21918, 43012, 37366, 2800, 14239, 21197, 26989, 38269, 16522, 25964, 18224, 47148, 21553, 19350, 37546, 41390, 1247, 2998]
19955 -> 30654: [19955, 18833, 24521, 9310, 29015, 5746, 46264, 7706, 4929, 11078, 41910, 30676, 26759, 16638, 3075, 23001, 9322, 38446, 20634, 1120, 30761, 47535, 15750, 10039, 34123, 42874, 22325, 24136, 30285, 34230, 23926, 9978, 4427, 23805, 10436, 41678, 46936, 37189, 30654]
45710 -> 21757: [45710, 45416, 1375, 16480, 21730, 22843, 15897, 33652, 12561, 46834, 23178, 44302, 21027, 15457, 38383, 14716, 26787, 20697, 41752, 42153, 44194, 21757]
--
16543 -> 43355: [16543, 44982, 27516, 6578, 27706, 39013, 35842, 33455, 30460, 22955, 579, 46537, 43224, 6811, 1651, 41054, 21637, 9496, 36577, 21896, 49329, 43355]
2856 -> 24431: [2856, 21766, 1449, 2525, 15156, 6325, 23773, 25733, 48449, 24269, 49865, 34213, 47119, 48167, 12609, 46284, 33395, 10107, 26726, 14078, 28431, 33884, 468, 39873, 42529, 32395, 49457, 44554, 2207, 47678, 4783, 14247, 39638, 8510, 9439, 20570, 18018, 34614, 37184, 17579, 49921, 8755, 44316, 24431]
17195 -> 21888: [17195, 38851, 28287, 18829, 14051, 28305, 32206, 11044, 6989, 30201, 49002, 19410, 6456, 47912, 35145, 9286, 17782, 10294, 14344, 49966, 49634, 5262, 12496, 45270, 20093, 11298, 7202, 15409, 41313, 35934, 14510, 17221, 23121, 49522, 38138, 45948, 43564, 7840, 4456, 32016, 16660, 5832, 7578, 380, 9925, 18908, 38131, 36929, 28073, 21888]
Total search time: 3.41871s