Clean up xy_index.h. (#9)

Removes and simplifies code from free standing functions in xy_index.h.

README.md

@@ -11,7 +11,7 @@ See https://github.com/googlecartographer/cartographer_ros
 
 ## Installation without ROS
 
-On Ubuntu 14.04 (Trusty):
+On Ubuntu 14.04 (Trusty) or Ubuntu 16.04 (Xenial):
 
     # Install the required libraries that are available as debs
     sudo apt-get install \

cartographer/mapping_2d/map_limits.h

@@ -74,9 +74,12 @@ class MapLimits {
   // false for Contains().
   Eigen::Array2i GetXYIndexOfCellContainingPoint(const double x,
                                                  const double y) const {
-    return mapping_2d::GetXYIndexOfCellContainingPoint(
+    // Index values are row major and the top left has Eigen::Array2i::Zero()
-        x, y, centered_limits_.max().x(), centered_limits_.max().y(),
+    // and contains (centered_max_x, centered_max_y). We need to flip and
-        resolution_);
+    // rotate.
+    return Eigen::Array2i(
+        common::RoundToInt((centered_limits_.max().y() - y) / resolution_),
+        common::RoundToInt((centered_limits_.max().x() - x) / resolution_));
   }
 
   // Returns true of the ProbabilityGrid contains 'xy_index'.
@@ -129,20 +132,24 @@ 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 cell size to the specified resolution in meters and the limits of
   // the grid to the specified bounding box in meters.
   void SetLimits(double resolution, const Eigen::AlignedBox2d& limits) {
     CHECK(!limits.isEmpty());
-    const int num_x_cells =
+    resolution_ = resolution;
-        common::RoundToInt((mapping_2d::Center(limits.max().y(), resolution) -
+    const int num_x_cells = common::RoundToInt((Center(limits.max().y()) -
-                            mapping_2d::Center(limits.min().y(), resolution)) /
+                                                Center(limits.min().y())) /
-                           resolution) +
+                                               resolution) +
-        1;
+                            1;
-    const int num_y_cells =
+    const int num_y_cells = common::RoundToInt((Center(limits.max().x()) -
-        common::RoundToInt((mapping_2d::Center(limits.max().x(), resolution) -
+                                                Center(limits.min().x())) /
-                            mapping_2d::Center(limits.min().x(), resolution)) /
+                                               resolution) +
-                           resolution) +
+                            1;
-        1;
     SetLimits(resolution, limits.max().x(), limits.max().y(),
               CellLimits(num_x_cells, num_y_cells));
   }
@@ -163,8 +170,8 @@ class MapLimits {
 
     resolution_ = resolution;
     cell_limits_ = limits;
-    centered_limits_.max().x() = Center(max_x, resolution);
+    centered_limits_.max().x() = Center(max_x);
-    centered_limits_.max().y() = Center(max_y, resolution);
+    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() -

cartographer/mapping_2d/probability_grid.h

@@ -111,15 +111,6 @@ class ProbabilityGrid {
                mapping::kUnknownProbabilityValue;
   }
 
-  // Returns the center of the cell at 'xy_index'.
-  void GetCenterOfCellWithXYIndex(const Eigen::Array2i& xy_index,
-                                  double* const x, double* const y) const {
-    *CHECK_NOTNULL(x) = limits_.edge_limits().max().x() -
-                        (xy_index.y() + 0.5) * limits_.resolution();
-    *CHECK_NOTNULL(y) = limits_.edge_limits().max().y() -
-                        (xy_index.x() + 0.5) * limits_.resolution();
-  }
-
   // Fills in 'offset' and 'limits' to define a subregion of that contains all
   // known cells.
   void ComputeCroppedLimits(Eigen::Array2i* const offset,
@@ -168,8 +159,7 @@ class ProbabilityGrid {
   // Returns the index of the cell at 'xy_index'.
   int GetIndexOfCell(const Eigen::Array2i& xy_index) const {
     CHECK(limits_.Contains(xy_index)) << xy_index;
-    return mapping_2d::GetIndexOfCell(xy_index,
+    return limits_.cell_limits().num_x_cells * xy_index.y() + xy_index.x();
-                                      limits_.cell_limits().num_x_cells);
   }
 
   void UpdateBounds(const Eigen::Array2i& xy_index) {

cartographer/mapping_2d/probability_grid_test.cc

@@ -144,26 +144,6 @@ TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) {
                   .all());
 }
 
-TEST(ProbabilityGridTest, GetCenterOfCell) {
-  ProbabilityGrid probability_grid(MapLimits(
-      2.,
-      Eigen::AlignedBox2d(Eigen::Vector2d(-4., 0.), Eigen::Vector2d(-0., 6.))));
-
-  // Limits are on the edge of a cell. Make sure that the grid grows (in
-  // positive direction).
-  const MapLimits& limits = probability_grid.limits();
-  const CellLimits& cell_limits = limits.cell_limits();
-  EXPECT_EQ(4, cell_limits.num_x_cells);
-  EXPECT_EQ(3, cell_limits.num_y_cells);
-
-  const Eigen::Array2i xy_index(3, 2);
-  double x, y;
-  probability_grid.GetCenterOfCellWithXYIndex(xy_index, &x, &y);
-  EXPECT_NEAR(-3., x, 1e-6);
-  EXPECT_NEAR(1., y, 1e-6);
-  EXPECT_TRUE((xy_index == limits.GetXYIndexOfCellContainingPoint(x, y)).all());
-}
-
 TEST(ProbabilityGridTest, CorrectCropping) {
   // Create a probability grid with random values.
   std::mt19937 rng(42);

cartographer/mapping_2d/ray_casting.cc

@@ -150,20 +150,25 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
 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 resolution = limits.resolution() / kSubpixelScale;
+  const double superscaled_resolution = limits.resolution() / kSubpixelScale;
   const Eigen::Vector2d superscaled_centered_max =
-      limits.edge_limits().max() - resolution / 2. * Eigen::Vector2d::Ones();
+      limits.edge_limits().max() -
-  const Eigen::Array2i begin = GetXYIndexOfCellContainingPoint(
+      superscaled_resolution / 2. * Eigen::Vector2d::Ones();
-      laser_fan.origin.x(), laser_fan.origin.y(), superscaled_centered_max.x(),
+  const MapLimits superscaled_limits(
-      superscaled_centered_max.y(), resolution);
+      superscaled_resolution, superscaled_centered_max.x(),
+      superscaled_centered_max.y(),
+      CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
+                 limits.cell_limits().num_y_cells * kSubpixelScale));
+  const Eigen::Array2i begin =
+      superscaled_limits.GetXYIndexOfCellContainingPoint(laser_fan.origin.x(),
+                                                         laser_fan.origin.y());
 
   // Compute and add the end points.
   std::vector<Eigen::Array2i> ends;
   ends.reserve(laser_fan.point_cloud.size());
   for (const Eigen::Vector2f& laser_return : laser_fan.point_cloud) {
-    ends.push_back(GetXYIndexOfCellContainingPoint(
+    ends.push_back(superscaled_limits.GetXYIndexOfCellContainingPoint(
-        laser_return.x(), laser_return.y(), superscaled_centered_max.x(),
+        laser_return.x(), laser_return.y()));
-        superscaled_centered_max.y(), resolution));
     hit_visitor(ends.back() / kSubpixelScale);
   }
 
@@ -175,10 +180,8 @@ void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
   // Finally, compute and add empty rays based on missing echos in the scan.
   for (const Eigen::Vector2f& missing_echo :
        laser_fan.missing_echo_point_cloud) {
-    CastRay(begin, GetXYIndexOfCellContainingPoint(
+    CastRay(begin, superscaled_limits.GetXYIndexOfCellContainingPoint(
-                       missing_echo.x(), missing_echo.y(),
+                       missing_echo.x(), missing_echo.y()),
-                       superscaled_centered_max.x(),
-                       superscaled_centered_max.y(), resolution),
             miss_visitor);
   }
 }

cartographer/mapping_2d/xy_index.h

@@ -117,31 +117,6 @@ class XYIndexRangeIterator
   Eigen::Array2i xy_index_;
 };
 
-// Returns the center of the resolution interval containing x.
-inline double Center(double x, double resolution) {
-  return (std::floor(x / resolution) + 0.5) * resolution;
-}
-
-// Returns the index of the cell containing the point (x, y) which is allowed to
-// lie outside the map.
-inline Eigen::Array2i GetXYIndexOfCellContainingPoint(double x, double y,
-                                                      double centered_max_x,
-                                                      double centered_max_y,
-                                                      double resolution) {
-  // Index values are row major and the top left has Eigen::Array2i::Zero() and
-  // contains (centered_max_x, centered_max_y). We need to flip and rotate.
-  const int cell_x = common::RoundToInt((centered_max_y - y) / resolution);
-  const int cell_y = common::RoundToInt((centered_max_x - x) / resolution);
-  return Eigen::Array2i(cell_x, cell_y);
-}
-
-// Returns the index into the vector with 'stride' for the cell with 'xy_index'.
-inline int GetIndexOfCell(const Eigen::Array2i& xy_index, int stride) {
-  CHECK_GE(xy_index.x(), 0);
-  CHECK_GE(xy_index.y(), 0);
-  return stride * xy_index.y() + xy_index.x();
-}
-
 }  // namespace mapping_2d
 }  // namespace cartographer