"""
Normal Distribution Transform (NDTGrid) mapping sample
"""
import matplotlib.pyplot as plt
import numpy as np
from collections import defaultdict
from Mapping.grid_map_lib.grid_map_lib import GridMap
from utils.plot import plot_covariance_ellipse
[docs]
class NDTMap:
"""
Normal Distribution Transform (NDT) map class
:param ox: obstacle x position list
:param oy: obstacle y position list
:param resolution: grid resolution [m]
"""
[docs]
class NDTGrid:
"""
NDT grid
"""
def __init__(self):
#: Number of points in the NDTGrid grid
self.n_points = 0
#: Mean x position of points in the NDTGrid cell
self.mean_x = None
#: Mean y position of points in the NDTGrid cell
self.mean_y = None
#: Center x position of the NDT grid
self.center_grid_x = None
#: Center y position of the NDT grid
self.center_grid_y = None
#: Covariance matrix of the NDT grid
self.covariance = None
#: Eigen vectors of the NDT grid
self.eig_vec = None
#: Eigen values of the NDT grid
self.eig_values = None
def __init__(self, ox, oy, resolution):
#: Minimum number of points in the NDT grid
self.min_n_points = 3
#: Resolution of the NDT grid [m]
self.resolution = resolution
width = int((max(ox) - min(ox))/resolution) + 3 # rounding up + right and left margin
height = int((max(oy) - min(oy))/resolution) + 3
center_x = np.mean(ox)
center_y = np.mean(oy)
self.ox = ox
self.oy = oy
#: NDT grid index map
self.grid_index_map = self._create_grid_index_map(ox, oy)
#: NDT grid map. Each grid contains NDTGrid object
self._construct_grid_map(center_x, center_y, height, ox, oy, resolution, width)
def _construct_grid_map(self, center_x, center_y, height, ox, oy, resolution, width):
self.grid_map = GridMap(width, height, resolution, center_x, center_y, self.NDTGrid())
for grid_index, inds in self.grid_index_map.items():
ndt = self.NDTGrid()
ndt.n_points = len(inds)
if ndt.n_points >= self.min_n_points:
ndt.mean_x = np.mean(ox[inds])
ndt.mean_y = np.mean(oy[inds])
ndt.center_grid_x, ndt.center_grid_y = \
self.grid_map.calc_grid_central_xy_position_from_grid_index(grid_index)
ndt.covariance = np.cov(ox[inds], oy[inds])
ndt.eig_values, ndt.eig_vec = np.linalg.eig(ndt.covariance)
self.grid_map.data[grid_index] = ndt
def _create_grid_index_map(self, ox, oy):
grid_index_map = defaultdict(list)
for i in range(len(ox)):
grid_index = self.grid_map.calc_grid_index_from_xy_pos(ox[i], oy[i])
grid_index_map[grid_index].append(i)
return grid_index_map
def create_dummy_observation_data():
ox = []
oy = []
# left corridor
for y in range(-50, 50):
ox.append(-20.0)
oy.append(y)
# right corridor 1
for y in range(-50, 0):
ox.append(20.0)
oy.append(y)
# right corridor 2
for x in range(20, 50):
ox.append(x)
oy.append(0)
# right corridor 3
for x in range(20, 50):
ox.append(x)
oy.append(x/2.0+10)
# right corridor 4
for y in range(20, 50):
ox.append(20)
oy.append(y)
ox = np.array(ox)
oy = np.array(oy)
# Adding random noize
ox += np.random.rand(len(ox)) * 1.0
oy += np.random.rand(len(ox)) * 1.0
return ox, oy
def main():
print(__file__ + " start!!")
ox, oy = create_dummy_observation_data()
grid_resolution = 10.0
ndt_map = NDTMap(ox, oy, grid_resolution)
# plot raw observation
plt.plot(ox, oy, ".r")
# plot grid clustering
[plt.plot(ox[inds], oy[inds], "x") for inds in ndt_map.grid_index_map.values()]
# plot ndt grid map
[plot_covariance_ellipse(ndt.mean_x, ndt.mean_y, ndt.covariance, color="-k") for ndt in ndt_map.grid_map.data if ndt.n_points > 0]
plt.axis("equal")
plt.show()
if __name__ == '__main__':
main()