Optimizing the Dijkstra's algorithm

Question

I need a graph-search algorithm that is enough in our application of robot navigation and I chose Dijkstra's algorithm.

We are given the gridmap which contains free, occupied and unknown cells where the robot is only permitted to pass through the free cells. The user will input the starting position and the goal position. In return, I will retrieve the sequence of free cells leading the robot from starting position to the goal position which corresponds to the path.

Since executing the dijkstra's algorithm from start to goal would give us a reverse path coming from goal to start, I decided to execute the dijkstra's algorithm backwards such that I would retrieve the path from start to goal.

Starting from the goal cell, I would have 8 neighbors whose cost horizontally and vertically is 1 while diagonally would be sqrt(2) only if the cells are reachable (i.e. not out-of-bounds and free cell).

Here are the rules that should be observe in updating the neighboring cells, the current cell can only assume 8 neighboring cells to be reachable (e.g. distance of 1 or sqrt(2)) with the following conditions:

1. The neighboring cell is not out of bounds
2. The neighboring cell is unvisited.
3. The neighboring cell is a free cell which can be checked via the 2-D grid map.

Here is my implementation:

#include <opencv2/opencv.hpp>
#include <algorithm>
#include "Timer.h"

/// CONSTANTS
static const int UNKNOWN_CELL  = 197;
static const int FREE_CELL     = 255;
static const int OCCUPIED_CELL = 0;

/// STRUCTURES for easier management.
struct vertex {
    cv::Point2i id_;
    cv::Point2i from_;

    vertex(cv::Point2i id, cv::Point2i from)
    {
        id_ = id;
        from_ = from;
    }
};

/// To be used for finding an element in std::multimap STL.
struct CompareID
{
    CompareID(cv::Point2i val) : val_(val) {}
    bool operator()(const std::pair<double, vertex> & elem) const {
        return val_ == elem.second.id_;
    }
private:
    cv::Point2i val_;
};

/// Some helper functions for dijkstra's algorithm.
uint8_t get_cell_at(const cv::Mat & image, int x, int y)
{
    assert(x < image.rows);
    assert(y < image.cols);
    return image.data[x * image.cols + y];
}

/// Some helper functions for dijkstra's algorithm.
bool checkIfNotOutOfBounds(cv::Point2i current, int rows, int cols)
{
    return (current.x >= 0 && current.y >= 0 &&
            current.x < cols && current.y < rows);
}

/// Brief: Finds the shortest possible path from starting position to the goal position
/// Param gridMap: The stage where the tracing of the shortest possible path will be performed.
/// Param start: The starting position in the gridMap. It is assumed that start cell is a free cell.
/// Param goal: The goal position in the gridMap. It is assumed that the goal cell is a free cell.
/// Param path: Returns the sequence of free cells leading to the goal starting from the starting cell.
bool findPathViaDijkstra(const cv::Mat& gridMap, cv::Point2i start, cv::Point2i goal, std::vector<cv::Point2i>& path)
{
    // Clear the path just in case
    path.clear();
    // Create working and visited set.
    std::multimap<double,vertex> working, visited;

    // Initialize working set. We are going to perform the djikstra's
    // backwards in order to get the actual path without reversing the path.
    working.insert(std::make_pair(0, vertex(goal, goal)));

    // Conditions in continuing
    // 1.) Working is empty implies all nodes are visited.
    // 2.) If the start is still not found in the working visited set.
    // The Dijkstra's algorithm
    while(!working.empty() && std::find_if(visited.begin(), visited.end(), CompareID(start)) == visited.end())
    {

        // Get the top of the STL.
        // It is already given that the top of the multimap has the lowest cost.
        std::pair<double, vertex> currentPair = *working.begin();
        cv::Point2i current = currentPair.second.id_;
        visited.insert(currentPair);
        working.erase(working.begin());

        // Check all arcs
        // Only insert the cells into working under these 3 conditions:
        // 1. The cell is not in visited cell
        // 2. The cell is not out of bounds
        // 3. The cell is free
        for (int x = current.x-1; x <= current.x+1; x++)
            for (int y = current.y-1; y <= current.y+1; y++)
            {

                if (checkIfNotOutOfBounds(cv::Point2i(x, y), gridMap.rows, gridMap.cols) &&
                        get_cell_at(gridMap, x, y) == FREE_CELL &&
                        std::find_if(visited.begin(), visited.end(), CompareID(cv::Point2i(x, y))) == visited.end())
                {
                    vertex newVertex = vertex(cv::Point2i(x,y), current);
                    double cost = currentPair.first + sqrt(2);
                    // Cost is 1
                    if (x == current.x || y == current.y)
                        cost = currentPair.first + 1;
                    std::multimap<double, vertex>::iterator it =
                            std::find_if(working.begin(), working.end(), CompareID(cv::Point2i(x, y)));
                    if (it == working.end())
                        working.insert(std::make_pair(cost, newVertex));
                    else if(cost < (*it).first)
                    {
                        working.erase(it);
                        working.insert(std::make_pair(cost, newVertex));
                    }
                }
            }
    }

    // Now, recover the path.
    // Path is valid!
    if (std::find_if(visited.begin(), visited.end(), CompareID(start)) != visited.end())
    {
        std::pair <double, vertex> currentPair = *std::find_if(visited.begin(), visited.end(), CompareID(start));
        path.push_back(currentPair.second.id_);
        do
        {
            currentPair = *std::find_if(visited.begin(), visited.end(), CompareID(currentPair.second.from_));
            path.push_back(currentPair.second.id_);
        } while(currentPair.second.id_.x != goal.x || currentPair.second.id_.y != goal.y);
        return true;
    }
    // Path is invalid!
    else
        return false;

}

int main()
{
    //    cv::Mat image = cv::imread("filteredmap1.jpg", CV_LOAD_IMAGE_GRAYSCALE);
    cv::Mat image = cv::Mat(100,100,CV_8UC1);
    std::vector<cv::Point2i> path;

    for (int i = 0; i < image.rows; i++)
        for(int j = 0; j < image.cols; j++)
        {
            image.data[i*image.cols+j] = FREE_CELL;

            if (j == image.cols/2 && (i > 3 && i < image.rows - 3))
                image.data[i*image.cols+j] = OCCUPIED_CELL;

            //            if (image.data[i*image.cols+j] > 215)
            //                image.data[i*image.cols+j] = FREE_CELL;
            //            else if(image.data[i*image.cols+j] < 100)
            //                image.data[i*image.cols+j] = OCCUPIED_CELL;
            //            else
            //                image.data[i*image.cols+j] = UNKNOWN_CELL;
        }

    // Start top right
    cv::Point2i goal(image.cols-1, 0);
    // Goal bottom left
    cv::Point2i start(0, image.rows-1);

    // Time the algorithm.
    Timer timer;
    timer.start();
    findPathViaDijkstra(image, start, goal, path);
    std::cerr << "Time elapsed: " << timer.getElapsedTimeInMilliSec() << " ms";


    // Add the path in the image for visualization purpose.
    cv::cvtColor(image, image, CV_GRAY2BGRA);
    int cn = image.channels();
    for (int i = 0; i < path.size(); i++)
    {
        image.data[path[i].x*cn*image.cols+path[i].y*cn+0] = 0;
               image.data[path[i].x*cn*image.cols+path[i].y*cn+1] = 255;
               image.data[path[i].x*cn*image.cols+path[i].y*cn+2] = 0;

    }
    cv::imshow("Map with path", image);
    cv::waitKey();
    return 0;
}

For the algorithm implementation, I decided to have two sets namely the visited and working set whose each elements contain:

1. The location of itself in the 2D grid map.
2. The accumulated cost
3. Through what cell did it get its accumulated cost (for path recovery)

And here is the result:

The black pixels represent obstacles, the white pixels represent free space and the green line represents the path computed.

On this implementation, I would only search within the current working set for the minimum value and DO NOT need to scan throughout the cost matrix (where initially, the initially cost of all cells are set to infinity and the starting point 0). Maintaining a separate vector of the working set I think promises a better code performance because all the cells that have cost of infinity is surely to be not included in the working set but only those cells that have been touched.

I also took advantage of the STL which C++ provides. I decided to use the std::multimap since it can store duplicating keys (which is the cost) and it sorts the lists automatically. However, I was forced to use std::find_if() to find the id (which is the row,col of the current cell in the set) in the visited set to check if the current cell is on it which promises linear complexity. I really think this is the bottleneck of the Dijkstra's algorithm.

I am well aware that A* algorithm is much faster than Dijkstra's algorithm but what I wanted to ask is my implementation of Dijkstra's algorithm optimal? Even if I implemented A* algorithm using my current implementation in Dijkstra's which is I believe suboptimal, then consequently A* algorithm will also be suboptimal.

What improvement can I perform? What STL is the most appropriate for this algorithm? Particularly, how do I improve the bottleneck?

Answer 1

You're using a std::multimap for 'working' and 'visited'. That's not great.

The first thing you should do is change visited into a per-vertex flag so you can do your find_if in constant time instead of linear times and also so that operations on the list of visited vertices take constant instead of logarithmic time. You know what all the vertices are and you can map them to small integers trivially, so you can use either a std::vector or a std::bitset.

The second thing you should do is turn working into a priority queue, rather than a balanced binary tree structure, so that operations are a (largish) constant factor faster. std::priority_queue is a barebones binary heap. A higher-radix heap---say quaternary for concreteness---will probably be faster on modern computers due to its reduced depth. Andrew Goldberg suggests some bucket-based data structures; I can dig up references for you if you get to that stage. (They're not too complicated.)

Once you've taken care of these two things, you might look at A* or meet-in-the-middle tricks to speed things up even more.

Answer 2

Your performance is several orders of magnitude worse than it could be because you're using graph search algorithms for what looks like geometry. This geometry is much simpler and less general than the problems that graph search algorithms can solve. Also, with a vertex for every pixel your graph is huge even though it contains basically no information.

I heard you asking "how can I make this better without changing what I'm thinking" but nevertheless I'll tell you a completely different and better approach.

It looks like your robot can only go horizontally, vertically or diagonally. Is that for real or just a side effect of you choosing graph search algorithms? I'll assume the latter and let it go in any direction.

The algorithm goes like this: (0) Represent your obstacles as polygons by listing the corners. Work in real numbers so you can make them as thin as you like. (1) Try for a straight line between the end points. (2) Check if that line goes through an obstacle or not. To do that for any line, show that all corners of any particular obstacle lie on the same side of the line. To do that, translate all points by (-X,-Y) of one end of the line so that that point is at the origin, then rotate until the other point is on the X axis. Now all corners should have the same sign of Y if there's no obstruction. There might be a quicker way just using gradients. (3) If there's an obstruction, propose N two-segment paths going via the N corners of the obstacle. (4) Recurse for all segments, culling any paths with segments that go out of bounds. That won't be a problem unless you have obstacles that go out of bounds. (5) When it stops recursing, you should have a list of locally optimised paths from which you can choose the shortest. (6) If you really want to restrict bearings to multiples of 45 degrees, then you can do this algorithm first and then replace each segment by any 45-only wiggly version that avoids obstacles. We know that such a version exists because you can stay extremely close to the original line by wiggling very often. We also know that all such wiggly paths have the same length.