Changed VoxelMapper class to use axis-aligned voxels and added more unit tests

orlando21lara · fcladera · commit bbe594999652 · 2022-07-12T23:06:56.000-04:00
diff --git a/autonomy_core/map_plan/mapper/CMakeLists.txt b/autonomy_core/map_plan/mapper/CMakeLists.txt
@@ -28,14 +28,34 @@ target_link_libraries(local_global_mapper PUBLIC ${PROJECT_NAME} Boost::timer)
 
 #----------------------------------------------------------------------------------
 # Add testing with GoogleTest
+
+# To run test:
+#   catkin test mapper
 enable_testing()
 
 catkin_add_gtest(test_voxel_mapper test/test_voxel_mapper.cpp)
 target_link_libraries(test_voxel_mapper ${catkin_LIBRARIES} ${PROJECT_NAME})
 
 #----------------------------------------------------------------------------------
 # Add microbenchmarking with Google Benchmark
-find_package(benchmark REQUIRED)
+
+# To run benchmark:
+#   rosrun mapper benchmarking
+# if you want to save the results to a file: 
+#   rosrun mapper benchmarking --benchmark_out=file_name.json
+# if you want to compare two benchmark files, use the compare.py script located
+# in the build directory under mapper/_deps/benchmark-src/tools/ (you will need
+# to have pandas installed). Also changed the shebang to use python3
+# e.g. from the catkin root directory run:
+#   ./build/mapper/_deps/benchmark-src/tools/compare.py benchmarks file1.json file2.json 
+
+include(FetchContent)
+FetchContent_Declare(
+    benchmark
+    URL https://github.com/google/benchmark/archive/b7afda2cd2d81230737caa1073e160b6406798d7.zip
+)
+FetchContent_MakeAvailable(benchmark)
+
 add_executable(benchmarking test/benchmark_voxel_mapper.cpp)
 target_link_libraries(benchmarking benchmark::benchmark ${PROJECT_NAME})
 
diff --git a/autonomy_core/map_plan/mapper/include/mapper/voxel_mapper.h b/autonomy_core/map_plan/mapper/include/mapper/voxel_mapper.h
@@ -19,7 +19,6 @@ class VoxelMapper {
   // access private methods and members
   friend class VoxelMapperTest;
   FRIEND_TEST(VoxelMapperTest, TestAllocateRelocate);
-  FRIEND_TEST(VoxelMapperTest, TestDecayLocalCloud);
 
  public:
   /**
@@ -37,6 +36,9 @@ class VoxelMapper {
               int8_t val = 0,
               int decay_times_to_empty = 0);
 
+  /// set the map with some predefined values
+  void setMap(const Eigen::Vector3d &ori, const Eigen::Vector3i &dim,
+              const std::vector<signed char> &map, double res);
   /// Set all voxels as unknown
   void setMapUnknown();
   /// Set all voxels as free
diff --git a/autonomy_core/map_plan/mapper/src/voxel_mapper.cpp b/autonomy_core/map_plan/mapper/src/voxel_mapper.cpp
@@ -36,6 +36,25 @@ VoxelMapper::VoxelMapper(const Eigen::Vector3d& origin,
   }
 }
 
+void VoxelMapper::setMap(const Eigen::Vector3d &ori, const Eigen::Vector3i &dim,
+                         const std::vector<signed char> &map, double res) {
+  dim_ = dim;
+  origin_d_ = ori;
+  res_ = res;
+
+  map_.resize(boost::extents[dim_(0)][dim_(1)][dim_(2)]);
+
+  Eigen::Vector3i n;
+  for (n(0) = 0; n(0) < dim_(0); n(0)++) {
+    for (n(1) = 0; n(1) < dim_(1); n(1)++) {
+      for (n(2) = 0; n(2) < dim_(2); n(2)++) {
+        int idx = n(0) + dim_(0) * n(1) + dim_(0) * dim_(1) * n(2);
+        map_[n(0)][n(1)][n(2)] = map[idx];
+      }
+    }
+  }
+}
+
 void VoxelMapper::setMapUnknown() {
   val_default = val_unknown;
   std::fill(map_.data(), map_.data() + map_.num_elements(), val_unknown);
@@ -108,13 +127,14 @@ vec_Vec3d VoxelMapper::getLocalCloud(const Eigen::Vector3d& pos,
   for (int i = 0; i < 3; i++) dim_low(i) = dim1(i) < 0 ? 0 : dim1(i);
 
   Eigen::Vector3i dim2 = floatToInt(pos + ori + dim);
-  for (int i = 0; i < 3; i++) dim_up(i) = dim2(i) > dim_(i) ? dim_(i) : dim2(i);
+  for (int i = 0; i < 3; i++)
+    dim_up(i) = dim2(i) >= dim_(i) ? dim_(i)-1 : dim2(i);
 
   vec_Vec3d pts;
   Eigen::Vector3i n;
-  for (n(0) = dim_low(0); n(0) < dim_up(0); n(0)++) {
-    for (n(1) = dim_low(1); n(1) < dim_up(1); n(1)++) {
-      for (n(2) = dim_low(2); n(2) < dim_up(2); n(2)++) {
+  for (n(0) = dim_low(0); n(0) <= dim_up(0); n(0)++) {
+    for (n(1) = dim_low(1); n(1) <= dim_up(1); n(1)++) {
+      for (n(2) = dim_low(2); n(2) <= dim_up(2); n(2)++) {
         if (map_[n(0)][n(1)][n(2)] > val_even) pts.push_back(intToFloat(n));
       }
     }
@@ -140,14 +160,15 @@ void VoxelMapper::decayLocalCloud(const Eigen::Vector3d& pos,
   for (int i = 0; i < 3; i++) dim_low(i) = dim1(i) < 0 ? 0 : dim1(i);
 
   Eigen::Vector3i dim2 = floatToInt(end_pos);
-  for (int i = 0; i < 3; i++) dim_up(i) = dim2(i) > dim_(i) ? dim_(i) : dim2(i);
+  for (int i = 0; i < 3; i++)
+    dim_up(i) = dim2(i) >= dim_(i) ? dim_(i)-1 : dim2(i);
 
   // Decaying voxels within robot's local region (voxels will disappear if
   // unobserved for (val_occ - val_even) / val_decay times)
   Eigen::Vector3i n;
-  for (n(0) = dim_low(0); n(0) < dim_up(0); n(0)++) {
-    for (n(1) = dim_low(1); n(1) < dim_up(1); n(1)++) {
-      for (n(2) = dim_low(2); n(2) < dim_up(2); n(2)++) {
+  for (n(0) = dim_low(0); n(0) <= dim_up(0); n(0)++) {
+    for (n(1) = dim_low(1); n(1) <= dim_up(1); n(1)++) {
+      for (n(2) = dim_low(2); n(2) <= dim_up(2); n(2)++) {
         if (map_[n(0)][n(1)][n(2)] > val_even)
           map_[n(0)][n(1)][n(2)] = map_[n(0)][n(1)][n(2)] - val_decay;
         if (inflated_map_[n(0)][n(1)][n(2)] > val_even)
@@ -169,13 +190,14 @@ vec_Vec3d VoxelMapper::getInflatedLocalCloud(const Eigen::Vector3d& pos,
   for (int i = 0; i < 3; i++) dim_low(i) = dim1(i) < 0 ? 0 : dim1(i);
 
   Eigen::Vector3i dim2 = floatToInt(pos + ori + dim);
-  for (int i = 0; i < 3; i++) dim_up(i) = dim2(i) > dim_(i) ? dim_(i) : dim2(i);
+  for (int i = 0; i < 3; i++)
+    dim_up(i) = dim2(i) >= dim_(i) ? dim_(i)-1 : dim2(i);
 
   vec_Vec3d pts;
   Eigen::Vector3i n;
-  for (n(0) = dim_low(0); n(0) < dim_up(0); n(0)++) {
-    for (n(1) = dim_low(1); n(1) < dim_up(1); n(1)++) {
-      for (n(2) = dim_low(2); n(2) < dim_up(2); n(2)++) {
+  for (n(0) = dim_low(0); n(0) <= dim_up(0); n(0)++) {
+    for (n(1) = dim_low(1); n(1) <= dim_up(1); n(1)++) {
+      for (n(2) = dim_low(2); n(2) <= dim_up(2); n(2)++) {
         if (inflated_map_[n(0)][n(1)][n(2)] > val_free)
           pts.push_back(intToFloat(n));
       }
@@ -494,7 +516,8 @@ Eigen::Vector3i VoxelMapper::floatToInt(const Eigen::Vector3d& pt) {
 }
 
 Eigen::Vector3d VoxelMapper::intToFloat(const Eigen::Vector3i& pn) {
-  return pn.cast<double>() * res_ + origin_d_;
+  return (pn.cast<double>() + Eigen::Vector3d(0.5, 0.5, 0.5)) * res_
+    + origin_d_;
 }
 
 bool VoxelMapper::isOutSide(const Eigen::Vector3i& pn) {
diff --git a/autonomy_core/map_plan/mapper/test/benchmark_voxel_mapper.cpp b/autonomy_core/map_plan/mapper/test/benchmark_voxel_mapper.cpp
@@ -6,7 +6,7 @@
  * fourth argument is the resolution multiplied by 100 because arguments are
  * signed long.
  */
-static void BM_GetMap(benchmark::State& state) {
+static void BM_GetInflatedMap(benchmark::State& state) {
   int x_dim = state.range(0);
   int y_dim = state.range(1);
   int z_dim = state.range(2);
@@ -23,15 +23,10 @@ static void BM_GetMap(benchmark::State& state) {
                                   decay_times);
 
   for (auto _ : state) {
-    test_mapper.getMap();
+    test_mapper.getInflatedMap();
   }
 }
 
-BENCHMARK(BM_GetMap)
-  ->Args({200, 200, 10, 50})
-  ->Args({200, 200, 10, 25})
-  ->Args({200, 200, 10, 10});
-
 
 /**
  * @brief Arguments represent the x-dim, y-dim, and z-dim size respectively. The
@@ -62,11 +57,6 @@ static void BM_DecayLocalCloud(benchmark::State& state) {
   }
 }
 
-BENCHMARK(BM_DecayLocalCloud)
-  ->Args({200, 200, 10, 50})
-  ->Args({200, 200, 10, 25})
-  ->Args({200, 200, 10, 10});
-
 
 /**
  * @brief Arguments represent the x-dim, y-dim, and z-dim size respectively. The
@@ -126,11 +116,6 @@ static void BM_AddCloud(benchmark::State& state) {
   }
 }
 
-BENCHMARK(BM_AddCloud)
-  ->Args({200, 200, 10, 50})
-  ->Args({200, 200, 10, 25})
-  ->Args({200, 200, 10, 10});
-
 
 /**
  * @brief Arguments represent the x-dim, y-dim, and z-dim size respectively. The
@@ -161,42 +146,28 @@ static void BM_GetInflatedLocalMap(benchmark::State& state) {
   }
 }
 
-BENCHMARK(BM_GetInflatedLocalMap)
+
+BENCHMARK(BM_GetInflatedMap)
   ->Args({200, 200, 10, 50})
   ->Args({200, 200, 10, 25})
   ->Args({200, 200, 10, 10});
 
+BENCHMARK(BM_DecayLocalCloud)
+  ->Args({200, 200, 10, 50})
+  ->Args({200, 200, 10, 25})
+  ->Args({200, 200, 10, 10});
 
-// class VoxelMapperBenchmark : public benchmark::Fixture {
-//  public:
-//   void SetUp(const ::benchmark::State& state) {
-//     int x_origin_ = -100;
-//     int y_origin_ = -100;
-//     int z_origin_ = -5;
-//     int x_dim_ = 200;
-//     int y_dim_ = 200;
-//     int z_dim_ = 10;
-//     double resolution_ = 0.5;
-//     int8_t val_default_ = 0;
-//     int decay_times_ = 30;
-//     Eigen::Vector3d origin(x_origin_, y_origin_, z_origin_);
-//     Eigen::Vector3d dimensions(x_dim_, y_dim_, z_dim_);
-//     p_test_mapper_.reset(new mapper::VoxelMapper(origin, dimensions,
-//                                         resolution_, val_default_,
-//                                         decay_times_));
-//   }
-
-//   std::unique_ptr<mapper::VoxelMapper> p_test_mapper_;  
+BENCHMARK(BM_AddCloud)
+  ->Args({200, 200, 10, 50})
+  ->Args({200, 200, 10, 25})
+  ->Args({200, 200, 10, 10});
 
-// };
+BENCHMARK(BM_GetInflatedLocalMap)
+  ->Args({200, 200, 10, 50})
+  ->Args({200, 200, 10, 25})
+  ->Args({200, 200, 10, 10});
 
-// BENCHMARK_DEFINE_F(VoxelMapperBenchmark, GetMapTest)(benchmark::State& st) {
-//   for (auto _ : st) {
-//     p_test_mapper_->getMap();
-//   }
-// }
 
-// BENCHMARK_REGISTER_F(VoxelMapperBenchmark, GetMapTest);
+BENCHMARK_MAIN();
 
 
-BENCHMARK_MAIN();
diff --git a/autonomy_core/map_plan/mapper/test/test_voxel_mapper.cpp b/autonomy_core/map_plan/mapper/test/test_voxel_mapper.cpp
