Adding NDT mapping script and doc (AtsushiSakai#867)

* 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

Author: AtsushiSakai

Localization/extended_kalman_filter/extended_kalman_filter.py

@@ -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([
@@ -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!!")
 
@@ -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)

Mapping/grid_map_lib/grid_map_lib.py

@@ -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]
@@ -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
@@ -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
@@ -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:
@@ -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)
@@ -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]
@@ -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()