Graph Algorithms in C++
Following article presents fundamental algorithm, which are frequently used while we perform calculations on graphs. We will start with basics like Depth First Search (DFS) and Breadth First Search (BFS). Due to my robotics domain preference I will depict also to commonly used algorithms used in path planning. We will investigate Dijkstra algorithm and A* algorithm.
My C++ implementations of these algorithm can be found on my GitHub. Note, in following article I tried to my best to depict principles. Please, first try to implement these algorithms on your own and eventually compare with my.
Introduction to Graph
Graph is a data structure which can be associated with sort of network connecting a vertices (nodes using tree terminology) connected by edges (links). The graph is similar the tree data structure, however there are distinct features making graph unique and extremely powerful for broad range of applications.
Consider below figure where the graph and tree has been drawn. The main difference between these two common data structures can be discussed as follows.
Beside each node in both structures has the unique ID, the tree has the ROOT node which is associated with the start point of tree. The tree is a hierarchical structure grown from the root down to bottom.
The graph however does not follow that concept and all the nodes can be the the start for our investigations. Let consider the example. The map of the city/country logically is the graph, which connects the cities or homes by different roads, paths (graph edges). Using the same map we can look for the shortest path from place A to place B or from city C to city D, etc. In these cases the start point is the place A or city C.
While using the graph we can use the LOOPS, it means that passing one node we can come back to that node using different way (if exist). This feature does not exist while using the tree.
BFS and DFS algorithms
In this section we will discuss about the the main principles and common usage of these algorithms. We are going to specify the main differences and perform simulations (the implementation of these algorithm you will find on my GitHub).
Please consider below figure which I hope help you to visualize the main principles of traverse using discussed algorithms.
Concept of BFS algorithm is connected directly with Queue, a type of container which operates of FIFO context. FIFO stands for : First Input First Output and can be associate with the memory (container) where items come into one side and are extracted from the other side. Let us consider an example which will exemplified our discussion.
We can consider we need to traverse all the nodes in presented graph. We can start from every nodes since the graph is not hierarchical structure.
We start from 3, so 3 comes to queue and is marked as a visited node. Next we remove (pop) that node from queue container (FIFO) and in the same time we insert (push) the non — visited nodes directly connected to previously poped node. In our case, we insert node nr 6 and 2.
Now the process repeats, we take node nr 6 mark this node as a visited (pop from queue) and check (push) to queue the non — visited nodes connected directly to node 6 and push them to queue. Node nr 4 lands in the queue and it is marked as a visited.
As you consider the current status of queue, we have to evaluate the node 2, which is removed form the queue.
Next, we consider non — visited neighbors of node 2. In this case, we add node 1. Node 4 is visited so not considered
Now, we check the node 4 and add non — visited neigbors. The node 5 is non — visited. Further we check Nodes 1 and 5 which do not have any connection to non-visited nodes so the process can be terminated. We visited all the nodes (traversed the graph).
In both cases (BFS and DFS) I initialized graph as a list (one list for each node). The list (for each node) includes the neighbors to that node. List give direct opportunity to push and pop.
Contrary to BFS a DFS algotithm uses the Stack concept. This type of container operates as a LIFO, which means the element comes in as a last and is poped (goes out) as a first. It can be associates as a “one way door” container.
In order to understand this concept we can consider the same graph like before. We start traverse of the graph also from node nr 3, which is inserted to the stack and marked as a visited (when the visited so we remove the node from stack) Next we consider the non-visited nodes connected to the node 3 (similarly to BFS). Node 2 and 6 come into the stack and are marked at one as visited. Further (difference to BFS) we take and expend the node which is on the top of the stack. Here we have node 6.
Applying the same methodology we mark the node as a visited and check no — visited node connected to 6. In this case we can remove from stack node 6 end push the node 4. Now the node 4 is on the top of stack (and marked as a visited). We remove this node from stack, while expanding by directly linked nodes 1 and 5. We do not consider 2 since the node is still on the stack and marked as a visited. On the top of the stack is 5 which does not have any link to non-visited nodes so the node 5 i removed and we take next node on the stack. There is one non — visited node left. Next we take node 1 from the stack. The node is not expended since there are not more non — visited nodes. Next we take the remaining node on the stack, which in our case in node 2. This operation (stack is empty) terminates the traverse of graph. Please consider below figure.
C++ code
There are many applications, where both algorithms can be used. However in order to remember which algorithm matches best for certain application, we have to remember two cases which can be derived for the other scenarios (applications).
BFS is used mainly for the application, where we need to find the shortest path to the node since we expend all neighbors to the the node.
DFS is manly used for the situations like a playing chess or decision related games. We take a one decision (move). That decision (movement) influences on next move. If we go to node 6 (from node 3; see graph) so we do not consider the other option (in this case node 2). We follow the stack order and we look for node 4 , etc.
Dijkstra Algorithm
The main objective for the Dijkstra algorithm is to find the shortest path between any two node (vertices) in graph. We need to notice however, when applying a Dijkstra algorithm we find the shortest path from starting node to other nodes in the graph. We visit all the graph's nodes so it means we can easily extract shortest path between any node in the graph.
Consider below graph, which we can take to our consideration. As you can see the path between nodes includes an information about the distance or cost or other useful value, which provides the estimation of transition effort between nodes.
I would like to mention, the understanding of following algorithm will soon gives you smoothly understanding/deploying the A* algorithm which we are going to discuss in next section.
For the purposes of that algorithm we have to allocate a table. The memory which stores certain values for each node. Additionally we need to store information about visited nodes.
As you can see below we have created table for all existing in the graph nodes. The second column includes the information about the shortest path from the start node (in our case node 0). At the beginning the column is initialized to infinity value. The last column contains an information about the previous node, which indicates the name of previous node for which the shortest path from the start has been computed so far. Please note the information in the table is constantly updated while the algorithm is not terminated (we have to visit all the nodes in the graph).
Let consider our graph. We start from 0. The shortest path to 0 equals 0 since we start form this node.
From node 0 we can transit to node 1 and 7. The cost to node 1 is lower, equals 4 in compare to node node 7, where the cost equal 8.
Now the magic is happen. This is a base of this algorithm.
We need to see how we perform the update of shortest distance (cost) from the start node.
Transition to the node 1 has been perform from node 0. The travel cost to node is 0 (since there is a start node) however, the distance (cost) for traveling equals 4. The total distance (cost) to travel to node 1 equals 0 + 4 = 4. If this calculated value is lower then the value which exist in initialized table (for the certain node; in our case node 1) so we updated information about the shortest path and previous node. In our case we initialized the distance as a infinity so the distance which equals 4 is definitely lower.
Similarity to node 1 we calculate shortest path for the node 7. In this case the calculations are also straight forward. Infinity distance to node 7 is updated by value 8 = 0 + 8 (see the graph).
In both previews cases we transited from node 0 so this information is also included in table. We marked node 0 as a visited and we are not going to visit this node (vertex) again.
Now the next node taken into considerations, is the node which the distance from start node (our case 0) is the shortest. In our case is node 1 has the shortest distance. Next we expand the node 1 by adding to the list the neighbors of node 1. We add node 2 an node 7. The node 0 is not added since it is marked as a visited.
The distance to the node 2 and 7 has to be updated accordingly. For node 2 we updated using previous formula, where we take the distance to reach previous node and the distance to the node we consider. For node 2 we transited form node 2 (for this node, see table we calculated the distance equals 4). The real distance between (see graph) between node 1 and node 2 equals 8 so the total distance equals 12 = 8 + 4. We update our table since the previous value is infinity.
Similar to node 2 we consider node 7. To reach node 7 through node 1 costs (path distance) 15 = 4 + 11. 4 as you can check in the table is the distance to node 1 and the distance between 1 and 7. In this case is 11. Now we need to evaluate the distance to 7 which (for now) can be reach directly from start 0 or through 4. In first case the distance equals 8 (already in our table) and lastly calculated 15. The value of 8 is lower then 15 so we do not updated the table.
Next, as before we take into consideration the non — visited node with the lowest distance to reach from start. We check the table. The lowest value is for node 7 opposite to node node 2 with the value of 12. Please note, the node 1 is marked as a visited.
For the node 7 we expand (add) to the list non — visited neighbors. Node 8 and node 6. The process repeats until all the nodes are visited so the traverse of graph can be terminated.
After the process is terminated the table include all necessary information to extract the shortest path to each node from start node. In order to be more inspired I do really recommend to watch below video on YouTube channel (link at the end of article).
A* Algorithm
The last algorithm, which we are going to discuss it the A* algorithm. Algorithm (and its derivatives) is frequently used for robot path planning where the robot knowing the goal, perceives the environment (using camera, Lidar or other sensors) and takes a decision about next movement. In our example we compute path for the static environment.
At the beginning I would like to point for the certain differences to Dijkstra algorithm. Beside all the edges in the graph include the distances between nodes, however before we start, each node is initialized by heuristics function (in our case the certain value) which simplified the theory can be associated with additional information provided to the system in order to speed up searching process (algorithm converges to the goal faster).
The heuristic function can be computed arbitrary, using different methods and assumptions, however has be consistent (applicable) for the whole environment (nodes) our “robot” moves. Often the extra information (heuristic) is computed as a Manhattan distance between node and goal. As we show later the heuristic can be considered as an extra knowledge in order to estimate the remaining distance to the goal (while our robot transits).
Secondly we need to notices that the Dijkstra algorithm computes the shortest path to all nodes in the graph (all the node have to be visited to the algorithm can be terminated), however A* computes shortest path between two particular nodes (there is not need to visit all the nodes).
The last difference is trivial since we need to simulated (in the software) the table, which as before includes column for all node,s the distance from start node, heuristic — the estimated distance from considered node to the goal. Additionally, for each node, we have to compute (value will be updated while the robot transits and explores the graph) the function:
f(n) = g(n) + h(n),where n is associated with node, g(n) is the cost of the path from the start node to n, and h(n) is a heuristic function that estimates the cost of the shortest (cheapest) path from n to the goal.
The last column holds information (similarly to Dijkstra algorithm) about the previous node which “represents” the shortest distance to reach that node.
The A* algorithm assumes creation (similarly to Dijkstra algorithm) of two list. We need to holds information about visited nodes and neighbors from the the current node we consider.
Let us consider below graph. We will try to find shortest path from node 0 to node 5. Consider the natural distances. Bear also in mind that the “cost” of the distance can include beside the “natural” distance also other factors like type of way (for example the way can be slippery, uphill or completely dark, etc). That “feature” exists in our example. Check the connection between node 3 and destination 6. The cost is 20. We can imagine we have a slippery road and our robot moving on this type of road will use more energy than expected. The algorithm tries to “detect” the anomalies and find the optimal solution. As we can expected the shortest path should avoid this edge.
We start from node 0. Before the robot start (robot control system computes the path) the table has to be filled with heuristic values (for each node).
In our case there is a constant vector (size of number of nodes in the graph).
std::vector<int> heuristic{10, 8, 9, 5, 6, 3, 0};
//eg. heuristic value for node 1 to goal is 8.Next we expend the node 0 (we add to the list) neighbors: node 1 and node 2. For both of these nodes we calculate f(n).
f(n) = g(n) + h(n); //g - current distance from start, h - heuristicf(0) = 0 + 10 = 10;f(1) = 2 + 8 = 10; // 2 - distance from 0 to 1, h = 10 heuristic for node 1f(2) = 10 + 9 = 19; // 10 - distance from 0 to 2, h = 9 heuristic fro node 2
Now we marked node 0 as a visited and choose for further analysis the node with the lowest f(n) value. In our case is node 1 where f(1) = 10.
We expend the node 1 for neighbors: node 3 and node 4 and calculate the f(n) values.
f(3) = (2 + 1) + 5 = 8; // 2 + 1 - distance from 0 to 1 and 1 to 3, h = 5 for node 3 heuristicf(4) = (2 + 5) + 6 = 13; // 2 + 5 - distance from 0 to 1 and 1 to 4, h = 6 for node 4 heuristic
Remember to “update” our virtual table (normally it is done in software — see my code in C++). Node 1 lands as a visited.
We choose again the lowest value which has been computed for the neighbors of node 1. This time we choose the node 3, which than is expended for node 6, node 5 and node 4. Please note, the node 6 is our goal.
f(6) = (2 + 1 + 20) + 0 = 23; // 2 + 1 + 20 - distance from 0 to 1 and 1 to 3 and 3 to 6; h = 0 since node 6 is the goalf(4) = (2 + 1 + 4) + 6 = 13; // 2 + 1 + 4 - distance from 0 to 1, 1 to 3 and 3 to 4; h = 6 for node 4 heuristicf(5) = (2 + 1 + 2) + 3 = 8; // 2 + 1 + 2- distance from 0 to 1, 1 to 3 and 3 to 5; h = 3 for node 5 heuristic
Again we choose the lowest value which has been computed for the neighbors of node 3. This time we choose the node 5, which than is expended for node 6 and node 4. Node 3 is visited. Since the node 6 is our goal we terminated the algorithm. Our robot reach the destination and node 4.
Please, watch also excellent explanation of A* algorithm on following video.
Both recommended links:
I do recommend you to check SOTA YouTube channel:
Thank you for reading.
