Correctly use PrecomputationGridStack for 2D and 3D (#1040)

FIXES=#1038

cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.cc

@@ -23,6 +23,7 @@
 #include <limits>
 
 #include "Eigen/Geometry"
+#include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
 #include "cartographer/mapping/2d/probability_grid.h"
 #include "cartographer/sensor/point_cloud.h"
@@ -166,42 +167,30 @@ uint8 PrecomputationGrid2D::ComputeCellValue(const float probability) const {
   return cell_value;
 }
 
-class PrecomputationGridStack {
+PrecomputationGridStack2D::PrecomputationGridStack2D(
- public:
+    const ProbabilityGrid& probability_grid,
-  PrecomputationGridStack(
+    const proto::FastCorrelativeScanMatcherOptions2D& options) {
-      const ProbabilityGrid& probability_grid,
+  CHECK_GE(options.branch_and_bound_depth(), 1);
-      const proto::FastCorrelativeScanMatcherOptions2D& options) {
+  const int max_width = 1 << (options.branch_and_bound_depth() - 1);
-    CHECK_GE(options.branch_and_bound_depth(), 1);
+  precomputation_grids_.reserve(options.branch_and_bound_depth());
-    const int max_width = 1 << (options.branch_and_bound_depth() - 1);
+  std::vector<float> reusable_intermediate_grid;
-    precomputation_grids_.reserve(options.branch_and_bound_depth());
+  const CellLimits limits = probability_grid.limits().cell_limits();
-    std::vector<float> reusable_intermediate_grid;
+  reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) *
-    const CellLimits limits = probability_grid.limits().cell_limits();
+                                     limits.num_y_cells);
-    reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) *
+  for (int i = 0; i != options.branch_and_bound_depth(); ++i) {
-                                       limits.num_y_cells);
+    const int width = 1 << i;
-    for (int i = 0; i != options.branch_and_bound_depth(); ++i) {
+    precomputation_grids_.emplace_back(probability_grid, limits, width,
-      const int width = 1 << i;
+                                       &reusable_intermediate_grid);
-      precomputation_grids_.emplace_back(probability_grid, limits, width,
-                                         &reusable_intermediate_grid);
-    }
   }
-
+}
-  const PrecomputationGrid2D& Get(int index) {
-    return precomputation_grids_[index];
-  }
-
-  int max_depth() const { return precomputation_grids_.size() - 1; }
-
- private:
-  std::vector<PrecomputationGrid2D> precomputation_grids_;
-};
 
 FastCorrelativeScanMatcher2D::FastCorrelativeScanMatcher2D(
     const ProbabilityGrid& probability_grid,
     const proto::FastCorrelativeScanMatcherOptions2D& options)
     : options_(options),
       limits_(probability_grid.limits()),
-      precomputation_grid_stack_(
+      precomputation_grid_stack_(common::make_unique<PrecomputationGridStack2D>(
-          new PrecomputationGridStack(probability_grid, options)) {}
+          probability_grid, options)) {}
 
 FastCorrelativeScanMatcher2D::~FastCorrelativeScanMatcher2D() {}
 

cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h

@@ -92,7 +92,21 @@ class PrecomputationGrid2D {
   std::vector<uint8> cells_;
 };
 
-class PrecomputationGridStack;
+class PrecomputationGridStack2D {
+ public:
+  PrecomputationGridStack2D(
+      const ProbabilityGrid& probability_grid,
+      const proto::FastCorrelativeScanMatcherOptions2D& options);
+
+  const PrecomputationGrid2D& Get(int index) {
+    return precomputation_grids_[index];
+  }
+
+  int max_depth() const { return precomputation_grids_.size() - 1; }
+
+ private:
+  std::vector<PrecomputationGrid2D> precomputation_grids_;
+};
 
 // An implementation of "Real-Time Correlative Scan Matching" by Olson.
 class FastCorrelativeScanMatcher2D {
@@ -146,7 +160,7 @@ class FastCorrelativeScanMatcher2D {
 
   const proto::FastCorrelativeScanMatcherOptions2D options_;
   MapLimits limits_;
-  std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_;
+  std::unique_ptr<PrecomputationGridStack2D> precomputation_grid_stack_;
 };
 
 }  // namespace scan_matching

cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc

@@ -26,7 +26,6 @@
 #include "cartographer/common/math.h"
 #include "cartographer/mapping/3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.pb.h"
 #include "cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.h"
-#include "cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d.h"
 #include "cartographer/transform/transform.h"
 #include "glog/logging.h"
 
@@ -55,40 +54,27 @@ CreateFastCorrelativeScanMatcherOptions3D(
   return options;
 }
 
-class PrecomputationGridStack {
+PrecomputationGridStack3D::PrecomputationGridStack3D(
- public:
+    const HybridGrid& hybrid_grid,
-  PrecomputationGridStack(
+    const proto::FastCorrelativeScanMatcherOptions3D& options) {
-      const HybridGrid& hybrid_grid,
+  CHECK_GE(options.branch_and_bound_depth(), 1);
-      const proto::FastCorrelativeScanMatcherOptions3D& options) {
+  CHECK_GE(options.full_resolution_depth(), 1);
-    CHECK_GE(options.branch_and_bound_depth(), 1);
+  precomputation_grids_.reserve(options.branch_and_bound_depth());
-    CHECK_GE(options.full_resolution_depth(), 1);
+  precomputation_grids_.push_back(ConvertToPrecomputationGrid(hybrid_grid));
-    precomputation_grids_.reserve(options.branch_and_bound_depth());
+  Eigen::Array3i last_width = Eigen::Array3i::Ones();
-    precomputation_grids_.push_back(ConvertToPrecomputationGrid(hybrid_grid));
+  for (int depth = 1; depth != options.branch_and_bound_depth(); ++depth) {
-    Eigen::Array3i last_width = Eigen::Array3i::Ones();
+    const bool half_resolution = depth >= options.full_resolution_depth();
-    for (int depth = 1; depth != options.branch_and_bound_depth(); ++depth) {
+    const Eigen::Array3i next_width = ((1 << depth) * Eigen::Array3i::Ones());
-      const bool half_resolution = depth >= options.full_resolution_depth();
+    const int full_voxels_per_high_resolution_voxel =
-      const Eigen::Array3i next_width = ((1 << depth) * Eigen::Array3i::Ones());
+        1 << std::max(0, depth - options.full_resolution_depth());
-      const int full_voxels_per_high_resolution_voxel =
+    const Eigen::Array3i shift = (next_width - last_width +
-          1 << std::max(0, depth - options.full_resolution_depth());
+                                  (full_voxels_per_high_resolution_voxel - 1)) /
-      const Eigen::Array3i shift =
+                                 full_voxels_per_high_resolution_voxel;
-          (next_width - last_width +
+    precomputation_grids_.push_back(
-           (full_voxels_per_high_resolution_voxel - 1)) /
+        PrecomputeGrid(precomputation_grids_.back(), half_resolution, shift));
-          full_voxels_per_high_resolution_voxel;
+    last_width = next_width;
-      precomputation_grids_.push_back(
-          PrecomputeGrid(precomputation_grids_.back(), half_resolution, shift));
-      last_width = next_width;
-    }
   }
-
+}
-  const PrecomputationGrid3D& Get(int depth) const {
-    return precomputation_grids_.at(depth);
-  }
-
-  int max_depth() const { return precomputation_grids_.size() - 1; }
-
- private:
-  std::vector<PrecomputationGrid3D> precomputation_grids_;
-};
 
 struct DiscreteScan3D {
   transform::Rigid3f pose;
@@ -150,7 +136,7 @@ FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D(
       resolution_(hybrid_grid.resolution()),
       width_in_voxels_(hybrid_grid.grid_size()),
       precomputation_grid_stack_(
-          common::make_unique<PrecomputationGridStack>(hybrid_grid, options)),
+          common::make_unique<PrecomputationGridStack3D>(hybrid_grid, options)),
       low_resolution_hybrid_grid_(low_resolution_hybrid_grid),
       rotational_scan_matcher_(HistogramsAtAnglesFromNodes(nodes)) {}
 

cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h

@@ -28,6 +28,7 @@
 #include "cartographer/mapping/3d/hybrid_grid.h"
 #include "cartographer/mapping/3d/scan_matching/proto/fast_correlative_scan_matcher_options_3d.pb.h"
 #include "cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h"
+#include "cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d.h"
 #include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h"
 #include "cartographer/mapping/trajectory_node.h"
 #include "cartographer/sensor/point_cloud.h"
@@ -40,7 +41,22 @@ proto::FastCorrelativeScanMatcherOptions3D
 CreateFastCorrelativeScanMatcherOptions3D(
     common::LuaParameterDictionary* parameter_dictionary);
 
-class PrecomputationGridStack;
+class PrecomputationGridStack3D {
+ public:
+  PrecomputationGridStack3D(
+      const HybridGrid& hybrid_grid,
+      const proto::FastCorrelativeScanMatcherOptions3D& options);
+
+  const PrecomputationGrid3D& Get(int depth) const {
+    return precomputation_grids_.at(depth);
+  }
+
+  int max_depth() const { return precomputation_grids_.size() - 1; }
+
+ private:
+  std::vector<PrecomputationGrid3D> precomputation_grids_;
+};
+
 struct DiscreteScan3D;
 struct Candidate3D;
 
@@ -129,7 +145,7 @@ class FastCorrelativeScanMatcher3D {
   const proto::FastCorrelativeScanMatcherOptions3D options_;
   const float resolution_;
   const int width_in_voxels_;
-  std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_;
+  std::unique_ptr<PrecomputationGridStack3D> precomputation_grid_stack_;
   const HybridGrid* const low_resolution_hybrid_grid_;
   RotationalScanMatcher rotational_scan_matcher_;
 };