Source code for Mapping.ndt_map.ndt_map

"""
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()