Fix elevation map rotation issue with Row-Major indexing

- Fixed CUDA kernels to use proper Row-Major convention (Row=Y, Col=X)
- Updated is_inside() and get_idx() functions in all kernels
- Fixed normal vector calculation (removed swapped components)
- Adjusted data flipping for correct GridMap publishing
- Kept 180-degree rotation in internal representation
- Removed extra flip in elevation_mapping_ros.py

The elevation map now correctly aligns with LiDAR data.

Author: Lorenzo

elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py

@@ -797,6 +797,7 @@ def get_map_with_name_ref(self, name, data):
             else:
                 print("Layer {} is not in the map".format(name))
                 return
+        # Need 180 degree rotation to match coordinate system
         m = xp.flip(m, 0)
         m = xp.flip(m, 1)
         if use_stream:

elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping_ros.py

@@ -297,9 +297,11 @@ def publish_map(self, key: str) -> None:
         for layer in self.my_publishers[key].get("layers", []):
             gm.layers.append(layer)
             self._map.get_map_with_name_ref(layer, self._map_data)
-            map_data_for_gridmap = np.flip(self._map_data, axis=1)
+            # After fixing CUDA kernels and removing flips in elevation_mapping.py, no flip needed here
+            map_data_for_gridmap = self._map_data
             arr = Float32MultiArray()
             arr.layout = MAL()
+            # Keep original dimension order but fix strides
             arr.layout.dim.append(MAD(label="column_index", size=map_data_for_gridmap.shape[1], stride=map_data_for_gridmap.shape[0] * map_data_for_gridmap.shape[1]))
             arr.layout.dim.append(MAD(label="row_index", size=map_data_for_gridmap.shape[0], stride=map_data_for_gridmap.shape[0]))
             arr.data = map_data_for_gridmap.flatten().tolist()

elevation_mapping_cupy/elevation_mapping_cupy/kernels/custom_kernels.py

@@ -32,11 +32,16 @@ def map_utils(
             return i;
         }
         __device__ bool is_inside(int idx) {
-            int idx_x = idx / ${width};
-            int idx_y = idx % ${width};
+            // Fixed: Row-Major (Row=Y, Col=X)
+            // Row index (Y)
+            int idx_y = idx / ${width};
+            // Column index (X)
+            int idx_x = idx % ${width};
+            // Check Col bounds (Width)
             if (idx_x == 0 || idx_x == ${width} - 1) {
                 return false;
             }
+            // Check Row bounds (Height)
             if (idx_y == 0 || idx_y == ${height} - 1) {
                 return false;
             }
@@ -45,7 +50,8 @@ def map_utils(
         __device__ int get_idx(float16 x, float16 y, float16 center_x, float16 center_y) {
             int idx_x = clamp(get_x_idx(x, center_x), 0, ${width} - 1);
             int idx_y = clamp(get_y_idx(y, center_y), 0, ${height} - 1);
-            return ${width} * idx_x + idx_y;
+            // Fixed: Row-Major (Row=Y, Col=X)
+            return ${width} * idx_y + idx_x;
         }
         __device__ int get_map_idx(int idx, int layer_n) {
             const int layer = ${width} * ${height};
@@ -406,11 +412,16 @@ def dilation_filter_kernel(width, height, dilation_size):
                 return layer * layer_n + relative_idx;
             }
             __device__ bool is_inside(int idx) {
-                int idx_x = idx / ${width};
-                int idx_y = idx % ${width};
+                // Fixed: Row-Major (Row=Y, Col=X)
+                // Row index (Y)
+                int idx_y = idx / ${width};
+                // Column index (X)
+                int idx_x = idx % ${width};
+                // Check Col bounds (Width)
                 if (idx_x <= 0 || idx_x >= ${width} - 1) {
                     return false;
                 }
+                // Check Row bounds (Height)
                 if (idx_y <= 0 || idx_y >= ${height} - 1) {
                     return false;
                 }
@@ -466,11 +477,16 @@ def normal_filter_kernel(width, height, resolution):
                 return layer * layer_n + relative_idx;
             }
             __device__ bool is_inside(int idx) {
-                int idx_x = idx / ${width};
-                int idx_y = idx % ${width};
+                // Fixed: Row-Major (Row=Y, Col=X)
+                // Row index (Y)
+                int idx_y = idx / ${width};
+                // Column index (X)
+                int idx_x = idx % ${width};
+                // Check Col bounds (Width)
                 if (idx_x <= 0 || idx_x >= ${width} - 1) {
                     return false;
                 }
+                // Check Row bounds (Height)
                 if (idx_y <= 0 || idx_y >= ${height} - 1) {
                     return false;
                 }
@@ -491,8 +507,9 @@ def normal_filter_kernel(width, height, resolution):
                 if (!is_inside(idx_x) || !is_inside(idx_y)) { return; }
                 float dzdx = (map[idx_x] - h);
                 float dzdy = (map[idx_y] - h);
-                float nx = -dzdy / resolution();
-                float ny = -dzdx / resolution();
+                // Fixed: Normal = (-dH/dx, -dH/dy, 1)
+                float nx = -dzdx / resolution();
+                float ny = -dzdy / resolution();
                 float nz = 1;
                 float norm = sqrt((nx * nx) + (ny * ny) + 1);
                 newmap[get_map_idx(i, 0)] = nx / norm;
@@ -568,12 +585,14 @@ def polygon_mask_kernel(width, height, resolution):
             }
 
             __device__ int get_idx_x(int idx){
-                int idx_x = idx / ${width};
+                // Fixed: Column index (X)
+                int idx_x = idx % ${width};
                 return idx_x;
             }
 
             __device__ int get_idx_y(int idx){
-                int idx_y = idx % ${width};
+                // Fixed: Row index (Y)
+                int idx_y = idx / ${width};
                 return idx_y;
             }
 
@@ -599,7 +618,8 @@ def polygon_mask_kernel(width, height, resolution):
             __device__ int get_idx(float16 x, float16 y, float16 center_x, float16 center_y) {
                 int idx_x = clamp(get_x_idx(x, center_x), 0, ${width} - 1);
                 int idx_y = clamp(get_y_idx(y, center_y), 0, ${height} - 1);
-                return ${width} * idx_x + idx_y;
+                // Fixed: Row-Major (Row=Y, Col=X)
+                return ${width} * idx_y + idx_x;
             }
 
             """