fix(lidar_centerpoint): reintroduce voxel order from front (#1745)

* Merge pull request #1688 from technolojin/feat/centerpoint/voxel-fill-from-front feat: optimize voxel indexing in preprocess_kernel.cu Signed-off-by: Taekjin LEE <[email protected]> * fix: clip the index in range Signed-off-by: Taekjin LEE <[email protected]> --------- Signed-off-by: Taekjin LEE <[email protected]> Co-authored-by: Yoshi Ri <[email protected]>
tier4 · Dec 26, 2024 · 60efdcc · 60efdcc
1 parent 2a3e190
commit 60efdcc
Showing 1 changed file with 11 additions and 7 deletions.
diff --git a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu
@@ -148,7 +148,9 @@ __global__ void generateVoxels_random_kernel(
 
   int voxel_idx = floorf((point.x - min_x_range) / pillar_x_size);
   int voxel_idy = floorf((point.y - min_y_range) / pillar_y_size);
-  unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx;
+  voxel_idx = voxel_idx < 0 ? 0 : voxel_idx >= grid_x_size ? grid_x_size - 1 : voxel_idx;
+  voxel_idy = voxel_idy < 0 ? 0 : voxel_idy >= grid_y_size ? grid_y_size - 1 : voxel_idy;
+  unsigned int voxel_index = (grid_x_size - 1 - voxel_idx) * grid_y_size + voxel_idy;
 
   unsigned int point_id = atomicAdd(&(mask[voxel_index]), 1);
 
@@ -185,12 +187,13 @@ __global__ void generateBaseFeatures_kernel(
   unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, int max_voxel_size,
   unsigned int * pillar_num, float * voxel_features, float * voxel_num, int * voxel_idxs)
 {
-  unsigned int voxel_idx = blockIdx.x * blockDim.x + threadIdx.x;
-  unsigned int voxel_idy = blockIdx.y * blockDim.y + threadIdx.y;
+  // flip x and y to process in a row-major order
+  unsigned int voxel_idx_inverted = blockIdx.y * blockDim.y + threadIdx.y;
+  unsigned int voxel_idy = blockIdx.x * blockDim.x + threadIdx.x;
+  if (voxel_idx_inverted >= grid_x_size || voxel_idy >= grid_y_size) return;
+  unsigned int voxel_idx = grid_x_size - 1 - voxel_idx_inverted;
 
-  if (voxel_idx >= grid_x_size || voxel_idy >= grid_y_size) return;
-
-  unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx;
+  unsigned int voxel_index = voxel_idx_inverted * grid_y_size + voxel_idy;
   unsigned int count = mask[voxel_index];
   if (!(count > 0)) return;
   count = count < MAX_POINT_IN_VOXEL_SIZE ? count : MAX_POINT_IN_VOXEL_SIZE;
@@ -220,9 +223,10 @@ cudaError_t generateBaseFeatures_launch(
   unsigned int * pillar_num, float * voxel_features, float * voxel_num, int * voxel_idxs,
   cudaStream_t stream)
 {
+  // flip x and y to process in a row-major order
   dim3 threads = {32, 32};
   dim3 blocks = {
-    (grid_x_size + threads.x - 1) / threads.x, (grid_y_size + threads.y - 1) / threads.y};
+    (grid_y_size + threads.x - 1) / threads.x, (grid_x_size + threads.y - 1) / threads.y};
 
   generateBaseFeatures_kernel<<<blocks, threads, 0, stream>>>(
     mask, voxels, grid_y_size, grid_x_size, max_voxel_size, pillar_num, voxel_features, voxel_num,