Fix map-center indexing and add regression tests

Author: Idate96

elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py

@@ -330,12 +330,13 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori
         points = points_all[:, :3]
         # additional_fusion = self.get_fusion_of_pcl(channels)
         with self.map_lock:
-            self.shift_translation_to_map_center(t)
+            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,
@@ -356,8 +357,8 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori
                 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,

elevation_mapping_cupy/script/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/script/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