A*算法的c++实现+opencv动态显示
想了解算法原理的可以看 A*算法 这篇博客,个人觉得非常通俗易懂而且很详细。
先看一下效果图吧:蓝色的是找到的路径,其它颜色的找路径过程中遍历的点。
这里贴出代码,关键的地方都有注释。
- 使用的是opencv3的版本,用2的可能要修改一下。
- 这里我规定它只能上下左右走,如果想让它可以斜着走,可以修改getSurroundNotes() 这个方法。
- Astar.h文件是类的声明;Astar.cpp是类的实现;main.cpp是程序入口。
Astar.h
#pragma once
#include<vector>
#include<opencv2/opencv.hpp>
using namespace std;
using namespace cv;
struct Note
{
int x,y;
int F,G,H;
Note *parent;
Note(int _x, int _y) :x(_x), y(_y), F(0), G(0), H(0), parent(NULL) {} //变量初始化
};
class Astar
{
public:
Mat img,resize_img;
void InitAstar(vector<vector<int>> &_map); //初始化图
vector<Note *> GetPath(Note &starNote, Note &endNote);//获得最短的路径
private:
vector<vector<int>> map; //存放地图
VideoWriter writer;
vector<Note *> openList; //开集
vector<Note *> closeList; //闭集
Note *findPath(Note &startNote, Note &endNote);//找最短的路径
vector<Note *> getSurroundNotes(const Note *currentNote) const;//遍历当前点的周围点
bool isReachable(const Note *currentNote, const Note *targetNote) const; //判断某点是否可以用于下一步判断
bool isInList(const vector<Note *> &list, const Note *note) const; //判断开/闭列表中是否包含某点
void deleteNote(vector<Note *> &list,Note *note); //删除点
Note *getLeastFNote(); //从开列表中返回F值最小的节点
int calcG(Note *note);//计算FGH值
int calcH(Note *note, Note *end);
int calcF(Note *note);
};
Astar.cpp
#include<cmath>
#include"Astar.h"
#include<opencv2/opencv.hpp>
using namespace std;
void Astar::InitAstar(vector<vector<int>> &_map)
{
map = _map;
writer.open("Astar.avi", -1, 10, Size(675, 675), true);
img.create(map.size(), map[0].size(), CV_8UC3);
for (int i = 0; i < img.rows; i++)
for (int j = 0; j < img.cols; j++)
{
if(map[i][j]==0)
img.at<Vec3b>(i, j) = Vec3b(255,255,255);
else
img.at<Vec3b>(i, j) = Vec3b(0,0,0);
}
}
bool Astar::isInList(const vector<Note *> &list, const Note *note) const
{
for (auto p : list)
if (p->x == note->x && p->y == note->y)
return true;
return false;
}
bool Astar::isReachable(const Note *currentNote, const Note *targetNote) const
{
//如果点超出地图、不是上下左右、是障碍物、或者在闭列表中,返回false。反之。
if (targetNote->x < 0 || targetNote->x > (int)(map.size() - 1)
|| targetNote->y < 0 || targetNote->y > (int)(map[0].size() - 1)
|| (abs(currentNote->x - targetNote->x) + abs(currentNote->y - targetNote->y))!= 1
|| map[targetNote->x][targetNote->y] == 1
|| isInList(closeList, targetNote))
return false;
else
return true;
}
vector<Note *> Astar::getSurroundNotes(const Note *currentNote) const
{
vector<Note *> surroundNotes;
for (int x = currentNote->x - 1; x <= currentNote->x + 1; ++x)
for (int y = currentNote->y - 1; y <= currentNote->y + 1; ++y)
if (isReachable(currentNote, new Note(x, y)))
surroundNotes.push_back(new Note(x, y));
return surroundNotes;
}
Note *Astar::getLeastFNote()
{
if (!openList.empty())
{
auto minFNote = openList.front();
for (auto ¬e : openList)
if (note->F < minFNote->F)
minFNote = note;
return minFNote;
}
return NULL;
}
int Astar::calcG( Note *note)
{
int parentG = note->parent == NULL ? 0 : note->parent->G; //如果是初始节点,则其父节点是空
return ++parentG;
}
int Astar::calcH(Note *note, Note *end)
{
return abs(end->x - note->x)+ abs(end->y - note->y);
}
int Astar::calcF(Note *note)
{
return note->G + note->H;
}
void Astar::deleteNote(vector<Note *> &list, Note *note)
{
int pos=0;
for (auto i = 0; i != list.size(); ++i)
{
if (list[i]->x == note->x && list[i]->y == note->y)
break;
++pos;
}
list.erase(list.begin()+pos);
}
Note *Astar::findPath(Note &startNote, Note &endNote)
{
img.at<Vec3b>(startNote.x, startNote.y) = Vec3b(0, 0, 255);
img.at<Vec3b>(endNote.x, endNote.y) = Vec3b(0, 0, 255);
openList.push_back(new Note(startNote.x, startNote.y)); //起点放入开集
while (!openList.empty())
{
auto currentNote = getLeastFNote(); //找到F值最小的点
deleteNote(openList, currentNote); //从开集中删除
closeList.push_back(currentNote); //放到关闭集
img.at<Vec3b>(currentNote->x, currentNote->y) = Vec3b(0, 0, 255);
resize(img, resize_img, Size(675, 675), 0, 0, 3);
writer << resize_img;
imshow("find path", resize_img);
waitKey(120);
auto surroundPoints = getSurroundNotes(currentNote);//寻找周围点
for (auto &target : surroundPoints)
{
//对某一个格子,如果它不在开启列表中,加入到开启列表,设置当前格为其父节点,计算FGH
if (!isInList(openList, target))
{
target->parent = currentNote;
target->G = calcG(target);
target->H = calcH(target, &endNote);
target->F = calcF(target);
openList.push_back(target);
img.at<Vec3b>(target->x,target->y) = Vec3b(0, 255, 255);
}
//对某一个格子,它在开启列表中,计算G值, 如果比原来的大, 就什么都不做, 否则设置它的父节点为当前点,并更新G和F
else
{
int tempG = calcG(target);
if (tempG < target->G)
{
target->parent = currentNote;
target->G = tempG;
target->F = calcF(target);
}
}
//如果终点出现在开集中,表明找到了路径,并返回。
if (isInList(openList, &endNote))
return target; //返回列表里的节点指针
}
img.at<Vec3b>(currentNote->x, currentNote->y) = Vec3b(0,255, 0);
resize(img, resize_img, Size(675, 675), 0, 0, 3);
writer << resize_img;
imshow("find path", resize_img);
waitKey(20);
}
return NULL;
}
vector<Note *> Astar::GetPath(Note &starNote, Note &endNote)
{
Note *result = findPath(starNote, endNote);
vector<Note *> path;
//返回路径,如果没找到路径,返回空
while (result)
{
img.at<Vec3b>(result->x, result->y) = Vec3b(255, 0, 0);
resize(img, resize_img, Size(675, 675), 0, 0, 3);
writer << resize_img;
imshow("find path", resize_img);
waitKey(30);
path.insert(path.begin(), result);
result = result->parent;
}
writer.release();
return path;
}
main.cpp
#include<iostream>
#include"Astar.h"
using namespace std;
int main()
{
//创建地图,1代表障碍点,0代表可以走的点
vector<vector<int>> map =
{{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,1, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0,1, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0,1, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0,1, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0,1, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,1, 0, 0, 0 },
{ 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,1, 1, 1, 1 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 },
{ 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0 }};
Astar astar;
astar.InitAstar(map);
//设置起始和结束点
Note start(26, 0);
Note end(0, 26);
//A*算法找寻路径
vector<Note *> path = astar.GetPath(start, end );
//打印路径
cout << "路径坐标点:" << endl;
if (path.empty())
cout << "两点之间不存在路径" << endl;
else
{
for (auto &p : path)
{
cout << '(' << p->x << ',' << p->y << ')';
map[p->x][p->y] = 6;
}
cout << endl;
}
//打印路径图
cout <<"路径图: "<< endl;
for (auto i = 0; i != map.size(); i++) //打印地图
{
for (auto j = 0; j != map[i].size(); j++)
{
cout << map[i][j] << " ";
}
cout << endl;
}
system("pause");
destroyAllWindows();
return 0;
}