路径规划算法之栅格地图绘制

来源 | 古月居01 数据地图1.1 地图类型1.2 栅格地图表示方式a. 本栏路径规划利用矩阵（二维数组）来表示栅格地图（因为对于矩阵，无论MATLAB、C++还是Python，矩阵更适合数组的表达，更便于编程。）b. 矩阵表示地图与传统意识中坐标X,Y有所区别。如图所示的地图，在矩阵表示中，起点位置为[3,1]，终点为[3,5]，三个障碍物分别为[2,3],[3,3]和[4,3]。在坐标表示.

3Ｄ视觉工坊

4841人浏览 · 2023-03-14 07:00:03

3Ｄ视觉工坊 · 2023-03-14 07:00:03 发布

来源 | 古月居

01 数据地图

1.1 地图类型

1.2 栅格地图表示方式

a. 本栏路径规划利用矩阵（二维数组）来表示栅格地图（因为对于矩阵，无论MATLAB、C++还是Python，矩阵更适合数组的表达，更便于编程。）

b. 矩阵表示地图与传统意识中坐标X,Y有所区别。

如图所示的地图，在矩阵表示中，起点位置为[3,1]，终点为[3,5]，三个障碍物分别为[2,3],[3,3]和[4,3]。在坐标表示中，起点位置为(1,2)，终点为(5,2)，三个障碍物分别为(3,1),(3,2)和(3,3)

c. 矩阵表示地图还可以用线性索引来简化，这样将矩阵的二维数组表示成一位数组，编程更方便。

Tips：线性索引，是从第一列开始，沿着行按顺序增长，然后从第二列开始…，是竖着沿行增长的，而非横着沿着列增长。理解记忆有点类似于Excel表格按行递增。

02 位置的表示方法的区分

2.1 在栅格地图上绘制xy点发生的情况

对于程序

clear;close all;
rows = 4; cols = 5;                           % 地图行列尺寸


% 设置地图属性
field = ones(rows, cols);
field(1,3) = 4;          % 栅格矩阵表示1,3位置图像


% 颜色表征矩阵
cmap = [1 1 1; ...       % 1-白色-空地
    0 0 0; ...           % 2-黑色-静态障碍
    1 0 0; ...           % 3-红色-动态障碍
    1 1 0;...            % 4-黄色-起始点
    1 0 1;...            % 5-品红-目标点
    0 1 0; ...           % 6-绿色-到目标点的规划路径
    0 1 1];              % 7-青色-动态规划的路径
colormap(cmap);
image(1.5,1.5,field);


% % 设置栅格属性
grid on;hold on;
set(gca,'gridline','-','gridcolor','k','linewidth',0.5,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
set(gca, 'XAxisLocation','top')
axis image;


scatter(1,3,200,'filled');  % XY绘制1,3图像，其中200是圆圈大小

‍

这是因为在栅格地图上的坐标系X对应于列col，而坐标Y对于与行row

2.2 三种表述位置方法的关系

下面图片是针对于Python绘图表述的，MATLAB绘图转换关系是一致的，但是MATLAB数组从1开始，而Python数组从0开始

①sub2coord和sub2xy均是仿照MATLABsub2ind命名的函数，方便理解和记忆

②MATLAB只要用到sub2ind和sub2coord即可

③sub2xy主要是正对于Python进行plt.plot绘图使用的

几个代码的转换函数见后面的模块

03 MATLAB绘制栅格地图

Tips1：这里为了方便MATLAB语言、Python语言和C++编程的统一性，统一采用按行递增的方式存放的下标特征信息数据（如障碍物坐标[row1,col1;row2,col2;row3,col3;…]），表示地图特征信息的位置==；

Tips2：当你获得线性地图特征信息的线性索引关系或者是XY坐标值，可以将其装换成线性索引值套用程序。MATLAB的数组的应用可以参考：

MATLAB数组的一些操作

https://blog.csdn.net/qq_42727752/article/details/119765592

MATLAB绘制栅格步骤：

a. 创建行列大小的二维矩阵，初始状态全为数值1表示空地；

b. 利用矩阵(下标位置) = 赋值的方式，修改对应位置的数值(和colormap的颜色值对应)，表示地图特征；

c. 利用colormap函数和image函数绘制出颜色地图；

d. 添加坐标。

3.1 colormap函数

利用image函数可以画出栅格图像，具体操作可以查看help image的帮助文档。

3.2 几个位置表述转换函数

MATLAB只要用到sub2ind和sub2coord即可

sub2ind和ind2sub将矩阵对应的线性值化成下标，具体操作可以查看 help sub2ind和help ind2sub的帮助文档。

sub2coord.m和coord2sub.m函数：

function coord = sub2coord(sub)
%SUB2COORD 将行列式下标装换为坐标格式，此时的坐标格式和原本认知坐标方向也不一致（如下所示）
%        1 2 3 4 5 6 7 .... X坐标
%      1|——————————>
%      2|
%      3|
%      4|
%      5|
%  Y坐标\/


    [l,w] = size(sub);
    % 长度l=2表示sub为2*n矩阵
if l == 2
coord(1,:) = sub(2,:);
        coord(2,:) = sub(1,:);
    end


if w == 2
        coord(:,1) = sub(:,2);
        coord(:,2) = sub(:,1);
    end


end

function sub = coord2sub(coord)
%COORD2SUB 将坐标转换为矩阵行列格式，坐标格式为下图所示
%        1 2 3 4 5 6 7 .... X坐标
%      1|——————————>
%      2|
%      3|
%      4|
%      5|
%  Y坐标\/


    [l,w] = size(coord);
    % 长度l=2表示sub为2*n矩阵
if l == 2
sub(1,:) = coord(2,:);
sub(2,:) = coord(1,:);
    end


if w == 2
sub(:,1) = coord(:,2);
sub(:,2) = coord(:,1);
    end
end

‍

3.3 利用地图线性值信息绘制栅格图(FD_DrawRasterMap.m函数)

线性索引值的目的就是省略for循环，因为矩阵A(线性索引)=int可以很方便的全部赋值，而Python/C++等需要借助循环

其次是线性索引作为输入，只要一个参数即可，同时在表达矩阵上，可以简化成一维，而行列是二维较为复杂

function Fields = DrawRasterMap(rows,cols,startsub,endsub,obssub)
%DRAWRASTERMAO 绘制随机障碍物的栅格图,存在一个返回内容；输入格式（行数 列数 起点行列 终点行列置 障碍物行列）；
                                                       % 输出内容:数值化的栅格矩阵


% 定义栅格地图全域，并初始化空白区域
field = ones(rows, cols); %初始化空白区域 数值全为1,数值用于表示Cmap的颜色(因为Cmap数组索引只能从1开始)


% 起始点和目标点
start = startsub;                       % 起点的位置
goal = endsub;                          % 终点的位置
field(start(1),start(2)) = 4;           % 起点在数组的数值,数值用于表示Cmap的颜色
field(goal(1),goal(2)) = 5;             % 终点在数组的数值,数值用于表示Cmap的颜色


% 障碍物区域
obs = obssub;
field(obs(:,1),obs(:,2)) = 2;       %障碍物在数组的数值,数值用于表示Cmap的颜色


% ObsR = obssub(:,1);ObsC = obssub(:,2);
% obs = sub2ind([rows,cols],ObsR,ObsC);
% field(obs) = 2;                         % 障碍物在数组的数值,数值用于表示Cmap的颜色


% 函数返回值为数值化的栅格矩阵
Fields = field;


%构建数组的栅格图 可以查看帮助 help image
cmap = [1 1 1; ...       % 1-白色-空地
    0 0 0; ...           % 2-黑色-静态障碍
    1 0 0; ...           % 3-红色-动态障碍
    1 1 0;...            % 4-黄色-起始点 
    1 0 1;...            % 5-品红-目标点
    0 1 0; ...           % 6-绿色-到目标点的规划路径   
    0 1 1; ...           % 7-青色-动态规划的路径
    0 0 1];              % 8-蓝色


% 构建颜色MAP图
colormap(cmap);




% 绘制图像
image(1.5,1.5,field);
grid on;
set(gca,'gridline','-','gridcolor','k','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
set(gca, 'XAxisLocation','top')
axis image;


end

测试代码：生成行为4列为5的矩阵，起点位置在3，终点在19，障碍物位置在10、11和12。主要目的是为了理解image函数的线性值和ROW和COL的坐标关系。

%%%% TestMain.m
%%%% 脚本文件 用于直接调用函数 熟悉概念
clc;
clear;
close all;
rows = 4;cols = 5;


[startpos(1),startpos(2)] = ind2sub([rows,cols],3);
[endpos(1),endpos(2)] = ind2sub([rows,cols],19);
[obspos(:,1),obspos(:,2)] = ind2sub([rows,cols],[10;11;12]);


field = FD_DrawRasterMap(rows,cols,startpos,endpos,obspos);
field
% 如果出线障碍物信息在地图上显示不对
% 可能是在函数内的field(obsIndex(:,1),obsIndex(:,2)) = 2;产生错误
% 可以采用将行列转成线性索引即函数内该代码下面注释的内容

最后生成的图像如图所示。

04 Python绘制栅格地图

Python绘制栅格地图的流程：

a. 创建全1二维矩阵，表示空地信息。
b. 将下标位置的栅格信息装换成特征值。
c. 将矩阵可视化。

Python数组的一些操作：

https://blog.csdn.net/qq_42727752/article/details/108368891

输入数据依旧采用按列递增的方式（如障碍物坐标[[row1,col1],][row2,col2],[row3,col3];…]）

4.1 matplotlib和seaborn包

绘制栅格地图前需要pip install matplotlib和seaborn两个绘图功能包。

4.2 几种坐标转换函数PathPlanning.py

import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import seaborn as sns
import random
import copy
from matplotlib import colors








'''
# --------------------------------PATHPLANNING函数包------------------------------------
# =====Tips1:输入参数矩阵尺寸为[0-->rows]的所有个数，非rows的最大下标
# =====Tips2:区别行列矩阵，行列坐标，和XY坐标
# =====Tips3:在不同表示系，sz均为行列尺寸，即sz=[rows,cols],在XY坐标坐标系中sz=[leny,lenx]
#
# # sub2coord(possub)函数，输入单个行列位置，输出单个行列坐标位置(数组)
# # coord2sub(posxy)函数，输入单个行列坐标位置，输出单个行列位置(数组)
# # xy2sub(sz,x,y)函数，输入尺寸、x坐标和y坐标，输出行列位置(数组)
# # sub2xy(sz,r,c)函数，输入尺寸、XY坐标行位置和列位置，输出xy位置(数组)
# # sub2ind(sz,r,c)函数，输入尺寸，行位置和列位置，输出对应的线性索引值(从0开始)
# # ind2sub(sz,ind)函数，输入尺寸和线性索引值，输出矩阵位置(数组)
# # DrawHeatMap(field)函数，输入地图矩阵绘制热力图，无输出
# ----------------------------------------------------------------------------------------
'''








'''
# sub2coord和coord2sub函数，行列坐标系内，行列位置和坐标位置相互转换
# 其中的【行列坐标系】形式如图所示，从0开始，因为Python数组下标从0开始，Y方向沿下递增
#
#                   0 1 2 3 4
#                0+---------->X(X=cols)
#                1|
#                2|
#                3|
#               Y(Y=rows)
'''
def sub2coord(possub):
    posx = possub[1]
    posy = possub[0]
return [posx, posy]




def coord2sub(posxy):
    posr = posxy[1]
    posc = posxy[0]
return [posr, posc]




'''
# xy2sub和sub2xy函数，坐标系XY位置转换为矩阵格式的转换函数
# ++++sz为坐标尺寸，即元素个数，非最大坐标值
# 其中的XY形式如图所示，从0开始，到[sz]-1为止
#
#                Y/
#                3|
#                2|
#                1|
#                0+---------->X
#                   0 1 2 3 4
'''
def xy2sub(sz, x, y):
    r = sz[0]-1-y
    c = x
return [r, c]




def sub2xy(sz, r, c):
    x = c
    y = sz[0]-1-r
return [x, y]




'''
# sub2ind和ind2sub函数，将行列位置转换为索引位置
# 行列位置和索引关系，对应如图所示
#
#                   0 1 2 3 4
#                0+---------->cols
#                1| 0 3 6 ...
#                2| 1 4 7 ...
#                3| 2 5 8 ...
#               rows
'''


def sub2ind(sz,r,c):
    ind = c*sz[0]+r
return ind




def ind2sub(sz,ind):
    c = int(ind/sz[0])
    r = ind-c*sz[0]
return [r,c]








'''
# DrawHeatMap函数，用于通过栅格地图的信息，绘制出彩色地图
# INPUT：栅格电子地图矩阵
# OUTPUT:NONE
#
'''
def DrawHeatMap(field):
    rows = len(field)
    cols = len(field[0])
    cmap = colors.ListedColormap(['none', 'white', 'black', 'red', 'yellow', 'magenta', 'green', 'cyan', 'blue'])


# 绘图函数
# 其实默认为fig,ax = plt.figure(),后续发现fig没有用上
# 但是ax需要频繁使用，因此直接ax = plt.gca()替代掉
    plt.figure(figsize=(12, 8))
    ax = plt.gca()


# 绘制热力图
# 其中vmin和vmax对应栅格地图数值的颜色与cmap一一对应
# cbar设置false将色条设置为不可见
    ax = sns.heatmap(field, cmap=cmap, vmin=0, vmax=8, linewidths=0.8,linecolor='black', ax=ax, cbar=False)


# 设置图标题
    ax.set_ylabel('rows')
    ax.set_xlabel('cols')


# 将列标签移动到图像上方
    ax.xaxis.tick_top()
    ax.xaxis.set_label_position('top')


# 设置图标的数字个数文字，放在plt.show下面能居中
    ax.set_xticks(np.arange(cols))
    ax.set_yticks(np.arange(rows))

4.3 采用seaborn绘制静态地图

import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import seaborn as sns
from matplotlib import colors
from matplotlib import animation
import matplotlib.patches as pc
import copy




def sub2coord(possub):
    posx = possub[1]
    posy = possub[0]
return [posx,posy]




def coord2sub(posxy):
    posr = posxy[1]
    posc = posxy[0]
return [posr,posc]




'''
# # -------------------------------------这里定义绘图函数-------------------------------------------------
'''
def drawmap(rows,cols,startSub,goalSub,obsSub):
# 创建全部为空地的地图栅格，其中空地以数字1表征
    field = np.ones((rows,cols))


# 修改栅格地图中起始点和终点的数值，其中起点以数值4表征，终点以数值5表示
    field[startSub[0],startSub[1]] = 4
    field[goalSub[0],goalSub[1]] = 5


# 修改栅格地图中障碍物的数值，其中以数值5表示
for i in range(len(obsSub)):
        field[obsSub[i][0],obsSub[i][1]] = 2


# 绘制图像，利用matplotlib的热力图进行绘制
# 设置色条的范围，从0~8
    cmap = colors.ListedColormap(['none','white','black','red','yellow','magenta','green','cyan','blue'])


# 绘图函数
# 其实默认为fig,ax = plt.figure(),后续发现fig没有用上
# 但是ax需要频繁使用，因此直接ax = plt.gca()替代掉
    plt.figure(figsize=(cols,rows))
    ax = plt.gca()




# 绘制热力图
# 其中vmin和vmax对应栅格地图数值的颜色与cmap一一对应
# cbar设置false将色条设置为不可见
    sns.heatmap(field, cmap = cmap,vmin = 0,vmax = 8, linewidths = 1.25, linecolor= 'black', ax = ax, cbar = False)


# 设置图标题
    ax.set_ylabel('rows')
    ax.set_xlabel('cols')


# 将列标签移动到图像上方
    ax.xaxis.tick_top()
    ax.xaxis.set_label_position('top')




# 设置图标的数字个数文字，放在plt.show下面能居中
    ax.set_xticks(np.arange(cols))
    ax.set_yticks(np.arange(rows))


# 直接显示图像，如要类似于MATLAB的hold on建议在主函数最后使用plt.show()
# plt.show()


return field


'''
# # -------------------------------------这是是测试的主函数-------------------------------------------------
'''
rows = 4
cols = 5


startSub = [2,0]
goalSub = [2,4]
obsSub = [[1,2],[2,2],[3,2]]


drawmap(rows,cols,startSub,goalSub,obsSub)
# 采用热力图这种，坐标系简历和MATLAB是一致的，X=col，Y=rows，且递增方向都是一致的，因此绘制XY时候比较难理解
pointxy = [2,3]
plt.scatter(pointxy[0],pointxy[1],s = 200,c = 'r')
pointsub = coord2sub(pointxy)
plt.scatter(pointsub[0],pointsub[1],s = 200,c = 'y')
plt.show()

最后生成的图像效果如图。

4.4 采用plot在XY坐标填充矩形绘制地图

import numpy as np
import matplotlib
import matplotlib.pyplot as plt
from matplotlib import colors
import matplotlib.patches as pc




def xy2sub(len,x,y):
    r = len - y -1
    c = x
return [r,c]


def sub2xy(len,r,c):
    x = c
    y = len - r - 1
return [x,y]




def drawmap_xy(Xs,Ys,startxy,goalxy,obsxy):
# 其中X和矩阵地图的cols对应
    rows = Ys
    cols = Xs
# 创建全部为空地的地图栅格，其中空地以数字1表征
# ！！！注意ones(行列个数，因此rows需要+1)
    field = np.ones([rows,cols])


# 修改栅格地图中起始点和终点的数值，其中起点以数值4表征，终点以数值5表示
    startsub = xy2sub(rows,startxy[0],startxy[1])
    goalsub = xy2sub(rows,goalxy[0],goalxy[1])
    field[startsub[0],startsub[1]] = 4
    field[goalsub[0],goalsub[1]] = 5


# 修改栅格地图中障碍物的数值，其中以数值5表示
for i in range(len(obsxy)):
        obssub = xy2sub(rows,obsxy[i][0],obsxy[i][1])
        field[obssub[0],obssub[1]] = 2




# 设置画布属性
    plt.figure(figsize=(cols,rows))
    plt.xlim(-1, cols)
    plt.ylim(-1, rows)
    plt.xticks(np.arange(Xs))
    plt.yticks(np.arange(Ys))


# 绘制障碍物XY位置
for i in range(len(obsxy)):
        plt.gca().add_patch(pc.Rectangle((obsxy[i][0] - 0.5, obsxy[i][1] - 0.5), 1,1,color='k'))


# 绘制起点，终点
    plt.gca().add_patch(pc.Rectangle((startxy[0] - 0.5, startxy[1] - 0.5), 1,1,color='yellow'))
    plt.gca().add_patch(pc.Rectangle((goalxy[0] - 0.5, goalxy[1] - 0.5), 1,1,color='m'))


return field








'''
这里是主函数，将下列地图，用以为坐标XY形式绘制出
# Y/|1. 1. 1. 1. 1.
#   |1. 1. 2. 1. 1.
#   |4. 1. 2. 1. 5.
#   |1. 1. 2. 1. 1.---->X
'''


startxy = [0,1]
goalxy = [4,1]
obsxy = [[2,0],[2,1],[2,2]]


Ys = 4
Xs = 5


drawmap_xy(Xs,Ys,startxy,goalxy,obsxy)


# 采用XY坐标的优势在于绘制其他参数时候，如在X=2，Y=3 绘制一个大圆点，但是地图矩阵和XY又需要相互转变一下
plt.scatter(2,3,s = 200,c = 'r')
plt.show()

4.5 绘制动态地图信息的方法

绘制动态地图方法提供两种参考：

a. 使用imshow(field)联合plt.pause()显示栅格地图

b. 利用plt.plot()联合plt.pause()和plt.cla()进行刷新显示

tips：import PathPlanning是必须的

import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import seaborn as sns
from matplotlib import colors
from matplotlib import animation
import matplotlib.patches as pc
import copy
import random


import PathPlanning




'''
# ----------------------------------------------------------------------------------------------
# 绘制动态地图方法提供两种参考：
# 1、使用imshow(field)联合plt.pause()显示栅格地图
# 2、利用plt.plot()联合plt.pause()和plt.cla()进行刷新显示
# tips：import PathPlanning是必须的
# -----------------------------------------------------------------------------------------------
'''


'''
# ------------------------声明地图信息和某些固定信息，地图样式如图所示
# [[1. 1. 1. 1. 1. 1. 1.]
#  [1. 1. 1. 2. 1. 1. 1.]
#  [1. 4. 1. 2. 1. 5. 1.]
#  [1. 1. 1. 2. 1. 1. 1.]
#  [1. 1. 1. 1. 1. 1. 1.]
#  [1. 1. 1. 1. 1. 1. 1.]]
#
'''
rows = 6
cols = 7
startSub = [2,1]
goalSub = [2,5]
obsSub = [[1,3],[2,3],[3,3]]




# 栅格地图属性
field = np.ones((rows, cols))


field[startSub[0], startSub[1]] = 4
field[goalSub[0], goalSub[1]] = 5


for i in range(len(obsSub)):
    field[obsSub[i][0], obsSub[i][1]] = 2


# 新建画布指定大小
fig = plt.figure(figsize=(9,6))
# 新建子图
ax = fig.add_subplot(111)


# 没有用的变量，主要是区分绘制方法是第一种还是第二种
draw = False






'''
# 方法一利用imshow绘制动态地图
# 利用随机函数，随机选择地图位置改变数值
'''
cmap = colors.ListedColormap(['none', 'white', 'black', 'red', 'yellow', 'magenta', 'green', 'cyan', 'blue'])


# 动态刷新地图次数
if draw:
for i in range(10):
# 随机选择修改地图信息的位置
        temp_r = random.randint(0,rows-1)
        temp_c = random.randint(0,cols-1)


# 修改地图信息
        field[temp_r][temp_c] = 3


# 显示图像并设置图像属性
        ax.imshow(field, cmap=cmap, vmin=0, vmax=8)
        ax.set_ylabel('rows')
        ax.set_xlabel('cols')
        ax.xaxis.tick_top()
        ax.xaxis.set_label_position('top')
        ax.set_xticks(np.arange(cols))
        ax.set_yticks(np.arange(rows))
        plt.pause(0.05)


# 重置回白色
        field[temp_r][temp_c] = 1




'''
# 方法二：采用plt.plot联合plt.pause()和plt.cla()
# 利用随机函数，随机选择地图位置改变数值
'''
if not draw:
# plt.plot都是建立在xy坐标系上的内容可以利用pathplanning的sub2xy函数转换
# 将行列转成xy坐标系的xy值
    startXY = PathPlanning.sub2xy([rows,cols],startSub[0],startSub[1])
    goalXY = PathPlanning.sub2xy([rows,cols],goalSub[0],goalSub[1])
    obsX = []
    obsY = []


for i in range(len(obsSub)):
        obsxy = PathPlanning.sub2xy([rows,cols],obsSub[i][0],obsSub[i][1])
        obsX.append(obsxy[0])
        obsY.append(obsxy[1])


for i in range(100):
# 随机选择修改地图信息的位置
        temp_r = random.randint(0,rows-1)
        temp_c = random.randint(0,cols-1)
        temp_xy = PathPlanning.sub2xy([rows,cols],temp_r,temp_c)


# 修改地图信息
        field[temp_r][temp_c] = 3


# 显示图像并设置图像属性
        plt.plot(startXY[0],startXY[1],'r+')
        plt.plot(goalXY[0],goalXY[1],'b+')
        plt.plot(obsX,obsY,'sk')
        plt.plot(temp_xy[0],temp_xy[1],'sr')
        ax.set_xlim([-1,cols])
        ax.set_ylim([-1,rows])
        ax.set_xticks(np.arange(cols))
        ax.set_yticks(np.arange(rows))
        plt.pause(1)
        plt.cla()


# 重置回白色
        field[temp_r][temp_c] = 1

05 C++创建栅格地图矩阵

C++创建栅格地图矩阵的流程：

a. 创建全0二维数组，表示空地信息。

b. 将下标位置的栅格信息装换成特征值。

c. C++虽然有matplotlib包，但是可视化效果并不是很好，这里不使用C++绘制栅格地图，难度较大。

C++数组的一些基本操作：

#include<iostream>
#include<vector>


using namespace std;


int main()
{
int const rows = 4;
int const cols = 5;
int startpos[2] = {2,0};
int endpos[2] = {2,4};
int obspos[3][2] = {{1,2},{2,2},{3,2}};


// 定义栅格地图二维数组，默认初始化为0
int filed[rows][cols] = {0};


// 修改起点和终点在栅格数组中的数值，其中起点为1，终点为2
    filed[startpos[0]][startpos[1]] = 1;
    filed[endpos[0]][endpos[1]] = 2;


// 循环修改障碍物在栅格矩阵的数值，其中障碍物数值为3
int len = sizeof(obspos) / sizeof(obspos[0]);
for(int temp_len = 0;temp_len <len; temp_len++)
    {
        filed[obspos[temp_len][0]][obspos[temp_len][1]] = 3;
    }




// 打印栅格矩阵数值
for(int r = 0; r < rows; r++)
    {
for(int c = 0; c < cols; c++)
        {
cout<<filed[r][c]<<" ";
        }
cout<<endl;


    }
return 0;
}

Tips：对C++编程而言，特殊位置信息的二维数组，作为参数进行传递时候，是不方便编程的。后续优化代码，使其采用线性索引的方式进行编程，实现函数封装，提升通用性。

本文仅做学术分享，如有侵权，请联系删文。

点击进入—>3D视觉工坊学习交流群

干货下载与学习

后台回复：巴塞罗那自治大学课件，即可下载国外大学沉淀数年3D Vison精品课件

后台回复：计算机视觉书籍，即可下载3D视觉领域经典书籍pdf

后台回复：3D视觉课程，即可学习3D视觉领域精品课程

3D视觉工坊精品课程官网：3dcver.com

1.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
2.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
 3.国内首个面向工业级实战的点云处理课程
4.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
5.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
 6.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
 7.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

8.从零搭建一套结构光3D重建系统[理论+源码+实践]

14.国内首个3D缺陷检测教程：理论、源码与实战

15.基于Open3D的点云处理入门与实战教程

16.透彻理解视觉ORB-SLAM3：理论基础+代码解析+算法改进

17.机械臂抓取从入门到实战

重磅！粉丝学习交流群已成立

交流群主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、ORB-SLAM系列源码交流、深度估计、TOF、求职交流等方向。

扫描以下二维码，添加小助理微信（dddvisiona）,一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿，微信号：dddvisiona

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、源码分享、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答等进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，6000+星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看，3天内无条件退款

高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~

AtomGit 开源协作平台测评赛

瓜分20万奖金获得内推名额丰厚实物奖励易参与易上手

更多推荐

【Spring Boot 】Spring Boot + HikariCP 连接池使用示例

文章目录示例工具版本HikariCP 依赖HikariCP 配置1. connectionTimeout2. minimumIdle3. maximumPoolSize4. idleTimeout5. maxLifetime6. autoCommitSpring Boot Data + HikariCP + MySQL示例测试应用程序1. 使用 Maven 命令2. 使用 Eclipse3. 使用