More optimized approach of 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 <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <queue>
#include <algorithm>
#include "Timer.h"
using namespace cv;

#define UNKNOWN_CELL 197
#define FREE_CELL 255
#define OCCUPIED_CELL 0
#define map(x,y) image.data[x*image.cols+y]


typedef struct vertex{
    cv::Point2i id_;
    cv::Point2i from_;

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

} vertex;

struct CompareID
{
    CompareID(cv::Point2i val) : val_(val) {}
    bool operator()(std::pair<double, vertex> const elem) const {
        return val_ == elem.second.id_;
    }
private:
    cv::Point2i val_;
};

bool checkIfNotOutOfBounds(cv::Point2i current, int rows, int cols)
{
    return (current.x >= 0 && current.y >= 0 &&
            current.x < cols && current.y < rows);
}


int main()
{
    //    cv::Mat image = cv::imread("filteredmap1.jpg", CV_LOAD_IMAGE_GRAYSCALE);
    cv::Mat image = cv::Mat(100,100,CV_8UC1);

    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;
        }
    // Create working and visited set.
    std::multimap<double,vertex> working, visited;

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

    // Time the algorithm.
    Timer timer;
    timer.start();
    // Initialize working set
    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 (std::find_if(visited.begin(), visited.end(), CompareID(cv::Point2i(x, y))) == visited.end() &&
                        checkIfNotOutOfBounds(cv::Point2i(x, y), image.rows, image.cols) &&
                        map(x, y) == FREE_CELL)
                {
                    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)
                    {
                        std::multimap<double, vertex>::iterator itCopy = it;
                        ++it;
                        working.erase(itCopy);
                        working.insert(std::make_pair(cost, newVertex));
                    }
                }
            }

        std::multimap<double,vertex>::iterator it = working.begin();

        //        static int i = 0;
        //        std::cerr << "Cycle " << i++ << ":" << currentPair.first << "/" << currentPair.second.id_.x<< "," << currentPair.second.id_.y << std::endl;
        //        for (it = working.begin(); it != working.end(); it++)
        //            std::cerr << "Cost/ID = " << (*it).first << "/" << (*it).second.id_.x << "," << (*it).second.id_.y << std::endl;

        //        std::cerr << "Current = " << current.x << ", " << current.y << std::endl;
    }
    std::cerr << "Time elapsed: " << timer.getElapsedTimeInMilliSec() << " ms";

    // Recover Path
    cv::cvtColor(image, image, CV_GRAY2BGRA);
    int cn = image.channels();
    if (std::find_if(visited.begin(), visited.end(), CompareID(start)) != visited.end())
    {
        std::cerr << "Path found!" << std::endl;
        std::pair <double, vertex> currentPair = *std::find_if(visited.begin(), visited.end(), CompareID(start));

        std::cerr << "Point(x,y) = " << currentPair.second.id_.x << "," << currentPair.second.id_.y << std::endl;
        do
        {
            image.data[currentPair.second.id_.x*cn*image.cols+currentPair.second.id_.y*cn+0] = 0;
            image.data[currentPair.second.id_.x*cn*image.cols+currentPair.second.id_.y*cn+1] = 255;
            image.data[currentPair.second.id_.x*cn*image.cols+currentPair.second.id_.y*cn+2] = 0;
            currentPair = *std::find_if(visited.begin(), visited.end(), CompareID(currentPair.second.from_));
            std::cerr << "Point(x,y) = " << currentPair.second.id_.x << "," << currentPair.second.id_.y << std::endl;
        } while(currentPair.second.id_.x != goal.x || currentPair.second.id_.y != goal.y);

    }
    else
        std::cerr << "Path not found!" << std::endl;

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

I decided to have two sets whose each elements contains:

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 found that multimap may satisfy those needs.

What alternation can I perform in order to improve this program?

And here is the result:

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

Answer 1

The Dijkstra's algorithm, when compared to the A*, will expand more nodes. As a consequence, it might be slower than A*. However, the extra node expansion might be desirable in some cases. For example, in a game where you are moving your AI char to a destination but also wish it to stop and pickup any health items that might be in the path. With Dijkstra's algo, the char will probably find more health items then it would with A*.

Code review:

Those macro constants are a bit ugly. You should use a const (or constexpr if using C++11).

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

The map() macro is also not very pleasant. Plus you have mixed image.data[] use with map() calls. This can be a source of confusion.

I suggest turning it into a function (I also gave it a new name):

int 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];
}

// calling it:
get_cell_at(image, x, y);

A further benefit of this approach is being able to add assertions for debug bounds checking.

typedef struct is not needed in C++:

This:

typedef struct vertex {
    ...
} vertex;

Is not needed in C++. Just define struct vertex { }; and vertex is now a type.

CompareID:

In the functor ComareID, you should be taking the parameter by reference in the () operator, to avoid copying a vertex/double pair just to test it:

bool operator() (const std::pair<double, vertex> & elem) const {
    return val_ == elem.second.id;
}

Miscellaneous:

You have a using namespace cv at the top, but most of your calls to the library are prefixed with cv::. Either use namespace and don't prefix the types with cv:: or don't use namespace and prefix everything with cv::. The latter is preferable.

main() is too big at the moment. This also makes reviewing the code more difficult. If you refactor it into more manageable units, please post a followup question and we will be happy to give you more insights.

Answer 2

1. You should move the actual dijkstra algorithm out of the main function and create a new one that returns the path.

2. your code to erase a found vertex from working is flawed. it is as simple as:

   it=working.erase(it);
   

   but you don't use it after that so there is no need to keep it anyway.

3. You can upgrade the algorithm to A* by guesstimating the cost of the remaining path and comparing the cost_fromBegining + guess_toEnd. As long as the guess is lower than the resulting cost then it will remain correct.