C++結(jié)合OpenCV實(shí)現(xiàn)RRT算法(路徑規(guī)劃算法)
更新時(shí)間：2022年05月12日 15:05:01 作者：m0_54861649
這篇文章主要介紹了C++結(jié)合OpenCV實(shí)現(xiàn)RRT算法,RRT算法整體框架主要分為rand、near、new三點(diǎn)的建立和near與new之間的安全性檢查,需要的朋友可以參考下
1.RRT算法簡(jiǎn)介
代碼鏈接：RRT
動(dòng)圖展示
RRT
2.算法整體框架流程
RRT算法整體框架主要分為rand、near、new三點(diǎn)的建立和near與new之間的安全性檢查
2.1 rand點(diǎn)的建立
rand點(diǎn)表示在地圖 M M M中隨機(jī)采樣獲得，記住是隨機(jī)。我們可以通過(guò)設(shè)計(jì)隨機(jī)函數(shù)，讓盡可能的點(diǎn)進(jìn)入空曠區(qū)域，即算法框架中的Sample函數(shù)。下圖中紅色點(diǎn)表示起點(diǎn)，綠色的點(diǎn)表示終點(diǎn)。
2.2 near和new點(diǎn)的建立
near點(diǎn)表示從RRT樹(shù) Γ Gamma Γ中通過(guò)距離函數(shù)，判斷樹(shù)中哪個(gè)點(diǎn)距離當(dāng)前rand點(diǎn)最近，此時(shí)該點(diǎn)即為near點(diǎn)。對(duì)應(yīng)于算法框架中的Near函數(shù)。
new點(diǎn)表示以near點(diǎn)到rand為方向，以 E i E_i Ei為步長(zhǎng)，生成的一個(gè)新點(diǎn)。對(duì)應(yīng)于算法框架的Steer函數(shù)。
2.3 安全性檢查
若上述的new點(diǎn)在安全區(qū)域內(nèi)，且new與near點(diǎn)連線安全，則會(huì)在RRT樹(shù)中進(jìn)行擴(kuò)展，否則不會(huì)進(jìn)行擴(kuò)展。對(duì)應(yīng)于算法框架中的CollisionFree函數(shù)。
2.4 算法結(jié)束判斷
算法框架中的當(dāng)new點(diǎn)與goal相等，表示算法運(yùn)行成功，但是實(shí)際編程情況中，new點(diǎn)與goal點(diǎn)會(huì)存在一定的距離閾值。
3.RRT代碼框架
3.1 主函數(shù)
main.cpp ：首先通過(guò)地圖文件中讀取地圖數(shù)據(jù)(本次代碼提供兩張地圖，供測(cè)試使用)，然后設(shè)置RRT算法的起點(diǎn)和終點(diǎn)，以及相關(guān)參數(shù)設(shè)置，例如距離閾值、步長(zhǎng)、迭代次數(shù)等。其次通過(guò)RRT算法的接口函數(shù)RRTCore和CreatePath獲得RRT算法的路徑，最后通過(guò)顯示函數(shù)Display進(jìn)行數(shù)據(jù)可視化。
#include <iostream>
#include <vector>
#include <string>
#include "map.h"
#include "display.h"
#include "RRT.h"
using namespace std;
int main()
{
//讀取地圖點(diǎn)
//vector<vector<int>>mapData = MapData("map/map.txt");
定義起點(diǎn)和終點(diǎn),以及閾值
//int xStart = 10;
//int yStart = 10;
//int xGoal = 700;
//int yGoal = 700;
//int thr = 50;
//int delta = 30;
//int numer = 3000;
//讀取地圖點(diǎn)
vector<vector<int>>mapData = MapData("map/map6.txt");
//定義起點(diǎn)和終點(diǎn),以及閾值
int xStart = 134; //起點(diǎn)x值
int yStart = 161; //起點(diǎn)y值
int xGoal = 251; //終點(diǎn)x值
int yGoal = 61; //終點(diǎn)y值
int thr = 10; //結(jié)束與終點(diǎn)的距離閾值
int delta = 10; //步長(zhǎng)
int numer = 3000; //迭代參數(shù)
//創(chuàng)建RRT對(duì)象
CRRT rrt(xStart, yStart, xGoal, yGoal, thr, delta, mapData);
vector<pair<float, float>>nearList, newList;
vector<pair<int, int>>path;
//RRT核心函數(shù)
bool flag = rrt.RRTCore(nearList, newList,numer);
if (flag == true)
{
//通過(guò)RRT獲得路徑
rrt.CreatePath(path);
std::cout << "path size is:" << path.size() << std::endl;
//顯示函數(shù)
Display(xStart, yStart, xGoal, yGoal, mapData, path, nearList, newList);
}
return 0;
}
3.2 地圖數(shù)據(jù)的獲取
本次地圖數(shù)據(jù)通過(guò)python程序?qū)⒌貓D圖片中的障礙物的位置存儲(chǔ)起來(lái)，然后通過(guò)C++流的方式進(jìn)行讀取。
img2txt.py：該程序可以將彩蛇或者黑白地圖中的障礙物**(gray_img [ i ] [ j ] [i][j] [i][j]== 0,數(shù)據(jù)0在圖片中為純黑，表示障礙物；255在圖片中為純白，表示自由可通行區(qū)域)**讀取，然后以txt的格式進(jìn)行存儲(chǔ)。python程序需要opencv的環(huán)境，大家自己百度安裝。
# -*- coding: utf-8 -*-
import matplotlib.pyplot as plt # plt 用于顯示圖片
import numpy as np
import cv2
img = cv2.imread("map/map6.bmp")
print(img.shape)
if len(img.shape)==3:
print("圖片為彩色圖")
gray_img = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
elif len(img.shape)==2:
print("圖片為灰度圖")
gray_img=img
h=gray_img.shape[0]
w=gray_img.shape[1]
print (gray_img.shape)
f = open("map/map6.txt", "wb")
# 尺寸 h, w
f.write((str(h) + " " + str(w) + "
").encode("utf-8"))
for i in range(h):
for j in range(w):
if gray_img[i][j] == 0:
print("hello world")
f.write((str(i) + " " + str(j) + "
").encode("utf-8"))
f.close()
print ("map.txt save sucessful")
其中保存的地圖txt數(shù)據(jù)分為兩部分，第一行表示地圖的高和寬；從第二行開(kāi)始表示障礙物的坐標(biāo)值，例如234 648表示第648行，第234列那個(gè)點(diǎn)的圖像像素值為0。圖像坐標(biāo)系中原點(diǎn)坐標(biāo)在圖像的左上角，x軸向右，y軸向下。
800 800
234 648
234 649
234 650
234 651
234 652
234 653
234 654
234 655
234 656
234 657
234 658
234 659
…
map.h
#pragma once #ifndef __MAP__ #define __MAP__ #include <vector> #include<iostream> #include <string> using namespace std; vector<vector<int>> MapData(std::string _MapPath); #endif
map.cpp：通過(guò)C++流的方式進(jìn)行txt數(shù)據(jù)的讀取，按照上述存儲(chǔ)方式進(jìn)行讀取。
/*該函數(shù)是讀取map.txt形成一個(gè)二維數(shù)組num，其中num里面0表示障礙物,255為可自由區(qū)域*/
#include "map.h"
#include<fstream>
#include<sstream>
vector<vector<int>>MapData(std::string _MapPath)
{
ifstream f;
f.open(_MapPath);
string str;
vector<vector<int> > num;
bool FirstLine = true;
while (getline(f, str)) //讀取1行并將它賦值給字符串str
{
if (FirstLine)
{
istringstream in(str); // c++風(fēng)格的串流的輸入操作
int a;
in >> a;
int height = a;
in >> a;
int wight = a;
num.resize(height, vector<int>(wight, 255));
FirstLine = false;
}
else
{
istringstream input(str); // c++風(fēng)格的串流的輸入操作
vector<int> tmp;
int a;
while (input >> a) //通過(guò)input將第一行的數(shù)據(jù)一個(gè)一個(gè)的輸入給a
tmp.push_back(a);
num[tmp[0]][tmp[1]] = 0;
}
}
return num;
}
3.3 RRT算法的實(shí)現(xiàn)
RRT.h：主要通過(guò)RRTCore函數(shù)實(shí)現(xiàn)RRT算法的本體，通過(guò)CreatePath函數(shù)獲得RRT算法的路徑。
#pragma once
#ifndef __RRT__
#define __RRT__
#include <iostream>
#include <string>
#include <vector>
#include <math.h>
#include "ctime"
#define N 999
#define pi 3.1415926
using namespace std;
//定義RRT搜索樹(shù)結(jié)構(gòu)
struct Tree
{
float Sx; //當(dāng)前點(diǎn)的x值
float Sy; //當(dāng)前點(diǎn)的y值 //new點(diǎn)
float SxPrev; //該點(diǎn)先前點(diǎn)的x值
float SyPrev; //該點(diǎn)先前點(diǎn)的x值 //near點(diǎn)
float Sdist; //near點(diǎn)與new點(diǎn)的距離
int SindPrev; //new點(diǎn)的索引
};
class CRRT
{
public:
//RRT構(gòu)造函數(shù)
CRRT(int _xStart, int _yStart, int _xGoal, int _yGoal, int _thr, int _delta, vector<vector<int>>_map);
//距離計(jì)算函數(shù)
inline float GetDist(int _x1, int _y1, int _x2, int _y2);
//與rand點(diǎn)距離較近的點(diǎn)在RRT樹(shù)中的索引
int GetMinIndex(int _x1, int _y1, vector<Tree>_T);
//點(diǎn)的安全性判定
inline bool FeasiblePoint(float _x, float _y, vector<vector<int>>_map);
//near點(diǎn)與new點(diǎn)連線之間的碰撞檢測(cè)
bool CollisionChecking(vector<float> _startPose, vector<float> _goalPose, vector<vector<int>>_map);
//RRT核心函數(shù)
bool RRTCore(int _numer,vector<pair<float,float>>& _nearList, vector<pair<float, float>>& _newList);
//RRT生成路徑函數(shù)
void CreatePath(vector<pair<int, int>>& _path);
private:
vector<Tree> m_TreeList; //RRT搜索樹(shù)列表
Tree m_tree; //RRT搜索樹(shù)
vector<vector<int>>m_map; //二維地圖
int m_xStart; //起點(diǎn)X值
int m_yStart; //起點(diǎn)Y值
int m_xGoal; //終點(diǎn)X值
int m_yGoal; //終點(diǎn)Y值
int m_thr; //距離閾值
int m_delta; //步長(zhǎng)
int m_mapWight; //地圖寬度
int m_mapHight; //地圖高度
};
#endif
RRT.cpp：主要實(shí)現(xiàn)RRT.h頭文件中的各成員函數(shù)。
#include "RRT.h"
/***********************************************************
*@函數(shù)功能: RRT構(gòu)造函數(shù),對(duì)地圖寬度和高度進(jìn)行初始化
-----------------------------------------------------------
*@函數(shù)參數(shù): _xStart 起點(diǎn)X值
_yStart 起點(diǎn)Y值
_xGoal 終點(diǎn)X值
_yGoal 終點(diǎn)Y值
_thr 距離閾值
_delta 步長(zhǎng)
_map 地圖
-----------------------------------------------------------
*@函數(shù)返回: 無(wú)
***********************************************************/
CRRT::CRRT(int _xStart,
int _yStart,
int _xGoal,
int _yGoal,
int _thr,
int _delta,
vector<vector<int>>_map
):m_xStart(_xStart),m_yStart(_yStart),m_xGoal(_xGoal),m_yGoal(_yGoal),m_thr(_thr),m_delta(_delta),m_map(_map)
{
m_mapWight = _map[0].size();
m_mapHight = _map.size();
}
/***********************************************************
*@函數(shù)功能: 兩點(diǎn)距離計(jì)算函數(shù)
-----------------------------------------------------------
*@函數(shù)參數(shù): _x1 第一個(gè)點(diǎn)X值
_y1 第一個(gè)點(diǎn)Y值
_x2 第二個(gè)點(diǎn)X值
_y2 第二個(gè)點(diǎn)Y值
-----------------------------------------------------------
*@函數(shù)返回: 兩點(diǎn)之間的距離值
***********************************************************/
inline float CRRT::GetDist(int _x1, int _y1, int _x2, int _y2)
{
return sqrt(pow((_x1 - _x2), 2) + pow((_y1 - _y2), 2));
}
/***********************************************************
*@函數(shù)功能: 求rand點(diǎn)距離較近的點(diǎn)在RRT樹(shù)中的索引
-----------------------------------------------------------
*@函數(shù)參數(shù): _x1 rand點(diǎn)X值
_y1 rand點(diǎn)Y值
_T RRT搜索樹(shù)列表
-----------------------------------------------------------
*@函數(shù)返回: 最近索引值
***********************************************************/
int CRRT::GetMinIndex(int _x1, int _y1, vector<Tree>_T)
{
float distance = FLT_MAX; //FLT_MAX表示float最大值
int index = 0;
for (int i = 0; i < _T.size(); i++)
{
if (GetDist(_x1, _y1, _T[i].Sx, _T[i].Sy) < distance)
{
distance = GetDist(_x1, _y1, _T[i].Sx, _T[i].Sy);
index = i;
}
}
return index;
}
/***********************************************************
*@函數(shù)功能: 點(diǎn)的安全性判定
-----------------------------------------------------------
*@函數(shù)參數(shù): _x1 點(diǎn)X值
_y1 點(diǎn)Y值
_map 地圖點(diǎn)
-----------------------------------------------------------
*@函數(shù)返回: true表示該點(diǎn)安全,false表示不安全
***********************************************************/
inline bool CRRT::FeasiblePoint(float _x, float _y, vector<vector<int>>_map)
{
//判斷該點(diǎn)是否在地圖的高度和寬度范圍內(nèi),且是否在障礙物內(nèi)
if (!(_x >= 0 && _x < m_mapWight && _y >= 0 && _y < m_mapHight && _map[_y][_x] == 255))
return false;
return true;
}
/***********************************************************
*@函數(shù)功能: near點(diǎn)與new點(diǎn)連線之間的碰撞檢測(cè)
-----------------------------------------------------------
*@函數(shù)參數(shù): _startPose near點(diǎn)
_goalPose new點(diǎn)
_map 地圖點(diǎn)
-----------------------------------------------------------
*@函數(shù)返回: true表示安全,false表示不安全
***********************************************************/
bool CRRT::CollisionChecking(vector<float> _startPose, vector<float> _goalPose, vector<vector<int>>_map)
{
//new點(diǎn)若在障礙物內(nèi),直接返回false
if (!(FeasiblePoint(floor(_goalPose[0]), ceil(_goalPose[1]), _map)))
{
return false;
}
float dir = atan2(_goalPose[0] - _startPose[0], _goalPose[1] - _startPose[1]);
float poseCheck[2] = { 0.0,0.0 };
float stop = sqrt(pow(_startPose[0] - _goalPose[0], 2) + pow(_startPose[1] - _goalPose[1], 2));
//r+=2表示在near與new連線的基礎(chǔ)上,每次移動(dòng)2個(gè)步長(zhǎng)
for (float r = 0; r <= stop; r += 2)
{
poseCheck[0] = _startPose[0] + r*sin(dir);
poseCheck[1] = _startPose[1] + r*cos(dir);
//由于poseCheck點(diǎn)為float類(lèi)型,為亞像素級(jí),因此判斷該點(diǎn)四領(lǐng)域的像素值,ceil向上取整,floor向下取整
if (!(FeasiblePoint(ceil(poseCheck[0]), ceil(poseCheck[1]), _map) &&
FeasiblePoint(floor(poseCheck[0]), floor(poseCheck[1]), _map) &&
FeasiblePoint(ceil(poseCheck[0]), floor(poseCheck[1]), _map) &&
FeasiblePoint(floor(poseCheck[0]), ceil(poseCheck[1]), _map)))
{
return false;
}
}
return true;
}
/***********************************************************
*@函數(shù)功能: RRT核心函數(shù)
-----------------------------------------------------------
*@函數(shù)參數(shù): _nearList near點(diǎn)集合,為引用傳參,實(shí)際上也為返回值
_newList new點(diǎn)集合,為引用傳參,實(shí)際上也為返回值
_numer RRT算法迭代次數(shù)
-----------------------------------------------------------
*@函數(shù)返回: true表示RRT找到路徑,false表示沒(méi)找到
***********************************************************/
bool CRRT::RRTCore(int _numer,vector<pair<float, float>>& _nearList, vector<pair<float, float>>& _newList)
{
//將起點(diǎn)插入樹(shù)中
m_tree.Sx =m_xStart;
m_tree.Sy = m_yStart;
m_tree.SxPrev = m_xGoal;
m_tree.SyPrev = m_yGoal;
m_tree.Sdist = 0;
m_tree.SindPrev = 0;
m_TreeList.push_back(m_tree);
vector<float>Rand, Near, New;
Rand.resize(2, 0.0);
Near.resize(2, 0.0);
New.resize(2, 0.0);
srand(time(NULL));//設(shè)置隨機(jī)數(shù)種子，使每次產(chǎn)生的隨機(jī)序列不同
int count = 0;
for (int i = 1; i <= _numer; i++)
{
//隨機(jī)產(chǎn)生地圖點(diǎn)Rand
Rand[0] =m_mapWight*(rand() % (N + 1) / (float)(N + 1));
Rand[1] = m_mapHight*(rand() % (N + 1) / (float)(N + 1));
//通過(guò)距離判斷來(lái)計(jì)算與Rand最近的Near點(diǎn)
int minDisIndex = GetMinIndex(Rand[0], Rand[1], m_TreeList);
Near[0] = m_TreeList[minDisIndex].Sx;
Near[1] = m_TreeList[minDisIndex].Sy;
//Near與Rand連線,移動(dòng)delta步長(zhǎng)
float theta = atan2(Rand[1] - Near[1], Rand[0] - Near[0]);
New[0] = Near[0] + m_delta*(cos(theta));
New[1] = Near[1] + m_delta*(sin(theta));
//連線碰撞檢測(cè)
if (!CollisionChecking(Near, New, m_map))
continue;
//擴(kuò)展RRT搜索樹(shù)
std::cout << "i:" << i << std::endl;
m_tree.Sx = New[0];
m_tree.Sy = New[1];
m_tree.SxPrev = Near[0];
m_tree.SyPrev = Near[1];
m_tree.Sdist = m_delta;
m_tree.SindPrev = minDisIndex;
m_TreeList.emplace_back(m_tree);
//距離閾值判斷,是否搜索結(jié)束
if (GetDist(New[0], New[1], m_xGoal, m_yGoal) < m_thr)
{
return true;
}
//保存near點(diǎn)與new點(diǎn)
_nearList.emplace_back(std::make_pair(Near[0], Near[1]));
_newList.emplace_back(std::make_pair(New[0], New[1]));
}
return false;
}
/***********************************************************
*@函數(shù)功能: RRT生成路徑,逆向搜索
-----------------------------------------------------------
*@函數(shù)參數(shù): _path RRT生成路徑集合點(diǎn),為引用傳參,實(shí)際上也為返回值
-----------------------------------------------------------
*@函數(shù)返回: 無(wú)
***********************************************************/
void CRRT::CreatePath(vector<pair<int, int>>& _path)
{
pair<int, int>temp;
//將終點(diǎn)加入路徑集合點(diǎn)
_path.push_back(std::make_pair(m_xGoal, m_yGoal));
//由于搜索路徑結(jié)束存在一個(gè)閾值,故將搜索樹(shù)的最后一個(gè)點(diǎn)加入路徑集合點(diǎn)
_path.push_back(std::make_pair(m_TreeList[m_TreeList.size() - 1].Sx, m_TreeList[m_TreeList.size() - 1].Sy));
int pathIndex = m_TreeList[m_TreeList.size() - 1].SindPrev;
//逆向搜索
while (true)
{
_path.emplace_back(std::make_pair(m_TreeList[pathIndex].Sx, m_TreeList[pathIndex].Sy));
pathIndex = m_TreeList[pathIndex].SindPrev;
if (pathIndex == 0)
break;
}
//將起點(diǎn)加入路徑集合點(diǎn)
_path.push_back(std::make_pair(m_TreeList[0].Sx, m_TreeList[0].Sy));
}
接下里主要從RRT中的核心函數(shù)RRTCore進(jìn)行算法介紹。
3.3.1 起點(diǎn)入樹(shù)
#include "RRT.h"
/***********************************************************
*@函數(shù)功能: RRT構(gòu)造函數(shù),對(duì)地圖寬度和高度進(jìn)行初始化
-----------------------------------------------------------
*@函數(shù)參數(shù): _xStart 起點(diǎn)X值
_yStart 起點(diǎn)Y值
_xGoal 終點(diǎn)X值
_yGoal 終點(diǎn)Y值
_thr 距離閾值
_delta 步長(zhǎng)
_map 地圖
-----------------------------------------------------------
*@函數(shù)返回: 無(wú)
***********************************************************/
CRRT::CRRT(int _xStart,
int _yStart,
int _xGoal,
int _yGoal,
int _thr,
int _delta,
vector<vector<int>>_map
):m_xStart(_xStart),m_yStart(_yStart),m_xGoal(_xGoal),m_yGoal(_yGoal),m_thr(_thr),m_delta(_delta),m_map(_map)
{
m_mapWight = _map[0].size();
m_mapHight = _map.size();
}
/***********************************************************
*@函數(shù)功能: 兩點(diǎn)距離計(jì)算函數(shù)
-----------------------------------------------------------
*@函數(shù)參數(shù): _x1 第一個(gè)點(diǎn)X值
_y1 第一個(gè)點(diǎn)Y值
_x2 第二個(gè)點(diǎn)X值
_y2 第二個(gè)點(diǎn)Y值
-----------------------------------------------------------
*@函數(shù)返回: 兩點(diǎn)之間的距離值
***********************************************************/
inline float CRRT::GetDist(int _x1, int _y1, int _x2, int _y2)
{
return sqrt(pow((_x1 - _x2), 2) + pow((_y1 - _y2), 2));
}
/***********************************************************
*@函數(shù)功能: 求rand點(diǎn)距離較近的點(diǎn)在RRT樹(shù)中的索引
-----------------------------------------------------------
*@函數(shù)參數(shù): _x1 rand點(diǎn)X值
_y1 rand點(diǎn)Y值
_T RRT搜索樹(shù)列表
-----------------------------------------------------------
*@函數(shù)返回: 最近索引值
***********************************************************/
int CRRT::GetMinIndex(int _x1, int _y1, vector<Tree>_T)
{
float distance = FLT_MAX; //FLT_MAX表示float最大值
int index = 0;
for (int i = 0; i < _T.size(); i++)
{
if (GetDist(_x1, _y1, _T[i].Sx, _T[i].Sy) < distance)
{
distance = GetDist(_x1, _y1, _T[i].Sx, _T[i].Sy);
index = i;
}
}
return index;
}
/***********************************************************
*@函數(shù)功能: 點(diǎn)的安全性判定
-----------------------------------------------------------
*@函數(shù)參數(shù): _x1 點(diǎn)X值
_y1 點(diǎn)Y值
_map 地圖點(diǎn)
-----------------------------------------------------------
*@函數(shù)返回: true表示該點(diǎn)安全,false表示不安全
***********************************************************/
inline bool CRRT::FeasiblePoint(float _x, float _y, vector<vector<int>>_map)
{
//判斷該點(diǎn)是否在地圖的高度和寬度范圍內(nèi),且是否在障礙物內(nèi)
if (!(_x >= 0 && _x < m_mapWight && _y >= 0 && _y < m_mapHight && _map[_y][_x] == 255))
return false;
return true;
}
/***********************************************************
*@函數(shù)功能: near點(diǎn)與new點(diǎn)連線之間的碰撞檢測(cè)
-----------------------------------------------------------
*@函數(shù)參數(shù): _startPose near點(diǎn)
_goalPose new點(diǎn)
_map 地圖點(diǎn)
-----------------------------------------------------------
*@函數(shù)返回: true表示安全,false表示不安全
***********************************************************/
bool CRRT::CollisionChecking(vector<float> _startPose, vector<float> _goalPose, vector<vector<int>>_map)
{
//new點(diǎn)若在障礙物內(nèi),直接返回false
if (!(FeasiblePoint(floor(_goalPose[0]), ceil(_goalPose[1]), _map)))
{
return false;
}
float dir = atan2(_goalPose[0] - _startPose[0], _goalPose[1] - _startPose[1]);
float poseCheck[2] = { 0.0,0.0 };
float stop = sqrt(pow(_startPose[0] - _goalPose[0], 2) + pow(_startPose[1] - _goalPose[1], 2));
//r+=2表示在near與new連線的基礎(chǔ)上,每次移動(dòng)2個(gè)步長(zhǎng)
for (float r = 0; r <= stop; r += 2)
{
poseCheck[0] = _startPose[0] + r*sin(dir);
poseCheck[1] = _startPose[1] + r*cos(dir);
//由于poseCheck點(diǎn)為float類(lèi)型,為亞像素級(jí),因此判斷該點(diǎn)四領(lǐng)域的像素值,ceil向上取整,floor向下取整
if (!(FeasiblePoint(ceil(poseCheck[0]), ceil(poseCheck[1]), _map) &&
FeasiblePoint(floor(poseCheck[0]), floor(poseCheck[1]), _map) &&
FeasiblePoint(ceil(poseCheck[0]), floor(poseCheck[1]), _map) &&
FeasiblePoint(floor(poseCheck[0]), ceil(poseCheck[1]), _map)))
{
return false;
}
}
return true;
}
/***********************************************************
*@函數(shù)功能: RRT核心函數(shù)
-----------------------------------------------------------
*@函數(shù)參數(shù): _nearList near點(diǎn)集合,為引用傳參,實(shí)際上也為返回值
_newList new點(diǎn)集合,為引用傳參,實(shí)際上也為返回值
_numer RRT算法迭代次數(shù)
-----------------------------------------------------------
*@函數(shù)返回: true表示RRT找到路徑,false表示沒(méi)找到
***********************************************************/
bool CRRT::RRTCore(int _numer,vector<pair<float, float>>& _nearList, vector<pair<float, float>>& _newList)
{
//將起點(diǎn)插入樹(shù)中
m_tree.Sx =m_xStart;
m_tree.Sy = m_yStart;
m_tree.SxPrev = m_xGoal;
m_tree.SyPrev = m_yGoal;
m_tree.Sdist = 0;
m_tree.SindPrev = 0;
m_TreeList.push_back(m_tree);
vector<float>Rand, Near, New;
Rand.resize(2, 0.0);
Near.resize(2, 0.0);
New.resize(2, 0.0);
srand(time(NULL));//設(shè)置隨機(jī)數(shù)種子，使每次產(chǎn)生的隨機(jī)序列不同
int count = 0;
for (int i = 1; i <= _numer; i++)
{
//隨機(jī)產(chǎn)生地圖點(diǎn)Rand
Rand[0] =m_mapWight*(rand() % (N + 1) / (float)(N + 1));
Rand[1] = m_mapHight*(rand() % (N + 1) / (float)(N + 1));
//通過(guò)距離判斷來(lái)計(jì)算與Rand最近的Near點(diǎn)
int minDisIndex = GetMinIndex(Rand[0], Rand[1], m_TreeList);
Near[0] = m_TreeList[minDisIndex].Sx;
Near[1] = m_TreeList[minDisIndex].Sy;
//Near與Rand連線,移動(dòng)delta步長(zhǎng)
float theta = atan2(Rand[1] - Near[1], Rand[0] - Near[0]);
New[0] = Near[0] + m_delta*(cos(theta));
New[1] = Near[1] + m_delta*(sin(theta));
//連線碰撞檢測(cè)
if (!CollisionChecking(Near, New, m_map))
continue;
//擴(kuò)展RRT搜索樹(shù)
std::cout << "i:" << i << std::endl;
m_tree.Sx = New[0];
m_tree.Sy = New[1];
m_tree.SxPrev = Near[0];
m_tree.SyPrev = Near[1];
m_tree.Sdist = m_delta;
m_tree.SindPrev = minDisIndex;
m_TreeList.emplace_back(m_tree);
//距離閾值判斷,是否搜索結(jié)束
if (GetDist(New[0], New[1], m_xGoal, m_yGoal) < m_thr)
{
return true;
}
//保存near點(diǎn)與new點(diǎn)
_nearList.emplace_back(std::make_pair(Near[0], Near[1]));
_newList.emplace_back(std::make_pair(New[0], New[1]));
}
return false;
}
/***********************************************************
*@函數(shù)功能: RRT生成路徑,逆向搜索
-----------------------------------------------------------
*@函數(shù)參數(shù): _path RRT生成路徑集合點(diǎn),為引用傳參,實(shí)際上也為返回值
-----------------------------------------------------------
*@函數(shù)返回: 無(wú)
***********************************************************/
void CRRT::CreatePath(vector<pair<int, int>>& _path)
{
pair<int, int>temp;
//將終點(diǎn)加入路徑集合點(diǎn)
_path.push_back(std::make_pair(m_xGoal, m_yGoal));
//由于搜索路徑結(jié)束存在一個(gè)閾值,故將搜索樹(shù)的最后一個(gè)點(diǎn)加入路徑集合點(diǎn)
_path.push_back(std::make_pair(m_TreeList[m_TreeList.size() - 1].Sx, m_TreeList[m_TreeList.size() - 1].Sy));
int pathIndex = m_TreeList[m_TreeList.size() - 1].SindPrev;
//逆向搜索
while (true)
{
_path.emplace_back(std::make_pair(m_TreeList[pathIndex].Sx, m_TreeList[pathIndex].Sy));
pathIndex = m_TreeList[pathIndex].SindPrev;
if (pathIndex == 0)
break;
}
//將起點(diǎn)加入路徑集合點(diǎn)
_path.push_back(std::make_pair(m_TreeList[0].Sx, m_TreeList[0].Sy));
}
3.3.2 rand點(diǎn)的獲取
為了方便起見(jiàn)，并沒(méi)有設(shè)置隨機(jī)采樣函數(shù)，通過(guò)隨機(jī)種子進(jìn)行rand的確定。其中(rand() % (N + 1) / (float)(N + 1))是產(chǎn)生0~1的隨機(jī)樹(shù)，小數(shù)點(diǎn)與N有關(guān)。
m_tree.Sx =m_xStart; m_tree.Sy = m_yStart; m_tree.SxPrev = m_xGoal; m_tree.SyPrev = m_yGoal; m_tree.Sdist = 0; m_tree.SindPrev = 0; m_TreeList.push_back(m_tree); vector<float>Rand, Near, New; Rand.resize(2, 0.0); Near.resize(2, 0.0); New.resize(2, 0.0);
3.3.3 near點(diǎn)的獲取
通過(guò)簡(jiǎn)單的距離函數(shù)進(jìn)行near點(diǎn)的判斷。其中GetMinIndex就是通過(guò)遍歷獲取與rand點(diǎn)最近的near點(diǎn)，當(dāng)然可以通過(guò)kd-tree對(duì)這塊進(jìn)行改進(jìn)，大家感興趣可以自行發(fā)揮，這里為了方便起見(jiàn)，就采用最原始的方法。
//隨機(jī)產(chǎn)生地圖點(diǎn)Rand Rand[0] =m_mapWight*(rand() % (N + 1) / (float)(N + 1)); Rand[1] = m_mapHight*(rand() % (N + 1) / (float)(N + 1));
3.3.4 new點(diǎn)的獲取
//通過(guò)距離判斷來(lái)計(jì)算與Rand最近的Near點(diǎn) int minDisIndex = GetMinIndex(Rand[0], Rand[1], m_TreeList); Near[0] = m_TreeList[minDisIndex].Sx; Near[1] = m_TreeList[minDisIndex].Sy;
注意near點(diǎn)的獲取使用C++中的atan2函數(shù)，該函數(shù)是 atan() 的增強(qiáng)版，能夠確定角度所在的象限。
其中**double atan2(double y，double x)**返回 y/x 的反正切值，以弧度表示，取值范圍為(-π,π]。如下圖所示，紅色線為 s i n ( θ ) sin( heta) sin(θ)，綠色線為 c o s ( θ ) cos( heta) cos(θ)。
當(dāng) (x, y) 在象限中時(shí)：
第一象限
第二象限
第三象限
第四象限
0 < θ < π / 2 0< heta<pi/2 0<θ<π/2
π / 2 < θ < π pi/2 < heta <pi π/2<θ<π
π < θ < π / 2 -pi< heta<-pi/2 π<θ<π/2
π / 2 < θ < 0 -pi/2< heta<0 π/2<θ<0
當(dāng) (x, y) 在象限的邊界（也就是坐標(biāo)軸）上時(shí)：
y = 0 y=0 y=0且 x ≥ 0 x geq 0 x≥0
y = 0 y=0 y=0且 x < 0 x < 0 x<0
y > 0 y>0 y>0且 x = 0 x=0 x=0
y < 0 y<0 y<0且 x = 0 x=0 x=0
θ = 0 heta =0 θ=0
θ = π heta=pi θ=π
θ = π / 2 heta=pi/2 θ=π/2
θ = π / 2 heta=-pi/2 θ=π/2
那么
n e w _ x = n e a r _ x + d c o s ( θ ) n e w _ y = n e a e _ y + d s i n ( θ ) new_x=near_x+d*cos( heta) \ new_y=neae_y+d*sin( heta) \ new_x=near_x+dcos(θ)new_y=neae_y+dsin(θ)
表示new點(diǎn)的情況如下，均滿足new點(diǎn)在near與rand點(diǎn)之間。這就是atan2帶來(lái)的好處。
第一象限
第二象限
第三象限
第四象限
3.3.5 安全性檢測(cè)
near點(diǎn)與new點(diǎn)之間的安全性判斷通過(guò)CollisionChecking函數(shù)所實(shí)習(xí)，基本思想就是沿著near與new點(diǎn)的方向，每隔一定的微小步長(zhǎng)（代碼中用 r r r）取一點(diǎn)，判斷該點(diǎn)是否在障礙物內(nèi)。注意微小步長(zhǎng)所取的點(diǎn)，它的像素是亞像素級(jí)的，可通過(guò)雙線性插值方法判斷該像素的值。本文為了方便起見(jiàn)，判斷該點(diǎn)亞像素的周?chē)狞c(diǎn)領(lǐng)域，進(jìn)行安全性判斷。
舉個(gè)例子，例如該點(diǎn)為 p = ( 1.3 , 4.7 ) p=(1.3,4.7) p=(1.3,4.7)，通過(guò)向下取整floor和向上取整ceil得該亞像素點(diǎn)的四點(diǎn)領(lǐng)域
c e i l ( 1.3 ) = 2 , c e i l ( 4.7 ) = 5 > p 1 = ( 2 , 5 ) ceil(1.3)=2,ceil(4.7) =5 —>p_1=(2,5) ceil(1.3)=2,ceil(4.7)=5>p1=(2,5)
f l o o r ( 1.3 ) = 1 , f l o o r ( 4.7 ) = 4 > p 2 = ( 1 , 4 ) floor(1.3)=1,floor(4.7)=4–>p_2=(1,4) floor(1.3)=1,floor(4.7)=4>p2=(1,4)
c e i l ( 1.3 ) = 2 , f l o o r ( 4.7 ) = 4 > p 3 = ( 2 , 4 ) ceil(1.3)=2,floor(4.7)=4–>p_3=(2,4) ceil(1.3)=2,floor(4.7)=4>p3=(2,4)
f l o o r ( 1.3 ) = 1 , c e i l ( 4.7 ) = 5 > p 4 = ( 1 , 5 ) floor(1.3)=1,ceil(4.7) =5—>p_4=(1,5) floor(1.3)=1,ceil(4.7)=5>p4=(1,5)
bool CRRT::CollisionChecking(vector<float> _startPose, vector<float> _goalPose, vector<vector<int>>_map)
{
//new點(diǎn)若在障礙物內(nèi),直接返回false
if (!(FeasiblePoint(floor(_goalPose[0]), ceil(_goalPose[1]), _map)))
{
return false;
}
float dir = atan2(_goalPose[0] - _startPose[0], _goalPose[1] - _startPose[1]);
float poseCheck[2] = { 0.0,0.0 };
float stop = sqrt(pow(_startPose[0] - _goalPose[0], 2) + pow(_startPose[1] - _goalPose[1], 2));
//r+=2表示在near與new連線的基礎(chǔ)上,每次移動(dòng)2個(gè)步長(zhǎng)
for (float r = 0; r <= stop; r += 2)
{
poseCheck[0] = _startPose[0] + r*sin(dir);
poseCheck[1] = _startPose[1] + r*cos(dir);
//由于poseCheck點(diǎn)為float類(lèi)型,為亞像素級(jí),因此判斷該點(diǎn)四領(lǐng)域的像素值,ceil向上取整,floor向下取整
if (!(FeasiblePoint(ceil(poseCheck[0]), ceil(poseCheck[1]), _map) &&
FeasiblePoint(floor(poseCheck[0]), floor(poseCheck[1]), _map) &&
FeasiblePoint(ceil(poseCheck[0]), floor(poseCheck[1]), _map) &&
FeasiblePoint(floor(poseCheck[0]), ceil(poseCheck[1]), _map)))
{
return false;
}
}
return true;
}
3.4 可視化顯示
display.h
#pragma once #ifndef __DISPLAY__ #define __DISPLAY__ #include <opencv2/opencv.hpp> #include <vector> using namespace std; //顯示函數(shù) void Display(int _xStart,int _yStart,int _xGoal,int _yGoal, vector<vector<int>>_map, vector<pair<int, int>>_path, vector<pair<float, float>>_nearList, vector<pair<float, float>>_newList); #endif // !__DISPLAY__
display.cpp
注意該代碼會(huì)在當(dāng)前項(xiàng)目中的image文件夾(沒(méi)有該文件夾，手動(dòng)創(chuàng)建一個(gè)即可)中存儲(chǔ)rrt顯示過(guò)程圖片(為了后期作gif使用，其他沒(méi)什么用)，若是不想存儲(chǔ)，則注釋掉。
cv::imwrite(“image/image” + std::to_string(i) + “.jpg”, image);
#include "display.h"
#include <iostream>
#include <string>
#include <Windows.h>
#include <cstdlib>
#include <io.h> // _access
#include <direct.h> //_mkdir
/***********************************************************
*@函數(shù)功能: RRT函數(shù)顯示
-----------------------------------------------------------
*@函數(shù)參數(shù): _xStart 起點(diǎn)X值
_yStart 起點(diǎn)Y值
_xGoal 終點(diǎn)X值
_yGoal 終點(diǎn)Y值
_thr 距離閾值
_map 地圖
_path 路徑點(diǎn)
_nearList near點(diǎn)集合
_newList new點(diǎn)集合
-----------------------------------------------------------
*@函數(shù)返回: 無(wú)
***********************************************************/
void Display(int _xStart,
int _yStart,
int _xGoal,
int _yGoal,
vector<vector<int>>_map,
vector<pair<int, int>>_path,
vector<pair<float, float>>_nearList,
vector<pair<float, float>>_newList)
{
int mapWight = _map[0].size();
int mapHight = _map.size();
//如沒(méi)有image文件夾,則新建一個(gè),存放RRT擴(kuò)展樹(shù)的中間過(guò)程
std::string prefix = "image";
if (_access(prefix.c_str(), 0) == -1) //如果文件夾不存在
{
_mkdir(prefix.c_str()); //則創(chuàng)建
}
//通過(guò)地圖點(diǎn)繪制圖像RGB,白色可通行區(qū)域,黑色為障礙物區(qū)域
cv::namedWindow("RRT result", CV_WINDOW_AUTOSIZE);
cv::Mat image(mapHight, mapWight, CV_8UC3, cv::Scalar(0, 0, 0));
for (int row = 0; row < mapHight; row++)
{
for (int col = 0; col < mapWight; col++)
{
if (_map[row][col] == 255)
{
image.at<cv::Vec3b>(row, col)[0] = 255;
image.at<cv::Vec3b>(row, col)[1] = 255;
image.at<cv::Vec3b>(row, col)[2] = 255;
}
}
}
//顯示起點(diǎn)和終點(diǎn),紅色起點(diǎn),綠色終點(diǎn)
cv::circle(image, cv::Point(_xStart, _yStart), 4, cv::Scalar(0, 0, 255), -1, 4); //起點(diǎn)
cv::circle(image, cv::Point(_xGoal, _yGoal), 4, cv::Scalar(0, 255, 0), -1, 4); //終點(diǎn)
//顯示路徑探索過(guò)程
for (int i = 0; i < _nearList.size(); i++)
{
cv::line(image, cv::Point(_nearList[i].first, _nearList[i].second), cv::Point(_newList[i].first, _newList[i].second), cv::Scalar(255, 0, 0), 2, 2);
cv::imshow("RRT result", image);
cv::waitKey(100); //100ms刷新一下
cv::imwrite("image/image" + std::to_string(i) + ".jpg", image);
}
//顯示最終路徑,黃色
for (int i = 0; i < _path.size() - 1; i++)
{
cv::line(image, cv::Point(_path[i].first, _path[i].second), cv::Point(_path[i + 1].first, _path[i + 1].second), cv::Scalar(0, 255, 255), 2, 2);
}
//保存6張最終圖片,方便制作gif
for (int i = 0; i <= 5; i++)
{
cv::imwrite("image/image"+std::to_string(_nearList.size()+i)+".jpg", image);
}
cv::imshow("RRT result", image);
cv::waitKey(0);
}
4. 代碼運(yùn)行過(guò)程
注意顯示過(guò)程中的“樹(shù)枝”表示near點(diǎn)與new點(diǎn)的連接。
顯示過(guò)程
顯示結(jié)果
map6.bmp
顯示過(guò)程
顯示結(jié)果
map.png
<動(dòng)圖太大，CSDN僅支持5M，無(wú)法顯示>
一個(gè)批量將圖片轉(zhuǎn)為gif的python腳本，注意python代碼中一定要添加dir_list = natsort.natsorted(dir_list)，否則會(huì)出現(xiàn)圖片亂序的問(wèn)題。
import os
import cv2 as cv
import moviepy.editor as mpy
import numpy as np
import natsort
import imageio
def frame_to_gif(frame_list):
gif = imageio.mimsave('./result.gif', frame_list, 'GIF', duration=0.00085)
dir_list = os.listdir('image')
dir_list = natsort.natsorted(dir_list)
img_list=[]
for i in range(0,len(dir_list)):
print (dir_list[i])
img = cv.imread('image\' + dir_list[i])
#img = cv.cvtcolor(img, cv.color_bgr2rgb)
img_list.append(img)
frame_to_gif(img_list)到此這篇關(guān)于C++結(jié)合OpenCV實(shí)現(xiàn)RRT算法的文章就介紹到這了,更多相關(guān)C++ OpenCV RRT算法內(nèi)容請(qǐng)搜索腳本之家以前的文章或繼續(xù)瀏覽下面的相關(guān)文章希望大家以后多多支持腳本之家！
相關(guān)文章
C++協(xié)程實(shí)現(xiàn)序列生成器的案例分享
序列生成器通常的實(shí)現(xiàn)是在一個(gè)協(xié)程內(nèi)部通過(guò)某種方式向外部傳一個(gè)值出去,并且將自己掛起,本文圍繞序列生成器這個(gè)經(jīng)典的協(xié)程案例介紹了協(xié)程的銷(xiāo)毀、co_await 運(yùn)算符、await_transform 以及 yield_value 的用法,需要的朋友可以參考下2024-05-05
ubuntu系統(tǒng)下C++調(diào)用matlab程序的方法詳解
學(xué)習(xí)c++與matlab混合編程一般是通過(guò)c++調(diào)用matlab函數(shù)，因?yàn)閙atlab具有強(qiáng)大的數(shù)學(xué)函數(shù)庫(kù)，然而vc++具有界面設(shè)計(jì)靈活的優(yōu)點(diǎn)，下面這篇文章主要給大家介紹了關(guān)于在ubuntu系統(tǒng)下C++調(diào)用matlab程序的方法，需要的朋友可以參考下。2017-08-08
詳解C++編程中類(lèi)的聲明和對(duì)象成員的引用
這篇文章主要介紹了詳解C++編程中類(lèi)的聲明和對(duì)象成員的引用,是C++入門(mén)學(xué)習(xí)中的基礎(chǔ)知識(shí),需要的朋友可以參考下2015-09-09
C語(yǔ)言當(dāng)函數(shù)執(zhí)行成功時(shí)return1還是0
本文主要介紹了C語(yǔ)言當(dāng)函數(shù)執(zhí)行成功時(shí)return1還是0，文中通過(guò)示例代碼介紹的非常詳細(xì)，具有一定的參考價(jià)值，感興趣的小伙伴們可以參考一下2021-09-09
這篇文章主要為大家介紹了C?C++?題解LeetCode1417重新格式化字符串，有需要的朋友可以借鑒參考下，希望能夠有所幫助，祝大家多多進(jìn)步，早日升職加薪2022-10-10
Linux下C語(yǔ)言修改進(jìn)程名稱(chēng)的方法
這篇文章主要介紹了Linux下C語(yǔ)言修改進(jìn)程名稱(chēng)的方法,涉及Linux下使用C語(yǔ)言操作進(jìn)程的相關(guān)技巧,具有一定參考借鑒價(jià)值,需要的朋友可以參考下2015-07-07
這篇文章主要為大家詳細(xì)介紹了C語(yǔ)言實(shí)現(xiàn)三子棋小游戲，文中示例代碼介紹的非常詳細(xì)，具有一定的參考價(jià)值，感興趣的小伙伴們可以參考一下2018-01-01
這篇文章主要介紹了C++中的函數(shù)模板,包括函數(shù)模板的聲明和生成以及異常處理等基本知識(shí),需要的朋友可以參考下2016-04-04
基于MATLAB神經(jīng)網(wǎng)絡(luò)圖像識(shí)別的高識(shí)別率代碼
今天小編就為大家分享一篇關(guān)于基于MATLAB神經(jīng)網(wǎng)絡(luò)圖像識(shí)別的高識(shí)別率代碼，小編覺(jué)得內(nèi)容挺不錯(cuò)的，現(xiàn)在分享給大家，具有很好的參考價(jià)值，需要的朋友一起跟隨小編來(lái)看看吧2019-03-03
C語(yǔ)言中_string.h庫(kù)函數(shù)功能及其用法詳解
在計(jì)算機(jī)編程中,字符串處理是一項(xiàng)常見(jiàn)而重要的任務(wù),C語(yǔ)言的string.h頭文件提供了一系列函數(shù)和工具,用于對(duì)字符串進(jìn)行操作和處理,本文將對(duì)string.h頭文件中的所有函數(shù)進(jìn)行全面介紹,包括它們的功能和使用方法,以幫助大家更好地理解和利用該頭文件2023-12-12