Separate raycast and grid update (#1216)

Separate raycast and grid update logic.
Rename raycast to ray_to_pixel_mask.

cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc

@@ -21,12 +21,79 @@
 #include "Eigen/Core"
 #include "Eigen/Geometry"
 #include "cartographer/mapping/2d/xy_index.h"
-#include "cartographer/mapping/internal/2d/ray_casting.h"
+#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h"
 #include "cartographer/mapping/probability_values.h"
 #include "glog/logging.h"
 
 namespace cartographer {
 namespace mapping {
+namespace {
+
+// Factor for subpixel accuracy of start and end point for ray casts.
+constexpr int kSubpixelScale = 1000;
+
+void GrowAsNeeded(const sensor::RangeData& range_data,
+                  ProbabilityGrid* const probability_grid) {
+  Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
+  constexpr float kPadding = 1e-6f;
+  for (const Eigen::Vector3f& hit : range_data.returns) {
+    bounding_box.extend(hit.head<2>());
+  }
+  for (const Eigen::Vector3f& miss : range_data.misses) {
+    bounding_box.extend(miss.head<2>());
+  }
+  probability_grid->GrowLimits(bounding_box.min() -
+                               kPadding * Eigen::Vector2f::Ones());
+  probability_grid->GrowLimits(bounding_box.max() +
+                               kPadding * Eigen::Vector2f::Ones());
+}
+
+void CastRays(const sensor::RangeData& range_data,
+              const std::vector<uint16>& hit_table,
+              const std::vector<uint16>& miss_table,
+              const bool insert_free_space, ProbabilityGrid* probability_grid) {
+  GrowAsNeeded(range_data, probability_grid);
+
+  const MapLimits& limits = probability_grid->limits();
+  const double superscaled_resolution = limits.resolution() / kSubpixelScale;
+  const MapLimits superscaled_limits(
+      superscaled_resolution, limits.max(),
+      CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
+                 limits.cell_limits().num_y_cells * kSubpixelScale));
+  const Eigen::Array2i begin =
+      superscaled_limits.GetCellIndex(range_data.origin.head<2>());
+  // Compute and add the end points.
+  std::vector<Eigen::Array2i> ends;
+  ends.reserve(range_data.returns.size());
+  for (const Eigen::Vector3f& hit : range_data.returns) {
+    ends.push_back(superscaled_limits.GetCellIndex(hit.head<2>()));
+    probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
+  }
+
+  if (!insert_free_space) {
+    return;
+  }
+
+  // Now add the misses.
+  for (const Eigen::Array2i& end : ends) {
+    std::vector<Eigen::Array2i> ray =
+        RayToPixelMask(begin, end, kSubpixelScale);
+    for (const Eigen::Array2i& cell_index : ray) {
+      probability_grid->ApplyLookupTable(cell_index, miss_table);
+    }
+  }
+
+  // Finally, compute and add empty rays based on misses in the range data.
+  for (const Eigen::Vector3f& missing_echo : range_data.misses) {
+    std::vector<Eigen::Array2i> ray = RayToPixelMask(
+        begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()),
+        kSubpixelScale);
+    for (const Eigen::Array2i& cell_index : ray) {
+      probability_grid->ApplyLookupTable(cell_index, miss_table);
+    }
+  }
+}
+}  // namespace
 
 proto::ProbabilityGridRangeDataInserterOptions2D
 CreateProbabilityGridRangeDataInserterOptions2D(

cartographer/mapping/internal/2d/ray_casting.cc

@@ -1,206 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "cartographer/mapping/internal/2d/ray_casting.h"
-
-namespace cartographer {
-namespace mapping {
-namespace {
-
-// Factor for subpixel accuracy of start and end point.
-constexpr int kSubpixelScale = 1000;
-
-// We divide each pixel in kSubpixelScale x kSubpixelScale subpixels. 'begin'
-// and 'end' are coordinates at subpixel precision. We compute all pixels in
-// which some part of the line segment connecting 'begin' and 'end' lies.
-void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
-             const std::vector<uint16>& miss_table,
-             ProbabilityGrid* const probability_grid) {
-  // For simplicity, we order 'begin' and 'end' by their x coordinate.
-  if (begin.x() > end.x()) {
-    CastRay(end, begin, miss_table, probability_grid);
-    return;
-  }
-
-  CHECK_GE(begin.x(), 0);
-  CHECK_GE(begin.y(), 0);
-  CHECK_GE(end.y(), 0);
-
-  // Special case: We have to draw a vertical line in full pixels, as 'begin'
-  // and 'end' have the same full pixel x coordinate.
-  if (begin.x() / kSubpixelScale == end.x() / kSubpixelScale) {
-    Eigen::Array2i current(begin.x() / kSubpixelScale,
-                           std::min(begin.y(), end.y()) / kSubpixelScale);
-    const int end_y = std::max(begin.y(), end.y()) / kSubpixelScale;
-    for (; current.y() <= end_y; ++current.y()) {
-      probability_grid->ApplyLookupTable(current, miss_table);
-    }
-    return;
-  }
-
-  const int64 dx = end.x() - begin.x();
-  const int64 dy = end.y() - begin.y();
-  const int64 denominator = 2 * kSubpixelScale * dx;
-
-  // The current full pixel coordinates. We begin at 'begin'.
-  Eigen::Array2i current = begin / kSubpixelScale;
-
-  // To represent subpixel centers, we use a factor of 2 * 'kSubpixelScale' in
-  // the denominator.
-  // +-+-+-+ -- 1 = (2 * kSubpixelScale) / (2 * kSubpixelScale)
-  // | | | |
-  // +-+-+-+
-  // | | | |
-  // +-+-+-+ -- top edge of first subpixel = 2 / (2 * kSubpixelScale)
-  // | | | | -- center of first subpixel = 1 / (2 * kSubpixelScale)
-  // +-+-+-+ -- 0 = 0 / (2 * kSubpixelScale)
-
-  // The center of the subpixel part of 'begin.y()' assuming the
-  // 'denominator', i.e., sub_y / denominator is in (0, 1).
-  int64 sub_y = (2 * (begin.y() % kSubpixelScale) + 1) * dx;
-
-  // The distance from the from 'begin' to the right pixel border, to be divided
-  // by 2 * 'kSubpixelScale'.
-  const int first_pixel =
-      2 * kSubpixelScale - 2 * (begin.x() % kSubpixelScale) - 1;
-  // The same from the left pixel border to 'end'.
-  const int last_pixel = 2 * (end.x() % kSubpixelScale) + 1;
-
-  // The full pixel x coordinate of 'end'.
-  const int end_x = std::max(begin.x(), end.x()) / kSubpixelScale;
-
-  // Move from 'begin' to the next pixel border to the right.
-  sub_y += dy * first_pixel;
-  if (dy > 0) {
-    while (true) {
-      probability_grid->ApplyLookupTable(current, miss_table);
-      while (sub_y > denominator) {
-        sub_y -= denominator;
-        ++current.y();
-        probability_grid->ApplyLookupTable(current, miss_table);
-      }
-      ++current.x();
-      if (sub_y == denominator) {
-        sub_y -= denominator;
-        ++current.y();
-      }
-      if (current.x() == end_x) {
-        break;
-      }
-      // Move from one pixel border to the next.
-      sub_y += dy * 2 * kSubpixelScale;
-    }
-    // Move from the pixel border on the right to 'end'.
-    sub_y += dy * last_pixel;
-    probability_grid->ApplyLookupTable(current, miss_table);
-    while (sub_y > denominator) {
-      sub_y -= denominator;
-      ++current.y();
-      probability_grid->ApplyLookupTable(current, miss_table);
-    }
-    CHECK_NE(sub_y, denominator);
-    CHECK_EQ(current.y(), end.y() / kSubpixelScale);
-    return;
-  }
-
-  // Same for lines non-ascending in y coordinates.
-  while (true) {
-    probability_grid->ApplyLookupTable(current, miss_table);
-    while (sub_y < 0) {
-      sub_y += denominator;
-      --current.y();
-      probability_grid->ApplyLookupTable(current, miss_table);
-    }
-    ++current.x();
-    if (sub_y == 0) {
-      sub_y += denominator;
-      --current.y();
-    }
-    if (current.x() == end_x) {
-      break;
-    }
-    sub_y += dy * 2 * kSubpixelScale;
-  }
-  sub_y += dy * last_pixel;
-  probability_grid->ApplyLookupTable(current, miss_table);
-  while (sub_y < 0) {
-    sub_y += denominator;
-    --current.y();
-    probability_grid->ApplyLookupTable(current, miss_table);
-  }
-  CHECK_NE(sub_y, 0);
-  CHECK_EQ(current.y(), end.y() / kSubpixelScale);
-}
-
-void GrowAsNeeded(const sensor::RangeData& range_data,
-                  ProbabilityGrid* const probability_grid) {
-  Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
-  constexpr float kPadding = 1e-6f;
-  for (const Eigen::Vector3f& hit : range_data.returns) {
-    bounding_box.extend(hit.head<2>());
-  }
-  for (const Eigen::Vector3f& miss : range_data.misses) {
-    bounding_box.extend(miss.head<2>());
-  }
-  probability_grid->GrowLimits(bounding_box.min() -
-                               kPadding * Eigen::Vector2f::Ones());
-  probability_grid->GrowLimits(bounding_box.max() +
-                               kPadding * Eigen::Vector2f::Ones());
-}
-
-}  // namespace
-
-void CastRays(const sensor::RangeData& range_data,
-              const std::vector<uint16>& hit_table,
-              const std::vector<uint16>& miss_table,
-              const bool insert_free_space,
-              ProbabilityGrid* const probability_grid) {
-  GrowAsNeeded(range_data, probability_grid);
-
-  const MapLimits& limits = probability_grid->limits();
-  const double superscaled_resolution = limits.resolution() / kSubpixelScale;
-  const MapLimits superscaled_limits(
-      superscaled_resolution, limits.max(),
-      CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
-                 limits.cell_limits().num_y_cells * kSubpixelScale));
-  const Eigen::Array2i begin =
-      superscaled_limits.GetCellIndex(range_data.origin.head<2>());
-  // Compute and add the end points.
-  std::vector<Eigen::Array2i> ends;
-  ends.reserve(range_data.returns.size());
-  for (const Eigen::Vector3f& hit : range_data.returns) {
-    ends.push_back(superscaled_limits.GetCellIndex(hit.head<2>()));
-    probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
-  }
-
-  if (!insert_free_space) {
-    return;
-  }
-
-  // Now add the misses.
-  for (const Eigen::Array2i& end : ends) {
-    CastRay(begin, end, miss_table, probability_grid);
-  }
-
-  // Finally, compute and add empty rays based on misses in the range data.
-  for (const Eigen::Vector3f& missing_echo : range_data.misses) {
-    CastRay(begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()),
-            miss_table, probability_grid);
-  }
-}
-
-}  // namespace mapping
-}  // namespace cartographer

cartographer/mapping/internal/2d/ray_to_pixel_mask.cc

@@ -0,0 +1,159 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h"
+
+#include "Eigen/Dense"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+bool isEqual(const Eigen::Array2i& lhs, const Eigen::Array2i& rhs) {
+  return ((lhs - rhs).matrix().lpNorm<1>() == 0);
+}
+}  // namespace
+
+// Compute all pixels that contain some part of the line segment connecting
+// 'scaled_begin' and 'scaled_end'. 'scaled_begin' and 'scaled_end' are scaled
+// by 'subpixel_scale'. 'scaled_begin' and 'scaled_end' are expected to be
+// greater than zero. Return values are in pixels and not scaled.
+std::vector<Eigen::Array2i> RayToPixelMask(const Eigen::Array2i& scaled_begin,
+                                           const Eigen::Array2i& scaled_end,
+                                           int subpixel_scale) {
+  // For simplicity, we order 'scaled_begin' and 'scaled_end' by their x
+  // coordinate.
+  if (scaled_begin.x() > scaled_end.x()) {
+    return RayToPixelMask(scaled_end, scaled_begin, subpixel_scale);
+  }
+
+  CHECK_GE(scaled_begin.x(), 0);
+  CHECK_GE(scaled_begin.y(), 0);
+  CHECK_GE(scaled_end.y(), 0);
+  std::vector<Eigen::Array2i> pixel_mask;
+  // Special case: We have to draw a vertical line in full pixels, as
+  // 'scaled_begin' and 'scaled_end' have the same full pixel x coordinate.
+  if (scaled_begin.x() / subpixel_scale == scaled_end.x() / subpixel_scale) {
+    Eigen::Array2i current(
+        scaled_begin.x() / subpixel_scale,
+        std::min(scaled_begin.y(), scaled_end.y()) / subpixel_scale);
+    pixel_mask.push_back(current);
+    const int end_y =
+        std::max(scaled_begin.y(), scaled_end.y()) / subpixel_scale;
+    for (; current.y() <= end_y; ++current.y()) {
+      if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
+    }
+    return pixel_mask;
+  }
+
+  const int64 dx = scaled_end.x() - scaled_begin.x();
+  const int64 dy = scaled_end.y() - scaled_begin.y();
+  const int64 denominator = 2 * subpixel_scale * dx;
+
+  // The current full pixel coordinates. We scaled_begin at 'scaled_begin'.
+  Eigen::Array2i current = scaled_begin / subpixel_scale;
+  pixel_mask.push_back(current);
+
+  // To represent subpixel centers, we use a factor of 2 * 'subpixel_scale' in
+  // the denominator.
+  // +-+-+-+ -- 1 = (2 * subpixel_scale) / (2 * subpixel_scale)
+  // | | | |
+  // +-+-+-+
+  // | | | |
+  // +-+-+-+ -- top edge of first subpixel = 2 / (2 * subpixel_scale)
+  // | | | | -- center of first subpixel = 1 / (2 * subpixel_scale)
+  // +-+-+-+ -- 0 = 0 / (2 * subpixel_scale)
+
+  // The center of the subpixel part of 'scaled_begin.y()' assuming the
+  // 'denominator', i.e., sub_y / denominator is in (0, 1).
+  int64 sub_y = (2 * (scaled_begin.y() % subpixel_scale) + 1) * dx;
+
+  // The distance from the from 'scaled_begin' to the right pixel border, to be
+  // divided by 2 * 'subpixel_scale'.
+  const int first_pixel =
+      2 * subpixel_scale - 2 * (scaled_begin.x() % subpixel_scale) - 1;
+  // The same from the left pixel border to 'scaled_end'.
+  const int last_pixel = 2 * (scaled_end.x() % subpixel_scale) + 1;
+
+  // The full pixel x coordinate of 'scaled_end'.
+  const int end_x = std::max(scaled_begin.x(), scaled_end.x()) / subpixel_scale;
+
+  // Move from 'scaled_begin' to the next pixel border to the right.
+  sub_y += dy * first_pixel;
+  if (dy > 0) {
+    while (true) {
+      if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
+      while (sub_y > denominator) {
+        sub_y -= denominator;
+        ++current.y();
+        if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
+      }
+      ++current.x();
+      if (sub_y == denominator) {
+        sub_y -= denominator;
+        ++current.y();
+      }
+      if (current.x() == end_x) {
+        break;
+      }
+      // Move from one pixel border to the next.
+      sub_y += dy * 2 * subpixel_scale;
+    }
+    // Move from the pixel border on the right to 'scaled_end'.
+    sub_y += dy * last_pixel;
+    if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
+    while (sub_y > denominator) {
+      sub_y -= denominator;
+      ++current.y();
+      if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
+    }
+    CHECK_NE(sub_y, denominator);
+    CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale);
+    return pixel_mask;
+  }
+
+  // Same for lines non-ascending in y coordinates.
+  while (true) {
+    if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
+    while (sub_y < 0) {
+      sub_y += denominator;
+      --current.y();
+      if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
+    }
+    ++current.x();
+    if (sub_y == 0) {
+      sub_y += denominator;
+      --current.y();
+    }
+    if (current.x() == end_x) {
+      break;
+    }
+    sub_y += dy * 2 * subpixel_scale;
+  }
+  sub_y += dy * last_pixel;
+  if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
+  while (sub_y < 0) {
+    sub_y += denominator;
+    --current.y();
+    if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
+  }
+  CHECK_NE(sub_y, 0);
+  CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale);
+  return pixel_mask;
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/internal/2d/ray_to_pixel_mask.h

@@ -14,28 +14,25 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_CASTING_H_
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_TO_PIXEL_MASK_H_
-#define CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_CASTING_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_TO_PIXEL_MASK_H_
 
 #include <vector>
 
-#include "cartographer/common/port.h"
-#include "cartographer/mapping/2d/probability_grid.h"
-#include "cartographer/sensor/point_cloud.h"
-#include "cartographer/sensor/range_data.h"
 #include "cartographer/transform/transform.h"
 
 namespace cartographer {
 namespace mapping {
 
-// For each ray in 'range_data', inserts hits and misses into
+// Compute all pixels that contain some part of the line segment connecting
-// 'probability_grid'. Hits are handled before misses.
+// 'scaled_begin' and 'scaled_end'. 'scaled_begin' and 'scaled_end' are scaled
-void CastRays(const sensor::RangeData& range_data,
+// by 'subpixel_scale'. 'scaled_begin' and 'scaled_end' are expected to be
-              const std::vector<uint16>& hit_table,
+// greater than zero. Return values are in pixels and not scaled.
-              const std::vector<uint16>& miss_table, bool insert_free_space,
+std::vector<Eigen::Array2i> RayToPixelMask(const Eigen::Array2i& scaled_begin,
-              ProbabilityGrid* probability_grid);
+                                           const Eigen::Array2i& scaled_end,
+                                           int subpixel_scale);
 
 }  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_CASTING_H_
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_TO_PIXEL_MASK_H_

cartographer/mapping/internal/2d/ray_to_pixel_mask_test.cc

@@ -0,0 +1,207 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h"
+
+#include "cartographer/mapping/2d/map_limits.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+using ::testing::ElementsAre;
+
+MATCHER_P(PixelMaskEqual, value, "") {
+  Eigen::Array2i residual = value - arg;
+  return residual.matrix().lpNorm<1>() == 0;
+}
+
+TEST(RayToPixelMaskTest, SingleCell) {
+  const Eigen::Array2i& begin = {1, 1};
+  const Eigen::Array2i& end = {1, 1};
+  const int subpixel_scale = 1;
+  std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
+  std::vector<Eigen::Array2i> ray_reference =
+      std::vector<Eigen::Array2i>{begin};
+  EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(begin)));
+}
+
+TEST(RayToPixelMaskTest, AxisAlignedX) {
+  const Eigen::Array2i& begin = {1, 1};
+  const Eigen::Array2i& end = {3, 1};
+  const int subpixel_scale = 1;
+  std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
+  EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
+                               PixelMaskEqual(Eigen::Array2i({2, 1})),
+                               PixelMaskEqual(Eigen::Array2i({3, 1}))));
+  ray = RayToPixelMask(end, begin, subpixel_scale);
+  EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
+                               PixelMaskEqual(Eigen::Array2i({2, 1})),
+                               PixelMaskEqual(Eigen::Array2i({3, 1}))));
+}
+
+TEST(RayToPixelMaskTest, AxisAlignedY) {
+  const Eigen::Array2i& begin = {1, 1};
+  const Eigen::Array2i& end = {1, 3};
+  const int subpixel_scale = 1;
+  std::vector<Eigen::Array2i> ray_reference = std::vector<Eigen::Array2i>{
+      Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({1, 3})};
+  std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
+  EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
+                               PixelMaskEqual(Eigen::Array2i({1, 2})),
+                               PixelMaskEqual(Eigen::Array2i({1, 3}))));
+  ray = RayToPixelMask(end, begin, subpixel_scale);
+  EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
+                               PixelMaskEqual(Eigen::Array2i({1, 2})),
+                               PixelMaskEqual(Eigen::Array2i({1, 3}))));
+}
+
+TEST(RayToPixelMaskTest, Diagonal) {
+  Eigen::Array2i begin = {1, 1};
+  Eigen::Array2i end = {3, 3};
+  const int subpixel_scale = 1;
+  std::vector<Eigen::Array2i> ray_reference = std::vector<Eigen::Array2i>{
+      Eigen::Array2i({1, 1}), Eigen::Array2i({2, 2}), Eigen::Array2i({3, 3})};
+  std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
+  EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
+                               PixelMaskEqual(Eigen::Array2i({2, 2})),
+                               PixelMaskEqual(Eigen::Array2i({3, 3}))));
+  ray = RayToPixelMask(end, begin, subpixel_scale);
+  EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
+                               PixelMaskEqual(Eigen::Array2i({2, 2})),
+                               PixelMaskEqual(Eigen::Array2i({3, 3}))));
+  begin = Eigen::Array2i({1, 3});
+  end = Eigen::Array2i({3, 1});
+  ray = RayToPixelMask(begin, end, subpixel_scale);
+  EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 3})),
+                               PixelMaskEqual(Eigen::Array2i({2, 2})),
+                               PixelMaskEqual(Eigen::Array2i({3, 1}))));
+  ray = RayToPixelMask(end, begin, subpixel_scale);
+  EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 3})),
+                               PixelMaskEqual(Eigen::Array2i({2, 2})),
+                               PixelMaskEqual(Eigen::Array2i({3, 1}))));
+}
+
+TEST(RayToPixelMaskTest, SteepLine) {
+  Eigen::Array2i begin = {1, 1};
+  Eigen::Array2i end = {2, 5};
+  const int subpixel_scale = 1;
+  std::vector<Eigen::Array2i> ray_reference = std::vector<Eigen::Array2i>{
+      Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({1, 3}),
+      Eigen::Array2i({2, 3}), Eigen::Array2i({2, 4}), Eigen::Array2i({2, 5})};
+  std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
+  EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
+                               PixelMaskEqual(Eigen::Array2i({1, 2})),
+                               PixelMaskEqual(Eigen::Array2i({1, 3})),
+                               PixelMaskEqual(Eigen::Array2i({2, 3})),
+                               PixelMaskEqual(Eigen::Array2i({2, 4})),
+                               PixelMaskEqual(Eigen::Array2i({2, 5}))));
+  begin = {1, 1};
+  end = {2, 4};
+  ray_reference = std::vector<Eigen::Array2i>{
+      Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({2, 3}),
+      Eigen::Array2i({2, 4})};
+  ray = RayToPixelMask(begin, end, subpixel_scale);
+  EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
+                               PixelMaskEqual(Eigen::Array2i({1, 2})),
+                               PixelMaskEqual(Eigen::Array2i({2, 3})),
+                               PixelMaskEqual(Eigen::Array2i({2, 4}))));
+}
+
+TEST(RayToPixelMaskTest, FlatLine) {
+  Eigen::Array2i begin = {1, 1};
+  Eigen::Array2i end = {5, 2};
+  const int subpixel_scale = 1;
+  std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
+  EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
+                               PixelMaskEqual(Eigen::Array2i({2, 1})),
+                               PixelMaskEqual(Eigen::Array2i({3, 1})),
+                               PixelMaskEqual(Eigen::Array2i({3, 2})),
+                               PixelMaskEqual(Eigen::Array2i({4, 2})),
+                               PixelMaskEqual(Eigen::Array2i({5, 2}))));
+  begin = {1, 1};
+  end = {4, 2};
+  ray = RayToPixelMask(begin, end, subpixel_scale);
+  EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
+                               PixelMaskEqual(Eigen::Array2i({2, 1})),
+                               PixelMaskEqual(Eigen::Array2i({3, 2})),
+                               PixelMaskEqual(Eigen::Array2i({4, 2}))));
+}
+
+TEST(RayToPixelMaskTest, MultiScaleAxisAlignedX) {
+  int subpixel_scale;
+  const int num_cells_x = 20;
+  const int num_cells_y = 20;
+  double resolution = 0.1;
+  Eigen::Vector2d max = {1.0, 1.0};
+  std::vector<Eigen::Array2i> base_scale_ray;
+  for (subpixel_scale = 1; subpixel_scale < 10000; subpixel_scale *= 2) {
+    double superscaled_resolution = resolution / subpixel_scale;
+    MapLimits superscaled_limits(
+        superscaled_resolution, max,
+        CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale));
+    Eigen::Array2i begin =
+        superscaled_limits.GetCellIndex(Eigen::Vector2f({0.05, 0.05}));
+    Eigen::Array2i end =
+        superscaled_limits.GetCellIndex(Eigen::Vector2f({0.35, 0.05}));
+    std::vector<Eigen::Array2i> ray =
+        RayToPixelMask(begin, end, subpixel_scale);
+    EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({9, 6})),
+                                 PixelMaskEqual(Eigen::Array2i({9, 7})),
+                                 PixelMaskEqual(Eigen::Array2i({9, 8})),
+                                 PixelMaskEqual(Eigen::Array2i({9, 9}))));
+  }
+}
+
+TEST(RayToPixelMaskTest, MultiScaleSkewedLine) {
+  int subpixel_scale = 1;
+  const int num_cells_x = 20;
+  const int num_cells_y = 20;
+  double resolution = 0.1;
+  Eigen::Vector2d max = {1.0, 1.0};
+  double superscaled_resolution = resolution / subpixel_scale;
+  MapLimits superscaled_limits(
+      superscaled_resolution, max,
+      CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale));
+  Eigen::Array2i begin =
+      superscaled_limits.GetCellIndex(Eigen::Vector2f({0.01, 0.09}));
+  Eigen::Array2i end =
+      superscaled_limits.GetCellIndex(Eigen::Vector2f({0.21, 0.19}));
+
+  std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
+  EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({8, 7})),
+                               PixelMaskEqual(Eigen::Array2i({8, 8})),
+                               PixelMaskEqual(Eigen::Array2i({9, 8})),
+                               PixelMaskEqual(Eigen::Array2i({9, 9}))));
+  subpixel_scale = 20;
+  superscaled_resolution = resolution / subpixel_scale;
+  superscaled_limits = MapLimits(
+      superscaled_resolution, max,
+      CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale));
+  begin = superscaled_limits.GetCellIndex(Eigen::Vector2f({0.01, 0.09}));
+  end = superscaled_limits.GetCellIndex(Eigen::Vector2f({0.21, 0.19}));
+  ray = RayToPixelMask(begin, end, subpixel_scale);
+  EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({8, 7})),
+                               PixelMaskEqual(Eigen::Array2i({8, 8})),
+                               PixelMaskEqual(Eigen::Array2i({8, 9})),
+                               PixelMaskEqual(Eigen::Array2i({9, 9}))));
+}
+
+}  // namespace
+}  // namespace mapping
+}  // namespace cartographer