A星(A* or A star)算法C++实现及opencv可视化

A*算法,具体原理可参看已有的博客,下面是我觉得比较好的几个。

A*,那个传说中的算法

堪称最好的A*算法

自己在github上找到了一个比较简单的用C++实现的版本(点击打开链接),自己在此基础上添加了opencv绘制简单图块,将结果可视化了,如下图。其中,红色为障碍块,白色绿边为自由空间,蓝色为起始点,黑色为目标点,规划的路径用黄色块表示,程序设定可斜对角穿行。

#include <iostream>
#include <stdlib.h>
#include <algorithm>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;

class CPoint
{
public:
    CPoint(int x,int y):X(x),Y(y),G(0),H(0),F(0),m_parentPoint(NULL){ };
    ~CPoint();
    int X,Y,G,H,F;
    CPoint * m_parentPoint;
    void CalF(){
        F=G+H;
    };
};
class  CAStar
{
    private:
	int m_array[12][12];
	static const int STEP = 10;
    static const int OBLIQUE = 14;
    
	typedef std::vector<CPoint*> POINTVEC;
	POINTVEC m_openVec;
	POINTVEC m_closeVec;
    public:
        CAStar(int array[][12])
        {
            for (int i=0;i<12;i++)
                for(int j=0;j<12;j++)
                    m_array[i][j]=array[i][j];
        }
        CPoint* GetMinFPoint()
        {
            int idx=0,valueF=-9999;
            for(int i=0; i < m_openVec.size(); ++i)
                {
                    if(m_openVec[i]->F < valueF)
                    {
                        valueF = m_openVec[i]->F;
                        idx = i;
                    }
                }
		return m_openVec[idx];
        }

	bool RemoveFromOpenVec(CPoint* point)
	{
		for(POINTVEC::iterator it = m_openVec.begin(); it != m_openVec.end(); ++it)
		{
			if((*it)->X == point->X && (*it)->Y == point->Y)
			{
				m_openVec.erase(it);
				return true;
			}
		}
		return false;
	}
	
	bool canReach(int x, int y)
	{
		return 0 == m_array[x][y];
	}

	bool IsAccessiblePoint(CPoint* point, int x, int y, bool isIgnoreCorner)
	{
		if(!canReach(x, y) || isInCloseVec(x, y))
			return false;
		else
		{
			//可到达的点
			if(abs(x - point->X) + abs(y - point->Y) == 1)    // 左右上下点
				return true;
			else
			{
				if(canReach(abs(x - 1), y) && canReach(x, abs(y - 1)))   // 对角点
					return true;
				else
					return isIgnoreCorner;   //墙的边角
			}
		}
	}

	std::vector<CPoint*> GetAdjacentPoints(CPoint* point, bool isIgnoreCorner)
	{
		POINTVEC adjacentPoints;
		for(int x = point->X-1; x <= point->X+1; ++x)
			for(int y = point->Y-1; y <= point->Y+1; ++y)
			{
				if(IsAccessiblePoint(point, x, y, isIgnoreCorner))
				{
					CPoint* tmpPoint = new CPoint(x, y);
					adjacentPoints.push_back(tmpPoint);
				}
			}

		return adjacentPoints;
	}

	bool isInOpenVec(int x, int y)
	{
		for(POINTVEC::iterator it = m_openVec.begin(); it != m_openVec.end(); ++it)
		{
			if((*it)->X == x && (*it)->Y == y)
				return true;
		}
		return false;
	}

	bool isInCloseVec(int x, int y)
	{
		for(POINTVEC::iterator it = m_closeVec.begin(); it != m_closeVec.end(); ++it)
		{
			if((*it)->X == x && (*it)->Y == y)
				return true;
		}
		return false;
	}
	
	void RefreshPoint(CPoint* tmpStart, CPoint* point)
	{
		int valueG = CalcG(tmpStart, point);
		if(valueG < point->G)
		{
			point->m_parentPoint = tmpStart;
			point->G = valueG;
			point->CalF();
		}
	}
	
	void NotFoundPoint(CPoint* tmpStart, CPoint* end, CPoint* point)
	{
		point->m_parentPoint = tmpStart;
		point->G = CalcG(tmpStart, point);
		point->G = CalcH(end, point);
		point->CalF();
		m_openVec.push_back(point);
	}
	
	int CalcG(CPoint* start, CPoint* point)
	{
		int G = (abs(point->X - start->X) + abs(point->Y - start->Y)) == 2 ? STEP : OBLIQUE;
		int parentG = point->m_parentPoint != NULL ? point->m_parentPoint->G : 0;
		return G + parentG;
	}
	
	int CalcH(CPoint* end, CPoint* point)
	{
		int step = abs(point->X - end->X) + abs(point->Y - end->Y);
		return STEP * step;
	}

	// 搜索路径
	CPoint* FindPath(CPoint* start, CPoint* end, bool isIgnoreCorner)
	{
		m_openVec.push_back(start);
		while(0 != m_openVec.size())
		{
			CPoint* tmpStart = GetMinFPoint();   // 获取F值最小的点
			RemoveFromOpenVec(tmpStart);
			m_closeVec.push_back(tmpStart);
			
			POINTVEC adjacentPoints = GetAdjacentPoints(tmpStart, isIgnoreCorner);
			for(POINTVEC::iterator it=adjacentPoints.begin(); it != adjacentPoints.end(); ++it)
			{
				CPoint* point = *it;
				if(isInOpenVec(point->X, point->Y))   // 在开启列表
					RefreshPoint(tmpStart, point);
				//else if(inCloseVec(point))
				//{
				// 检查节点的g值,如果新计算得到的路径开销比该g值低,那么要重新打开该节点(即重新放入OPEN集)		
				//}
				else
					NotFoundPoint(tmpStart, end, point);
			}
			if(isInOpenVec(end->X, end->Y)) // 目标点已经在开启列表中
			{
				for(int i=0; i < m_openVec.size(); ++i)
				{
					if(end->X == m_openVec[i]->X && end->Y == m_openVec[i]->Y)
						return m_openVec[i];
				}
			}
		}
		return end;
	}


};

int main()
{
    int start_point_x=1;
    int start_point_y=1;
    int goal_point_x=4;
    int goal_point_y=3;
    int array[12][12] = 
    { //  0  1  2  3  4  5  6  7  8  9 10  11    
        { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},// 0 
        { 1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1},// 1
	{ 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1},// 2
	{ 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1},// 3
	{ 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1},// 4
	{ 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1},// 5
	{ 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1},// 6
	{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1} // 7
    };
	
    CAStar* pAStar = new CAStar(array);
    if(array[start_point_x][start_point_y]||array[goal_point_x][goal_point_y])
    {
        cout<<"start point or goal point set error!!!"<<endl;
        return 0;
    }
    CPoint* start = new CPoint(start_point_x, start_point_y);
    CPoint* end = new CPoint(goal_point_x, goal_point_y);
    CPoint* point = pAStar->FindPath(start, end, false);

    Rect rect;
    Point left_up,right_bottom;
    
    Mat img(600,600,CV_8UC3,Scalar(255,255,255));
    namedWindow("Test"); 
    
    while(point != NULL)
    {
        left_up.x = point->Y*50;  //存储数组的列(point->Y)对应矩形的x轴,一个格大小50像素
        left_up.y = point->X*50;  
        right_bottom.x = left_up.x+50;  
        right_bottom.y = left_up.y+50;
        rectangle(img,left_up,right_bottom,Scalar(0,255,255),CV_FILLED,8,0);//path yellow(full)
        std::cout << "(" << point->X << "," << point->Y << ");" << std::endl;
        point = point->m_parentPoint;
        
    }

    for(int i=0;i<8;i++)
    {
        for(int j=0;j<12;j++)
        {   
            left_up.x = j*50; //存储数组的列(j)对应矩形的x轴
            left_up.y = i*50;  
            right_bottom.x = left_up.x+50;  
            right_bottom.y = left_up.y+50;
            if(array[i][j])
            {
                rectangle(img,left_up,right_bottom,Scalar(0,0,255),CV_FILLED,8,0);//obstacles red
            }
            else
            {
                if(i==start_point_x&&j==start_point_y)
                    rectangle(img,left_up,right_bottom,Scalar(255,0,0),CV_FILLED,8,0);//start point blue(full)
                else if(i==goal_point_x&&j==goal_point_y)
                    rectangle(img,left_up,right_bottom,Scalar(0,0,0),CV_FILLED,8,0);//goal point black(full)
                else
                    rectangle(img,left_up,right_bottom,Scalar(0,255,0),2,8,0);//free white content,green edge
            }    
        }
    }

    
    imshow("Test",img);   //窗口中显示图像 
	waitKey(0);
	return 0;
    
    
}

引用:https://github.com/yangxt225/AStar/blob/master/myAstar.cpp

阅读更多
文章标签: A * opencv 可视化
下一篇人工势场法 C++版 及利用 opencv (or matlab)实现可视化
想对作者说点什么? 我来说一句

没有更多推荐了,返回首页

关闭
关闭
关闭