Fix map-center indexing and add regression test

Author: Idate96

elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py

@@ -351,23 +351,20 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori
         points = points_all[:, :3]
 
         with self.map_lock:
-            self.shift_translation_to_map_center(t)
-            
-            # Log before kernel execution
-            
+            t = t.copy()
+            t[2] -= self.center[2]
             self.error_counting_kernel(
                 self.elevation_map,
                 points,
-                cp.array([0.0], dtype=self.data_type),
-                cp.array([0.0], dtype=self.data_type),
+                self.center[:1],
+                self.center[1:2],
                 R,
                 t,
                 self.new_map,
                 error,
                 error_cnt,
                 size=(points.shape[0]),
             )
-            
             if (
                 self.param.enable_drift_compensation
                 and error_cnt > self.param.min_height_drift_cnt
@@ -380,11 +377,9 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori
                 self.additive_mean_error += self.mean_error
                 if np.abs(self.mean_error) < self.param.max_drift:
                     self.elevation_map[0] += self.mean_error * self.param.drift_compensation_alpha
-
-            
             self.add_points_kernel(
-                cp.array([0.0], dtype=self.data_type),
-                cp.array([0.0], dtype=self.data_type),
+                self.center[:1],
+                self.center[1:2],
                 R,
                 t,
                 self.normal_map,
@@ -393,8 +388,6 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori
                 self.new_map,
                 size=(points.shape[0]),
             )
-            
-            # Log after adding points
 
             self.average_map_kernel(self.new_map, self.elevation_map, size=(self.cell_n * self.cell_n))
 
@@ -415,7 +408,6 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori
                 (traversability.shape[2], traversability.shape[3])
             )
 
-        # Log final state
         self.update_normal(self.traversability_input)
 
     def clear_overlap_map(self, t):

elevation_mapping_cupy/elevation_mapping_cupy/kernels/custom_kernels.py

@@ -24,11 +24,13 @@ def map_utils(
             return max(min(x, max_x), min_x);
         }
         __device__ int get_x_idx(float16 x, float16 center) {
-            int i = (x - center) / ${resolution} + 0.5 * ${width};
+            float fi = (x - center) / ${resolution} + 0.5 * (${width} - 1);
+            int i = (int)floorf(fi + 0.5f);
             return i;
         }
         __device__ int get_y_idx(float16 y, float16 center) {
-            int i = (y - center) / ${resolution} + 0.5 * ${height};
+            float fi = (y - center) / ${resolution} + 0.5 * (${height} - 1);
+            int i = (int)floorf(fi + 0.5f);
             return i;
         }
         __device__ bool is_inside(int idx) {

elevation_mapping_cupy/elevation_mapping_cupy/tests/test_indexing_center.py

@@ -0,0 +1,44 @@
+import cupy as cp
+
+from elevation_mapping_cupy.kernels.custom_kernels import map_utils
+
+
+def _probe_get_idx_kernel(axis: str, width: int, height: int, resolution: float):
+    if axis not in ("x", "y"):
+        raise ValueError(f"Unsupported axis '{axis}'. Expected 'x' or 'y'.")
+    op = "out[i] = get_x_idx(coords[i], center[0]);" if axis == "x" else "out[i] = get_y_idx(coords[i], center[0]);"
+    return cp.ElementwiseKernel(
+        in_params="raw U coords, raw U center",
+        out_params="raw int32 out",
+        preamble=map_utils(
+            resolution=resolution,
+            width=width,
+            height=height,
+            sensor_noise_factor=0.0,
+            min_valid_distance=0.0,
+            max_height_range=1000.0,
+            ramped_height_range_a=0.0,
+            ramped_height_range_b=0.0,
+            ramped_height_range_c=1000.0,
+        ),
+        operation=op,
+        name=f"probe_get_{axis}_idx_kernel",
+    )
+
+
+def test_get_x_idx_rounds_left_of_boundary_before_clamp():
+    kernel = _probe_get_idx_kernel(axis="x", width=200, height=200, resolution=1.0)
+    coords = cp.asarray([-100.2], dtype=cp.float32)
+    center = cp.asarray([0.0], dtype=cp.float32)
+    out = cp.zeros((1,), dtype=cp.int32)
+    kernel(coords, center, out, size=1)
+    assert int(out[0].item()) == -1
+
+
+def test_get_y_idx_at_center_matches_middle_cell():
+    kernel = _probe_get_idx_kernel(axis="y", width=200, height=200, resolution=1.0)
+    coords = cp.asarray([0.0], dtype=cp.float32)
+    center = cp.asarray([0.0], dtype=cp.float32)
+    out = cp.zeros((1,), dtype=cp.int32)
+    kernel(coords, center, out, size=1)
+    assert int(out[0].item()) == 100