github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-11 20:05:13 -05:00
Code Issues Packages Projects Releases Wiki Activity

Using scipy.spatial.rotation matrix (#335)

Browse Source
This commit is contained in:
Atsushi Sakai
2020-06-07 20:28:29 +09:00
committed by GitHub
parent d1bde5835f
commit b8afdb10d8
26 changed files with 1002 additions and 931 deletions
Download Patch File Download Diff File Expand all files Collapse all files

187
Mapping/lidar_to_grid_map/lidar_to_grid_map.py
View File

@@ -2,15 +2,16 @@
LIDAR to 2D grid map example
author: Erno Horvath, Csaba Hajdu based on Atsushi Sakai's scripts (@Atsushi_twi)
author: Erno Horvath, Csaba Hajdu based on Atsushi Sakai's scripts
"""
import math
import numpy as np
import matplotlib.pyplot as plt
from collections import deque
import matplotlib.pyplot as plt
import numpy as np
EXTEND_AREA = 1.0
@@ -44,47 +45,49 @@ def bresenham(start, end):
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
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
swapped = False # swap start and end points if necessary and store swap state
# 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
ystep = 1 if y1 < y2 else -1
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)
points.append(coord)
error -= abs(dy)
if error < 0:
y += ystep
y += y_step
error += dx
if swapped: # reverse the list if the coordinates were swapped
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, xyreso):
def calc_grid_map_config(ox, oy, xy_resolution):
"""
Calculates the size, and the maximum distances according to the the measurement center
Calculates the size, and the maximum distances according to the the
measurement center
"""
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))
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 minx, miny, maxx, maxy, xw, yw
return min_x, min_y, max_x, max_y, xw, yw
def atan_zero_to_twopi(y, x):
@@ -94,96 +97,110 @@ def atan_zero_to_twopi(y, x):
return angle
def init_floodfill(cpoint, opoints, xypoints, mincoord, xyreso):
def init_flood_fill(center_point, obstacle_points, xy_points, min_coord,
xy_resolution):
"""
cpoint: center point
opoints: detected obstacles points (x,y)
xypoints: (x,y) point pairs
center_point: center point
obstacle_points: detected obstacles points (x,y)
xy_points: (x,y) point pairs
"""
centix, centiy = cpoint
prev_ix, prev_iy = centix - 1, centiy
ox, oy = opoints
xw, yw = xypoints
minx, miny = mincoord
pmap = (np.ones((xw, yw))) * 0.5
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):
ix = int(round((x - minx) / xyreso)) # x coordinate of the the occupied area
iy = int(round((y - miny) / xyreso)) # y coordinate of the the occupied area
# 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:
pmap[fa[0]][fa[1]] = 0 # free area 0.0
occupancy_map[fa[0]][fa[1]] = 0 # free area 0.0
prev_ix = ix
prev_iy = iy
return pmap
return occupancy_map
def flood_fill(cpoint, pmap):
def flood_fill(center_point, occupancy_map):
"""
cpoint: starting point (x,y) of fill
pmap: occupancy map generated from Bresenham ray-tracing
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 = pmap.shape
sx, sy = occupancy_map.shape
fringe = deque()
fringe.appendleft(cpoint)
fringe.appendleft(center_point)
while fringe:
n = fringe.pop()
nx, ny = n
# West
if nx > 0:
if pmap[nx - 1, ny] == 0.5:
pmap[nx - 1, ny] = 0.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 pmap[nx + 1, ny] == 0.5:
pmap[nx + 1, ny] = 0.0
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 pmap[nx, ny - 1] == 0.5:
pmap[nx, ny - 1] = 0.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 pmap[nx, ny + 1] == 0.5:
pmap[nx, ny + 1] = 0.0
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, xyreso, breshen=True):
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)
The breshen boolean tells if it's computed with bresenham ray casting
(True) or with flood fill (False)
"""
minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso)
pmap = np.ones((xw, yw))/2 # default 0.5 -- [[0.5 for i in range(yw)] for i in range(xw)]
centix = int(round(-minx / xyreso)) # center x coordinate of the grid map
centiy = int(round(-miny / xyreso)) # center y coordinate of the grid map
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):
ix = int(round((x - minx) / xyreso)) # x coordinate of the the occupied area
iy = int(round((y - miny) / xyreso)) # y coordinate of the the occupied area
laser_beams = bresenham((centix, centiy), (ix, iy)) # line form the lidar to the cooupied point
# 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:
pmap[laser_beam[0]][laser_beam[1]] = 0.0 # free area 0.0
pmap[ix][iy] = 1.0 # occupied area 1.0
pmap[ix+1][iy] = 1.0 # extend the occupied area
pmap[ix][iy+1] = 1.0 # extend the occupied area
pmap[ix+1][iy+1] = 1.0 # extend the occupied area
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:
pmap = init_floodfill((centix, centiy), (ox, oy), (xw, yw), (minx, miny), xyreso)
flood_fill((centix, centiy), pmap)
pmap = np.array(pmap, dtype=np.float)
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 - minx) / xyreso))
iy = int(round((y - miny) / xyreso))
pmap[ix][iy] = 1.0 # occupied area 1.0
pmap[ix+1][iy] = 1.0 # extend the occupied area
pmap[ix][iy+1] = 1.0 # extend the occupied area
pmap[ix+1][iy+1] = 1.0 # extend the occupied area
return pmap, minx, maxx, miny, maxy, xyreso
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():
@@ -191,18 +208,20 @@ def main():
Example usage
"""
print(__file__, "start")
xyreso = 0.02 # x-y grid resolution
xy_resolution = 0.02 # x-y grid resolution
ang, dist = file_read("lidar01.csv")
ox = np.sin(ang) * dist
oy = np.cos(ang) * dist
pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map(ox, oy, xyreso, True)
xyres = np.array(pmap).shape
plt.figure(1, figsize=(10,4))
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(pmap, cmap="PiYG_r") # cmap = "binary" "PiYG_r" "PiYG_r" "bone" "bone_r" "RdYlGn_r"
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, xyres[1], 1), minor=True)
plt.gca().set_yticks(np.arange(-.5, xyres[0], 1), minor=True)
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)
@@ -210,11 +229,11 @@ def main():
plt.axis("equal")
plt.plot(0.0, 0.0, "ob")
plt.gca().set_aspect("equal", "box")
bottom, top = plt.ylim() # return the current ylim
plt.ylim((top, bottom)) # rescale y axis, to match the grid orientation
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()
main()