c++ 实现 prm

1. 代码

采样 + 搜索，这里搜索用的就是 dijkstra， 用 Astar 也可以

#include <iostream>
#include <vector>
#include <string>
#include <queue>

#include "opencv2/highgui.hpp"
#include "opencv2/opencv.hpp"

// #include <direct.h> 
#include <unistd.h> //目录
#include <typeinfo>

#define INF  13421772
#define sampleNum 200
#define START  0
#define GOAL   1

double toDegree(double radian) {return radian * 180 / M_PI;}

double toRadian(double degree) {return degree * M_PI / 180;}

struct GraphNode{
    int label;
    std::vector<GraphNode*> neighbors;
    GraphNode(int x): label(x) {};
};

void loadMap(){
    // char *root_path = NULL;
    // root_path = getcwd(NULL,0);
    // std::cout << root_path << std::endl;
    // cv::Mat dismap = cv::imread("../map/map_2.bmp");
    // cv::imshow("dismap", dismap);
    // cv::waitKey(1000);
}

bool checkCollision(const std::vector<int> point, const cv::Mat map){
    bool reach = true;
    if(map.at<cv::Vec3b>(point[1],point[0])[0]==0 && 
       map.at<cv::Vec3b>(point[1],point[0])[1]==0 && 
       map.at<cv::Vec3b>(point[1],point[0])[2]==0){
           reach = false;
       }

    return reach;
}

//这里只是设置了10个点,也可以根据间距最大的设置采样点
bool checkPath(const std::vector<int> point_a, 
               const std::vector<int> point_b, 
               const cv::Mat map, int split_num){
    
    std::vector<double> path_x;
    std::vector<double> path_y;
    // path_x.reserve(split_num + 1);
    // path_y.reserve(split_num + 1);

    // int min_x = point_a[0] > point_b[0] ? point_a[0] : point_b[0];
    // int min_y = point_a[1] > point_b[1] ? point_a[1] : point_b[1];
    double interval_x = (point_b[0] - point_a[0]) / (split_num);
    double interval_y = (point_b[1] - point_a[1]) / (split_num);

    // std::cout << point_a[0] << "," << point_b[0] << std::endl;

    for(int i = 0; i <= split_num; ++i){
        path_x.push_back(point_a[0] + i * interval_x);
        path_y.push_back(point_a[1] + i * interval_y);
        // std::cout << point_a[0] + i * interval_x << "," << point_a[1] + i * interval_y << std::endl;
        // std::cout << "i:" << i << std::endl;
    }

    for(int i = 0; i <= split_num; ++i){
        if(!checkCollision({int(path_x[i]), int(path_y[i])}, map)){
            return false;
        }
    }

    return true;
}

double calcDistance(const std::vector<int> point_a, const std::vector<int> point_b){
    return sqrt(pow(point_a[0]-point_b[0],2) + pow(point_a[1]-point_b[1],2));
}

struct Dijkstra {
    struct node{
        int point;
        double value;
        node(int _point, double _value):point(_point), value(_value){}
        bool operator < (const node& rhs) const{
                return value > rhs.value;
        }
    };

    std::vector<node> edges[sampleNum];
    double dist[sampleNum];
    int path[sampleNum];

    void init()
	{
		for(int i = 0; i < sampleNum; i++)
        {
            edges[i].clear();
            dist[i] = 0;
            path[i] = 0;
        }
	}

    void addEdge(int from, int to, double dist)
	{
		edges[from].push_back(node(to,dist));
        // edges[to].push_back(node(from,dist));
	}

    void showEdge()
    {
        std::cout << "------------------------" << std::endl;

        for(int i = 0; i< sampleNum; i++)
        {
            // cout << "-----" << i << "-----" << endl;

            for(int j=0; j<edges[i].size(); j++)
            {
                std::cout << i << "," << edges[i][j].point << "," << edges[i][j].value << std::endl;
            }

            // cout << "**********************" << endl;

        }

        std::cout << "------------------------" << std::endl;
    }

    std::vector<int> dijkstra(int s, int t)
	{
		std::priority_queue <node> q;

		for(int i = 0; i < sampleNum; i++)
            dist[i] = INF;

		dist[s] = 0;
		q.push(node(s, dist[s]));

		while(!q.empty())
		{
            node x = q.top(); q.pop();
            for (int i = 0; i < edges[x.point].size(); i++)
            {
                // std::cout << edges[x.point].size() << std::endl;
                node y = edges[x.point][i];
                if (dist[y.point] > dist[x.point] + y.value)
                {
                    dist[y.point] = dist[x.point] + y.value;
                    path[y.point] = x.point;
                    q.push(node(y.point, dist[y.point]));
                }
            }
		}

        std::vector<int> result;

        // 存距离
        // result.push_back(dist[t]);
        std::cout << "dist[t]:" << dist[t] << std::endl;

        while(t)
        {
            result.push_back(t);
            t = path[t];
        }
        
        result.push_back(path[0]);


        reverse(result.begin(),result.end()); //起点->终点 ，+ 距离

        // for show result (start -> end,cost)
        // for(int i=0; i < result.size(); i++)
        // {
        //     std::cout << result[i] << " ";
        // }

        return result;
    }
};

struct Dijkstra DijkstraPlanning;  

int main() {

    cv::Mat dismap = cv::imread("../map/map_2.bmp");
    // cv::imshow("dismap", dismap);
    // cv::waitKey(1000);
    int mapLength = dismap.cols;
    int mapWidth = dismap.rows; 

    // std::cout << "mapLength:" << mapLength << ",mapWidth:" << mapWidth << std::endl;
    // std::cout << dismap.size[0] << typeid(dismap.size).name() << std::endl;


    //改为结构体
    // std::vector<int> pStrat = {50, 50};
    // std::vector<int> pGoal = {450, 450};

    std::vector<int> pStrat = {10, 10};
    std::vector<int> pGoal = {490, 490};

    std::vector<std::vector<int> > sampleMap;
    sampleMap.reserve(sampleNum + 2);
    sampleMap.push_back(pStrat);
    sampleMap.push_back(pGoal);

    // std::cout << sampleMap.size() << std::endl;

    //初始化采样点
    while (sampleMap.size() < sampleNum + 2)
    // while (sampleMap.size() < 10)
    {

        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<> dis(0, dismap.size[0]-1);
        
        
        std::vector<int> res;
        // sampleMap.push_back(std::vector<int>());
        for(int i=0; i<2; ++i){
            res.push_back(dis(gen));
            // sampleMap.back().push_back(dis(gen));
        }

        if(checkCollision(res, dismap)){
            sampleMap.push_back({res[0], res[1]});
        }
        
        // std::cout << res[0] << "," << res[1] << std::endl;
    }

    const int MAX_N = sampleNum + 2;
    GraphNode *Graph[MAX_N];

    const double DISTANCE = 100; 
    const int SPLIT_N = 10;
    
    // for(int i = 0; i < MAX_N; i++){
    //     Graph[i] = new GraphNode(i);
    // }

    DijkstraPlanning.init();

    for (int i = 0; i < MAX_N; i++)
    {
        for(int j = 0; j < MAX_N; j++){
            if (calcDistance(sampleMap[i], sampleMap[j]) <= DISTANCE && 
                checkPath(sampleMap[i], sampleMap[j], dismap, SPLIT_N) && i!= j)
            {
                cv::line(dismap, cv::Point(sampleMap[i][0], sampleMap[i][1]), 
                                 cv::Point(sampleMap[j][0], sampleMap[j][1]), 
                                 cv::Scalar(0, 0, 255), 1);
                // Graph[i]->neighbors.push_back(Graph(i));

                DijkstraPlanning.addEdge(i, j, calcDistance(sampleMap[i], sampleMap[j]));
            }
            
        }
    }

    // for(int i=0; i<sampleMap.size(); i++){
    //     // std::cout << i << ":";
    //     for(int j=0; j<sampleMap[i].size(); j++){
    //         // std::cout << sampleMap[i][j] << ",";
    //        std::cout << sampleMap[i][j] << ",";

    //     }
    //     std::cout << std::endl;
    // }
    
    DijkstraPlanning.showEdge();


    std::vector<int> result = DijkstraPlanning.dijkstra(START, GOAL);
    // std::cout << "result.size(): " << result.size() << std::endl;

    // std::cout << "result: ";

    // for(int i=0; i < result.size(); i++)
    // {
    //     std::cout << result[i] << " ";
    // }

    if(result.size() > 2){
        for(int i=0;i < result.size() - 1; i++)
        {
            // std::cout << result[i] << " ";
            cv::line(dismap, cv::Point(sampleMap[result[i]][0], sampleMap[result[i]][1]), 
                    cv::Point(sampleMap[result[i+1]][0], sampleMap[result[i+1]][1]), 
                    cv::Scalar(0, 255, 0), 5);
        }
    }

    for(int i=0; i<sampleMap.size(); i++){
        // std::cout << i << ":";
        for(int j=0; j<sampleMap[i].size(); j++){
            // std::cout << sampleMap[i][j] << ",";
            dismap.at<cv::Vec3b>(sampleMap[i][1],sampleMap[i][0])[0] = 0;
            dismap.at<cv::Vec3b>(sampleMap[i][1],sampleMap[i][0])[1] = 255; 
            dismap.at<cv::Vec3b>(sampleMap[i][1],sampleMap[i][0])[2] = 0;

        }
        // std::cout << std::endl;
    }
    
    // std::cout << sampleMap.size() << std::endl;

    cv::imshow("dismap", dismap);
    cv::waitKey(10000);
    
    // if(result.size() > 2){
    //     cv::imwrite("/home/seivl/me/代码/规划/prm/planning_result/prm.bmp", dismap);
    // }
    
  return 0;
}

2. 结果