파이썬 로보틱스 - 지도 작성, 광선 투사 격자 지도 작성

 조금전에 특징 기반 지도를 가우시안 분포로 나타내는

가우시안 격자 지도 작성 시뮬레이션을 돌려보고

 

 광선 투사를 사용하는 다양한 관측 모델들에 대해서 살펴보았습니다.

이번에는 광선 투사를 이용한 격자 지도 작성 시뮬레이션 코드를 분석해 봅시다.

 

 

"""
Ray casting 2D grid map example
author: Atsushi Sakai (@Atsushi_twi)
"""

import math
import numpy as np
import matplotlib.pyplot as plt

EXTEND_AREA = 10.0

show_animation = True

 

 일단 가져오는 라이브러리들은 이전과 비슷하고,

파라미터도 확장영역 밖에 없습니다.

 

 바로 메인 함수로 가서 전체 코드를 살펴보겠습니다.

 

def main():
    print(__file__ + " start!!")

    xyreso = 0.25  # x-y grid resolution [m]
    yawreso = np.deg2rad(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:  # pragma: no cover
            plt.cla()
            # for stopping simulation with the esc key.
            plt.gcf().canvas.mpl_connect('key_release_event',
                    lambda event: [exit(0) if event.key == 'escape' else None])
            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()

 

 메인 함수 처음에는

xy에 대한 격자 해상도와 회전각 yaw 해상도를 설정해주고

 

 루프문을 도는데

가우시안 격자 지도와 마찬가지로 4개의 랜드마크를 생성하나 봅니다.

 

 핵심인 광선 투사 격자 지도 함수를 살펴보겠습니다.

 

 

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.hypot(x, y)
        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

 맨 처음 만나는 그리드 지도 설정 계산 함수는

격자 지도의 크기와, 격자 갯수등을 생성하는것으로 이전에 봤으니 pass 하겠습니다.

 

 다음으로 2차원 격자 지도 pmap을 생성하고 0으로 초기화해준 다음

전투사 precasting 함수를 수행하는데 한번 살펴보겠습니다.

 

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.hypot(px, py)
            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

 전투사 함수는 우선 아래의 연산을 하고

2 * pi / 각도 해상도 -> 6.24 / 0.25 = 24.96 

24.9을 반올림 해준후 + 1 을 하면 26이 됩니다.

 

 26번 만큼 빈 리스트를 생성해주면

precast는 (1, 26) 크기의 이차원 행렬이 되겠습니다.

 

 이제 반복문 내부를 위주로 살펴보겠습니다.

    for ix in range(xw):
        for iy in range(yw):
            px = ix * xyreso + minx
            py = iy * xyreso + miny

            d = math.hypot(px, py)
            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

 일단 모든 격자에 대해 반복을 하는데

첫번째 격자의 x, y 좌표를 구하고

원점에서 첫 격자까지의 거리 d와 각도 angle, 각도를 해상도 만큼 나누어 만든 정수를 angleid라 합니다.

 

 이제 첫번째 셀에 대한 정보를 담을 전투사 데이터베이스 precastDB()를 생성해 주는데

잠시 전투사 데이터베이스를 살펴보겠습니다.

 

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)

 

 전투사 데이터베이스는 별 내용은 없고

해당 셀의 위치 px, py와 원점으로부터 거리 d, 각 angle, 그리고 격자 인덱스 ix, iy를 가지는 클래스입니다.

 

 대강 살펴봤으니 다시 전투사 precasting 함수로 돌아오겠습니다.

 

    for ix in range(xw):
        for iy in range(yw):
            px = ix * xyreso + minx
            py = iy * xyreso + miny

            d = math.hypot(px, py)
            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

 

 이 전투사 데이터베이스에다가 첫번째 셀의 위치와 거리, 각도, 인덱스 등을 담고

아까 텅빈 리스트들로 이루어진 (1, 26)크기의 2차원 행렬에 

해당 angle id 번호에 전투사 데이터베이스를 추가 해줍니다.

 

 모든 셀에 대해 전투사 데이터베이스가 만들어지면 전체 행렬을 반환하게 됩니다.

전투사 함수를 마쳤으니 다시 광선 투사 격자 지도 작성 함수로 돌아갑시다.

 

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.hypot(x, y)
        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

 

 전투사 함수까지 살펴보았으니

반복문 위주로 보겠습니다.

 

    for (x, y) in zip(ox, oy):

        d = math.hypot(x, y)
        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

 일단 ox, oy는 각 랜드마크의 좌표로

zip을 해준후 루프를 시키므로 첫번째 랜드마크 좌표, 두번째 랜드마크 좌표 순으로 반복이 수행 될겁니다.

 

 우선 랜드마크에 대한 거리와 각, 각 아이디를 계산해주고

해당 랜드마크의 angleid와 동일한 격자 셀들의 목록 gridlist를 가져옵시다.

 

 첫번째 랜드마크의 격자 인덱스를 계산하고 ... ix, iy

 

 

 격자 목록에 존재하는 각 격자들을 돌리면서

현재 랜드마크의 거리보다 격자의 거리가 먼지 비교를 합니다.

 

 현재 랜드마크보다 격자가 뒤에 있다면 알수없음이므로 0.5

현재 격자는 알고 있으므로 1

그 이외 격자는 값이 변하지 않을 겁니다.

 

 이를 모든 랜드마크에 반복시키면

원점을 중심으로 랜드마크 뒷부분은 알수없음 0.5인

광선투사 격자 지도가 만들어 지게 됩니다.

 

 

 

"""
Ray casting 2D grid map example
author: Atsushi Sakai (@Atsushi_twi)
"""

import math
import numpy as np
import matplotlib.pyplot as plt

EXTEND_AREA = 10.0

show_animation = True


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.hypot(px, py)
            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.hypot(x, y)
        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


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 = np.deg2rad(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:  # pragma: no cover
            plt.cla()
            # for stopping simulation with the esc key.
            plt.gcf().canvas.mpl_connect('key_release_event',
                    lambda event: [exit(0) if event.key == 'escape' else None])
            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()