A*算法的c++实现+opencv动态显示

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 &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;
}

---