diff --git a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp
index 350b5f2a3c7..3da642262d0 100644
--- a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp
+++ b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp
@@ -267,7 +267,7 @@ class VoxelGrid
       int error_y = abs_dx / 2;
       int error_z = abs_dx / 2;
       // Since initial point has been updated above, subtracting min_length from the total length
-      length = (unsigned int)(scale * abs_dx) - abs(min_shift_x);
+      length = (unsigned int)abs(scale * (x1 - min_x0));
       bresenham3D(
         at, grid_off, grid_off, z_off, abs_dx, abs_dy, abs_dz, error_y, error_z,
         offset_dx, offset_dy, offset_dz, offset, z_mask, length);
@@ -279,7 +279,7 @@ class VoxelGrid
       int error_x = abs_dy / 2;
       int error_z = abs_dy / 2;
       // Since initial point has been updated above, subtracting min_length from the total length
-      length = (unsigned int)(scale * abs_dy) - abs(min_shift_y);
+      length = (unsigned int)abs(scale * (y1 - min_y0));
       bresenham3D(
         at, grid_off, grid_off, z_off, abs_dy, abs_dx, abs_dz, error_x, error_z,
         offset_dy, offset_dx, offset_dz, offset, z_mask, length);
@@ -290,7 +290,7 @@ class VoxelGrid
     int error_x = abs_dz / 2;
     int error_y = abs_dz / 2;
     // Since initial point has been updated above, subtracting min_length from the total length
-    length = (unsigned int)(scale * abs_dz) - abs(min_shift_z);
+    length = (unsigned int)abs(scale * (z1 - min_z0));
 
     bresenham3D(
       at, z_off, grid_off, grid_off, abs_dz, abs_dx, abs_dy, error_x, error_y, offset_dz,