mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-13 10:48:01 -05:00
0fc769421f
* 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
136 lines
4.2 KiB
Python
136 lines
4.2 KiB
Python
"""
|
|
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
|
|
|
|
|
|
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]
|
|
"""
|
|
|
|
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()
|