v0.0.7

 - bugfix with local occupancy map creation

mpi_sim/__init__.py

@@ -22,4 +22,4 @@ from mpi_sim.utils.attrdict import combine_dicts
 # import the module library last, so that it can find all other modules in the mpi_sim to load them
 import mpi_sim.core.module_library
 
-__version__ = '0.0.6'
+__version__ = '0.0.7'

mpi_sim/utils/occupancy_grid_map.py

@@ -166,6 +166,10 @@ def create_local_perspective_occupancy_grid_map(global_map, depth=5.0, position=
     """
 
     # see https://theailearner.com/tag/cv2-warpperspective/
+    # Attention: opencv uses a different coordinate frame to us, thus the code additionally transforms the coordinate frames by
+    # having the opposite rotation direction, and using transpose operations
+
+    # TODO: there should be a way to avoid the transpose operations, by choosing a different points that should be
 
     local_map_length = int(depth * 2 / global_map.resolution)
 
@@ -176,9 +180,9 @@ def create_local_perspective_occupancy_grid_map(global_map, depth=5.0, position=
     )
     orientation = mpi_sim.utils.constraint_angle(orientation)
 
-    hvect = np.array([np.cos(orientation), -np.sin(orientation)])
-    vvect = np.array([np.sin(orientation), np.cos(orientation)])
-    # print('')
+    # rotation in for cv operator is in opposite direction
+    hvect = np.array([np.cos(-orientation), -np.sin(-orientation)])
+    vvect = np.array([np.sin(-orientation), np.cos(-orientation)])
     # print('hvect', hvect)
     # print('vvect', vvect)
 
@@ -202,10 +206,11 @@ def create_local_perspective_occupancy_grid_map(global_map, depth=5.0, position=
     # print('dst_pts', dst_pts)
 
     m = cv2.getPerspectiveTransform(src_pts, dst_pts)
+    # use transpose operations to bring image into coordinate frame of cv and then back to our coordinate system
     local_map = cv2.warpPerspective(
-        global_map.map,
+        global_map.map.T,
         m,
         (local_map_length, local_map_length)
-    )
+    ).T
 
-    return OccupancyGridMap(map=local_map, resolution=global_map.resolution, area=None)
\ No newline at end of file
+    return OccupancyGridMap(map=local_map, resolution=global_map.resolution, area=None)

readme.md

 # Multi-Agent Interaction Simulator
 
-Version 0.0.6 (09.12.2022)
+Version 0.0.7 (13.12.2022)
 
 Simulator for multi-agent interaction scenarios in a 2D environment.
 

setup.cfg

 [metadata]
 name = mpi_sim
-version = 0.0.6
+version = 0.0.7
 
 [options]
 packages = find:

test/utils/test_occupancy_grid_map.py

@@ -394,8 +394,8 @@ def test_local_perspective_no_rotation_area2():
     global_map = np.array([
         [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
         [0., 1., 1., 1., 0., 0., 0., 0., 0., 0.],
-        [0., 1., 1., 1., 0., 0., 0., 0., 0., 0.],
-        [0., 1., 1., 1., 0., 0., 0., 0., 0., 0.],
+        [1., 1., 1., 1., 0., 0., 0., 0., 0., 0.],
+        [0., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
         [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
     ])
     global_map = np.flip(global_map, axis=0).transpose()  # in correct (x,y) array form
@@ -405,14 +405,106 @@ def test_local_perspective_no_rotation_area2():
 
     expected_local_map = np.array([
         [0., 0., 0., 1., 1., 1.],
+        [0., 0., 1., 1., 1., 1.],
+        [0., 0., 0., 1., 1., 0.],
+        [0., 0., 0., 0., 0., 0.],
+        [0., 0., 0., 0., 0., 0.],
+        [0., 0., 0., 0., 0., 0.],
+    ])
+    expected_local_map = np.flip(expected_local_map, axis=0).transpose()
+
+    lm = np.flip(local_map.map.transpose(), axis=0)
+
+    np_test.assert_array_almost_equal(expected_local_map, local_map.map)
+
+
+def test_local_perspective_no_rotation_area3():
+
+    # map in human view form with axes on bottom and y axes on the left
+    global_map = np.array([
+        [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+        [0., 0., 1., 1., 1., 0., 0., 0., 0., 0.],
+        [0., 0., 0., 1., 1., 0., 0., 0., 0., 0.],
+        [0., 0., 1., 1., 0., 0., 0., 0., 0., 0.],
+        [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+    ])
+    global_map = np.flip(global_map, axis=0).transpose()  # in correct (x,y) array form
+    global_map = OccupancyGridMap(map=global_map, area=((-5, 5), (-2, 3)), resolution=1.0)
+
+    local_map = create_local_perspective_occupancy_grid_map(global_map=global_map, position=(-3.0, -1.0), orientation=0.0, depth=3)
+
+    expected_local_map = np.array([
         [0., 0., 0., 1., 1., 1.],
-        [0., 0., 0., 1., 1., 1.],
+        [0., 0., 0., 0., 1., 1.],
+        [0., 0., 0., 1., 1., 0.],
         [0., 0., 0., 0., 0., 0.],
         [0., 0., 0., 0., 0., 0.],
         [0., 0., 0., 0., 0., 0.],
     ])
     expected_local_map = np.flip(expected_local_map, axis=0).transpose()
 
+    lm = np.flip(local_map.map.transpose(), axis=0)
+
+    np_test.assert_array_almost_equal(expected_local_map, local_map.map)
+
+
+def test_local_perspective_rotation_east_area3():
+
+    # map in human view form with axes on bottom and y axes on the left
+    global_map = np.array([
+        [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+        [0., 0., 1., 1., 1., 0., 0., 0., 0., 0.],
+        [0., 0., 0., 1., 1., 0., 0., 0., 0., 0.],
+        [0., 0., 1., 1., 0., 0., 0., 0., 0., 0.],
+        [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+    ])
+    global_map = np.flip(global_map, axis=0).transpose()  # in correct (x,y) array form
+    global_map = OccupancyGridMap(map=global_map, area=((-5, 5), (-2, 3)), resolution=1.0)
+
+    local_map = create_local_perspective_occupancy_grid_map(global_map=global_map, position=(-3.0, -1.0), orientation=-np.pi/2, depth=3)
+
+    expected_local_map = np.array([
+        [0., 1., 1., 0., 0., 0.],
+        [0., 1., 1., 1., 0., 0.],
+        [0., 1., 0., 1., 0., 0.],
+        [0., 0., 0., 0., 0., 0.],
+        [0., 0., 0., 0., 0., 0.],
+        [0., 0., 0., 0., 0., 0.],
+    ])
+    expected_local_map = np.flip(expected_local_map, axis=0).transpose()
+
+    lm = np.flip(local_map.map.transpose(), axis=0)
+
+    np_test.assert_array_almost_equal(expected_local_map, local_map.map)
+
+
+def test_local_perspective_rotation_west_area3():
+
+    # map in human view form with axes on bottom and y axes on the left
+    global_map = np.array([
+        [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+        [0., 0., 1., 1., 1., 0., 0., 0., 0., 0.],
+        [0., 0., 0., 1., 1., 0., 0., 0., 0., 0.],
+        [0., 0., 1., 1., 0., 0., 0., 0., 0., 0.],
+        [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+    ])
+    global_map = np.flip(global_map, axis=0).transpose()  # in correct (x,y) array form
+    global_map = OccupancyGridMap(map=global_map, area=((-5, 5), (-2, 3)), resolution=1.0)
+
+    local_map = create_local_perspective_occupancy_grid_map(global_map=global_map, position=(-3.0, -1.0), orientation=np.pi/2, depth=3)
+
+    expected_local_map = np.array([
+        [0., 0., 0., 0., 0., 0.],
+        [0., 0., 0., 0., 0., 0.],
+        [0., 0., 0., 1., 0., 1.],
+        [0., 0., 0., 1., 1., 1.],
+        [0., 0., 0., 0., 1., 1.],
+        [0., 0., 0., 0., 0., 0.],
+    ])
+    expected_local_map = np.flip(expected_local_map, axis=0).transpose()
+
+    # lm = np.flip(local_map.map.transpose(), axis=0)
+
     np_test.assert_array_almost_equal(expected_local_map, local_map.map)