Merge pull request #135 from leggedrobotics/rotation_fix

Fix 90° CCW rotation in masked_replace, load and save

Author: Idate96

elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py

@@ -875,15 +875,60 @@ def get_map_with_name_ref(self, name, data):
         # Old 180° rotation (incorrect - missing transpose, caused 90° CCW error in RViz):
         # m = xp.flip(m, 0)
         # m = xp.flip(m, 1)
-        m = m.T
-        m = xp.flip(m, 0)
-        m = xp.flip(m, 1)
+        m = self._transform_to_grid_map_coordinate_convention(m)
         if use_stream:
             stream = cp.cuda.Stream(non_blocking=False)
         else:
             stream = None
         self.copy_to_cpu(m, data, stream=stream)
 
+    def _transform_to_grid_map_coordinate_convention(self, m):
+        """Transform the map to the grid_map coordinate convention.
+
+        elevation_mapping_cupy uses Row=Y, Col=X (see kernels/custom_kernels.py:35)
+        grid_map uses Row→-X, Col→-Y (see grid_map_core/src/GridMapMath.cpp:64-67
+        transformBufferOrderToMapFrame returns {-index[0], -index[1]})
+        Required transformation:
+           1. Transpose: swap axes so Row=X, Col=Y (matching grid_map's axis assignment)
+           2. Flip axis 0: so increasing row → decreasing X (matching grid_map's -X)
+           3. Flip axis 1: so increasing col → decreasing Y (matching grid_map's -Y)
+
+        This is equivalent to: rot90(m.T, k=2) or flip(flip(m.T, 0), 1)
+
+        Args:
+            m (cupy._core.core.ndarray):
+
+        Returns:
+            cupy._core.core.ndarray:
+        """
+        m = m.T
+        m = xp.flip(m, 0)
+        m = xp.flip(m, 1)
+        return m
+
+    def _transform_to_elevation_mapping_coordinate_convention(self, m):
+        """Transform the map to the grid_map coordinate convention.
+
+        elevation_mapping_cupy uses Row=Y, Col=X (see kernels/custom_kernels.py:35)
+        grid_map uses Row→-X, Col→-Y (see grid_map_core/src/GridMapMath.cpp:64-67
+        transformBufferOrderToMapFrame returns {-index[0], -index[1]})
+        To transform back to a normal array, we need to apply the inverse transformation:
+        Flip axis 0: so increasing row → decreasing X (matching grid_map's -X)
+        Flip axis 1: so increasing col → decreasing Y (matching grid_map's -Y)
+        Transpose: swap axes so Row=X, Col=Y (matching grid_map's axis assignment)
+        This is equivalent to: flip(flip(m, 0), 1).T
+
+        Args:
+            m (cupy._core.core.ndarray):
+
+        Returns:
+            cupy._core.core.ndarray:
+        """
+        m = xp.flip(m, 0)
+        m = xp.flip(m, 1)
+        m = m.T
+        return m
+
     def get_normal_maps(self):
         """Get the normal maps.
 
@@ -1062,6 +1107,12 @@ def apply_masked_replace(
         if not layer_data:
             raise ValueError("No layer data provided for masked replace.")
 
+        # Transform the layer data from grid_map coordinate convention to the elevation_mapping_cupy coordinate convention
+        for name, array in layer_data.items():
+            layer_data[name] = self._transform_to_elevation_mapping_coordinate_convention(array)
+        if mask is not None:
+            mask = self._transform_to_elevation_mapping_coordinate_convention(mask)
+
         sample_shape: Optional[Tuple[int, int]] = None
         for array in layer_data.values():
             if sample_shape is None:
@@ -1134,6 +1185,11 @@ def set_full_map(
     ) -> None:
         if not raw_layers:
             raise ValueError("Raw layer data required to restore the map.")
+
+        # Transform the raw layer data from grid_map coordinate convention to the elevation_mapping_cupy coordinate convention
+        for name, array in raw_layers.items():
+            raw_layers[name] = self._transform_to_elevation_mapping_coordinate_convention(array)
+
         sample_shape = next(iter(raw_layers.values())).shape
         self._validate_geometry_against_shape(sample_shape, geometry)
 

elevation_mapping_cupy/scripts/elevation_mapping_node.py

@@ -442,7 +442,6 @@ def handle_load_map(self, request, response):
 
             fused_layers, _ = self._grid_map_to_numpy(fused_msg)
             raw_layers, geometry = self._grid_map_to_numpy(raw_msg)
-            raw_layers = self._restore_internal_layer_orientation(raw_layers)
 
             self._map.set_full_map(fused_layers, raw_layers, geometry)
 
@@ -499,17 +498,6 @@ def _grid_map_to_numpy(self, grid_map_msg: GridMap):
         )
         return arrays, geometry
 
-    def _restore_internal_layer_orientation(self, layers: Dict[str, np.ndarray]) -> Dict[str, np.ndarray]:
-        """Undo the double-axis flip applied when exporting layers for GridMap messages."""
-        restored: Dict[str, np.ndarray] = {}
-        for name, array in layers.items():
-            if array.ndim >= 2:
-                flipped = np.flip(np.flip(array, axis=0), axis=1)
-                restored[name] = np.ascontiguousarray(flipped)
-            else:
-                restored[name] = np.ascontiguousarray(array)
-        return restored
-
     def _extract_layout_shape(self, array_msg: Float32MultiArray) -> tuple:
         if array_msg.layout.dim:
             cols = array_msg.layout.dim[0].size or 1

scripts/masked_replace_tool.py

@@ -297,6 +297,7 @@ def _numpy_to_multiarray(array: np.ndarray) -> Float32MultiArray:
         msg = Float32MultiArray()
         layout = MultiArrayLayout()
         rows, cols = array.shape
+        # numpy
         layout.dim.append(MultiArrayDimension(label="column_index", size=cols, stride=rows * cols))
         layout.dim.append(MultiArrayDimension(label="row_index", size=rows, stride=rows))
         msg.layout = layout