C++结合OpenCV实现RRT算法

文章目录

  • C++结合OpenCV实现RRT算法
    • 1.RRT算法简介
    • 2.算法整体框架流程
      • 2.1 rand点的建立
      • 2.2 near和new点的建立
      • 2.3 安全性检查
      • 2.4 算法结束判断
    • 3.RRT代码框架
      • 3.1 主函数
      • 3.2 地图数据的获取
      • 3.3 RRT算法的实现
        • 3.3.1 起点入树
        • 3.3.2 rand点的获取
        • 3.3.3 near点的获取
        • 3.3.4 new点的获取
        • 3.3.5 安全性检测
      • 3.4 可视化显示
    • 4. 代码运行过程

1.RRT算法简介

代码链接：RRT
动图展示

RRT

2.算法整体框架流程


RRT算法整体框架主要分为rand、near、new三点的建立和near与new之间的安全性检查

2.1 rand点的建立

rand点表示在地图MMM中随机采样获得，记住是随机。我们可以通过设计随机函数，让尽可能的点进入空旷区域，即算法框架中的Sample函数。下图中红色点表示起点，绿色的点表示终点。

2.2 near和new点的建立

near点表示从RRT树Γ\Gamma Γ中通过距离函数，判断树中哪个点距离当前rand点最近，此时该点即为near点。对应于算法框架中的Near函数。

new点表示以near点到rand为方向，以EiE_iEi​为步长，生成的一个新点。对应于算法框架的Steer函数。

2.3 安全性检查

若上述的new点在安全区域内，且new与near点连线安全，则会在RRT树中进行扩展，否则不会进行扩展。对应于算法框架中的CollisionFree函数。

2.4 算法结束判断

算法框架中的当new点与goal相等，表示算法运行成功，但是实际编程情况中，new点与goal点会存在一定的距离阈值。

3.RRT代码框架

3.1 主函数

main.cpp ：首先通过地图文件中读取地图数据(本次代码提供两张地图，供测试使用)，然后设置RRT算法的起点和终点，以及相关参数设置，例如距离阈值、步长、迭代次数等。其次通过RRT算法的接口函数RRTCore和CreatePath获得RRT算法的路径，最后通过显示函数Display进行数据可视化。

#include <iostream>
#include <vector>
#include <string>
#include "map.h"
#include "display.h"
#include "RRT.h"
using namespace std;int main()
{//读取地图点//vector<vector<int>>mapData = MapData("map/map.txt");定义起点和终点,以及阈值//int xStart = 10;//int yStart = 10;//int xGoal = 700;//int yGoal = 700;//int thr = 50;//int delta = 30;//int numer = 3000;//读取地图点vector<vector<int>>mapData = MapData("map/map6.txt");//定义起点和终点,以及阈值int xStart = 134;       //起点x值int yStart = 161;       //起点y值int xGoal = 251;        //终点x值int yGoal = 61;         //终点y值int thr = 10;           //结束与终点的距离阈值int delta = 10;         //步长int numer = 3000;       //迭代参数//创建RRT对象CRRT rrt(xStart, yStart, xGoal, yGoal, thr, delta, mapData);vector<pair<float, float>>nearList, newList;vector<pair<int, int>>path;//RRT核心函数bool flag = rrt.RRTCore(nearList, newList,numer);if (flag == true){//通过RRT获得路径rrt.CreatePath(path);std::cout << "path size is:" << path.size() << std::endl;//显示函数Display(xStart, yStart, xGoal, yGoal, mapData, path, nearList, newList);}return 0;
}

3.2 地图数据的获取

本次地图数据通过python程序将地图图片中的障碍物的位置存储起来，然后通过C++流的方式进行读取。

img2txt.py：该程序可以将彩蛇或者黑白地图中的障碍物**(gray_img[i][j][i][j][i][j]== 0,数据0在图片中为纯黑，表示障碍物；255在图片中为纯白，表示自由可通行区域)**读取，然后以txt的格式进行存储。python程序需要opencv的环境，大家自己百度安装。

# -*- coding: utf-8 -*-import matplotlib.pyplot as plt # plt 用于显示图片
import numpy as np
import cv2img = 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) + "\n").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) + "\n").encode("utf-8"))f.close()
print ("map.txt save sucessful")

其中保存的地图txt数据分为两部分，第一行表示地图的高和宽；从第二行开始表示障碍物的坐标值，例如234 648表示第648行，第234列那个点的图像像素值为0。图像坐标系中原点坐标在图像的左上角，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：通过C++流的方式进行txt数据的读取，按照上述存储方式进行读取。

/*该函数是读取map.txt形成一个二维数组num，其中num里面0表示障碍物,255为可自由区域*/
#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++风格的串流的输入操作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++风格的串流的输入操作vector<int> tmp;int a;while (input >> a)          //通过input将第一行的数据一个一个的输入给atmp.push_back(a);num[tmp[0]][tmp[1]] = 0;}}return num;
}

3.3 RRT算法的实现

RRT.h：主要通过RRTCore函数实现RRT算法的本体，通过CreatePath函数获得RRT算法的路径。

#pragma once
#ifndef __RRT__
#define __RRT__
#include <iostream>
#include <string>
#include <vector>
#include <math.h>
#include "ctime"
#define N 999
#define pi 3.1415926using namespace std;
//定义RRT搜索树结构
struct Tree
{float Sx;          //当前点的x值float Sy;          //当前点的y值           //new点float SxPrev;      //该点先前点的x值float SyPrev;      //该点先前点的x值       //near点float Sdist;       //near点与new点的距离int SindPrev;      //new点的索引
};
class CRRT
{
public://RRT构造函数CRRT(int _xStart, int _yStart, int _xGoal, int _yGoal, int _thr, int _delta, vector<vector<int>>_map);//距离计算函数inline float GetDist(int _x1, int _y1, int _x2, int _y2);//与rand点距离较近的点在RRT树中的索引int GetMinIndex(int _x1, int _y1, vector<Tree>_T);//点的安全性判定inline bool FeasiblePoint(float _x, float _y, vector<vector<int>>_map);//near点与new点连线之间的碰撞检测bool CollisionChecking(vector<float> _startPose, vector<float> _goalPose, vector<vector<int>>_map);//RRT核心函数bool RRTCore(int _numer,vector<pair<float,float>>& _nearList, vector<pair<float, float>>& _newList);//RRT生成路径函数void CreatePath(vector<pair<int, int>>& _path);
private:vector<Tree> m_TreeList;       //RRT搜索树列表Tree m_tree;                   //RRT搜索树vector<vector<int>>m_map;      //二维地图int m_xStart;                  //起点X值int m_yStart;                  //起点Y值int m_xGoal;                   //终点X值int m_yGoal;                   //终点Y值int m_thr;                     //距离阈值int m_delta;                   //步长int m_mapWight;                //地图宽度int m_mapHight;                //地图高度
};#endif

RRT.cpp：主要实现RRT.h头文件中的各成员函数。

#include "RRT.h"/***********************************************************
*@函数功能:       RRT构造函数,对地图宽度和高度进行初始化
-----------------------------------------------------------
*@函数参数:       _xStart  起点X值_yStart  起点Y值_xGoal   终点X值_yGoal   终点Y值_thr     距离阈值_delta   步长_map     地图
-----------------------------------------------------------
*@函数返回:      无
***********************************************************/
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();
}/***********************************************************
*@函数功能:       两点距离计算函数
-----------------------------------------------------------
*@函数参数:       _x1  第一个点X值_y1  第一个点Y值_x2  第二个点X值_y2  第二个点Y值
-----------------------------------------------------------
*@函数返回:      两点之间的距离值
***********************************************************/
inline float CRRT::GetDist(int _x1, int _y1, int _x2, int _y2)
{return sqrt(pow((_x1 - _x2), 2) + pow((_y1 - _y2), 2));
}/***********************************************************
*@函数功能:      求rand点距离较近的点在RRT树中的索引
-----------------------------------------------------------
*@函数参数:       _x1  rand点X值_y1  rand点Y值_T   RRT搜索树列表
-----------------------------------------------------------
*@函数返回:      最近索引值
***********************************************************/
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;
}/***********************************************************
*@函数功能:      点的安全性判定
-----------------------------------------------------------
*@函数参数:       _x1  点X值_y1  点Y值_map 地图点
-----------------------------------------------------------
*@函数返回:      true表示该点安全,false表示不安全
***********************************************************/
inline bool CRRT::FeasiblePoint(float _x, float _y, vector<vector<int>>_map)
{//判断该点是否在地图的高度和宽度范围内,且是否在障碍物内if (!(_x >= 0 && _x < m_mapWight && _y >= 0 && _y < m_mapHight && _map[_y][_x] == 255))return false;return true;
}/***********************************************************
*@函数功能:      near点与new点连线之间的碰撞检测
-----------------------------------------------------------
*@函数参数:       _startPose  near点_goalPose   new点_map 地图点
-----------------------------------------------------------
*@函数返回:      true表示安全,false表示不安全
***********************************************************/
bool CRRT::CollisionChecking(vector<float> _startPose, vector<float> _goalPose, vector<vector<int>>_map)
{//new点若在障碍物内,直接返回falseif (!(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连线的基础上,每次移动2个步长for (float r = 0; r <= stop; r += 2){poseCheck[0] = _startPose[0] + r*sin(dir);poseCheck[1] = _startPose[1] + r*cos(dir);//由于poseCheck点为float类型,为亚像素级,因此判断该点四领域的像素值,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;
}/***********************************************************
*@函数功能:   RRT核心函数
-----------------------------------------------------------
*@函数参数:   _nearList  near点集合,为引用传参,实际上也为返回值_newList    new点集合,为引用传参,实际上也为返回值_numer     RRT算法迭代次数
-----------------------------------------------------------
*@函数返回:   true表示RRT找到路径,false表示没找到
***********************************************************/
bool CRRT::RRTCore(int _numer,vector<pair<float, float>>& _nearList, vector<pair<float, float>>& _newList)
{//将起点插入树中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));//设置随机数种子，使每次产生的随机序列不同int count = 0;for (int i = 1; i <= _numer; i++){//随机产生地图点RandRand[0] =m_mapWight*(rand() % (N + 1) / (float)(N + 1));Rand[1] = m_mapHight*(rand() % (N + 1) / (float)(N + 1));//通过距离判断来计算与Rand最近的Near点int minDisIndex = GetMinIndex(Rand[0], Rand[1], m_TreeList);Near[0] = m_TreeList[minDisIndex].Sx;Near[1] = m_TreeList[minDisIndex].Sy;//Near与Rand连线,移动delta步长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));//连线碰撞检测if (!CollisionChecking(Near, New, m_map))continue;//扩展RRT搜索树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);//距离阈值判断,是否搜索结束if (GetDist(New[0], New[1], m_xGoal, m_yGoal) < m_thr){return true;}//保存near点与new点_nearList.emplace_back(std::make_pair(Near[0], Near[1]));_newList.emplace_back(std::make_pair(New[0], New[1]));}return false;}/***********************************************************
*@函数功能:   RRT生成路径,逆向搜索
-----------------------------------------------------------
*@函数参数:   _path  RRT生成路径集合点,为引用传参,实际上也为返回值
-----------------------------------------------------------
*@函数返回:   无
***********************************************************/
void CRRT::CreatePath(vector<pair<int, int>>& _path)
{pair<int, int>temp;//将终点加入路径集合点_path.push_back(std::make_pair(m_xGoal, m_yGoal));//由于搜索路径结束存在一个阈值,故将搜索树的最后一个点加入路径集合点_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;}//将起点加入路径集合点_path.push_back(std::make_pair(m_TreeList[0].Sx, m_TreeList[0].Sy));}

接下里主要从RRT中的核心函数RRTCore进行算法介绍。

3.3.1 起点入树

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.2 rand点的获取

为了方便起见，并没有设置随机采样函数，通过随机种子进行rand的确定。其中(rand() % (N + 1) / (float)(N + 1))是产生0~1的随机树，小数点与N有关。

//随机产生地图点Rand
Rand[0] =m_mapWight*(rand() % (N + 1) / (float)(N + 1));
Rand[1] = m_mapHight*(rand() % (N + 1) / (float)(N + 1));

3.3.3 near点的获取

通过简单的距离函数进行near点的判断。其中GetMinIndex就是通过遍历获取与rand点最近的near点，当然可以通过kd-tree对这块进行改进，大家感兴趣可以自行发挥，这里为了方便起见，就采用最原始的方法。

//通过距离判断来计算与Rand最近的Near点
int minDisIndex = GetMinIndex(Rand[0], Rand[1], m_TreeList);
Near[0] = m_TreeList[minDisIndex].Sx;
Near[1] = m_TreeList[minDisIndex].Sy;

3.3.4 new点的获取

//Near与Rand连线,移动delta步长
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));

注意near点的获取使用C++中的atan2函数，该函数是 atan() 的增强版，能够确定角度所在的象限。

其中**double atan2(double y，double x)**返回 y/x 的反正切值，以弧度表示，取值范围为(-π,π]。如下图所示，红色线为sin(θ)sin(\theta)sin(θ)，绿色线为cos(θ)cos(\theta)cos(θ)。

当 (x, y) 在象限中时：

第一象限 第二象限 第三象限 第四象限
0<θ<π/20<\theta<\pi/20<θ<π/2 π/2<θ<π\pi/2 <\theta <\piπ/2<θ<π −π<θ<−π/2-\pi<\theta<-\pi/2−π<θ<−π/2 −π/2<θ<0-\pi/2<\theta<0−π/2<θ<0

当 (x, y) 在象限的边界（也就是坐标轴）上时：

y=0y=0y=0且x≥0x \geq 0x≥0 y=0y=0y=0且x<0x < 0x<0 y>0y>0y>0且x=0x=0x=0 y<0y<0y<0且x=0x=0x=0
θ=0\theta =0θ=0 θ=π\theta=\piθ=π θ=π/2\theta=\pi/2θ=π/2 θ=−π/2\theta=-\pi/2θ=−π/2

那么
new_x=near_x+d∗cos(θ)new_y=neae_y+d∗sin(θ)new\_x=near\_x+d*cos(\theta) \\ new\_y=neae\_y+d*sin(\theta) \\ new_x=near_x+d∗cos(θ)new_y=neae_y+d∗sin(θ)

表示new点的情况如下，均满足new点在near与rand点之间。这就是atan2带来的好处。

第一象限
第二象限
第三象限
第四象限

3.3.5 安全性检测

near点与new点之间的安全性判断通过CollisionChecking函数所实习，基本思想就是沿着near与new点的方向，每隔一定的微小步长（代码中用rrr）取一点，判断该点是否在障碍物内。注意微小步长所取的点，它的像素是亚像素级的，可通过双线性插值方法判断该像素的值。本文为了方便起见，判断该点亚像素的周围四点领域，进行安全性判断。

举个例子，例如该点为p=(1.3,4.7)p=(1.3,4.7)p=(1.3,4.7)，通过向下取整floor和向上取整ceil得该亚像素点的四点领域

ceil(1.3)=2,ceil(4.7)=5−−−>p1=(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)

floor(1.3)=1,floor(4.7)=4−−>p2=(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)

ceil(1.3)=2,floor(4.7)=4−−>p3=(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)

floor(1.3)=1,ceil(4.7)=5−−−>p4=(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点若在障碍物内,直接返回falseif (!(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连线的基础上,每次移动2个步长for (float r = 0; r <= stop; r += 2){poseCheck[0] = _startPose[0] + r*sin(dir);poseCheck[1] = _startPose[1] + r*cos(dir);//由于poseCheck点为float类型,为亚像素级,因此判断该点四领域的像素值,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;
//显示函数
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

注意该代码会在当前项目中的image文件夹(没有该文件夹，手动创建一个即可)中存储rrt显示过程图片(为了后期作gif使用，其他没什么用)，若是不想存储，则注释掉。

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
/***********************************************************
*@函数功能:       RRT函数显示
-----------------------------------------------------------
*@函数参数:       _xStart  起点X值_yStart  起点Y值_xGoal   终点X值_yGoal   终点Y值_thr     距离阈值_map     地图_path    路径点_nearList near点集合_newList  new点集合
-----------------------------------------------------------
*@函数返回:      无
***********************************************************/
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();//如没有image文件夹,则新建一个,存放RRT扩展树的中间过程std::string prefix = "image";if (_access(prefix.c_str(), 0) == -1)   //如果文件夹不存在{_mkdir(prefix.c_str());    //则创建}//通过地图点绘制图像RGB,白色可通行区域,黑色为障碍物区域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;}}}//显示起点和终点,红色起点,绿色终点cv::circle(image, cv::Point(_xStart, _yStart), 4, cv::Scalar(0, 0, 255), -1, 4);   //起点cv::circle(image, cv::Point(_xGoal, _yGoal), 4, cv::Scalar(0, 255, 0), -1, 4);    //终点//显示路径探索过程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张最终图片,方便制作giffor (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. 代码运行过程

注意显示过程中的“树枝”表示near点与new点的连接。

显示过程
显示结果
map6.bmp
显示过程
显示结果
map.png
<动图太大，CSDN仅支持5M，无法显示>

一个批量将图片转为gif的python脚本，注意python代码中一定要添加dir_list = natsort.natsorted(dir_list)，否则会出现图片乱序的问题。

import os
import cv2 as cv
import moviepy.editor as mpy
import numpy as np
import natsort
import imageiodef 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)

参考连接：https://blog.csdn.net/qq_44965314/article/details/107706145

啰里啰唆说了这么多，就到这里吧，图中的部分图片摘自深蓝学院的路径与规划课程，博客转载请注明出处，谢谢。

(二)路径规划算法---C++结合OpenCV实现RRT算法相关推荐

  1. 左上角到右下角的路径 oj_【机器人路径规划】快速扩展随机树(RRT)算法

    内容无关:最近的课题内容和机器人运动规划方法有关,我把学习的内容整理成为工具箱上传到了我的github仓库,稍后将会发一篇说明介绍使用方法. XM522706601/robotics_tutorial ...

  2. 【APF三维路径规划】基于matlab人工势场算法无人机三维路径规划【含Matlab源码 168期】

    一.获取代码方式 获取代码方式1: 通过订阅紫极神光博客付费专栏,凭支付凭证,私信博主,可获得此代码. 获取代码方式2: 完整代码已上传我的资源:[三维路径规划]基于matlab人工势场算法无人机三维 ...

  3. C#，人工智能，机器人，路径规划，A*（AStar Algorithm）算法、源代码及计算数据可视化

    Peter Hart Nils Nilsson Bertram Raphael 参考: C#,人工智能(AI)机器人路径规划(Path Planning)的ARA*(Anytime Replannin ...

  4. BS1036-基于java+路径规划+CS架构实现的A星算法求解最短路径问题演示程序

    本基于java+路径规划+CS架构实现的A星算法求解最短路径问题演示程序,系统采用多层C/S软件架构,采用java 编程语言开发技术实现A*算法求解地图中的最短路径问题,实时获取计算用户在地图中设置的 ...

  5. 【蚁群路径规划】基于MATLAB的蚁群算法的二维路径规划

    %% 清空环境 clc;clear%% 障碍物数据 position = load('barrier.txt'); plot([0,200],[0,200],'.'); hold on B = loa ...

  6. 二.路径规划---二维路径规划实车实现---gmapping+amcl+map_server+move_base

    专栏系列文章如下: 一.路径规划---二维路径规划仿真实现-gmapping+amcl+map_server+move_base_goldqiu的博客-CSDN博客 本次实验是利用gmapping只使 ...

  7. 【DE三维路径规划】基于matlab改进的差分算法多无人机协同三维路径规划【含Matlab源码 169期】

    ⛄一.无人机简介 0 引言 随着现代技术的发展,飞行器种类不断变多,应用也日趋专一化.完善化,如专门用作植保的大疆PS-X625无人机,用作街景拍摄与监控巡察的宝鸡行翼航空科技的X8无人机,以及用作水 ...

  8. 【路径规划】基于matlab遗传优化模拟退火算法避障路径规划【含Matlab源码 889期】

    ⛄一.简介 路径规划主要是让目标对象在规定范围内的区域内找到一条从起点到终点的无碰撞安全路径.路径规划中有静态路径规划以及动态路径规划,本文所讨论的问题仅针对静态路径规划.具体问题描述如下: 给定起点 ...

  9. 【BA三维路径规划】基于matlab改进的蝙蝠算法农用无人机三维路径规划【含Matlab源码 1514期】

    一.无人机简介 0 引言 随着现代技术的发展,飞行器种类不断变多,应用也日趋专一化.完善化,如专门用作植保的大疆PS-X625无人机,用作街景拍摄与监控巡察的宝鸡行翼航空科技的X8无人机,以及用作水下 ...

  10. 【ACO三维路径规划】基于matlab GUI蚁群算法无人机三维路径规划【含Matlab源码 254期】

    一.无人机简介 0 引言 随着现代技术的发展,飞行器种类不断变多,应用也日趋专一化.完善化,如专门用作植保的大疆PS-X625无人机,用作街景拍摄与监控巡察的宝鸡行翼航空科技的X8无人机,以及用作水下 ...

最新文章

  1. 微信红包如何抢大包、直播反垃圾、老骗局翻新……这一期宅客周刊你值得拥有...
  2. hdu_Anniversary party_(树形DP入门题)
  3. Python Cookbook (2) 文件
  4. CrystalDecisions.CrystalReports.Engine.LoadSaveReportException:載入報表失敗6/25
  5. ODS(Operational Data Store)定义
  6. js学习 字符串常用方法
  7. axios 上传文件_聚是一团火散作满天星，前端Vue.js+elementUI结合后端FastAPI实现大文件分片上传...
  8. Docker系列(四)守护式容器
  9. asp.net 从客户端中检测到有潜在危险的 Request.Form 值
  10. A级学科计算机技术,东南大学a类学科排名！附东大a类学科名单
  11. 使用Python快速实现人脸融合
  12. FDTD Solutions初学笔记
  13. ubuntu16.04设置自启动wifi热点
  14. 获取基因的所有转录本（不同亚型）的外显子区域
  15. 资深项目经理2019年总结
  16. 电脑控制android 源代码,[源代码]电脑通过adb控制安卓手机
  17. Learned-Mixin +H（LMH）
  18. 腾讯云服务器域名申请 备案和绑定IP地址详细步骤带清晰图
  19. 模拟肯德基KFC快餐店收银系统
  20. 什么是阻抗匹配以及为什么要阻抗匹配

热门文章

  1. office2016 + visio2016
  2. 屏幕尺寸、分辨率、DPI、PPI
  3. FFmpeg下载网络视频流
  4. 基于Java的TCP Socket通信详解
  5. 黑马程序员Python教程的代码--植物大战僵尸游戏代码
  6. 西门子200程序案例集
  7. sql和mysql 语法区别吗_sql和mysql语法有什么不同
  8. 2022年下半年深圳地区数据分析师认证（CPDA），[进入查看]
  9. endnotex7怎么导入中文文献_EndNote导入CNKI文献的方法 | 科研动力
  10. PUN☀️四、服务器大厅建房解析