Merge pull request #6 from leggedrobotics/fix/cell_indexing

fix cell indexing

Author: mktk1117

elevation_mapping_cupy/script/custom_kernels.py

@@ -13,12 +13,12 @@ def map_utils(resolution, width, height, sensor_noise_factor, min_valid_distance
 
             return max(min(x, max_x), min_x);
         }
-        __device__ float16 round(float16 x) {
-            return (int)x + (int)(2 * (x - (int)x));
+        __device__ int get_x_idx(float16 x, float16 center) {
+            int i = (x - center) / ${resolution} + 0.5 * ${width};
+            return i;
         }
-        __device__ int get_xy_idx(float16 x, float16 center) {
-            const float resolution = ${resolution};
-            int i = round((x - center) / resolution);
+        __device__ int get_y_idx(float16 y, float16 center) {
+            int i = (y - center) / ${resolution} + 0.5 * ${height};
             return i;
         }
         __device__ bool is_inside(int idx) {
@@ -33,8 +33,8 @@ def map_utils(resolution, width, height, sensor_noise_factor, min_valid_distance
             return true;
         }
         __device__ int get_idx(float16 x, float16 y, float16 center_x, float16 center_y) {
-            int idx_x = clamp(get_xy_idx(x, center_x) + ${width} / 2, 0, ${width} - 1);
-            int idx_y = clamp(get_xy_idx(y, center_y) + ${height} / 2, 0, ${height} - 1);
+            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;
         }
         __device__ int get_map_idx(int idx, int layer_n) {
@@ -515,14 +515,21 @@ def polygon_mask_kernel(width, height, resolution):
             __device__ float16 round(float16 x) {
                 return (int)x + (int)(2 * (x - (int)x));
             }
-            __device__ int get_xy_idx(float16 x, float16 center) {
+            __device__ int get_x_idx(float16 x, float16 center) {
+                const float resolution = ${resolution};
+                const float width = ${width};
+                int i = (x - center) / resolution + 0.5 * width;
+                return i;
+            }
+            __device__ int get_y_idx(float16 y, float16 center) {
                 const float resolution = ${resolution};
-                int i = round((x - center) / resolution);
+                const float height = ${height};
+                int i = (y - center) / resolution + 0.5 * height;
                 return i;
             }
             __device__ int get_idx(float16 x, float16 y, float16 center_x, float16 center_y) {
-                int idx_x = clamp(get_xy_idx(x, center_x) + ${width} / 2, 0, ${width} - 1);
-                int idx_y = clamp(get_xy_idx(y, center_y) + ${height} / 2, 0, ${height} - 1);
+                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;
             }
 

elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp

@@ -77,6 +77,7 @@ void ElevationMappingWrapper::setParameters(ros::NodeHandle& nh) {
   resolution_ = py::cast<float>(param_.attr("get_value")("resolution"));
   map_length_ = py::cast<float>(param_.attr("get_value")("map_length"));
   map_n_ = static_cast<int>(round(map_length_ / resolution_));
+  map_length_ = resolution_ * map_n_;  // get true length after rounding
 
   nh.param<bool>("enable_normal", enable_normal_, false);
   nh.param<bool>("enable_normal_color", enable_normal_color_, false);