mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-14 06:17:59 -05:00
154 lines
3.9 KiB
Python
154 lines
3.9 KiB
Python
"""
|
|
|
|
2D grid map sample
|
|
|
|
author: Atsushi Sakai (@Atsushi_twi)
|
|
|
|
"""
|
|
|
|
from scipy.stats import norm
|
|
import math
|
|
|
|
|
|
# function gm=RayCastingUpdate(gm,z)
|
|
# %レイキャスティングによるGridの更新
|
|
|
|
# %事前レイキャスティングモデルの作成
|
|
# gm=PreCasting(gm,z.ANGLE_TICK);
|
|
|
|
# rayId=0;
|
|
# %事前レイキャスティングモデルに従ってグリッドの確率の更新
|
|
# for iz=1:length(z.data(:,1))%それぞれの観測点に対して
|
|
# range=z.data(iz,1);
|
|
|
|
# rayId=rayId+1;%レイキャスティングクラスタにおけるデータID
|
|
# %各観測点はそれぞれのクラスタから取得できるとする。
|
|
|
|
# %クラスタ内の各gridのデータからビームモデルによるupdate
|
|
# for ir=1:length(gm.precasting(rayId).grid(:,1))
|
|
# grange=gm.precasting(rayId).grid(ir,1);
|
|
# gid=gm.precasting(rayId).grid(ir,5);
|
|
|
|
# if grange<(range-gm.RESO/2) %free
|
|
# gm.data(gid)=0;
|
|
# elseif grange<(range+gm.RESO/2) %hit
|
|
# gm.data(gid)=1;
|
|
# end %それ以上の距離のgridはunknownなので何もしない
|
|
# end
|
|
# end
|
|
|
|
# function gm=PreCasting(gm,angleTick)
|
|
# %事前レイキャスティングモデルの作成
|
|
|
|
# %各角度について対応するグリッドを追加していく
|
|
# precasting=[];%プレキャスティングの結果 [最小角度,最大角度,中に入るgridのデータ]
|
|
# for ia=(0-angleTick/2):angleTick:(360+angleTick/2)
|
|
# %角度範囲の保存
|
|
# ray.minAngle=ia;
|
|
# ray.maxAngle=ia+angleTick;
|
|
# grid=[];%角度範囲に入ったグリッドのデータ
|
|
# for ig=1:(gm.nGrid)
|
|
# %各グリッドのxy値を取得
|
|
# gxy=GetXYFromDataIndex(ig,gm);
|
|
# range=norm(gxy);
|
|
# angle=atan2(gxy(2),gxy(1));
|
|
# if angle<0 %[0 360]度に変換
|
|
# angle=angle+2*pi;
|
|
# end
|
|
# if ray.minAngle<=toDegree(angle) && ray.maxAngle>=toDegree(angle)
|
|
# grid=[grid;[range,angle,gxy,ig]];
|
|
# end
|
|
# end
|
|
# %rangeの値でソーティングしておく
|
|
# if ~isempty(grid)
|
|
# ray.grid=sortrows(grid,1);
|
|
# end
|
|
# precasting=[precasting;ray];
|
|
# end
|
|
# gm.precasting=precasting;%Grid Mapデータに追加
|
|
|
|
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
|
|
AREA_WIDTH = 30.0
|
|
|
|
|
|
def generate_gaussian_grid_map(ox, oy, reso):
|
|
|
|
minx = min(ox) - AREA_WIDTH / 2.0
|
|
miny = min(oy) - AREA_WIDTH / 2.0
|
|
maxx = max(ox) + AREA_WIDTH / 2.0
|
|
maxy = max(oy) + AREA_WIDTH / 2.0
|
|
xw = round((maxx - minx) / reso)
|
|
yw = round((maxy - miny) / reso)
|
|
|
|
# calc each potential
|
|
pmap = [[0.0 for i in range(yw)] for i in range(xw)]
|
|
|
|
STD = 10.0 # standard diviation
|
|
|
|
for ix in range(xw):
|
|
for iy in range(yw):
|
|
|
|
x = ix / reso + minx
|
|
y = iy / reso + miny
|
|
|
|
# Search minimum distance
|
|
mindis = float("inf")
|
|
for (iox, ioy) in zip(ox, oy):
|
|
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
|
|
if mindis >= d:
|
|
mindis = d
|
|
|
|
pdf = (1.0 - norm.cdf(mindis, 0.0, STD))
|
|
pmap[ix][iy] = pdf
|
|
|
|
draw_heatmap(pmap)
|
|
plt.show()
|
|
|
|
|
|
def generate_ray_casting_grid_map(ox, oy, reso):
|
|
|
|
minx = min(ox) - AREA_WIDTH / 2.0
|
|
miny = min(oy) - AREA_WIDTH / 2.0
|
|
maxx = max(ox) + AREA_WIDTH / 2.0
|
|
maxy = max(oy) + AREA_WIDTH / 2.0
|
|
xw = round((maxx - minx) / reso)
|
|
yw = round((maxy - miny) / reso)
|
|
|
|
# calc each potential
|
|
pmap = [[0.0 for i in range(yw)] for i in range(xw)]
|
|
|
|
for (x, y) in zip(ox, oy):
|
|
|
|
ix = round(x * reso - minx)
|
|
iy = round(y * reso - miny)
|
|
|
|
pmap[ix][iy] = 100.0
|
|
|
|
# print(norm.cdf(x, mean, std))
|
|
|
|
draw_heatmap(pmap)
|
|
plt.show()
|
|
|
|
|
|
def draw_heatmap(data):
|
|
data = np.array(data).T
|
|
plt.pcolor(data, vmax=1.0, cmap=plt.cm.Blues)
|
|
plt.axis("equal")
|
|
|
|
|
|
def main():
|
|
print(__file__ + " start!!")
|
|
|
|
ox = [0.0, 5.0, 0.0]
|
|
oy = [0.0, 5.0, 10.0]
|
|
reso = 1.0
|
|
|
|
generate_gaussian_grid_map(ox, oy, reso)
|
|
# generate_ray_casting_grid_map(ox, oy, reso)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|