Move implementation of ProbabilityGrid to .cc file. (#924)

cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h

@@ -19,6 +19,7 @@
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
+#include "cartographer/mapping/probability_values.h"
 #include "cartographer/mapping_2d/probability_grid.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "ceres/ceres.h"

cartographer/io/probability_grid_points_processor.cc

@@ -23,6 +23,7 @@
 #include "cartographer/io/draw_trajectories.h"
 #include "cartographer/io/image.h"
 #include "cartographer/io/points_batch.h"
+#include "cartographer/mapping/probability_values.h"
 
 namespace cartographer {
 namespace io {

cartographer/mapping_2d/probability_grid.cc

@@ -0,0 +1,164 @@
+#include "cartographer/mapping_2d/probability_grid.h"
+
+#include <limits>
+
+#include "cartographer/mapping/probability_values.h"
+
+namespace cartographer {
+namespace mapping_2d {
+namespace {
+
+// Converts a 'cell_index' into an index into 'cells_'.
+int ToFlatIndex(const Eigen::Array2i& cell_index, const MapLimits& limits) {
+  CHECK(limits.Contains(cell_index)) << cell_index;
+  return limits.cell_limits().num_x_cells * cell_index.y() + cell_index.x();
+}
+
+}  // namespace
+
+ProbabilityGrid::ProbabilityGrid(const MapLimits& limits)
+    : limits_(limits),
+      cells_(
+          limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
+          mapping::kUnknownProbabilityValue) {}
+
+ProbabilityGrid::ProbabilityGrid(const proto::ProbabilityGrid& proto)
+    : limits_(proto.limits()), cells_() {
+  if (proto.has_known_cells_box()) {
+    const auto& box = proto.known_cells_box();
+    known_cells_box_ =
+        Eigen::AlignedBox2i(Eigen::Vector2i(box.min_x(), box.min_y()),
+                            Eigen::Vector2i(box.max_x(), box.max_y()));
+  }
+  cells_.reserve(proto.cells_size());
+  for (const auto& cell : proto.cells()) {
+    CHECK_LE(cell, std::numeric_limits<uint16>::max());
+    cells_.push_back(cell);
+  }
+}
+
+// Finishes the update sequence.
+void ProbabilityGrid::FinishUpdate() {
+  while (!update_indices_.empty()) {
+    DCHECK_GE(cells_[update_indices_.back()], mapping::kUpdateMarker);
+    cells_[update_indices_.back()] -= mapping::kUpdateMarker;
+    update_indices_.pop_back();
+  }
+}
+
+// Sets the probability of the cell at 'cell_index' to the given
+// 'probability'. Only allowed if the cell was unknown before.
+void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,
+                                     const float probability) {
+  uint16& cell = cells_[ToFlatIndex(cell_index, limits_)];
+  CHECK_EQ(cell, mapping::kUnknownProbabilityValue);
+  cell = mapping::ProbabilityToValue(probability);
+  known_cells_box_.extend(cell_index.matrix());
+}
+
+// Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
+// to the probability of the cell at 'cell_index' if the cell has not already
+// been updated. Multiple updates of the same cell will be ignored until
+// FinishUpdate() is called. Returns true if the cell was updated.
+//
+// If this is the first call to ApplyOdds() for the specified cell, its value
+// will be set to probability corresponding to 'odds'.
+bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
+                                       const std::vector<uint16>& table) {
+  DCHECK_EQ(table.size(), mapping::kUpdateMarker);
+  const int flat_index = ToFlatIndex(cell_index, limits_);
+  uint16* cell = &cells_[flat_index];
+  if (*cell >= mapping::kUpdateMarker) {
+    return false;
+  }
+  update_indices_.push_back(flat_index);
+  *cell = table[*cell];
+  DCHECK_GE(*cell, mapping::kUpdateMarker);
+  known_cells_box_.extend(cell_index.matrix());
+  return true;
+}
+
+// Returns the probability of the cell with 'cell_index'.
+float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const {
+  if (limits_.Contains(cell_index)) {
+    return mapping::ValueToProbability(
+        cells_[ToFlatIndex(cell_index, limits_)]);
+  }
+  return mapping::kMinProbability;
+}
+
+// Returns true if the probability at the specified index is known.
+bool ProbabilityGrid::IsKnown(const Eigen::Array2i& cell_index) const {
+  return limits_.Contains(cell_index) &&
+         cells_[ToFlatIndex(cell_index, limits_)] !=
+             mapping::kUnknownProbabilityValue;
+}
+
+// Fills in 'offset' and 'limits' to define a subregion of that contains all
+// known cells.
+void ProbabilityGrid::ComputeCroppedLimits(Eigen::Array2i* const offset,
+                                           CellLimits* const limits) const {
+  if (known_cells_box_.isEmpty()) {
+    *offset = Eigen::Array2i::Zero();
+    *limits = CellLimits(1, 1);
+    return;
+  }
+  *offset = known_cells_box_.min().array();
+  *limits = CellLimits(known_cells_box_.sizes().x() + 1,
+                       known_cells_box_.sizes().y() + 1);
+}
+
+// Grows the map as necessary to include 'point'. This changes the meaning of
+// these coordinates going forward. This method must be called immediately
+// after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
+void ProbabilityGrid::GrowLimits(const Eigen::Vector2f& point) {
+  CHECK(update_indices_.empty());
+  while (!limits_.Contains(limits_.GetCellIndex(point))) {
+    const int x_offset = limits_.cell_limits().num_x_cells / 2;
+    const int y_offset = limits_.cell_limits().num_y_cells / 2;
+    const MapLimits new_limits(
+        limits_.resolution(),
+        limits_.max() +
+            limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
+        CellLimits(2 * limits_.cell_limits().num_x_cells,
+                   2 * limits_.cell_limits().num_y_cells));
+    const int stride = new_limits.cell_limits().num_x_cells;
+    const int offset = x_offset + stride * y_offset;
+    const int new_size = new_limits.cell_limits().num_x_cells *
+                         new_limits.cell_limits().num_y_cells;
+    std::vector<uint16> new_cells(new_size, mapping::kUnknownProbabilityValue);
+    for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
+      for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
+        new_cells[offset + j + i * stride] =
+            cells_[j + i * limits_.cell_limits().num_x_cells];
+      }
+    }
+    cells_ = new_cells;
+    limits_ = new_limits;
+    if (!known_cells_box_.isEmpty()) {
+      known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
+    }
+  }
+}
+
+proto::ProbabilityGrid ProbabilityGrid::ToProto() const {
+  proto::ProbabilityGrid result;
+  *result.mutable_limits() = cartographer::mapping_2d::ToProto(limits_);
+  result.mutable_cells()->Reserve(cells_.size());
+  for (const auto& cell : cells_) {
+    result.mutable_cells()->Add(cell);
+  }
+  CHECK(update_indices_.empty()) << "Serializing a grid during an update is "
+                                    "not supported. Finish the update first.";
+  if (!known_cells_box_.isEmpty()) {
+    auto* const box = result.mutable_known_cells_box();
+    box->set_max_x(known_cells_box_.max().x());
+    box->set_max_y(known_cells_box_.max().y());
+    box->set_min_x(known_cells_box_.min().x());
+    box->set_min_y(known_cells_box_.min().y());
+  }
+  return result;
+}
+
+}  // namespace mapping_2d
+}  // namespace cartographer

cartographer/mapping_2d/probability_grid.h

@@ -17,21 +17,12 @@
 #ifndef CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_
 #define CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_
 
-#include <algorithm>
-#include <cmath>
-#include <iostream>
-#include <limits>
-#include <memory>
-#include <string>
 #include <vector>
 
-#include "cartographer/common/math.h"
 #include "cartographer/common/port.h"
-#include "cartographer/mapping/probability_values.h"
 #include "cartographer/mapping_2d/map_limits.h"
 #include "cartographer/mapping_2d/proto/probability_grid.pb.h"
 #include "cartographer/mapping_2d/xy_index.h"
-#include "glog/logging.h"
 
 namespace cartographer {
 namespace mapping_2d {
@@ -39,48 +30,19 @@ namespace mapping_2d {
 // Represents a 2D grid of probabilities.
 class ProbabilityGrid {
  public:
-  explicit ProbabilityGrid(const MapLimits& limits)
+  explicit ProbabilityGrid(const MapLimits& limits);
-      : limits_(limits),
+  explicit ProbabilityGrid(const proto::ProbabilityGrid& proto);
-        cells_(limits_.cell_limits().num_x_cells *
-                   limits_.cell_limits().num_y_cells,
-               mapping::kUnknownProbabilityValue) {}
-
-  explicit ProbabilityGrid(const proto::ProbabilityGrid& proto)
-      : limits_(proto.limits()), cells_() {
-    if (proto.has_known_cells_box()) {
-      const auto& box = proto.known_cells_box();
-      known_cells_box_ =
-          Eigen::AlignedBox2i(Eigen::Vector2i(box.min_x(), box.min_y()),
-                              Eigen::Vector2i(box.max_x(), box.max_y()));
-    }
-    cells_.reserve(proto.cells_size());
-    for (const auto cell : proto.cells()) {
-      CHECK_LE(cell, std::numeric_limits<uint16>::max());
-      cells_.push_back(cell);
-    }
-  }
 
   // Returns the limits of this ProbabilityGrid.
   const MapLimits& limits() const { return limits_; }
 
   // Finishes the update sequence.
-  void FinishUpdate() {
+  void FinishUpdate();
-    while (!update_indices_.empty()) {
-      DCHECK_GE(cells_[update_indices_.back()], mapping::kUpdateMarker);
-      cells_[update_indices_.back()] -= mapping::kUpdateMarker;
-      update_indices_.pop_back();
-    }
-  }
 
   // Sets the probability of the cell at 'cell_index' to the given
   // 'probability'. Only allowed if the cell was unknown before.
   void SetProbability(const Eigen::Array2i& cell_index,
-                      const float probability) {
+                      const float probability);
-    uint16& cell = cells_[ToFlatIndex(cell_index)];
-    CHECK_EQ(cell, mapping::kUnknownProbabilityValue);
-    cell = mapping::ProbabilityToValue(probability);
-    known_cells_box_.extend(cell_index.matrix());
-  }
 
   // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
   // to the probability of the cell at 'cell_index' if the cell has not already
@@ -90,108 +52,27 @@ class ProbabilityGrid {
   // If this is the first call to ApplyOdds() for the specified cell, its value
   // will be set to probability corresponding to 'odds'.
   bool ApplyLookupTable(const Eigen::Array2i& cell_index,
-                        const std::vector<uint16>& table) {
+                        const std::vector<uint16>& table);
-    DCHECK_EQ(table.size(), mapping::kUpdateMarker);
-    const int flat_index = ToFlatIndex(cell_index);
-    uint16& cell = cells_[flat_index];
-    if (cell >= mapping::kUpdateMarker) {
-      return false;
-    }
-    update_indices_.push_back(flat_index);
-    cell = table[cell];
-    DCHECK_GE(cell, mapping::kUpdateMarker);
-    known_cells_box_.extend(cell_index.matrix());
-    return true;
-  }
 
   // Returns the probability of the cell with 'cell_index'.
-  float GetProbability(const Eigen::Array2i& cell_index) const {
+  float GetProbability(const Eigen::Array2i& cell_index) const;
-    if (limits_.Contains(cell_index)) {
-      return mapping::ValueToProbability(cells_[ToFlatIndex(cell_index)]);
-    }
-    return mapping::kMinProbability;
-  }
 
   // Returns true if the probability at the specified index is known.
-  bool IsKnown(const Eigen::Array2i& cell_index) const {
+  bool IsKnown(const Eigen::Array2i& cell_index) const;
-    return limits_.Contains(cell_index) &&
-           cells_[ToFlatIndex(cell_index)] != mapping::kUnknownProbabilityValue;
-  }
 
   // Fills in 'offset' and 'limits' to define a subregion of that contains all
   // known cells.
   void ComputeCroppedLimits(Eigen::Array2i* const offset,
-                            CellLimits* const limits) const {
+                            CellLimits* const limits) const;
-    if (known_cells_box_.isEmpty()) {
-      *offset = Eigen::Array2i::Zero();
-      *limits = CellLimits(1, 1);
-    } else {
-      *offset = known_cells_box_.min().array();
-      *limits = CellLimits(known_cells_box_.sizes().x() + 1,
-                           known_cells_box_.sizes().y() + 1);
-    }
-  }
 
   // Grows the map as necessary to include 'point'. This changes the meaning of
   // these coordinates going forward. This method must be called immediately
   // after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
-  void GrowLimits(const Eigen::Vector2f& point) {
+  void GrowLimits(const Eigen::Vector2f& point);
-    CHECK(update_indices_.empty());
-    while (!limits_.Contains(limits_.GetCellIndex(point))) {
-      const int x_offset = limits_.cell_limits().num_x_cells / 2;
-      const int y_offset = limits_.cell_limits().num_y_cells / 2;
-      const MapLimits new_limits(
-          limits_.resolution(),
-          limits_.max() +
-              limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
-          CellLimits(2 * limits_.cell_limits().num_x_cells,
-                     2 * limits_.cell_limits().num_y_cells));
-      const int stride = new_limits.cell_limits().num_x_cells;
-      const int offset = x_offset + stride * y_offset;
-      const int new_size = new_limits.cell_limits().num_x_cells *
-                           new_limits.cell_limits().num_y_cells;
-      std::vector<uint16> new_cells(new_size,
-                                    mapping::kUnknownProbabilityValue);
-      for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
-        for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
-          new_cells[offset + j + i * stride] =
-              cells_[j + i * limits_.cell_limits().num_x_cells];
-        }
-      }
-      cells_ = new_cells;
-      limits_ = new_limits;
-      if (!known_cells_box_.isEmpty()) {
-        known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
-      }
-    }
-  }
 
-  proto::ProbabilityGrid ToProto() const {
+  proto::ProbabilityGrid ToProto() const;
-    proto::ProbabilityGrid result;
-    *result.mutable_limits() = cartographer::mapping_2d::ToProto(limits_);
-    result.mutable_cells()->Reserve(cells_.size());
-    for (const auto cell : cells_) {
-      result.mutable_cells()->Add(cell);
-    }
-    CHECK(update_indices_.empty()) << "Serializing a grid during an update is "
-                                      "not supported. Finish the update first.";
-    if (!known_cells_box_.isEmpty()) {
-      auto* const box = result.mutable_known_cells_box();
-      box->set_max_x(known_cells_box_.max().x());
-      box->set_max_y(known_cells_box_.max().y());
-      box->set_min_x(known_cells_box_.min().x());
-      box->set_min_y(known_cells_box_.min().y());
-    }
-    return result;
-  }
 
  private:
-  // Converts a 'cell_index' into an index into 'cells_'.
-  int ToFlatIndex(const Eigen::Array2i& cell_index) const {
-    CHECK(limits_.Contains(cell_index)) << cell_index;
-    return limits_.cell_limits().num_x_cells * cell_index.y() + cell_index.x();
-  }
-
   MapLimits limits_;
   std::vector<uint16> cells_;  // Highest bit is update marker.
   std::vector<int> update_indices_;

cartographer/mapping_2d/probability_grid_test.cc

@@ -17,14 +17,17 @@
 #include "cartographer/mapping_2d/probability_grid.h"
 
 #include <random>
-#include <vector>
 
+#include "cartographer/mapping/probability_values.h"
 #include "gtest/gtest.h"
 
 namespace cartographer {
 namespace mapping_2d {
 namespace {
 
+using Eigen::Array2i;
+using Eigen::Vector2f;
+
 TEST(ProbabilityGridTest, ProtoConstructor) {
   proto::ProbabilityGrid proto;
   const MapLimits limits(1., {2., 3.}, CellLimits(4., 5.));
@@ -61,49 +64,47 @@ TEST(ProbabilityGridTest, ApplyOdds) {
       MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)));
   const MapLimits& limits = probability_grid.limits();
 
-  EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 0)));
+  EXPECT_TRUE(limits.Contains(Array2i(0, 0)));
-  EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 1)));
+  EXPECT_TRUE(limits.Contains(Array2i(0, 1)));
-  EXPECT_TRUE(limits.Contains(Eigen::Array2i(1, 0)));
+  EXPECT_TRUE(limits.Contains(Array2i(1, 0)));
-  EXPECT_TRUE(limits.Contains(Eigen::Array2i(1, 1)));
+  EXPECT_TRUE(limits.Contains(Array2i(1, 1)));
-  EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(0, 0)));
+  EXPECT_FALSE(probability_grid.IsKnown(Array2i(0, 0)));
-  EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(0, 1)));
+  EXPECT_FALSE(probability_grid.IsKnown(Array2i(0, 1)));
-  EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(1, 0)));
+  EXPECT_FALSE(probability_grid.IsKnown(Array2i(1, 0)));
-  EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(1, 1)));
+  EXPECT_FALSE(probability_grid.IsKnown(Array2i(1, 1)));
 
-  probability_grid.SetProbability(Eigen::Array2i(1, 0), 0.5);
+  probability_grid.SetProbability(Array2i(1, 0), 0.5);
 
   probability_grid.ApplyLookupTable(
-      Eigen::Array2i(1, 0),
+      Array2i(1, 0),
       mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
   probability_grid.FinishUpdate();
-  EXPECT_GT(probability_grid.GetProbability(Eigen::Array2i(1, 0)), 0.5);
+  EXPECT_GT(probability_grid.GetProbability(Array2i(1, 0)), 0.5);
 
-  probability_grid.SetProbability(Eigen::Array2i(0, 1), 0.5);
+  probability_grid.SetProbability(Array2i(0, 1), 0.5);
 
   probability_grid.ApplyLookupTable(
-      Eigen::Array2i(0, 1),
+      Array2i(0, 1),
       mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1)));
   probability_grid.FinishUpdate();
-  EXPECT_LT(probability_grid.GetProbability(Eigen::Array2i(0, 1)), 0.5);
+  EXPECT_LT(probability_grid.GetProbability(Array2i(0, 1)), 0.5);
 
   // Tests adding odds to an unknown cell.
   probability_grid.ApplyLookupTable(
-      Eigen::Array2i(1, 1),
+      Array2i(1, 1),
       mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42)));
-  EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42,
+  EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4);
-              1e-4);
 
   // Tests that further updates are ignored if FinishUpdate() isn't called.
   probability_grid.ApplyLookupTable(
-      Eigen::Array2i(1, 1),
+      Array2i(1, 1),
       mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
-  EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42,
+  EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4);
-              1e-4);
   probability_grid.FinishUpdate();
   probability_grid.ApplyLookupTable(
-      Eigen::Array2i(1, 1),
+      Array2i(1, 1),
       mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
-  EXPECT_GT(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42);
+  EXPECT_GT(probability_grid.GetProbability(Array2i(1, 1)), 0.42);
 }
 
 TEST(ProbabilityGridTest, GetProbability) {
@@ -118,16 +119,14 @@ TEST(ProbabilityGridTest, GetProbability) {
   ASSERT_EQ(2, cell_limits.num_x_cells);
   ASSERT_EQ(2, cell_limits.num_y_cells);
 
-  probability_grid.SetProbability(
+  probability_grid.SetProbability(limits.GetCellIndex(Vector2f(-0.5f, 0.5f)),
-      limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f)),
                                   mapping::kMaxProbability);
   EXPECT_NEAR(probability_grid.GetProbability(
-                  limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f))),
+                  limits.GetCellIndex(Vector2f(-0.5f, 0.5f))),
               mapping::kMaxProbability, 1e-6);
-  for (const Eigen::Array2i& xy_index :
+  for (const Array2i& xy_index : {limits.GetCellIndex(Vector2f(-0.5f, 1.5f)),
-       {limits.GetCellIndex(Eigen::Vector2f(-0.5f, 1.5f)),
+                                  limits.GetCellIndex(Vector2f(0.5f, 0.5f)),
-        limits.GetCellIndex(Eigen::Vector2f(0.5f, 0.5f)),
+                                  limits.GetCellIndex(Vector2f(0.5f, 1.5f))}) {
-        limits.GetCellIndex(Eigen::Vector2f(0.5f, 1.5f))}) {
     EXPECT_TRUE(limits.Contains(xy_index));
     EXPECT_FALSE(probability_grid.IsKnown(xy_index));
   }
@@ -142,34 +141,25 @@ TEST(ProbabilityGridTest, GetCellIndex) {
   ASSERT_EQ(14, cell_limits.num_x_cells);
   ASSERT_EQ(8, cell_limits.num_y_cells);
   EXPECT_TRUE(
-      (Eigen::Array2i(0, 0) == limits.GetCellIndex(Eigen::Vector2f(7.f, 13.f)))
+      (Array2i(0, 0) == limits.GetCellIndex(Vector2f(7.f, 13.f))).all());
-          .all());
-  EXPECT_TRUE((Eigen::Array2i(13, 0) ==
-               limits.GetCellIndex(Eigen::Vector2f(7.f, -13.f)))
-                  .all());
   EXPECT_TRUE(
-      (Eigen::Array2i(0, 7) == limits.GetCellIndex(Eigen::Vector2f(-7.f, 13.f)))
+      (Array2i(13, 0) == limits.GetCellIndex(Vector2f(7.f, -13.f))).all());
-          .all());
+  EXPECT_TRUE(
-  EXPECT_TRUE((Eigen::Array2i(13, 7) ==
+      (Array2i(0, 7) == limits.GetCellIndex(Vector2f(-7.f, 13.f))).all());
-               limits.GetCellIndex(Eigen::Vector2f(-7.f, -13.f)))
+  EXPECT_TRUE(
-                  .all());
+      (Array2i(13, 7) == limits.GetCellIndex(Vector2f(-7.f, -13.f))).all());
 
   // Check around the origin.
   EXPECT_TRUE(
-      (Eigen::Array2i(6, 3) == limits.GetCellIndex(Eigen::Vector2f(0.5f, 0.5f)))
+      (Array2i(6, 3) == limits.GetCellIndex(Vector2f(0.5f, 0.5f))).all());
-          .all());
   EXPECT_TRUE(
-      (Eigen::Array2i(6, 3) == limits.GetCellIndex(Eigen::Vector2f(1.5f, 1.5f)))
+      (Array2i(6, 3) == limits.GetCellIndex(Vector2f(1.5f, 1.5f))).all());
-          .all());
+  EXPECT_TRUE(
-  EXPECT_TRUE((Eigen::Array2i(7, 3) ==
+      (Array2i(7, 3) == limits.GetCellIndex(Vector2f(0.5f, -0.5f))).all());
-               limits.GetCellIndex(Eigen::Vector2f(0.5f, -0.5f)))
+  EXPECT_TRUE(
-                  .all());
+      (Array2i(6, 4) == limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).all());
-  EXPECT_TRUE((Eigen::Array2i(6, 4) ==
+  EXPECT_TRUE(
-               limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f)))
+      (Array2i(7, 4) == limits.GetCellIndex(Vector2f(-0.5f, -0.5f))).all());
-                  .all());
-  EXPECT_TRUE((Eigen::Array2i(7, 4) ==
-               limits.GetCellIndex(Eigen::Vector2f(-0.5f, -0.5f)))
-                  .all());
 }
 
 TEST(ProbabilityGridTest, CorrectCropping) {
@@ -179,14 +169,14 @@ TEST(ProbabilityGridTest, CorrectCropping) {
       mapping::kMinProbability, mapping::kMaxProbability);
   ProbabilityGrid probability_grid(
       MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)));
-  for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(
+  for (const Array2i& xy_index :
-           Eigen::Array2i(100, 100), Eigen::Array2i(299, 299))) {
+       XYIndexRangeIterator(Array2i(100, 100), Array2i(299, 299))) {
     probability_grid.SetProbability(xy_index, value_distribution(rng));
   }
-  Eigen::Array2i offset;
+  Array2i offset;
   CellLimits limits;
   probability_grid.ComputeCroppedLimits(&offset, &limits);
-  EXPECT_TRUE((offset == Eigen::Array2i(100, 100)).all());
+  EXPECT_TRUE((offset == Array2i(100, 100)).all());
   EXPECT_EQ(limits.num_x_cells, 200);
   EXPECT_EQ(limits.num_y_cells, 200);
 }

cartographer/mapping_2d/range_data_inserter.cc

@@ -20,6 +20,7 @@
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
+#include "cartographer/mapping/probability_values.h"
 #include "cartographer/mapping_2d/ray_casting.h"
 #include "cartographer/mapping_2d/xy_index.h"
 #include "glog/logging.h"

cartographer/mapping_2d/range_data_inserter_test.cc

@@ -21,6 +21,7 @@
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
 #include "cartographer/common/make_unique.h"
+#include "cartographer/mapping/probability_values.h"
 #include "cartographer/mapping_2d/probability_grid.h"
 #include "gmock/gmock.h"
 

cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc

@@ -21,6 +21,7 @@
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
 #include "cartographer/common/make_unique.h"
+#include "cartographer/mapping/probability_values.h"
 #include "cartographer/mapping_2d/probability_grid.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/rigid_transform_test_helpers.h"

cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h

@@ -30,6 +30,7 @@
 
 #include "Eigen/Core"
 #include "cartographer/common/port.h"
+#include "cartographer/mapping/probability_values.h"
 #include "cartographer/mapping_2d/probability_grid.h"
 #include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h"
 #include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"