More optimized approach of Dijkstra's algorithm

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:

• The neighboring cell is not out of bounds

• The neighboring cell is unvisited.

• The neighboring cell is a free cell which can be checked via the 2-D grid map.

Here is my implementation:

```
#include
#include
#include
#include
#include
#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 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 && cur

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

-
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.

-
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.

StackExchange Code Review Q#66724, answer score: 2