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 <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:
- The location of itself in the 2D grid map.
- The accumulated cost
- 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.
std::find_if
(it's a linear search). Perhaps also try anunordered_multimap
instead ofmultimap
. \$\endgroup\$