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函數：

functioncoord=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

functionsub=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++等需要借助循環

其次是線性索引作為輸入，只要一個參數即可，同時在表達矩陣上，可以簡化成一維，而行列是二維較為復雜

functionFields=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繪制靜態地圖

importnumpyasnp
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坐標填充矩形繪制地圖

importnumpyasnp
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是必須的

importnumpyasnp
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

C++創建柵格地圖矩陣的流程：

a. 創建全0二維數組，表示空地信息。 b. 將下標位置的柵格信息裝換成特征值。 c. C++雖然有matplotlib包，但是可視化效果并不是很好，這里不使用C++繪制柵格地圖，難度較大。

C++數組的一些基本操作：

#include
#include


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 

	

	Tips：對C++編程而言，特殊位置信息的二維數組，作為參數進行傳遞時候，是不方便編程的。后續優化代碼，使其采用線性索引的方式進行編程，實現函數封裝，提升通用性。

	審核編輯：湯梓紅