Clean up and simplify MapLimits. (#12)

MapLimits no longer keeps redundant data.

cartographer/mapping/submaps.cc

@@ -80,10 +80,10 @@ void Submaps::AddProbabilityGridToResponse(
   response->set_height(limits.num_y_cells);
   const double resolution = probability_grid.limits().resolution();
   response->set_resolution(resolution);
-  const double max_x = probability_grid.limits().edge_limits().max().x() -
+  const double max_x =
-                       resolution * offset.y();
+      probability_grid.limits().max().x() - resolution * offset.y();
-  const double max_y = probability_grid.limits().edge_limits().max().y() -
+  const double max_y =
-                       resolution * offset.x();
+      probability_grid.limits().max().y() - resolution * offset.x();
   *response->mutable_slice_pose() = transform::ToProto(
       local_submap_pose.inverse() *
       transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));

cartographer/mapping_2d/CMakeLists.txt

@@ -144,6 +144,7 @@ google_library(mapping_2d_submaps
   DEPENDS
     common_lua_parameter_dictionary
     common_make_unique
+    common_port
     mapping_2d_laser_fan_inserter
     mapping_2d_map_limits
     mapping_2d_probability_grid

cartographer/mapping_2d/laser_fan_inserter_test.cc

@@ -32,12 +32,7 @@ class LaserFanInserterTest : public ::testing::Test {
  protected:
   LaserFanInserterTest()
       : probability_grid_(
-            MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-4., 0.),
+            MapLimits(1., Eigen::Vector2d(1., 5.), CellLimits(5, 5))) {
-                                              Eigen::Vector2d(0., 4.)))) {
-    const CellLimits& cell_limits = probability_grid_.limits().cell_limits();
-    CHECK_EQ(5, cell_limits.num_x_cells);
-    CHECK_EQ(5, cell_limits.num_y_cells);
-
     auto parameter_dictionary = common::MakeDictionary(
         "return { "
         "insert_free_space = true, "
@@ -68,12 +63,8 @@ class LaserFanInserterTest : public ::testing::Test {
 TEST_F(LaserFanInserterTest, InsertPointCloud) {
   InsertPointCloud();
 
-  const Eigen::AlignedBox2d& edge_limits =
+  EXPECT_NEAR(1., probability_grid_.limits().max().x(), 1e-9);
-      probability_grid_.limits().edge_limits();
+  EXPECT_NEAR(5., probability_grid_.limits().max().y(), 1e-9);
-  EXPECT_NEAR(-4., edge_limits.min().x(), 1e-9);
-  EXPECT_NEAR(0., edge_limits.min().y(), 1e-9);
-  EXPECT_NEAR(1., edge_limits.max().x(), 1e-9);
-  EXPECT_NEAR(5., edge_limits.max().y(), 1e-9);
 
   const CellLimits& cell_limits = probability_grid_.limits().cell_limits();
   EXPECT_EQ(5, cell_limits.num_x_cells);

cartographer/mapping_2d/map_limits.h

@@ -38,29 +38,21 @@ namespace mapping_2d {
 // performance reasons.
 class MapLimits {
  public:
-  MapLimits(const double resolution, const Eigen::AlignedBox2d& edge_limits)
+  MapLimits(const double resolution, const Eigen::Vector2d& max,
-      : resolution_(resolution) {
-    SetLimits(edge_limits);
-  }
-
-  MapLimits(const double resolution, const double max_x, const double max_y,
             const CellLimits& cell_limits)
-      : resolution_(resolution) {
+      : resolution_(resolution), max_(max), cell_limits_(cell_limits) {
-    SetLimits(max_x, max_y, cell_limits);
+    CHECK_GT(resolution_, 0.);
+    CHECK_GT(cell_limits.num_x_cells, 0.);
+    CHECK_GT(cell_limits.num_y_cells, 0.);
   }
 
   // Returns the cell size in meters. All cells are square and the resolution is
   // the length of one side.
   double resolution() const { return resolution_; }
 
-  // Returns the limits of the grid from edge to edge in meters.
+  // Returns the corner of the limits, i.e., all pixels have positions with
-  const Eigen::AlignedBox2d& edge_limits() const { return edge_limits_; }
+  // smaller coordinates.
-
+  const Eigen::Vector2d& max() const { return max_; }
-  // Returns the limits of the grid between the centers of the edge cells in
-  // meters.
-  const Eigen::AlignedBox2d& centered_limits() const {
-    return centered_limits_;
-  }
 
   // Returns the limits of the grid in number of cells.
   const CellLimits& cell_limits() const { return cell_limits_; }
@@ -74,8 +66,8 @@ class MapLimits {
     // and contains (centered_max_x, centered_max_y). We need to flip and
     // rotate.
     return Eigen::Array2i(
-        common::RoundToInt((centered_limits_.max().y() - y) / resolution_),
+        common::RoundToInt((max_.y() - y) / resolution_ - 0.5),
-        common::RoundToInt((centered_limits_.max().x() - x) / resolution_));
+        common::RoundToInt((max_.x() - x) / resolution_ - 0.5));
   }
 
   // Returns true of the ProbabilityGrid contains 'xy_index'.
@@ -97,7 +89,9 @@ class MapLimits {
     const float kPadding = 3.f * resolution;
     bounding_box.min() -= kPadding * Eigen::Vector2f::Ones();
     bounding_box.max() += kPadding * Eigen::Vector2f::Ones();
-    return MapLimits(resolution, bounding_box.cast<double>());
+    return MapLimits(resolution, bounding_box.max().cast<double>(),
+                     CellLimits(common::RoundToInt(bounding_box.sizes().y()),
+                                common::RoundToInt(bounding_box.sizes().x())));
   }
 
   static Eigen::AlignedBox2f ComputeMapBoundingBox(
@@ -128,49 +122,8 @@ class MapLimits {
   }
 
  private:
-  // Returns the center of the resolution interval containing 'x'.
-  double Center(const double x) {
-    return (std::floor(x / resolution_) + 0.5) * resolution_;
-  }
-
-  // Sets the limits of the grid to the specified bounding box in meters.
-  void SetLimits(const Eigen::AlignedBox2d& limits) {
-    CHECK(!limits.isEmpty());
-    const int num_x_cells = common::RoundToInt((Center(limits.max().y()) -
-                                                Center(limits.min().y())) /
-                                               resolution_) +
-                            1;
-    const int num_y_cells = common::RoundToInt((Center(limits.max().x()) -
-                                                Center(limits.min().x())) /
-                                               resolution_) +
-                            1;
-    SetLimits(limits.max().x(), limits.max().y(),
-              CellLimits(num_x_cells, num_y_cells));
-  }
-
-  void SetLimits(double max_x, double max_y, const CellLimits& limits) {
-    CHECK_GT(resolution_, 0.);
-    CHECK_GT(limits.num_x_cells, 0.);
-    CHECK_GT(limits.num_y_cells, 0.);
-
-    cell_limits_ = limits;
-    centered_limits_.max().x() = Center(max_x);
-    centered_limits_.max().y() = Center(max_y);
-    centered_limits_.min().x() = centered_limits_.max().x() -
-                                 resolution_ * (cell_limits_.num_y_cells - 1);
-    centered_limits_.min().y() = centered_limits_.max().y() -
-                                 resolution_ * (cell_limits_.num_x_cells - 1);
-
-    const double half_resolution = resolution_ / 2.;
-    edge_limits_.min().x() = centered_limits_.min().x() - half_resolution;
-    edge_limits_.min().y() = centered_limits_.min().y() - half_resolution;
-    edge_limits_.max().x() = centered_limits_.max().x() + half_resolution;
-    edge_limits_.max().y() = centered_limits_.max().y() + half_resolution;
-  }
-
   double resolution_;
-  Eigen::AlignedBox2d edge_limits_;
+  Eigen::Vector2d max_;
-  Eigen::AlignedBox2d centered_limits_;
   CellLimits cell_limits_;
 };
 

cartographer/mapping_2d/map_limits_test.cc

@@ -22,43 +22,18 @@ namespace cartographer {
 namespace mapping_2d {
 namespace {
 
-TEST(MapLimits, SetEdgeLimits) {
+TEST(MapLimits, ConstructAndGet) {
-  const MapLimits limits(2., Eigen::AlignedBox2d(Eigen::Vector2d(-0.5, 1.5),
+  const MapLimits limits(42., Eigen::Vector2d(3., 0.), CellLimits(2, 3));
-                                                 Eigen::Vector2d(0.5, 5.5)));
-
-  const Eigen::AlignedBox2d& edge_limits = limits.edge_limits();
-  EXPECT_EQ(-2., edge_limits.min().x());
-  EXPECT_EQ(0., edge_limits.min().y());
-  EXPECT_EQ(2., edge_limits.max().x());
-  EXPECT_EQ(6., edge_limits.max().y());
-
-  const CellLimits& cell_limits = limits.cell_limits();
-  EXPECT_EQ(3, cell_limits.num_x_cells);
-  EXPECT_EQ(2, cell_limits.num_y_cells);
-}
-
-TEST(MapLimits, SetCellLimits) {
-  const MapLimits limits(3., 1.5, -0.5, CellLimits(2, 3));
 
   const CellLimits& cell_limits = limits.cell_limits();
   EXPECT_EQ(2, cell_limits.num_x_cells);
   EXPECT_EQ(3, cell_limits.num_y_cells);
 
-  const Eigen::AlignedBox2d& edge_limits = limits.edge_limits();
+  const Eigen::Vector2d& max = limits.max();
-  EXPECT_EQ(-6., edge_limits.min().x());
+  EXPECT_EQ(3., max.x());
-  EXPECT_EQ(-6., edge_limits.min().y());
+  EXPECT_EQ(0., max.y());
-  EXPECT_EQ(3., edge_limits.max().x());
-  EXPECT_EQ(0., edge_limits.max().y());
-}
 
-TEST(MapLimits, GetCenteredLimits) {
+  EXPECT_EQ(42., limits.resolution());
-  const MapLimits limits(2., -0.5, 1.5, CellLimits(3, 2));
-
-  const Eigen::AlignedBox2d& centered_limits = limits.centered_limits();
-  EXPECT_EQ(-3, centered_limits.min().x());
-  EXPECT_EQ(-3., centered_limits.min().y());
-  EXPECT_EQ(-1., centered_limits.max().x());
-  EXPECT_EQ(1., centered_limits.max().y());
 }
 
 }  // namespace

cartographer/mapping_2d/probability_grid.h

@@ -130,8 +130,8 @@ class ProbabilityGrid {
       const int y_offset = limits_.cell_limits().num_y_cells / 2;
       const MapLimits new_limits(
           limits_.resolution(),
-          limits_.centered_limits().max().x() + y_offset * limits_.resolution(),
+          limits_.max() +
-          limits_.centered_limits().max().y() + x_offset * limits_.resolution(),
+              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;

cartographer/mapping_2d/probability_grid_test.cc

@@ -25,7 +25,8 @@ namespace mapping_2d {
 namespace {
 
 TEST(ProbabilityGridTest, ApplyOdds) {
-  ProbabilityGrid probability_grid(MapLimits(1., 0.5, 0.5, CellLimits(2, 2)));
+  ProbabilityGrid probability_grid(
+      MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)));
   const MapLimits& limits = probability_grid.limits();
 
   EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 0)));
@@ -77,15 +78,11 @@ TEST(ProbabilityGridTest, ApplyOdds) {
 
 TEST(ProbabilityGridTest, GetProbability) {
   ProbabilityGrid probability_grid(
-      MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-0.5, 0.5),
+      MapLimits(1., Eigen::Vector2d(1., 2.), CellLimits(2, 2)));
-                                        Eigen::Vector2d(0.5, 1.5))));
 
   const MapLimits& limits = probability_grid.limits();
-  const Eigen::AlignedBox2d& edge_limits = limits.edge_limits();
+  EXPECT_EQ(1., limits.max().x());
-  EXPECT_EQ(-1., edge_limits.min().x());
+  EXPECT_EQ(2., limits.max().y());
-  EXPECT_EQ(0., edge_limits.min().y());
-  EXPECT_EQ(1., edge_limits.max().x());
-  EXPECT_EQ(2., edge_limits.max().y());
 
   const CellLimits& cell_limits = limits.cell_limits();
   ASSERT_EQ(2, cell_limits.num_x_cells);
@@ -107,7 +104,8 @@ TEST(ProbabilityGridTest, GetProbability) {
 }
 
 TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) {
-  ProbabilityGrid probability_grid(MapLimits(2., 7, 13, CellLimits(14, 8)));
+  ProbabilityGrid probability_grid(
+      MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8)));
 
   const MapLimits& limits = probability_grid.limits();
   const CellLimits& cell_limits = limits.cell_limits();
@@ -150,7 +148,7 @@ TEST(ProbabilityGridTest, CorrectCropping) {
   std::uniform_real_distribution<float> value_distribution(
       mapping::kMinProbability, mapping::kMaxProbability);
   ProbabilityGrid probability_grid(
-      MapLimits(0.05, 10., 10., CellLimits(400, 400)));
+      MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)));
   probability_grid.StartUpdate();
   for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(
            Eigen::Array2i(100, 100), Eigen::Array2i(299, 299))) {

cartographer/mapping_2d/ray_casting.cc

@@ -151,12 +151,8 @@ void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
               const std::function<void(const Eigen::Array2i&)>& hit_visitor,
               const std::function<void(const Eigen::Array2i&)>& miss_visitor) {
   const double superscaled_resolution = limits.resolution() / kSubpixelScale;
-  const Eigen::Vector2d superscaled_centered_max =
-      limits.edge_limits().max() -
-      superscaled_resolution / 2. * Eigen::Vector2d::Ones();
   const MapLimits superscaled_limits(
-      superscaled_resolution, superscaled_centered_max.x(),
+      superscaled_resolution, limits.max(),
-      superscaled_centered_max.y(),
       CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
                  limits.cell_limits().num_y_cells * kSubpixelScale));
   const Eigen::Array2i begin =

cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc

@@ -35,8 +35,7 @@ class CeresScanMatcherTest : public ::testing::Test {
  protected:
   CeresScanMatcherTest()
       : probability_grid_(
-            MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-10., -10.),
+            MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20))) {
-                                              Eigen::Vector2d(10., 10.)))) {
     probability_grid_.SetProbability(
         probability_grid_.limits().GetXYIndexOfCellContainingPoint(-3.5, 2.5),
         mapping::kMaxProbability);

cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc

@@ -79,9 +79,8 @@ TEST(DiscretizeScans, DiscretizeScans) {
   point_cloud.emplace_back(-0.125, 0.125);
   point_cloud.emplace_back(-0.125, 0.075);
   point_cloud.emplace_back(-0.125, 0.025);
-  const MapLimits map_limits(
+  const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25),
-      0.05, Eigen::AlignedBox2d(Eigen::Vector2d(-0.225, -0.025),
+                             CellLimits(6, 6));
-                                Eigen::Vector2d(0.025, 0.225)));
   const std::vector<sensor::PointCloud2D> scans =
       GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.));
   const std::vector<DiscreteScan> discrete_scans =

cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc

@@ -232,8 +232,11 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
       1e6 * limits_.resolution(),  // Linear search window, 1e6 cells/direction.
       M_PI,  // Angular search window, 180 degrees in both directions.
       point_cloud, limits_.resolution());
-  const transform::Rigid2d center =
+  const transform::Rigid2d center = transform::Rigid2d::Translation(
-      transform::Rigid2d::Translation(limits_.edge_limits().center());
+      limits_.max() -
+      0.5 * limits_.resolution() *
+          Eigen::Vector2d(limits_.cell_limits().num_y_cells,
+                          limits_.cell_limits().num_x_cells));
   return MatchWithSearchParameters(search_parameters, center, point_cloud,
                                    min_score, score, pose_estimate, covariance);
 }

cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc

@@ -40,7 +40,7 @@ TEST(PrecomputationGridTest, CorrectValues) {
   std::mt19937 prng(42);
   std::uniform_int_distribution<int> distribution(0, 255);
   ProbabilityGrid probability_grid(
-      MapLimits(0.05, 5., 5., CellLimits(250, 250)));
+      MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250)));
   probability_grid.StartUpdate();
   for (const Eigen::Array2i& xy_index :
        XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
@@ -73,7 +73,8 @@ TEST(PrecomputationGridTest, CorrectValues) {
 TEST(PrecomputationGridTest, TinyProbabilityGrid) {
   std::mt19937 prng(42);
   std::uniform_int_distribution<int> distribution(0, 255);
-  ProbabilityGrid probability_grid(MapLimits(0.05, 0.1, 0.1, CellLimits(4, 4)));
+  ProbabilityGrid probability_grid(
+      MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4)));
   probability_grid.StartUpdate();
   for (const Eigen::Array2i& xy_index :
        XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
@@ -147,7 +148,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
         0.5 * distribution(prng));
 
     ProbabilityGrid probability_grid(
-        MapLimits(0.05, 5., 5., CellLimits(200, 200)));
+        MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
     probability_grid.StartUpdate();
     laser_fan_inserter.Insert(sensor::LaserFan{expected_pose.translation(),
                                                sensor::TransformPointCloud2D(
@@ -198,7 +199,7 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
         perturbation.inverse();
 
     ProbabilityGrid probability_grid(
-        MapLimits(0.05, 5., 5., CellLimits(200, 200)));
+        MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
     probability_grid.StartUpdate();
     laser_fan_inserter.Insert(
         sensor::LaserFan{

cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h

@@ -62,13 +62,12 @@ class OccupiedSpaceCostFunctor {
       const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].x())),
                                          (T(point_cloud_[i].y())), T(1.));
       const Eigen::Matrix<T, 3, 1> world = transform * point;
-      interpolator.Evaluate((limits.centered_limits().max().x() - world[0]) /
+      interpolator.Evaluate(
-                                    limits.resolution() +
+          (limits.max().x() - world[0]) / limits.resolution() - 0.5 +
-                                T(kPadding),
+              T(kPadding),
-                            (limits.centered_limits().max().y() - world[1]) /
+          (limits.max().y() - world[1]) / limits.resolution() - 0.5 +
-                                    limits.resolution() +
+              T(kPadding),
-                                T(kPadding),
+          &residual[i]);
-                            &residual[i]);
       residual[i] = scaling_factor_ * (1. - residual[i]);
     }
     return true;

cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc

@@ -37,9 +37,8 @@ namespace {
 class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
  protected:
   RealTimeCorrelativeScanMatcherTest()
-      : probability_grid_(MapLimits(
+      : probability_grid_(
-            0.05, Eigen::AlignedBox2d(Eigen::Vector2d(-0.225, -0.025),
+            MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6))) {
-                                      Eigen::Vector2d(0.025, 0.225)))) {
     {
       auto parameter_dictionary = common::MakeDictionary(
           "return { "

cartographer/mapping_2d/submaps.cc

@@ -24,6 +24,7 @@
 
 #include "Eigen/Geometry"
 #include "cartographer/common/make_unique.h"
+#include "cartographer/common/port.h"
 #include "glog/logging.h"
 #include "webp/encode.h"
 
@@ -70,11 +71,10 @@ ProbabilityGrid ComputeCroppedProbabilityGrid(
   CellLimits limits;
   probability_grid.ComputeCroppedLimits(&offset, &limits);
   const double resolution = probability_grid.limits().resolution();
-  const double max_x = probability_grid.limits().centered_limits().max().x() -
+  const Eigen::Vector2d max =
-                       resolution * offset.y();
+      probability_grid.limits().max() -
-  const double max_y = probability_grid.limits().centered_limits().max().y() -
+      resolution * Eigen::Vector2d(offset.y(), offset.x());
-                       resolution * offset.x();
+  ProbabilityGrid cropped_grid(MapLimits(resolution, max, limits));
-  ProbabilityGrid cropped_grid(MapLimits(resolution, max_x, max_y, limits));
   cropped_grid.StartUpdate();
   for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
     if (probability_grid.IsKnown(xy_index + offset)) {
@@ -164,13 +164,14 @@ void Submaps::AddSubmap(const Eigen::Vector2f& origin) {
   if (size() > 1) {
     FinishSubmap(size() - 2);
   }
+  const int num_cells_per_dimension =
+      common::RoundToInt(2. * options_.half_length() / options_.resolution()) +
+      1;
   submaps_.push_back(common::make_unique<Submap>(
       MapLimits(options_.resolution(),
-                Eigen::AlignedBox2d(
+                origin.cast<double>() +
-                    origin.cast<double>() -
+                    options_.half_length() * Eigen::Vector2d::Ones(),
-                        options_.half_length() * Eigen::Vector2d::Ones(),
+                CellLimits(num_cells_per_dimension, num_cells_per_dimension)),
-                    origin.cast<double>() +
-                        options_.half_length() * Eigen::Vector2d::Ones())),
       origin, num_laser_fans_));
   LOG(INFO) << "Added submap " << size();
   num_laser_fans_in_last_submap_ = 0;