draw covariance ellipse in each grids

src/components/mapping/ndt/ndt_global_mapper.py

@@ -4,6 +4,11 @@
 Author: Shisato Yano
 """
 
+import sys
+from pathlib import Path
+
+sys.path.append(str(Path(__file__).absolute().parent) + "/../../common")
+from matrix_lib import hom_mat_33
 from ndt_map import NdtMap
 
 
@@ -13,7 +18,8 @@ class NdtGlobalMapper:
     """
 
     def __init__(self, width_m=60.0, height_m=60.0, resolution_m=5.0,
-                 center_x_m=0.0, center_y_m=0.0, min_points_num=3):
+                 center_x_m=0.0, center_y_m=0.0, min_points_num=3,
+                 sensor_params=None):
         """
         Constructor
         width_m: Width size of map[m]
@@ -22,9 +28,11 @@ def __init__(self, width_m=60.0, height_m=60.0, resolution_m=5.0,
         center_x_m: Center x position of map[m]
         center_y_m: Center y position of map[m]
         min_points_num: Minimum number of points for normal distribution transform
+        sensor_params: Parameters object of sensor
         """
 
         self.map = NdtMap(width_m, height_m, resolution_m, center_x_m, center_y_m, min_points_num)
+        self.params = sensor_params
 
     def update(self, point_cloud, state):
         """
@@ -37,13 +45,28 @@ def update(self, point_cloud, state):
 
         points_x_list, points_y_list = [], []
         for point in point_cloud:
-            global_point_xy = point.get_transformed_data(vehicle_pose[0, 0],
-                                                         vehicle_pose[1, 0],
-                                                         vehicle_pose[2, 0])
-            points_x_list.append(global_point_xy[0, 0])
-            points_y_list.append(global_point_xy[1, 0])
+            # x, y position on sensor coordinate
+            point_xy = point.get_point_array()
+            sensor_tf = hom_mat_33(point_xy[0, 0], point_xy[1, 0], 0.0)
+
+            # transformation matrix into vehicle coordinate
+            vehicle_tf = hom_mat_33(self.params.INST_LON_M, self.params.INST_LAT_M, self.params.INST_YAW_RAD)
+
+            # transformation matrix into global coordinate
+            global_tf = hom_mat_33(vehicle_pose[0, 0], vehicle_pose[1, 0], vehicle_pose[2, 0])
+
+            # homogeneous transformation into global coordinate
+            global_points_matrix = global_tf @ vehicle_tf @ sensor_tf
+            points_x_list.append(global_points_matrix[0, 2])
+            points_y_list.append(global_points_matrix[1, 2])
 
         self.map.update_map(points_x_list, points_y_list)
 
     def draw(self, axes, elems):
-        print("draw map")
+        """
+        Function to draw map data
+        axes: Axes object of figure
+        elems: List of plot object
+        """
+
+        self.map.draw_map(axes, elems)

src/components/mapping/ndt/ndt_map.py

@@ -10,8 +10,10 @@
 from collections import defaultdict
 
 sys.path.append(str(Path(__file__).absolute().parent) + "/../grid")
+sys.path.append(str(Path(__file__).absolute().parent) + "/../../common")
 from grid_map import GridMap
 from ndt_grid import NdtGrid
+from plot_lib import draw_covariance_ellipse
 
 
 class NdtMap:
@@ -73,3 +75,12 @@ def update_map(self, points_x_list, points_y_list):
                                          points_y_array[points_indices])
                 grid.eigen_values, grid.eigen_vectors = np.linalg.eig(grid.covariance)
                 self.map.set_grid_data(grid_idx, grid)
+
+    def draw_map(self, axes, elems):
+        """
+        Function to draw map data
+        axes: Axes object of figure
+        elems: List of plot object
+        """
+
+        [draw_covariance_ellipse(axes, elems, grid.mean_x_m, grid.mean_y_m, grid.covariance) for grid in self.map.data if grid.points_num > 0]

src/simulations/mapping/ndt_map_construction/ndt_map_construction.py

@@ -52,14 +52,15 @@ def main():
 
     # create obstacle instances
     obst_list = ObstacleList()
-    obst1 = Obstacle(State(x_m=10.0, y_m=10.0))
+    obst1 = Obstacle(State(x_m=15.0, y_m=15.0), length_m=10, width_m=5)
     obst_list.add_obstacle(obst1)
     vis.add_object(obst_list)
 
     # create vehicle instance
     spec = VehicleSpecification()
-    lidar = OmniDirectionalLidar(obst_list, SensorParameters(lon_m=spec.wheel_base_m/2, angle_std_scale=0.0, dist_std_rate=0.0))
-    mapper = NdtGlobalMapper()
+    sensor_params = SensorParameters(lon_m=spec.wheel_base_m/2)
+    lidar = OmniDirectionalLidar(obst_list, sensor_params)
+    mapper = NdtGlobalMapper(sensor_params=sensor_params)
     vehicle = FourWheelsVehicle(State(color=spec.color), spec, sensors=Sensors(lidar=lidar), mapper=mapper, show_zoom=False)
     vis.add_object(vehicle)