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 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_;
    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;
    // 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_;

        // 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;
                        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;
            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);

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

    cv::imshow("Map with path", image);
    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:

enter image description here

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

  • \$\begingroup\$ You shouldn't edit the original post, you have to post a new question. Read this: meta.codereview.stackexchange.com/questions/1763/… \$\endgroup\$
    – glampert
    Oct 15, 2014 at 17:47
  • \$\begingroup\$ My bad i thought the follow up question should be added here also \$\endgroup\$
    – Xegara
    Oct 15, 2014 at 17:51
  • \$\begingroup\$ @glampert, I removed the revised code. However, I think I still didn't get a clear answer. How do I optimize my code? \$\endgroup\$
    – Xegara
    Oct 16, 2014 at 1:24
  • 1
    \$\begingroup\$ Well, optimizing code is not something obvious. You would have to, first of all, do some profiling/measurement to figure out what to optimize. Can you maybe try using A*? It should be faster. Other things that might be slowing it down are the calls to std::find_if (it's a linear search). Perhaps also try an unordered_multimap instead of multimap. \$\endgroup\$
    – glampert
    Oct 16, 2014 at 1:42
  • 1
    \$\begingroup\$ Definitely, I really think that the std::find_if is the bottleneck since it is linear complexity in nature. I am aware that A* algorithm is faster than Dijkstra's but if my implementation of Dijkstra's is suboptimal. Modifying my code to make it A* algorithm would also be suboptimal. Hence, I first want to improve that bottleneck first. \$\endgroup\$
    – Xegara
    Oct 16, 2014 at 2:11

2 Answers 2


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++:


typedef struct vertex {
} vertex;

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


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;


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.

  • \$\begingroup\$ I revised the code. :) Please read the follow-up question. :) \$\endgroup\$
    – Xegara
    Oct 15, 2014 at 17:18
  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:


    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.

  • \$\begingroup\$ Thank you for your answer. Though applying the change of 1 and 2 would not give boost in the performance, wouldn't it? Is the djikstra's that really slow? Or maybe there exists a more efficient way of implementing this? I'm not even quite sure if multimap is best suited for this. I just took the auto sort in my advantage but finding the id promises a linear complexity since it will be doing linear search which might prove to be the bottleneck of this implementation. Hence, I think choosing the right STL/data structure may promise a great performance \$\endgroup\$
    – Xegara
    Oct 15, 2014 at 10:51
  • 2
    \$\begingroup\$ performance and readability/maintainability are both important, long functions typically don't lend themselves for easy reading \$\endgroup\$ Oct 15, 2014 at 10:55
  • \$\begingroup\$ I was just prototyping my program before I place it in the actual program I'm working. Nevertheless, thank you for the reminder. I also believe that maintainability is equally important too. \$\endgroup\$
    – Xegara
    Oct 15, 2014 at 11:00

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.