• ROS1云课→20迷宫不惑之A*大法（一种虽古老但实用全局路径规划算法）

    ROS1云课→19仿真turtlebot（stage）

    19提及的机器人如何实现全局路径规划？A*算法是一种可行的选择。 

    www.gamedev.net/reference/articles/article2003.asp 

    A*算法的基本介绍可以查询网络资源。这里，列出一些案例：

    蓝桥ROS扩展笔记CppRobotics编译_zhangrelay的博客-CSDN博客

    第一种：

    1. /*************************************************************************
    2. 	> File Name: a_star.cpp
    3. 	> Author: TAI Lei
    4. 	> Mail: ltai@ust.hk
    5. 	> Created Time: Sat Jul 20 12:38:43 2019
    6.  ************************************************************************/
    7.  
    8. #include
    9. #include
    10. #include
    11. #include
    12. #include
    13. #include
    14. #include
    15. #include
    16.  
    17. using namespace std;
    18.  
    19.  
    20. class Node{
    21. public:
    22.   int x;
    23.   int y;
    24.   float sum_cost;
    25.   Node* p_node;
    26.  
    27.   Node(int x_, int y_, float sum_cost_=0, Node* p_node_=NULL):x(x_), y(y_), sum_cost(sum_cost_), p_node(p_node_){};
    28. };
    29.  
    30.  
    31. std::vectorfloat> > calc_final_path(Node * goal, float reso, cv::Mat& img, float img_reso){
    32.   std::vector<float> rx;
    33.   std::vector<float> ry;
    34.   Node* node = goal;
    35.   while (node->p_node != NULL){
    36.     node = node->p_node;
    37.     rx.push_back(node->x * reso);
    38.     ry.push_back(node->y * reso);
    39.     cv::rectangle(img,
    40.         cv::Point(node->x*img_reso+1, node->y*img_reso+1),
    41.         cv::Point((node->x+1)*img_reso, (node->y+1)*img_reso),
    42.         cv::Scalar(255, 0, 0), -1);
    43.   }
    44.   return {rx, ry};
    45. }
    46.  
    47.  
    48. std::vectorint> > calc_obstacle_map(
    49.     std::vector<int> ox, std::vector<int> oy,
    50.     const int min_ox, const int max_ox,
    51.     const int min_oy, const int max_oy,
    52.     float reso, float vr,
    53.     cv::Mat& img, int img_reso){
    54.  
    55.   int xwidth = max_ox-min_ox;
    56.   int ywidth = max_oy-min_oy;
    57.  
    58.   std::vectorint> > obmap(ywidth, vector<int>(xwidth, 0));
    59.  
    60.   for(int i=0; i
    61.     int x = i + min_ox;
    62.     for(int j=0; j
    63.       int y = j + min_oy;
    64.       for(int k=0; ksize(); k++){
    65.         float d = std::sqrt(std::pow((ox[k]-x), 2)+std::pow((oy[k]-y), 2));
    66.         if (d <= vr/reso){
    67.           obmap[i][j] = 1;
    68.           cv::rectangle(img,
    69.                         cv::Point(i*img_reso+1, j*img_reso+1),
    70.                         cv::Point((i+1)*img_reso, (j+1)*img_reso),
    71.                         cv::Scalar(0, 0, 0), -1);
    72.           break;
    73.         }
    74.       }
    75.     }
    76.   }
    77.   return obmap;
    78. }
    79.  
    80.  
    81. bool verify_node(Node* node,
    82.                  vectorint> > obmap,
    83.                  int min_ox, int max_ox,
    84.                  int min_oy, int max_oy){
    85.   if (node->x < min_ox || node->y < min_oy || node->x >= max_ox || node->y >= max_oy){
    86.     return false;
    87.   }
    88.  
    89.   if (obmap[node->x-min_ox][node->y-min_oy]) return false;
    90.  
    91.   return true;
    92. }
    93.  
    94.  
    95. float calc_heristic(Node* n1, Node* n2, float w=1.0){
    96.   return w * std::sqrt(std::pow(n1->x-n2->x, 2)+std::pow(n1->y-n2->y, 2));
    97. }
    98.  
    99. std::vector get_motion_model(){
    100.   return {Node(1,   0,  1),
    101.           Node(0,   1,  1),
    102.           Node(-1,   0,  1),
    103.           Node(0,   -1,  1),
    104.           Node(-1,   -1,  std::sqrt(2)),
    105.           Node(-1,   1,  std::sqrt(2)),
    106.           Node(1,   -1,  std::sqrt(2)),
    107.           Node(1,    1,  std::sqrt(2))};
    108. }
    109.  
    110. void a_star_planning(float sx, float sy,
    111.                      float gx, float gy,
    112.                      vector<float> ox_, vector<float> oy_,
    113.                      float reso, float rr){
    114.  
    115.   Node* nstart = new Node((int)std::round(sx/reso), (int)std::round(sy/reso), 0.0);
    116.   Node* ngoal = new Node((int)std::round(gx/reso), (int)std::round(gy/reso), 0.0);
    117.  
    118.  
    119.   vector<int> ox;
    120.   vector<int> oy;
    121.  
    122.   int min_ox = std::numeric_limits<int>::max();
    123.   int max_ox = std::numeric_limits<int>::min();
    124.   int min_oy = std::numeric_limits<int>::max();
    125.   int max_oy = std::numeric_limits<int>::min();
    126.  
    127.  
    128.   for(float iox:ox_){
    129.       int map_x = (int)std::round(iox*1.0/reso);
    130.       ox.push_back(map_x);
    131.       min_ox = std::min(map_x, min_ox);
    132.       max_ox = std::max(map_x, max_ox);
    133.   }
    134.  
    135.   for(float ioy:oy_){
    136.       int map_y = (int)std::round(ioy*1.0/reso);
    137.       oy.push_back(map_y);
    138.       min_oy = std::min(map_y, min_oy);
    139.       max_oy = std::max(map_y, max_oy);
    140.   }
    141.  
    142.   int xwidth = max_ox-min_ox;
    143.   int ywidth = max_oy-min_oy;
    144.  
    145.   //visualization
    146.   cv::namedWindow("astar", cv::WINDOW_NORMAL);
    147.   int count = 0;
    148.   int img_reso = 5;
    149.   cv::Mat bg(img_reso*xwidth,
    150.              img_reso*ywidth,
    151.              CV_8UC3,
    152.              cv::Scalar(255,255,255));
    153.  
    154.     cv::rectangle(bg,
    155.                   cv::Point(nstart->x*img_reso+1, nstart->y*img_reso+1),
    156.                   cv::Point((nstart->x+1)*img_reso, (nstart->y+1)*img_reso),
    157.                   cv::Scalar(255, 0, 0), -1);
    158.     cv::rectangle(bg,
    159.                   cv::Point(ngoal->x*img_reso+1, ngoal->y*img_reso+1),
    160.                   cv::Point((ngoal->x+1)*img_reso, (ngoal->y+1)*img_reso),
    161.                   cv::Scalar(0, 0, 255), -1);
    162.  
    163.   std::vectorint> > visit_map(xwidth, vector<int>(ywidth, 0));
    164.   
    165.   std::vectorfloat> > path_cost(xwidth, vector<float>(ywidth, std::numeric_limits<float>::max()));
    166.  
    167.   path_cost[nstart->x][nstart->y] = 0;
    168.  
    169.   std::vectorint> > obmap = calc_obstacle_map(
    170.                                                   ox, oy,
    171.                                                   min_ox, max_ox,
    172.                                                   min_oy, max_oy,
    173.                                                   reso, rr,
    174.                                                   bg, img_reso);
    175.  
    176.   // NOTE: d_ary_heap should be a better choice here
    177.   auto cmp = [](const Node* left, const Node* right){return left->sum_cost > right->sum_cost;};
    178.   std::priority_queue, decltype(cmp)> pq(cmp);
    179.  
    180.   pq.push(nstart);
    181.   std::vector motion = get_motion_model();
    182.  
    183.   while (true){
    184.     Node * node = pq.top();
    185.  
    186.     if (visit_map[node->x-min_ox][node->y-min_oy] == 1){
    187.       pq.pop();
    188.       delete node;
    189.       continue;
    190.     }else{
    191.       pq.pop();
    192.       visit_map[node->x-min_ox][node->y-min_oy] = 1;
    193.     }
    194.  
    195.     if (node->x == ngoal->x && node->y==ngoal->y){
    196.       ngoal->sum_cost = node->sum_cost;
    197.       ngoal->p_node = node;
    198.       break;
    199.     }
    200.  
    201.     for(int i=0; isize(); i++){
    202.       Node * new_node = new Node(
    203.         node->x + motion[i].x,
    204.         node->y + motion[i].y,
    205.         path_cost[node->x][node->y] + motion[i].sum_cost + calc_heristic(ngoal, node),
    206.         node);
    207.  
    208.       if (!verify_node(new_node, obmap, min_ox, max_ox, min_oy, max_oy)){
    209.         delete new_node;
    210.         continue;
    211.       }
    212.  
    213.       if (visit_map[new_node->x-min_ox][new_node->y-min_oy]){
    214.         delete new_node;
    215.         continue;
    216.       }
    217.  
    218.       cv::rectangle(bg,
    219.                     cv::Point(new_node->x*img_reso+1, new_node->y*img_reso+1),
    220.                     cv::Point((new_node->x+1)*img_reso, (new_node->y+1)*img_reso),
    221.                     cv::Scalar(0, 255, 0));
    222.  
    223.       // std::string int_count = std::to_string(count);
    224.       // cv::imwrite("./pngs/"+std::string(5-int_count.length(), '0').append(int_count)+".png", bg);
    225.       count++;
    226.       cv::imshow("astar", bg);
    227.       cv::waitKey(5);
    228.  
    229.       if (path_cost[node->x][node->y]+motion[i].sum_cost < path_cost[new_node->x][new_node->y]){
    230.         path_cost[new_node->x][new_node->y]=path_cost[node->x][node->y]+motion[i].sum_cost; 
    231.         pq.push(new_node);
    232.       }
    233.     }
    234.   }
    235.  
    236.   calc_final_path(ngoal, reso, bg, img_reso);
    237.   delete ngoal;
    238.   delete nstart;
    239.  
    240.   // std::string int_count = std::to_string(count);
    241.   // cv::imwrite("./pngs/"+std::string(5-int_count.length(), '0').append(int_count)+".png", bg);
    242.   cv::imshow("astar", bg);
    243.   cv::waitKey(5);
    244. };
    245.  
    246.  
    247. int main(){
    248.   float sx = 10.0;
    249.   float sy = 10.0;
    250.   float gx = 50.0;
    251.   float gy = 50.0;
    252.  
    253.   float grid_size = 1.0;
    254.   float robot_size = 1.0;
    255.  
    256.   vector<float> ox;
    257.   vector<float> oy;
    258.  
    259.   // add edges
    260.   for(float i=0; i<60; i++){
    261.     ox.push_back(i);
    262.     oy.push_back(60.0);
    263.   }
    264.   for(float i=0; i<60; i++){
    265.     ox.push_back(60.0);
    266.     oy.push_back(i);
    267.   }
    268.   for(float i=0; i<61; i++){
    269.     ox.push_back(i);
    270.     oy.push_back(60.0);
    271.   }
    272.   for(float i=0; i<61; i++){
    273.     ox.push_back(0.0);
    274.     oy.push_back(i);
    275.   }
    276.   for(float i=0; i<40; i++){
    277.     ox.push_back(20.0);
    278.     oy.push_back(i);
    279.   }
    280.   for(float i=0; i<40; i++){
    281.     ox.push_back(40.0);
    282.     oy.push_back(60.0 - i);
    283.   }
    284.  
    285.   a_star_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size);
    286.   cin.ignore();
    287.   return 0;
    288. }

    效果如下：

     

     但是，需要通过修改代码，定位起点终点和障碍物等。

    如何，解决下面这种迷宫呢（mazegenerator.net）？

    迷宫有中杯，大杯，超大杯。

    中杯版本如下：

    大杯版本如下：

     

    超超大杯版本：

     

    咱就不玩超大号的了。 

    就用下面这个地图吧：

    中：

    大：

    代码参考： 

    1. #include 
    2. #include
    3. #include
    4. #include
    5. #include
    6. #include
    7. using namespace cv;
    8. using namespace std;
    9. //
    10. Mat pool_max(Mat image_source, int size);
    11. Mat pool_min(Mat image_source, int size);
    12. int f(int x1,int x2,int z1,int z2);
    13.  
    14. class A
    15. {
    16.     private:
    17.     int rate=6;
    18.     int map[300][300];
    19.     Mat m;
    20.     //
    21.     int tag_open=0,tag_close=0;
    22.     //
    23.     int map_root[300][300][2];
    24.     //
    25.     int x1;
    26.     int x2;
    27.     //
    28.     int z1;
    29.     int z2;
    30.     //
    31.     int way[1000][2];
    32.     //
    33.     //
    34.     int close[24400][3];
    35.     //
    36.     int open[24400][4];
    37.     public:
    38.     //
    39.     Mat image_source;
    40.     //
    41.     void deal_image();
    42.     //
    43.     void find_ori();
    44.     //
    45.     void deal_A();
    46.     //
    47.     void draw_road();
    48.  
    49. };
    50. void A::draw_road()
    51. {
    52.     Mat rgb;
    53.     rgb=imread("maze.png",1);
    54.     VideoWriter witer = VideoWriter("maze.avi",CV_FOURCC('M','P','4','2'),40,Size(image_source.cols,image_source.cols),1);//
    55.     int mm=0;
    56.     //
    57.     while(1)
    58.     {
    59.         mm++;
    60.         //
    61.         for(int tag_a=4;tag_a<7;tag_a++)
    62.         for(int tag_b=4;tag_b<7;tag_b++)
    63.         for(int tag_c=0;tag_c<2;tag_c++)
    64.         rgb.at((map_root[z1][z2][0]*rate+tag_a),(map_root[z1][z2][1]*rate+tag_b))[tag_c%3]=0;
    65.         imshow("a",rgb);
    66.         waitKey(1);
    67.     z1=map_root[z1][z2][0];
    68.     z2=map_root[z1][z2][1];
    69.     //
    70.     witer<
    71.     printf("%d %d \n",z1,z2);
    72.     //
    73.     if(((z1==x1)&&(z2==x2))||mm>1000)break;
    74.     }
    75.     //
    76.     imshow("a",rgb);
    77.  
    78.     witer.release();
    79. }
    80. void A::find_ori()
    81. {
    82.     Mat image_b;
    83.         GaussianBlur(image_source,image_b,Size(3,3),5);
    84.     //
    85.     for(int tag_a=0;tag_a
    86.     {
    87.     for(int tag_b=0;tag_b
    88.     if(image_b.at(tag_a,tag_b)>100)image_b.at(tag_a,tag_b)=255;
    89.     else image_b.at(tag_a,tag_b)=0;
    90.     }
    91.     //
    92.     for(int tag_b=0;tag_b
    93.     if(image_b.at(tag_b,0)==255)
    94.     {
    95.         x1=tag_b/rate-1;
    96.         x2=0;
    97.     }
    98.  
    99.     for(int tag_b=0;tag_b
    100.     if(image_b.at(tag_b,image_b.rows-1)==255)
    101.     {
    102.         z1=tag_b/rate-1;
    103.         z2=(image_b.rows)/rate-1;
    104.     }
    105.  
    106. }
    107. void A::deal_image()
    108. {   //
    109.     image_source = imread("maze.png",0);
    110.     //
    111.     Mat b;
    112.     //
    113.     GaussianBlur(image_source,b,Size(3,3),5);
    114.     //
    115.     for(int tag_a=0;tag_a
    116.     {
    117.     for(int tag_b=0;tag_b
    118.     if(b.at(tag_a,tag_b)>100)b.at(tag_a,tag_b)=255;
    119.     else b.at(tag_a,tag_b)=0;
    120.     }
    121.     //
    122.     //m=pool_min(pool_max(b,3),3);
    123.     m=pool_min(b,6);
    124.     //
    125.     for(int tag_a=0;tag_a
    126.     {
    127.     for(int tag_b=0;tag_b
    128.     if(m.at(tag_a,tag_b)>100)m.at(tag_a,tag_b)=255;
    129.     else m.at(tag_a,tag_b)=0;
    130.     }
    131.     //
    132.     imshow("c",m);
    133.     //
    134.     for(int tag_a=0;tag_a
    135.     for(int tag_b=0;tag_b
    136.     map[tag_a][tag_b]=m.at(tag_a,tag_b);
    137. }
    138. void A::deal_A()
    139. {
    140.     int keyy1=0,keyy2=0,keyy3=0,keyy4=0;
    141.     map_root[z1][z2][0]=5;
    142.     //
    143.     close[0][0]=x1;
    144.     close[0][1]=x2;
    145.     tag_close++;
    146.     //
    147.     int com[2]={10000,0};
    148. while(1)
    149. {
    150. //
    151. if(map[close[tag_close-1][0]][close[tag_close-1][1]+1]==255&&close[tag_close-1][0]>=0&&(close[tag_close-1][1]+1)>=0)
    152. {   //close
    153.  
    154.         for(int tag_a=0;tag_a
    155.     {
    156.         //
    157.         if((close[tag_close-1][0])==open[tag_a][0]&&(close[tag_close-1][1]+1)==open[tag_a][1])
    158.     {   //
    159.         //open[tag_a][2]=close[tag_close-1][2]+1;
    160.         keyy1=1;
    161.         break;
    162.     }
    163.         if(keyy1==0)
    164.     {
    165.         map_root[close[tag_close-1][0]][close[tag_close-1][1]+1][0]=close[tag_close-1][0];
    166.         map_root[close[tag_close-1][0]][close[tag_close-1][1]+1][1]=close[tag_close-1][1];
    167.     }
    168.     keyy1=0;
    169.     
    170.     }
    171.     //
    172.     if(open[com[1]][3]==100000)
    173.     {
    174.         open[com[1]][0]=close[tag_close-1][0];
    175.         open[com[1]][1]=close[tag_close-1][1]+1;
    176.         open[com[1]][2]=close[tag_close-1][2]+1;
    177.         open[com[1]][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);
    178.  
    179.     }
    180.     else
    181.     {
    182.     //
    183.         open[tag_open][0]=close[tag_close-1][0];
    184.         open[tag_open][1]=close[tag_close-1][1]+1;
    185.         open[tag_open][2]=close[tag_close-1][2]+1;
    186.         open[tag_open][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);
    187.         tag_open++;
    188.     }
    189.     
    190.  
    191. }
    192. if(map[close[tag_close-1][0]][close[tag_close-1][1]-1]==255&&close[tag_close-1][0]>=0&&(close[tag_close-1][1]-1)>=0)
    193. {
    194.  
    195.         for(int tag_a=0;tag_a
    196.     {
    197.         if((close[tag_close-1][0])==open[tag_a][0]&&(close[tag_close-1][1]-1)==open[tag_a][1])
    198.     {
    199.         //open[tag_a][2]=close[tag_close-1][2]+1;
    200.         keyy2=1;
    201.         break;
    202.     }
    203.         if(keyy2==0)
    204.     {
    205.         map_root[close[tag_close-1][0]][close[tag_close-1][1]-1][0]=close[tag_close-1][0];
    206.         map_root[close[tag_close-1][0]][close[tag_close-1][1]-1][1]=close[tag_close-1][1];
    207.     }
    208.     keyy2=0;
    209.     
    210.     }
    211.         if(open[com[1]][3]==100000)
    212.     {
    213.         open[com[1]][0]=close[tag_close-1][0];
    214.         open[com[1]][1]=close[tag_close-1][1]-1;
    215.         open[com[1]][2]=close[tag_close-1][2]+1;
    216.         open[com[1]][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);
    217.  
    218.     }
    219.     else
    220.     {
    221.         open[tag_open][0]=close[tag_close-1][0];
    222.         open[tag_open][1]=close[tag_close-1][1]-1;
    223.         open[tag_open][2]=close[tag_close-1][2]+1;
    224.         open[tag_open][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);
    225.         tag_open++;
    226.         
    227.  
    228.     }
    229.     
    230.  
    231. }
    232. if(map[close[tag_close-1][0]+1][close[tag_close-1][1]]==255&&(close[tag_close-1][0]+1)>=0&&(close[tag_close-1][1])>=0)
    233. {
    234.  
    235.     for(int tag_a=0;tag_a
    236.     {
    237.         if((close[tag_close-1][0]+1)==open[tag_a][0]&&(close[tag_close-1][1])==open[tag_a][1])
    238.     {
    239.         //open[tag_a][2]=close[tag_close-1][2]+1;
    240.         keyy3=1;
    241.         break;
    242.     }
    243.         if(keyy3==0)
    244.         {
    245.         map_root[close[tag_close-1][0]+1][close[tag_close-1][1]][0]=close[tag_close-1][0];
    246.         map_root[close[tag_close-1][0]+1][close[tag_close-1][1]][1]=close[tag_close-1][1];
    247.         }
    248.     keyy3=0;
    249.     
    250.     }
    251.             if(open[com[1]][3]==100000)
    252.     {
    253.         open[com[1]][0]=close[tag_close-1][0]+1;
    254.         open[com[1]][1]=close[tag_close-1][1];
    255.         open[com[1]][2]=close[tag_close-1][2]+1;
    256.         open[com[1]][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);
    257.  
    258.     }
    259.     else
    260.     {
    261.             open[tag_open][0]=close[tag_close-1][0]+1;
    262.             open[tag_open][1]=close[tag_close-1][1];
    263.             open[tag_open][2]=close[tag_close-1][2]+1;
    264.             open[tag_open][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);
    265.             tag_open++;
    266.         
    267.  
    268.     }
    269.     
    270.  
    271. }
    272. if(map[close[tag_close-1][0]-1][close[tag_close-1][1]]==255&&(close[tag_close-1][0]-1)>=0&&(close[tag_close-1][1])>=0)
    273. {
    274.  
    275.     
    276.     for(int tag_a=0;tag_a
    277.     {
    278.         if((close[tag_close-1][0]-1)==open[tag_a][0]&&(close[tag_close-1][1])==open[tag_a][1])
    279.     {
    280.         //open[tag_a][2]=close[tag_close-1][2]+1;
    281.         keyy4=1;
    282.         break;
    283.     }
    284.         if(keyy4==0)
    285.         {
    286.         map_root[close[tag_close-1][0]-1][close[tag_close-1][1]][0]=close[tag_close-1][0];
    287.         map_root[close[tag_close-1][0]-1][close[tag_close-1][1]][1]=close[tag_close-1][1];
    288.         }
    289.     keyy4=0;
    290.     }
    291.  
    292.             if(open[com[1]][3]==100000)
    293.     {
    294.         open[com[1]][0]=close[tag_close-1][0]-1;
    295.         open[com[1]][1]=close[tag_close-1][1];
    296.         open[com[1]][2]=close[tag_close-1][2]+1;
    297.         open[com[1]][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);
    298.  
    299.     }
    300.     else
    301.     {
    302.     open[tag_open][0]=close[tag_close-1][0]-1;
    303.     open[tag_open][1]=close[tag_close-1][1];
    304.     open[tag_open][2]=close[tag_close-1][2]+1;
    305.     open[tag_open][3]=f(open[tag_open][0],open[tag_open][1],z1,z2);
    306.     tag_open++;
    307.  
    308.     }
    309.  
    310. }
    311. com[0]=10000;
    312. com[1]=0;
    313. //
    314. for(int tag_a=0;tag_a
    315. {
    316. if((open[tag_a][2]+open[tag_a][3])0])
    317. {
    318.     com[1]=tag_a;
    319.     com[0]=open[tag_a][2]+open[tag_a][3];
    320. }
    321.  
    322. }
    323. //
    324. close[tag_close][0]=open[com[1]][0];
    325. close[tag_close][1]=open[com[1]][1];
    326. //map_root[close[tag_close][0]][close[tag_close][1]][0]=close[tag_close-1][0];
    327. //map_root[close[tag_close][0]][close[tag_close][1]][1]=close[tag_close-1][1];
    328. //
    329. map[close[tag_close-1][0]][close[tag_close-1][1]]=0;
    330. //
    331. open[com[1]][3]=100000;
    332. tag_close++;
    333.     //printf("\n %d %d\n",close[tag_close-1][0],close[tag_close-1][1]);
    334. //waitKey(5);
    335. if(map[z1][z2]==0)break;
    336. }
    337.  
    338.  
    339. }
    340.  
    341.  
    342. int f(int x1,int x2,int z1,int z2)
    343. {
    344.  
    345. return abs(x1-z1)+abs(x2-z2);
    346. }
    347. Mat pool_max(Mat image_source, int size)
    348. {
    349.     int rows = image_source.rows;
    350.     int cols = image_source.cols;
    351.     int tag_x=0,tag_y=0,tag1=0,tag2=0;
    352.     int tag3[3]={0,0,0};
    353.     Mat image_new(image_source.rows/size,image_source.cols/size,CV_8UC1);
    354.     while(tag_x>=0&&tag_x<=rows-size)//
    355.     {
    356.         while(tag_y>=0&&tag_y<=cols-size)
    357.         {
    358.             while(tag1>=0&&tag1<=size)
    359.             {
    360.                 while(tag2>=0&&tag2<=size)
    361.                 {
    362.                     if(image_source.at(tag_x+tag1,tag_y+tag2)>tag3[1])(tag3[1]=image_source.at(tag_x+tag1,tag_y+tag2));
    363.                     tag2++;
    364.                 }
    365.                 tag1++;
    366.                 tag2=0; 
    367.             }
    368.             image_new.at(tag_x/size,tag_y/size)=tag3[1];
    369.             tag3[1]=0;
    370.             tag_y+=size;tag1=0;tag2=0;
    371.         }
    372.         tag_x+=size;
    373.         tag_y=0;tag1=0;tag2=0;
    374.     }
    375.     return image_new;
    376.  
    377. }
    378.  
    379. Mat pool_min(Mat image_source, int size)
    380. {
    381.     int rows = image_source.rows;
    382.     int cols = image_source.cols;
    383.     int tag_x=0,tag_y=0,tag1=0,tag2=0;
    384.     int tag3[3]={0,0,0};
    385.     Mat image_new(image_source.rows/size,image_source.cols/size,CV_8UC1);
    386.     while(tag_x>=0&&tag_x<=rows-size)//
    387.     {
    388.         while(tag_y>=0&&tag_y<=cols-size-1)
    389.         {
    390.             while(tag1>=0&&tag1<=size)
    391.             {
    392.                 while(tag2>=0&&tag2<=size)
    393.                 {
    394.                     if(image_source.at(tag_x+tag1,tag_y+tag2)1])(tag3[1]=image_source.at(tag_x+tag1,tag_y+tag2));
    395.                     tag2++;
    396.                 }
    397.                 tag1++;
    398.                 tag2=0; 
    399.             }
    400.             image_new.at(tag_x/size,tag_y/size)=tag3[1];
    401.             tag3[1]=255;
    402.             tag_y+=size;tag1=0;tag2=0;
    403.         }
    404.         tag_x+=size;
    405.         tag_y=0;tag1=0;tag2=0;
    406.     }
    407.     return image_new;
    408.  
    409. }
    410.  
    411.  
    412. int main()
    413. {
    414.     A a;
    415.     a.deal_image();
    416.     a.find_ori();
    417.     a.deal_A();
    418.     a.draw_road();
    419.     cin.ignore();
    420. }

    www.gamedev.net/reference/articles/article2003.asp 

    A*（发音为 A-star）算法对于初学者来说可能很复杂。虽然网上有很多解释 A* 的文章，但大多数都是为已经了解基础知识的人编写的。这篇文章是为真正的初学者准备的。

    本文并不试图成为该主题的权威著作。相反，它描述了基础知识，并让准备好出去阅读所有其他材料并理解他们在说什么。本文末尾的进一步阅读下提供了一些最佳链接。

    最后，本文不是特定于程序的。应该能够使这里的内容适应任何计算机语言。然而，正如所料，我在本文末尾提供了一个示例程序的链接。示例包包含两个版本：一个是 C++ 版本，一个是 Blitz Basic 版本。如果只想查看 A* 的运行情况，它还包含可执行文件。

    将其从打开列表中删除并将其添加到关闭列表中。
    检查所有相邻的方块。忽略那些在封闭列表中或无法行走（有墙壁、水或其他非法地形的地形）的那些，如果它们已经不在开放列表中，则将它们添加到开放列表中。使选定的方块成为新方块的“父级”。
    如果相邻的方格已经在打开列表中，请检查通往该方格的这条路径是否更好。换句话说，如果我们使用当前方格到达那里，请检查该方格的 G 分数是否较低。如果没有，不要做任何事情。
    另一方面，如果新路径的 G 成本较低，则将相邻方格的父方更改为选定方格（在上图中，将指针的方向更改为指向选定方格）。最后，重新计算该方格的 F 和 G 分数。如果这看起来令人困惑，您将在下面看到它。
    将起始方块（或节点）添加到打开列表中。
    重复以下操作：
    a) 在开放列表中寻找最低的 F 成本方。我们将其称为当前方格。
    b) 将其切换到关闭列表。
    c) 对于与当前方格相邻的 8 个方格中的每一个 ...
    如果它不可步行或在封闭列表中，请忽略它。否则请执行以下操作。
    如果它不在打开列表中，请将其添加到打开列表中。使当前方格成为该方格的父方。记录正方形的 F、G 和 H 成本。
    如果它已经在开放列表中，请使用 G 成本作为衡量标准，检查通往该方格的这条路径是否更好。较低的 G 成本意味着这是一条更好的路径。如果是，则将方块的父级更改为当前方块，并重新计算该方块的 G 和 F 分数。如果您保持您的开放列表按 F 分数排序，您可能需要借助该列表来说明更改。
    将目标方块添加到封闭列表中，在这种情况下已找到路径（请参见下面的注释），或
    找不到目标方格，打开列表为空。在这种情况下，没有路径。
    保存路径。从目标方格向后工作，从每个方格到其父方格，直到到达起始方格。那是你的道路。注意：在本文的早期版本中，建议您可以在目标方格（或节点）已添加到打开列表而不是关闭列表时停止。这样做会更快，它几乎总是会给你最短的路径，但并非总是如此。这样做可能会产生影响的情况是，从第二个节点移动到最后一个节点到最后一个（目标）节点的移动成本可能会有很大差异 - 例如，在两个节点之间的河流交叉的情况下。 

  • 相关阅读:
    drone ci 是什么
    用递归算法得到Java的树形结构
    【Cesium4UE】使用问题及解法统计
    Rebex Total Pack 6.0 for .Net Crack
    数据采集之：巧用布隆过滤器提取数据摘要
    cocos creator实现浏览星球的功能，附源码
    算法竞赛进阶指南：装满的油箱（Python）
    【java_wxid项目】【第六章】【Spring Cloud Gateway集成】
    金融业务系统云原生技术转型：从传统架构到云原生的跨越
    win10 上PHP artisan storage:link 出现 symlink (): Protocol error的解决办法
  • 原文地址：https://blog.csdn.net/ZhangRelay/article/details/126735454