PythonRobotics机器人算法-映射算法之光线投射网格映射(Ray casting grid map)


1.光线投射网格映射(Ray casting grid map)实例

下面展示一下基于python实现的光线投射网格映射。

(1)定义初始参数

1
2
3
4
5
6
7
import math
import numpy as np
import matplotlib.pyplot as plt

EXTEND_AREA = 10.0

show_animation = True

(2)算法过程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
def calc_grid_map_config(ox, oy, xyreso):
    minx = round(min(ox) - EXTEND_AREA / 2.0)
    miny = round(min(oy) - EXTEND_AREA / 2.0)
    maxx = round(max(ox) + EXTEND_AREA / 2.0)
    maxy = round(max(oy) + EXTEND_AREA / 2.0)
    xw = int(round((maxx - minx) / xyreso))
    yw = int(round((maxy - miny) / xyreso))
    return minx, miny, maxx, maxy, xw, yw

class precastDB:
    def _ _init__(self):
        self.px = 0.0
        self.py = 0.0
        self.d = 0.0
        self.angle = 0.0
        self.ix = 0
        self.iy = 0
    def _ _str__(self):
        return str(self.px) + "," + str(self.py) + "," + str(self.d) + "," + str(self.angle)

def atan_zero_to_twopi(y, x):
    angle = math.atan2(y, x)
    if angle < 0.0:
        angle += math.pi * 2.0
    return angle

def precasting(minx, miny, xw, yw, xyreso, yawreso):
    precast = [[] for i in range(int(round((math.pi * 2.0) / yawreso)) + 1)]
    for ix in range(xw):
        for iy in range(yw):
            px = ix * xyreso + minx
            py = iy * xyreso + miny
            d = math.sqrt(px<strong>2 + py</strong>2)
            angle = atan_zero_to_twopi(py, px)
            angleid = int(math.floor(angle / yawreso))
            pc = precastDB()
            pc.px = px
            pc.py = py
            pc.d = d
            pc.ix = ix
            pc.iy = iy
            pc.angle = angle
            precast[angleid].append(pc)
    return precast

def generate_ray_casting_grid_map(ox, oy, xyreso, yawreso):
    minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
    pmap = [[0.0 for i in range(yw)] for i in range(xw)]
    precast = precasting(minx, miny, xw, yw, xyreso, yawreso)
    for (x, y) in zip(ox, oy):
        d = math.sqrt(x* * 2 + y* * 2)
        angle = atan_zero_to_twopi(y, x)
        angleid = int(math.floor(angle / yawreso))
        gridlist = precast[angleid]
        ix = int(round((x - minx) / xyreso))
        iy = int(round((y - miny) / xyreso))
        for grid in gridlist:
            if grid.d > d:
                pmap[grid.ix][grid.iy] = 0.5
        pmap[ix][iy] = 1.0
    return pmap, minx, maxx, miny, maxy, xyreso

(3)结果可视化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
def draw_heatmap(data, minx, maxx, miny, maxy, xyreso):
    x, y = np.mgrid[slice(minx - xyreso / 2.0, maxx + xyreso / 2.0, xyreso),
                    slice(miny - xyreso / 2.0, maxy + xyreso / 2.0, xyreso)]
    plt.pcolor(x, y, data, vmax=1.0, cmap=plt.cm.Blues)
    plt.axis("equal")

def main():
    print(_ _file__ + " start!!")
    xyreso = 0.25  # x-y grid resolution [m]
    yawreso = math.radians(10.0)  # yaw angle resolution [rad]
    for i in range(5):
        ox = (np.random.rand(4) - 0.5) * 10.0
        oy = (np.random.rand(4) - 0.5) * 10.0
        pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map(
            ox, oy, xyreso, yawreso)
        if show_animation:
            plt.cla()
            draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso)
            plt.plot(ox, oy, "xr")
            plt.plot(0.0, 0.0, "ob")
            plt.pause(1.0)

if _ _ name__ == '_ _main__':
    main()

执行结果如下。下面为二维光线投射网格映射的实例

参考文献

  1. https://en.wikipedia.org/wiki/Ray_casting
  2. https://pythonrobotics.readthedocs.io/en/latest/
  3. https://github.com/AtsushiSakai/PythonRobotics/
  4. PythonRobotics: a Python code collection of robotics algorithms

进化学习团队将会根据大家意见和建议持续修改、维护与更新。转载请注明出处(进化学习: https://www.evolutionarylearn.com/paper/ra-pythonrobotics-mapping-rcgm/)。

赞赏

微信赞赏支付宝赞赏

Have any Question or Comment?

发表评论

电子邮件地址不会被公开。 必填项已用*标注

热门主题 & 页面

分类目录

博客统计

无点击次数。