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,9 +167,7 @@ uint8 PrecomputationGrid2D::ComputeCellValue(const float probability) const {
   return cell_value;
 }
 
-class PrecomputationGridStack {
+PrecomputationGridStack2D::PrecomputationGridStack2D(
- public:
-  PrecomputationGridStack(
     const ProbabilityGrid& probability_grid,
     const proto::FastCorrelativeScanMatcherOptions2D& options) {
   CHECK_GE(options.branch_and_bound_depth(), 1);
@@ -185,23 +184,13 @@ class PrecomputationGridStack {
   }
 }
 
-  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,9 +54,7 @@ CreateFastCorrelativeScanMatcherOptions3D(
   return options;
 }
 
-class PrecomputationGridStack {
+PrecomputationGridStack3D::PrecomputationGridStack3D(
- public:
-  PrecomputationGridStack(
     const HybridGrid& hybrid_grid,
     const proto::FastCorrelativeScanMatcherOptions3D& options) {
   CHECK_GE(options.branch_and_bound_depth(), 1);
@@ -70,8 +67,7 @@ class PrecomputationGridStack {
     const Eigen::Array3i next_width = ((1 << depth) * Eigen::Array3i::Ones());
     const int full_voxels_per_high_resolution_voxel =
         1 << std::max(0, depth - options.full_resolution_depth());
-      const Eigen::Array3i shift =
+    const Eigen::Array3i shift = (next_width - last_width +
-          (next_width - last_width +
                                   (full_voxels_per_high_resolution_voxel - 1)) /
                                  full_voxels_per_high_resolution_voxel;
     precomputation_grids_.push_back(
@@ -80,16 +76,6 @@ class PrecomputationGridStack {
   }
 }
 
-  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;
   // Contains a vector of discretized scans for each 'depth'.
@@ -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_;
 };