여기서 점들의 갯수는 1000개 이하로 적으며, 점들 간에 구분하기 쉬울수록 잘 동작합니다.
EKF SLAM은 특징 검출기가 필요한데, 특징으로 인공 비콘이나 랜드마크를 사용합니다.
2. 가우시안 노이즈
EKF SLAM은 로봇의 동작과 인지 시 가우시안 노이즈를 따른다고 가정하며
사후 확률의 불확실성은 상대적으로 작으나, EKF 선형화시 견딜수 없는 에러가 발생할 수 있습니다.
3. 긍정 측정치
EKF SLAM은 랜드마크에 대한 긍정 정보만을 사용할수 있으며,
랜드마크가 사라지는 것같은 부정 정보를 처리할수 없습니다.
EKF SLAM을 사용하는 경우 대응관계가 주어진 경우와 대응관계가 주어지지 않은 경우가 있으나
일반적으로 대응관계는 주어지지 않으며 파이썬 로보틱스에서 제공하는 예제 또한 그러하므로
이 경우에 대한 시뮬레이션 예제를 살펴보겠습니다.
대응 관계가 주어지지 않은 EKF SLAM 알고리즘
이 슬램 알고리즘은 대응 관계를 구하기 위해
증문 최대 우도 추정기 incremental maximum likelihood(ML)을 사용합니다.
import numpy as np
import math
import matplotlib.pyplot as plt
# EKF state covariance
Cx = np.diag([0.5, 0.5, math.radians(30.0)])**2
# Simulation parameter
Qsim = np.diag([0.2, math.radians(1.0)])**2
Rsim = np.diag([1.0, math.radians(10.0)])**2
DT = 0.1 # time tick [s]
SIM_TIME = 60.0 # simulation time [s]
MAX_RANGE = 20.0 # maximum observation range
M_DIST_TH = 2.0 # Threshold of Mahalanobis distance for data association.
STATE_SIZE = 3 # State size [x,y,yaw]
LM_SIZE = 2 # LM srate size [x,y]
show_animation = True
늘 하던대로
임포트 라이브러리와 전역변수들을 살펴봅시다.
상태 공분산 Cx와 동작, 관측 노이즈 Q,Rsim을 초기화하고,
별도 파라미터들을 설정해 주었습니다.
def main():
print(__file__ + " start!!")
time = 0.0
# RFID positions [x, y]
RFID = np.array([[10.0, -2.0],
[15.0, 10.0],
[3.0, 15.0],
[-5.0, 20.0]])
# State Vector [x y yaw v]'
xEst = np.matrix(np.zeros((STATE_SIZE, 1)))
xTrue = np.matrix(np.zeros((STATE_SIZE, 1)))
PEst = np.eye(STATE_SIZE)
xDR = np.matrix(np.zeros((STATE_SIZE, 1))) # Dead reckoning
# history
hxEst = xEst
hxTrue = xTrue
hxDR = xTrue
while SIM_TIME >= time:
time += DT
u = calc_input()
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
xEst, PEst = ekf_slam(xEst, PEst, ud, z)
x_state = xEst[0:STATE_SIZE]
# store data history
hxEst = np.hstack((hxEst, x_state))
hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue))
if show_animation:
plt.cla()
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
plt.plot(xEst[0], xEst[1], ".r")
# plot landmark
for i in range(calc_n_LM(xEst)):
plt.plot(xEst[STATE_SIZE + i * 2],
xEst[STATE_SIZE + i * 2 + 1], "xg")
plt.plot(np.array(hxTrue[0, :]).flatten(),
np.array(hxTrue[1, :]).flatten(), "-b")
plt.plot(np.array(hxDR[0, :]).flatten(),
np.array(hxDR[1, :]).flatten(), "-k")
plt.plot(np.array(hxEst[0, :]).flatten(),
np.array(hxEst[1, :]).flatten(), "-r")
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
if __name__ == '__main__':
main()
메인 함수를 보면, 플로팅 부분을 제외하고 루프 앞까지를 보면
랜드마크로 사용할 RFID 4개를 정의 하였습니다.
이후 추정 상태 변수 xEst, 실제 상태 변수 xTrue
추정 상태 공분산 PEst를 만들어줍니다.
그 다음 추측항법도 비교해줄수 있도록 xDR도 만들고
추측 항법 상태, 추정 상태, 실제 상태들을 담기위해
hx 변수들도 만들어 줍니다.
이제 루프문 내부를 확인하겠습니다.
while SIM_TIME >= time:
time += DT
u = calc_input()
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
xEst, PEst = ekf_slam(xEst, PEst, ud, z)
x_state = xEst[0:STATE_SIZE]
# store data history
hxEst = np.hstack((hxEst, x_state))
hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue))
맨 앞은 우리가 자주보던 입력 구하는 함수 calc_input
그 다음에는 관측 함수
ekf slam
뒤에는 로봇의 상태변수들만 hx 변수에 담는 과정이 되겠습니다.
calc input 함수는 입력값으로 사용할 선속도와 각속도를 반환하니 패스하고
관측 함수 부터 살펴보겠습니다.
def observation(xTrue, xd, u, RFID):
xTrue = motion_model(xTrue, u)
# add noise to gps x-y
z = np.matrix(np.zeros((0, 3)))
for i in range(len(RFID[:, 0])):
dx = RFID[i, 0] - xTrue[0, 0]
dy = RFID[i, 1] - xTrue[1, 0]
d = math.sqrt(dx**2 + dy**2)
angle = pi_2_pi(math.atan2(dy, dx))
if d <= MAX_RANGE:
dn = d + np.random.randn() * Qsim[0, 0] # add noise
anglen = angle + np.random.randn() * Qsim[1, 1] # add noise
zi = np.matrix([dn, anglen, i])
z = np.vstack((z, zi))
# add noise to input
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
ud = np.matrix([ud1, ud2]).T
xd = motion_model(xd, ud)
return xTrue, z, xd, ud
관측 함수에서 처음에는 실제 로봇의 주행 궤적을 구하기 위해 xTrue를 동작시키고,
다음에는 관측치의 노이즈로 사용할 행렬을 생성 합니다.
관측치인 거리와 방위는 실제 로봇의 위치와 랜드마크 사이의 거리로 구할수 있으므로
dx, dy를 통해 대각선 거리 d와 방위 angle을 구합니다.
하지만 측정 가능 거리에 존재하는 랜드마크만 검출할수 있으므로 거리재한을 두고,
이 거리재한 안에 존재하는 랜드마크에 한해 가우시안 노이즈를 추가한 dn, anglen을 관측치 z로 추가합니다.
다음으로 동작 모델을 사용하는데, 기존의 입력에 가우시안 노이즈를 추가하여
추측 항법 상태를 구합니다.
while SIM_TIME >= time:
time += DT
u = calc_input()
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
xEst, PEst = ekf_slam(xEst, PEst, ud, z)
x_state = xEst[0:STATE_SIZE]
관측 함수를 수행하였으니
다음으로 ekf slam 함수를 보겠습니다.
여기서 이전 상태와 공분산, 노이즈가 추가된 입력,
관측 함수로 구한 관측치들이 매개변수로 사용됩니다.
def ekf_slam(xEst, PEst, u, z):
# Predict
S = STATE_SIZE
xEst[0:S] = motion_model(xEst[0:S], u)
G, Fx = jacob_motion(xEst[0:S], u)
PEst[0:S, 0:S] = G.T * PEst[0:S, 0:S] * G + Fx.T * Cx * Fx
initP = np.eye(2)
# Update
for iz in range(len(z[:, 0])): # for each observation
minid = search_correspond_LM_ID(xEst, PEst, z[iz, 0:2])
nLM = calc_n_LM(xEst)
if minid == nLM:
print("New LM")
# Extend state and covariance matrix
xAug = np.vstack((xEst, calc_LM_Pos(xEst, z[iz, :])))
PAug = np.vstack((np.hstack((PEst, np.zeros((len(xEst), LM_SIZE)))),
np.hstack((np.zeros((LM_SIZE, len(xEst))), initP))))
xEst = xAug
PEst = PAug
lm = get_LM_Pos_from_state(xEst, minid)
y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)
K = PEst * H.T * np.linalg.inv(S)
xEst = xEst + K * y
PEst = (np.eye(len(xEst)) - K * H) * PEst
xEst[2] = pi_2_pi(xEst[2])
return xEst, PEst
def ekf_slam(xEst, PEst, u, z):
# Predict
S = STATE_SIZE
xEst[0:S] = motion_model(xEst[0:S], u)
G, Fx = jacob_motion(xEst[0:S], u)
PEst[0:S, 0:S] = G.T * PEst[0:S, 0:S] * G + Fx.T * Cx * Fx
initP = np.eye(2)
다시 ekf slam의 앞부분으로 돌아오면
방금 구한 자코비안을 통해 로봇의 추정 자세 변수들에 대한 예측 공분산을 구할수 있게 됩니다.
이 아래에는 관측치에 대한 루프를 돌리겠습니다.
# Update
for iz in range(len(z[:, 0])): # for each observation
minid = search_correspond_LM_ID(xEst, PEst, z[iz, 0:2])
nLM = calc_n_LM(xEst)
if minid == nLM:
print("New LM")
# Extend state and covariance matrix
xAug = np.vstack((xEst, calc_LM_Pos(xEst, z[iz, :])))
PAug = np.vstack((np.hstack((PEst, np.zeros((len(xEst), LM_SIZE)))),
np.hstack((np.zeros((LM_SIZE, len(xEst))), initP))))
xEst = xAug
PEst = PAug
lm = get_LM_Pos_from_state(xEst, minid)
y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)
K = PEst * H.T * np.linalg.inv(S)
xEst = xEst + K * y
PEst = (np.eye(len(xEst)) - K * H) * PEst
xEst[2] = pi_2_pi(xEst[2])
return xEst, PEst
이 루프문의 첫번째로
추정 상태와 추정 공분산, 관측이 주어질때 해당 랜드마크의 아이디를 찾는 함수입니다.
def search_correspond_LM_ID(xAug, PAug, zi):
"""
Landmark association with Mahalanobis distance
"""
nLM = calc_n_LM(xAug)
mdist = []
for i in range(nLM):
lm = get_LM_Pos_from_state(xAug, i)
y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
mdist.append(y.T * np.linalg.inv(S) * y)
mdist.append(M_DIST_TH) # new landmark
minid = mdist.index(min(mdist))
return minid
이 랜드마크 아이디 탐색 함수는
마할라노비스 거리를 이용한 랜드마크 연관으로
매개변수로부터 전달받은 추정 상태를 통해 랜드마크의 갯수를 확인해 봅니다.
def calc_n_LM(x):
n = int((len(x) - STATE_SIZE) / LM_SIZE)
return n
초기 추정 상태는 로봇의 자세 3개밖에 없지만
랜드마크가 검출 될수록 각 랜드마크의 위치 x, y 2개씩 추가 되게 됩니다.
그래서 전체 x의 길이 - 로봇 자세 3을 한 후 2를 나누어주면 그동안 관측한 랜드마크 갯수가 나오게 됩니다.
def search_correspond_LM_ID(xAug, PAug, zi):
"""
Landmark association with Mahalanobis distance
"""
nLM = calc_n_LM(xAug)
mdist = []
for i in range(nLM):
lm = get_LM_Pos_from_state(xAug, i)
y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
mdist.append(y.T * np.linalg.inv(S) * y)
mdist.append(M_DIST_TH) # new landmark
minid = mdist.index(min(mdist))
return minid
추정 상태공간으로부터 랜드마크 갯수를 구한 후
마할라노비스 거리를 담을 텅빈 리스트를 만들어 주고
각 랜드마크에 대해 루프를 돌려봅시다.
for i in range(nLM):
lm = get_LM_Pos_from_state(xAug, i)
y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
mdist.append(y.T * np.linalg.inv(S) * y)
mdist.append(M_DIST_TH) # new landmark
minid = mdist.index(min(mdist))
return minid
def generate_ray_casting_grid_map(ox, oy, xy_resolution, breshen=True):
"""
The breshen boolean tells if it's computed with bresenham ray casting
(True) or with flood fill (False)
"""
min_x, min_y, max_x, max_y, x_w, y_w = calc_grid_map_config(
ox, oy, xy_resolution)
# default 0.5 -- [[0.5 for i in range(y_w)] for i in range(x_w)]
occupancy_map = np.ones((x_w, y_w)) / 2
center_x = int(
round(-min_x / xy_resolution)) # center x coordinate of the grid map
center_y = int(
round(-min_y / xy_resolution)) # center y coordinate of the grid map
# occupancy grid computed with bresenham ray casting
if breshen:
for (x, y) in zip(ox, oy):
# x coordinate of the the occupied area
ix = int(round((x - min_x) / xy_resolution))
# y coordinate of the the occupied area
iy = int(round((y - min_y) / xy_resolution))
laser_beams = bresenham((center_x, center_y), (
ix, iy)) # line form the lidar to the occupied point
for laser_beam in laser_beams:
occupancy_map[laser_beam[0]][
laser_beam[1]] = 0.0 # free area 0.0
occupancy_map[ix][iy] = 1.0 # occupied area 1.0
occupancy_map[ix + 1][iy] = 1.0 # extend the occupied area
occupancy_map[ix][iy + 1] = 1.0 # extend the occupied area
occupancy_map[ix + 1][iy + 1] = 1.0 # extend the occupied area
# occupancy grid computed with with flood fill
else:
occupancy_map = init_flood_fill((center_x, center_y), (ox, oy),
(x_w, y_w),
(min_x, min_y), xy_resolution)
flood_fill((center_x, center_y), occupancy_map)
occupancy_map = np.array(occupancy_map, dtype=np.float)
for (x, y) in zip(ox, oy):
ix = int(round((x - min_x) / xy_resolution))
iy = int(round((y - min_y) / xy_resolution))
occupancy_map[ix][iy] = 1.0 # occupied area 1.0
occupancy_map[ix + 1][iy] = 1.0 # extend the occupied area
occupancy_map[ix][iy + 1] = 1.0 # extend the occupied area
occupancy_map[ix + 1][iy + 1] = 1.0 # extend the occupied area
return occupancy_map, min_x, max_x, min_y, max_y, xy_resolution
이 함수를 보니 격자 지도 생성방법중에
breshen 광선투사와 홍수 알고리즘을 사용하는 두가지가 있나 봅니다.
우리는 광선투사 방법을 다룰것이니 일단 처음부터 내려가봅시다.
격자 지도 설정 계산 함수로 최대 최소 길이와 격자 갯수를 구하고
모든 값이 0.5인 점유 격자를 생성합니다.
다음 점유 격자 지도의 중시 값을 찾은뒤, 브레즌햄 광선 투사로 점유 격자 계산을 시작합니다.
* 브레슨햄
브레즌햄(Bresenham)알고리즘브레즌햄알고리즘은 컴퓨터 그래픽스에서 복잡하고 계산을 느리게 만드는 실수 계산을 배제하고 정수 계산만으로 직선을 그리기 위해 만들어진알고리즘입니다
for (x, y) in zip(ox, oy):
# x coordinate of the the occupied area
ix = int(round((x - min_x) / xy_resolution))
# y coordinate of the the occupied area
iy = int(round((y - min_y) / xy_resolution))
laser_beams = bresenham((center_x, center_y), (
ix, iy)) # line form the lidar to the occupied point
for laser_beam in laser_beams:
occupancy_map[laser_beam[0]][
laser_beam[1]] = 0.0 # free area 0.0
occupancy_map[ix][iy] = 1.0 # occupied area 1.0
occupancy_map[ix + 1][iy] = 1.0 # extend the occupied area
occupancy_map[ix][iy + 1] = 1.0 # extend the occupied area
occupancy_map[ix + 1][iy + 1] = 1.0 # extend the occupied area
브래즌햄 광선투사를 사용하는 경우 일단 모든 관측치에 대한 x, y 값을 각각 반복시킵니다.
해당 좌표에 존재하는 격자 인덱스 ix, iy를 구한뒤
브래즌햄 함수로 레이저 빔을 구합니다.
def bresenham(start, end):
"""
Implementation of Bresenham's line drawing algorithm
See en.wikipedia.org/wiki/Bresenham's_line_algorithm
Bresenham's Line Algorithm
Produces a np.array from start and end (original from roguebasin.com)
>>> points1 = bresenham((4, 4), (6, 10))
>>> print(points1)
np.array([[4,4], [4,5], [5,6], [5,7], [5,8], [6,9], [6,10]])
"""
# setup initial conditions
x1, y1 = start
x2, y2 = end
dx = x2 - x1
dy = y2 - y1
is_steep = abs(dy) > abs(dx) # determine how steep the line is
if is_steep: # rotate line
x1, y1 = y1, x1
x2, y2 = y2, x2
# swap start and end points if necessary and store swap state
swapped = False
if x1 > x2:
x1, x2 = x2, x1
y1, y2 = y2, y1
swapped = True
dx = x2 - x1 # recalculate differentials
dy = y2 - y1 # recalculate differentials
error = int(dx / 2.0) # calculate error
y_step = 1 if y1 < y2 else -1
# iterate over bounding box generating points between start and end
y = y1
points = []
for x in range(x1, x2 + 1):
coord = [y, x] if is_steep else (x, y)
points.append(coord)
error -= abs(dy)
if error < 0:
y += y_step
error += dx
if swapped: # reverse the list if the coordinates were swapped
points.reverse()
points = np.array(points)
return points
브레즈햄 광선 투사 알고리즘을 통해 해당 광선이 지나가는 격자들의 좌표 목록을 구하게 됩니다.
* 브레즈햄 직선 알고리즘 예시
(0,0)은 그리드 왼쪽 위 구석, (1,1) 선의 왼쪽 맨위, (11, 5) 선의 오른쪽 바닥
for (x, y) in zip(ox, oy):
# x coordinate of the the occupied area
ix = int(round((x - min_x) / xy_resolution))
# y coordinate of the the occupied area
iy = int(round((y - min_y) / xy_resolution))
laser_beams = bresenham((center_x, center_y), (
ix, iy)) # line form the lidar to the occupied point
for laser_beam in laser_beams:
occupancy_map[laser_beam[0]][
laser_beam[1]] = 0.0 # free area 0.0
occupancy_map[ix][iy] = 1.0 # occupied area 1.0
occupancy_map[ix + 1][iy] = 1.0 # extend the occupied area
occupancy_map[ix][iy + 1] = 1.0 # extend the occupied area
occupancy_map[ix + 1][iy + 1] = 1.0 # extend the occupied area
광선 투사가 지나가는 격자 목록들을 laser_beams로 받아
모든 값들을 텅빈 공간이므로(레이저가 지나가니) 0.0으로 지정하고
빔이 도달한 지점을 점유된 공간이므로 1로 지정해줍니다.
def generate_ray_casting_grid_map(ox, oy, xy_resolution, breshen=True):
"""
The breshen boolean tells if it's computed with bresenham ray casting
(True) or with flood fill (False)
"""
min_x, min_y, max_x, max_y, x_w, y_w = calc_grid_map_config(
ox, oy, xy_resolution)
# default 0.5 -- [[0.5 for i in range(y_w)] for i in range(x_w)]
occupancy_map = np.ones((x_w, y_w)) / 2
center_x = int(
round(-min_x / xy_resolution)) # center x coordinate of the grid map
center_y = int(
round(-min_y / xy_resolution)) # center y coordinate of the grid map
# occupancy grid computed with bresenham ray casting
if breshen:
for (x, y) in zip(ox, oy):
# x coordinate of the the occupied area
ix = int(round((x - min_x) / xy_resolution))
# y coordinate of the the occupied area
iy = int(round((y - min_y) / xy_resolution))
laser_beams = bresenham((center_x, center_y), (
ix, iy)) # line form the lidar to the occupied point
for laser_beam in laser_beams:
occupancy_map[laser_beam[0]][
laser_beam[1]] = 0.0 # free area 0.0
occupancy_map[ix][iy] = 1.0 # occupied area 1.0
occupancy_map[ix + 1][iy] = 1.0 # extend the occupied area
occupancy_map[ix][iy + 1] = 1.0 # extend the occupied area
occupancy_map[ix + 1][iy + 1] = 1.0 # extend the occupied area
# occupancy grid computed with with flood fill
else:
occupancy_map = init_flood_fill((center_x, center_y), (ox, oy),
(x_w, y_w),
(min_x, min_y), xy_resolution)
flood_fill((center_x, center_y), occupancy_map)
occupancy_map = np.array(occupancy_map, dtype=np.float)
for (x, y) in zip(ox, oy):
ix = int(round((x - min_x) / xy_resolution))
iy = int(round((y - min_y) / xy_resolution))
occupancy_map[ix][iy] = 1.0 # occupied area 1.0
occupancy_map[ix + 1][iy] = 1.0 # extend the occupied area
occupancy_map[ix][iy + 1] = 1.0 # extend the occupied area
occupancy_map[ix + 1][iy + 1] = 1.0 # extend the occupied area
return occupancy_map, min_x, max_x, min_y, max_y, xy_resolution
이렇게 계산된 점유 격자와 지도 크기, 해상도 등의 정보가 메인함수로 반환됩니다.
xy_res = np.array(occupancy_map).shape
plt.figure(1, figsize=(10, 4))
plt.subplot(122)
plt.imshow(occupancy_map, cmap="PiYG_r")
# cmap = "binary" "PiYG_r" "PiYG_r" "bone" "bone_r" "RdYlGn_r"
plt.clim(-0.4, 1.4)
plt.gca().set_xticks(np.arange(-.5, xy_res[1], 1), minor=True)
plt.gca().set_yticks(np.arange(-.5, xy_res[0], 1), minor=True)
plt.grid(True, which="minor", color="w", linewidth=0.6, alpha=0.5)
plt.colorbar()
plt.subplot(121)
plt.plot([oy, np.zeros(np.size(oy))], [ox, np.zeros(np.size(oy))], "ro-")
plt.axis("equal")
plt.plot(0.0, 0.0, "ob")
plt.gca().set_aspect("equal", "box")
bottom, top = plt.ylim() # return the current y-lim
plt.ylim((top, bottom)) # rescale y axis, to match the grid orientation
plt.grid(True)
plt.show()
메인 함수의 플롯 부분을 살펴보면
우측에 점유 격자 지도를 놓고 좌측에는 레이저 빔 결과를 보여주고 있습니다.
이런 식으로 플로팅한 결과는 아래와 같습니다.
전체 코드
"""
LIDAR to 2D grid map example
author: Erno Horvath, Csaba Hajdu based on Atsushi Sakai's scripts
"""
import math
from collections import deque
import matplotlib.pyplot as plt
import numpy as np
EXTEND_AREA = 1.0
def file_read(f):
"""
Reading LIDAR laser beams (angles and corresponding distance data)
"""
with open(f) as data:
measures = [line.split(",") for line in data]
angles = []
distances = []
for measure in measures:
angles.append(float(measure[0]))
distances.append(float(measure[1]))
angles = np.array(angles)
distances = np.array(distances)
return angles, distances
def bresenham(start, end):
"""
Implementation of Bresenham's line drawing algorithm
See en.wikipedia.org/wiki/Bresenham's_line_algorithm
Bresenham's Line Algorithm
Produces a np.array from start and end (original from roguebasin.com)
>>> points1 = bresenham((4, 4), (6, 10))
>>> print(points1)
np.array([[4,4], [4,5], [5,6], [5,7], [5,8], [6,9], [6,10]])
"""
# setup initial conditions
x1, y1 = start
x2, y2 = end
dx = x2 - x1
dy = y2 - y1
is_steep = abs(dy) > abs(dx) # determine how steep the line is
if is_steep: # rotate line
x1, y1 = y1, x1
x2, y2 = y2, x2
# swap start and end points if necessary and store swap state
swapped = False
if x1 > x2:
x1, x2 = x2, x1
y1, y2 = y2, y1
swapped = True
dx = x2 - x1 # recalculate differentials
dy = y2 - y1 # recalculate differentials
error = int(dx / 2.0) # calculate error
y_step = 1 if y1 < y2 else -1
# iterate over bounding box generating points between start and end
y = y1
points = []
for x in range(x1, x2 + 1):
coord = [y, x] if is_steep else (x, y)
points.append(coord)
error -= abs(dy)
if error < 0:
y += y_step
error += dx
if swapped: # reverse the list if the coordinates were swapped
points.reverse()
points = np.array(points)
return points
def calc_grid_map_config(ox, oy, xy_resolution):
"""
Calculates the size, and the maximum distances according to the the
measurement center
"""
min_x = round(min(ox) - EXTEND_AREA / 2.0)
min_y = round(min(oy) - EXTEND_AREA / 2.0)
max_x = round(max(ox) + EXTEND_AREA / 2.0)
max_y = round(max(oy) + EXTEND_AREA / 2.0)
xw = int(round((max_x - min_x) / xy_resolution))
yw = int(round((max_y - min_y) / xy_resolution))
print("The grid map is ", xw, "x", yw, ".")
return min_x, min_y, max_x, max_y, xw, yw
def atan_zero_to_twopi(y, x):
angle = math.atan2(y, x)
if angle < 0.0:
angle += math.pi * 2.0
return angle
def init_flood_fill(center_point, obstacle_points, xy_points, min_coord,
xy_resolution):
"""
center_point: center point
obstacle_points: detected obstacles points (x,y)
xy_points: (x,y) point pairs
"""
center_x, center_y = center_point
prev_ix, prev_iy = center_x - 1, center_y
ox, oy = obstacle_points
xw, yw = xy_points
min_x, min_y = min_coord
occupancy_map = (np.ones((xw, yw))) * 0.5
for (x, y) in zip(ox, oy):
# x coordinate of the the occupied area
ix = int(round((x - min_x) / xy_resolution))
# y coordinate of the the occupied area
iy = int(round((y - min_y) / xy_resolution))
free_area = bresenham((prev_ix, prev_iy), (ix, iy))
for fa in free_area:
occupancy_map[fa[0]][fa[1]] = 0 # free area 0.0
prev_ix = ix
prev_iy = iy
return occupancy_map
def flood_fill(center_point, occupancy_map):
"""
center_point: starting point (x,y) of fill
occupancy_map: occupancy map generated from Bresenham ray-tracing
"""
# Fill empty areas with queue method
sx, sy = occupancy_map.shape
fringe = deque()
fringe.appendleft(center_point)
while fringe:
n = fringe.pop()
nx, ny = n
# West
if nx > 0:
if occupancy_map[nx - 1, ny] == 0.5:
occupancy_map[nx - 1, ny] = 0.0
fringe.appendleft((nx - 1, ny))
# East
if nx < sx - 1:
if occupancy_map[nx + 1, ny] == 0.5:
occupancy_map[nx + 1, ny] = 0.0
fringe.appendleft((nx + 1, ny))
# North
if ny > 0:
if occupancy_map[nx, ny - 1] == 0.5:
occupancy_map[nx, ny - 1] = 0.0
fringe.appendleft((nx, ny - 1))
# South
if ny < sy - 1:
if occupancy_map[nx, ny + 1] == 0.5:
occupancy_map[nx, ny + 1] = 0.0
fringe.appendleft((nx, ny + 1))
def generate_ray_casting_grid_map(ox, oy, xy_resolution, breshen=True):
"""
The breshen boolean tells if it's computed with bresenham ray casting
(True) or with flood fill (False)
"""
min_x, min_y, max_x, max_y, x_w, y_w = calc_grid_map_config(
ox, oy, xy_resolution)
# default 0.5 -- [[0.5 for i in range(y_w)] for i in range(x_w)]
occupancy_map = np.ones((x_w, y_w)) / 2
center_x = int(
round(-min_x / xy_resolution)) # center x coordinate of the grid map
center_y = int(
round(-min_y / xy_resolution)) # center y coordinate of the grid map
# occupancy grid computed with bresenham ray casting
if breshen:
for (x, y) in zip(ox, oy):
# x coordinate of the the occupied area
ix = int(round((x - min_x) / xy_resolution))
# y coordinate of the the occupied area
iy = int(round((y - min_y) / xy_resolution))
laser_beams = bresenham((center_x, center_y), (
ix, iy)) # line form the lidar to the occupied point
for laser_beam in laser_beams:
occupancy_map[laser_beam[0]][
laser_beam[1]] = 0.0 # free area 0.0
occupancy_map[ix][iy] = 1.0 # occupied area 1.0
occupancy_map[ix + 1][iy] = 1.0 # extend the occupied area
occupancy_map[ix][iy + 1] = 1.0 # extend the occupied area
occupancy_map[ix + 1][iy + 1] = 1.0 # extend the occupied area
# occupancy grid computed with with flood fill
else:
occupancy_map = init_flood_fill((center_x, center_y), (ox, oy),
(x_w, y_w),
(min_x, min_y), xy_resolution)
flood_fill((center_x, center_y), occupancy_map)
occupancy_map = np.array(occupancy_map, dtype=np.float)
for (x, y) in zip(ox, oy):
ix = int(round((x - min_x) / xy_resolution))
iy = int(round((y - min_y) / xy_resolution))
occupancy_map[ix][iy] = 1.0 # occupied area 1.0
occupancy_map[ix + 1][iy] = 1.0 # extend the occupied area
occupancy_map[ix][iy + 1] = 1.0 # extend the occupied area
occupancy_map[ix + 1][iy + 1] = 1.0 # extend the occupied area
return occupancy_map, min_x, max_x, min_y, max_y, xy_resolution
def main():
"""
Example usage
"""
print(__file__, "start")
xy_resolution = 0.02 # x-y grid resolution
ang, dist = file_read("lidar01.csv")
ox = np.sin(ang) * dist
oy = np.cos(ang) * dist
occupancy_map, min_x, max_x, min_y, max_y, xy_resolution = \
generate_ray_casting_grid_map(ox, oy, xy_resolution, True)
xy_res = np.array(occupancy_map).shape
plt.figure(1, figsize=(10, 4))
plt.subplot(122)
plt.imshow(occupancy_map, cmap="PiYG_r")
# cmap = "binary" "PiYG_r" "PiYG_r" "bone" "bone_r" "RdYlGn_r"
plt.clim(-0.4, 1.4)
plt.gca().set_xticks(np.arange(-.5, xy_res[1], 1), minor=True)
plt.gca().set_yticks(np.arange(-.5, xy_res[0], 1), minor=True)
plt.grid(True, which="minor", color="w", linewidth=0.6, alpha=0.5)
plt.colorbar()
plt.subplot(121)
plt.plot([oy, np.zeros(np.size(oy))], [ox, np.zeros(np.size(oy))], "ro-")
plt.axis("equal")
plt.plot(0.0, 0.0, "ob")
plt.gca().set_aspect("equal", "box")
bottom, top = plt.ylim() # return the current y-lim
plt.ylim((top, bottom)) # rescale y axis, to match the grid orientation
plt.grid(True)
plt.show()
if __name__ == '__main__':
main()
