Skip to content

Commit

Permalink
Adding NDT mapping script and doc (#867)
Browse files Browse the repository at this point in the history 
* Adding ndt mapping script and doc

* Adding ndt mapping script and doc

* Adding ndt mapping script and doc

* Adding ndt mapping script and doc

* Adding ndt mapping script and doc

* Adding ndt mapping script and doc

* Adding ndt mapping script and doc

* Adding ndt mapping script and doc
  • Loading branch information
@AtsushiSakai
AtsushiSakai authored Jul 6, 2023
1 parent 49dd1a9 commit 0fc7694
Show file tree
Hide file tree
Showing 12 changed files with 372 additions and 83 deletions.
28 changes: 3 additions & 25 deletions Localization/extended_kalman_filter/extended_kalman_filter.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,13 +7,14 @@
"""
import sys
import pathlib

sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))

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

from utils.angle import rot_mat_2d
from utils.plot import plot_covariance_ellipse

# Covariance for EKF simulation
Q = np.diag([
Expand Down Expand Up @@ -135,29 +136,6 @@ def ekf_estimation(xEst, PEst, z, u):
return xEst, PEst


def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
Pxy = PEst[0:2, 0:2]
eigval, eigvec = np.linalg.eig(Pxy)

if eigval[0] >= eigval[1]:
bigind = 0
smallind = 1
else:
bigind = 1
smallind = 0

t = np.arange(0, 2 * math.pi + 0.1, 0.1)
a = math.sqrt(eigval[bigind])
b = math.sqrt(eigval[smallind])
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
angle = math.atan2(eigvec[1, bigind], eigvec[0, bigind])
fx = rot_mat_2d(angle) @ (np.array([x, y]))
px = np.array(fx[0, :] + xEst[0, 0]).flatten()
py = np.array(fx[1, :] + xEst[1, 0]).flatten()
plt.plot(px, py, "--r")


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

Expand Down Expand Up @@ -202,7 +180,7 @@ def main():
hxDR[1, :].flatten(), "-k")
plt.plot(hxEst[0, :].flatten(),
hxEst[1, :].flatten(), "-r")
plot_covariance_ellipse(xEst, PEst)
plot_covariance_ellipse(xEst[0, 0], xEst[1, 0], PEst)
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
Expand Down
125 changes: 85 additions & 40 deletions Mapping/grid_map_lib/grid_map_lib.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,22 +5,42 @@
author: Atsushi Sakai
"""

from functools import total_ordering
import matplotlib.pyplot as plt
import numpy as np


@total_ordering
class FloatGrid:

def __init__(self, init_val=0.0):
self.data = init_val

def get_float_data(self):
return self.data

def __eq__(self, other):
if not isinstance(other, FloatGrid):
return NotImplemented
return self.get_float_data() == other.get_float_data()

def __lt__(self, other):
if not isinstance(other, FloatGrid):
return NotImplemented
return self.get_float_data() < other.get_float_data()


class GridMap:
"""
GridMap class
"""

def __init__(self, width, height, resolution,
center_x, center_y, init_val=0.0):
center_x, center_y, init_val=FloatGrid(0.0)):
"""__init__
:param width: number of grid for width
:param height: number of grid for heigt
:param height: number of grid for height
:param resolution: grid resolution [m]
:param center_x: center x position [m]
:param center_y: center y position [m]
Expand All @@ -35,8 +55,9 @@ def __init__(self, width, height, resolution,
self.left_lower_x = self.center_x - self.width / 2.0 * self.resolution
self.left_lower_y = self.center_y - self.height / 2.0 * self.resolution

self.ndata = self.width * self.height
self.data = [init_val] * self.ndata
self.n_data = self.width * self.height
self.data = [init_val] * self.n_data
self.data_type = type(init_val)

def get_value_from_xy_index(self, x_ind, y_ind):
"""get_value_from_xy_index
Expand All @@ -49,7 +70,7 @@ def get_value_from_xy_index(self, x_ind, y_ind):

grid_ind = self.calc_grid_index_from_xy_index(x_ind, y_ind)

if 0 <= grid_ind < self.ndata:
if 0 <= grid_ind < self.n_data:
return self.data[grid_ind]
else:
return None
Expand Down Expand Up @@ -101,7 +122,7 @@ def set_value_from_xy_index(self, x_ind, y_ind, val):

grid_ind = int(y_ind * self.width + x_ind)

if 0 <= grid_ind < self.ndata:
if 0 <= grid_ind < self.n_data and isinstance(val, self.data_type):
self.data[grid_ind] = val
return True # OK
else:
Expand Down Expand Up @@ -138,6 +159,27 @@ def calc_grid_index_from_xy_index(self, x_ind, y_ind):
grid_ind = int(y_ind * self.width + x_ind)
return grid_ind

def calc_xy_index_from_grid_index(self, grid_ind):
y_ind, x_ind = divmod(grid_ind, self.width)
return x_ind, y_ind

def calc_grid_index_from_xy_pos(self, x_pos, y_pos):
"""get_xy_index_from_xy_pos
:param x_pos: x position [m]
:param y_pos: y position [m]
"""
x_ind = self.calc_xy_index_from_position(
x_pos, self.left_lower_x, self.width)
y_ind = self.calc_xy_index_from_position(
y_pos, self.left_lower_y, self.height)

return self.calc_grid_index_from_xy_index(x_ind, y_ind)

def calc_grid_central_xy_position_from_grid_index(self, grid_ind):
x_ind, y_ind = self.calc_xy_index_from_grid_index(grid_ind)
return self.calc_grid_central_xy_position_from_xy_index(x_ind, y_ind)

def calc_grid_central_xy_position_from_xy_index(self, x_ind, y_ind):
x_pos = self.calc_grid_central_xy_position_from_index(
x_ind, self.left_lower_x)
Expand All @@ -156,39 +198,40 @@ def calc_xy_index_from_position(self, pos, lower_pos, max_index):
else:
return None

def check_occupied_from_xy_index(self, xind, yind, occupied_val=1.0):
def check_occupied_from_xy_index(self, x_ind, y_ind, occupied_val):

val = self.get_value_from_xy_index(xind, yind)
val = self.get_value_from_xy_index(x_ind, y_ind)

if val is None or val >= occupied_val:
return True
else:
return False

def expand_grid(self):
xinds, yinds = [], []
def expand_grid(self, occupied_val=FloatGrid(1.0)):
x_inds, y_inds, values = [], [], []

for ix in range(self.width):
for iy in range(self.height):
if self.check_occupied_from_xy_index(ix, iy):
xinds.append(ix)
yinds.append(iy)

for (ix, iy) in zip(xinds, yinds):
self.set_value_from_xy_index(ix + 1, iy, val=1.0)
self.set_value_from_xy_index(ix, iy + 1, val=1.0)
self.set_value_from_xy_index(ix + 1, iy + 1, val=1.0)
self.set_value_from_xy_index(ix - 1, iy, val=1.0)
self.set_value_from_xy_index(ix, iy - 1, val=1.0)
self.set_value_from_xy_index(ix - 1, iy - 1, val=1.0)
if self.check_occupied_from_xy_index(ix, iy, occupied_val):
x_inds.append(ix)
y_inds.append(iy)
values.append(self.get_value_from_xy_index(ix, iy))

for (ix, iy, value) in zip(x_inds, y_inds, values):
self.set_value_from_xy_index(ix + 1, iy, val=value)
self.set_value_from_xy_index(ix, iy + 1, val=value)
self.set_value_from_xy_index(ix + 1, iy + 1, val=value)
self.set_value_from_xy_index(ix - 1, iy, val=value)
self.set_value_from_xy_index(ix, iy - 1, val=value)
self.set_value_from_xy_index(ix - 1, iy - 1, val=value)

@staticmethod
def check_inside_polygon(iox, ioy, x, y):

npoint = len(x) - 1
n_point = len(x) - 1
inside = False
for i1 in range(npoint):
i2 = (i1 + 1) % (npoint + 1)
for i1 in range(n_point):
i2 = (i1 + 1) % (n_point + 1)

if x[i1] >= x[i2]:
min_x, max_x = x[i2], x[i1]
Expand All @@ -211,52 +254,54 @@ def print_grid_map_info(self):
print("center_y:", self.center_y)
print("left_lower_x:", self.left_lower_x)
print("left_lower_y:", self.left_lower_y)
print("ndata:", self.ndata)
print("n_data:", self.n_data)

def plot_grid_map(self, ax=None):

grid_data = np.reshape(np.array(self.data), (self.height, self.width))
float_data_array = np.array([d.get_float_data() for d in self.data])
grid_data = np.reshape(float_data_array, (self.height, self.width))
if not ax:
fig, ax = plt.subplots()
heat_map = ax.pcolor(grid_data, cmap="Blues", vmin=0.0, vmax=1.0)
plt.axis("equal")
# plt.show()

return heat_map


def test_polygon_set():
def polygon_set_demo():
ox = [0.0, 4.35, 20.0, 50.0, 100.0, 130.0, 40.0]
oy = [0.0, -4.15, -20.0, 0.0, 30.0, 60.0, 80.0]

grid_map = GridMap(600, 290, 0.7, 60.0, 30.5)

grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
grid_map.set_value_from_polygon(ox, oy, FloatGrid(1.0), inside=False)

grid_map.plot_grid_map()

plt.axis("equal")
plt.grid(True)


def test_position_set():
def position_set_demo():
grid_map = GridMap(100, 120, 0.5, 10.0, -0.5)

grid_map.set_value_from_xy_pos(10.1, -1.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, -0.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, 1.1, 1.0)
grid_map.set_value_from_xy_pos(11.1, 0.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, 0.1, 1.0)
grid_map.set_value_from_xy_pos(9.1, 0.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, -1.1, FloatGrid(1.0))
grid_map.set_value_from_xy_pos(10.1, -0.1, FloatGrid(1.0))
grid_map.set_value_from_xy_pos(10.1, 1.1, FloatGrid(1.0))
grid_map.set_value_from_xy_pos(11.1, 0.1, FloatGrid(1.0))
grid_map.set_value_from_xy_pos(10.1, 0.1, FloatGrid(1.0))
grid_map.set_value_from_xy_pos(9.1, 0.1, FloatGrid(1.0))

grid_map.plot_grid_map()

plt.axis("equal")
plt.grid(True)


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

test_position_set()
test_polygon_set()
position_set_demo()
polygon_set_demo()

plt.show()

Expand Down
Loading

0 comments on commit 0fc7694

Please sign in to comment.