Fix for voxel grid out-of-range issues due to loss of precision during floating-point calculations

nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp

@@ -226,18 +226,6 @@ class VoxelGrid
     double x1, double y1, double z1, unsigned int max_length = UINT_MAX,
     unsigned int min_length = 0)
   {
-    int dx = int(x1) - int(x0);  // NOLINT
-    int dy = int(y1) - int(y0);  // NOLINT
-    int dz = int(z1) - int(z0);  // NOLINT
-
-    unsigned int abs_dx = abs(dx);
-    unsigned int abs_dy = abs(dy);
-    unsigned int abs_dz = abs(dz);
-
-    int offset_dx = sign(dx);
-    int offset_dy = sign(dy) * size_x_;
-    int offset_dz = sign(dz);
-
     // we need to chose how much to scale our dominant dimension, based on the
     // maximum length of the line
     double dist = sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1) + (z0 - z1) * (z0 - z1));
@@ -247,51 +235,57 @@ class VoxelGrid
     double scale = std::min(1.0, max_length / dist);
 
     // Updating starting point to the point at distance min_length from the initial point
-    double min_x0 = x0 + dx / dist * min_length;
-    double min_y0 = y0 + dy / dist * min_length;
-    double min_z0 = z0 + dz / dist * min_length;
+    double min_x0 = x0 + (x1 - x0) / dist * min_length;
+    double min_y0 = y0 + (y1 - y0) / dist * min_length;
+    double min_z0 = z0 + (z1 - z0) / dist * min_length;
 
+    int dx = int(x1) - int(min_x0);  // NOLINT
+    int dy = int(y1) - int(min_y0);  // NOLINT
+    int dz = int(z1) - int(min_z0);  // NOLINT
+
+    unsigned int abs_dx = abs(dx);
+    unsigned int abs_dy = abs(dy);
+    unsigned int abs_dz = abs(dz);
+
+    int offset_dx = sign(dx);
+    int offset_dy = sign(dy) * size_x_;
+    int offset_dz = sign(dz);
 
     unsigned int z_mask = ((1 << 16) | 1) << (unsigned int)min_z0;
     unsigned int offset = (unsigned int)min_y0 * size_x_ + (unsigned int)min_x0;
 
     GridOffset grid_off(offset);
     ZOffset z_off(z_mask);
 
-    unsigned int length = 0;
     // is x dominant
     if (abs_dx >= max(abs_dy, abs_dz)) {
       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) - min_length;
+
       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);
+        offset_dx, offset_dy, offset_dz, offset, z_mask, (unsigned int)(scale * abs_dx));
       return;
     }
 
     // y is dominant
     if (abs_dy >= abs_dz) {
       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) - min_length;
+
       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);
+        offset_dy, offset_dx, offset_dz, offset, z_mask, (unsigned int)(scale * abs_dy));
       return;
     }
 
     // otherwise, z is dominant
     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) - min_length;
 
     bresenham3D(
       at, z_off, grid_off, grid_off, abs_dz, abs_dx, abs_dy, error_x, error_y, offset_dz,
-      offset_dx, offset_dy, offset, z_mask, length);
+      offset_dx, offset_dy, offset, z_mask, (unsigned int)(scale * abs_dz));
   }
 
 private:

nav2_voxel_grid/test/CMakeLists.txt

@@ -1,2 +1,5 @@
 ament_add_gtest(voxel_grid_tests voxel_grid_tests.cpp)
 target_link_libraries(voxel_grid_tests voxel_grid)
+
+ament_add_gtest(voxel_grid_bresenham_3d voxel_grid_bresenham_3d.cpp)
+target_link_libraries(voxel_grid_bresenham_3d voxel_grid)

nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp

@@ -0,0 +1,129 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2021 Samsung Research Russia
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Alexey Merzlyakov
+*********************************************************************/
+#include <nav2_voxel_grid/voxel_grid.hpp>
+#include <gtest/gtest.h>
+
+class TestVoxel
+{
+public:
+  explicit TestVoxel(uint32_t * data, int sz_x, int sz_y)
+  : data_(data)
+  {
+    size_ = sz_x * sz_y;
+  }
+  inline void operator()(unsigned int off, unsigned int val)
+  {
+    ASSERT_TRUE(off < size_);
+    data_[off] = val;
+  }
+
+private:
+  uint32_t * data_;
+  unsigned int size_;
+};
+
+TEST(voxel_grid, bresenham3DBoundariesCheck)
+{
+  const int sz_x = 60;
+  const int sz_y = 60;
+  const int sz_z = 2;
+  const unsigned int max_length = 60;
+  const unsigned int min_length = 6;
+  nav2_voxel_grid::VoxelGrid vg(sz_x, sz_y, sz_z);
+  TestVoxel tv(vg.getData(), sz_x, sz_y);
+
+  // Initial point - some assymetrically standing point in order to cover most corner cases
+  const double x0 = 2.2;
+  const double y0 = 3.8;
+  const double z0 = 0.4;
+  // z-axis won't be domimant
+  const double z1 = 0.5;
+  // (x1, y1) point will move
+  double x1, y1;
+
+  // Epsilon for outer boundaries of voxel grid array
+  const double epsilon = 0.02;
+
+  // Running on (x, 0) edge
+  y1 = 0.0;
+  for (int i = 0; i <= sz_x; i++) {
+    if (i != sz_x) {
+      x1 = i;
+    } else {
+      x1 = i - epsilon;
+    }
+    vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length);
+  }
+
+  // Running on (x, sz_y) edge
+  y1 = sz_y - epsilon;
+  for (int i = 0; i <= sz_x; i++) {
+    if (i != sz_x) {
+      x1 = i;
+    } else {
+      x1 = i - epsilon;
+    }
+    vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length);
+  }
+
+  // Running on (0, y) edge
+  x1 = 0.0;
+  for (int j = 0; j <= sz_y; j++) {
+    if (j != sz_y) {
+      y1 = j;
+    } else {
+      y1 = j - epsilon;
+    }
+    vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length);
+  }
+
+  // Running on (sz_x, y) edge
+  x1 = sz_x - epsilon;
+  for (int j = 0; j <= sz_y; j++) {
+    if (j != sz_y) {
+      y1 = j;
+    } else {
+      y1 = j - epsilon;
+    }
+    vg.raytraceLine(tv, x0, y0, z0, x1, y1, z1, max_length, min_length);
+  }
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}