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);elseimg.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;elsereturn 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 &note : 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; }

---

總結

以上是生活随笔為你收集整理的A*算法的c++实现+opencv动态显示的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。