Extend raycast microbenchmarks

beluga/test/beluga/algorithm/test_raycasting.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "beluga/algorithm/raycasting.hpp"
+#include "beluga/test/raycasting.hpp"
 #include "beluga/test/static_occupancy_grid.hpp"
 
 #include <gmock/gmock.h>
@@ -125,4 +126,82 @@ TEST(Raycasting, NonIdentityGridOrigin) {
   }
 }
 
+TEST(BaselineRaycasting, Nominal) {
+  constexpr double kResolution = 0.5;
+  // Note that axes are:
+  // Positive X -> Right
+  // Positive Y -> Down
+
+  // clang-format off
+  const auto grid = StaticOccupancyGrid<5, 5>{{
+    false, false, false, false, false,
+    false, false, false, false, false,
+    false, false, true , false, false,
+    false, false, false, false, false,
+    false, false, false, false, false},
+    kResolution};
+  // clang-format on
+
+  constexpr double kMaxRange = 5.;
+
+  using Constants = Sophus::Constants<double>;
+
+  {
+    // Horizontal ray that hits the map boundary.
+    const auto pose = Eigen::Vector2d{0.5, 0.};
+    const auto source = grid.cell_near(pose);
+    const auto target = grid.cell_near(pose + kMaxRange * Sophus::SO2d{0.}.unit_complex());
+    EXPECT_EQ(beluga::testing::raycast(grid, source, target), std::nullopt);
+  }
+
+  {
+    // Horizontal ray that hits the occupied cell.
+    const auto pose = Eigen::Vector2d{0., 1.};
+    const auto source = grid.cell_near(pose);
+    const auto target = grid.cell_near(pose + kMaxRange * Sophus::SO2d{0.}.unit_complex());
+    EXPECT_EQ(beluga::testing::raycast(grid, source, target), 1.);
+  }
+
+  {
+    // Downwards ray that hits the map boundary.
+    const auto pose = Eigen::Vector2d{0., 1.};
+    const auto source = grid.cell_near(pose);
+    const auto target = grid.cell_near(pose + kMaxRange * Sophus::SO2d{Constants::pi() / 2.}.unit_complex());
+    EXPECT_EQ(beluga::testing::raycast(grid, source, target), std::nullopt);
+  }
+
+  {
+    // Start cell is occupied, should return 0.
+    const auto pose = Eigen::Vector2d{1., 1.};
+    const auto source = grid.cell_near(pose);
+    const auto target = grid.cell_near(pose + kMaxRange * Sophus::SO2d{Constants::pi() / 2.}.unit_complex());
+    EXPECT_EQ(beluga::testing::raycast(grid, source, target), 0.);
+  }
+
+  {
+    // Downwards ray that is limited by beam range.
+    constexpr double kShortenedRange = 1.;
+    const auto pose = Eigen::Vector2d{0., 0.};
+    const auto source = grid.cell_near(pose);
+    const auto target = grid.cell_near(pose + kShortenedRange * Sophus::SO2d{Constants::pi() / 2.}.unit_complex());
+    EXPECT_EQ(beluga::testing::raycast(grid, source, target), std::nullopt);
+  }
+
+  {
+    // Downwards ray that hits the occupied cell.
+    const auto pose = Eigen::Vector2d{1., 0.};
+    const auto source = grid.cell_near(pose);
+    const auto target = grid.cell_near(pose + kMaxRange * Sophus::SO2d{Constants::pi() / 2.}.unit_complex());
+    EXPECT_EQ(beluga::testing::raycast(grid, source, target), 1.);
+  }
+
+  {
+    // Diagonal ray that hits the occupied cell.
+    const auto pose = Eigen::Vector2d{0., 0.};
+    const auto source = grid.cell_near(pose);
+    const auto target = grid.cell_near(pose + kMaxRange * Sophus::SO2d{Constants::pi() / 4.}.unit_complex());
+    EXPECT_EQ(beluga::testing::raycast(grid, source, target), std::sqrt(2));
+  }
+}
+
 }  // namespace beluga

beluga/test/beluga/include/beluga/test/raycasting.hpp

@@ -0,0 +1,81 @@
+// Copyright 2023 Ekumen, Inc.
+//
+// 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.
+
+#ifndef BELUGA_TEST_RAYCASTING_HPP
+#define BELUGA_TEST_RAYCASTING_HPP
+
+#include <Eigen/Core>
+
+namespace beluga::testing {
+
+template <class Map>
+std::optional<double> raycast(const Map& map, Eigen::Vector2i source, Eigen::Vector2i target) {
+  const bool steep = std::abs(target.y() - source.y()) > std::abs(target.x() - source.x());
+
+  if (steep) {
+    std::swap(source.x(), source.y());
+    std::swap(target.x(), target.y());
+  }
+
+  const auto delta = Eigen::Vector2i{
+      std::abs(target.x() - source.x()),
+      std::abs(target.y() - source.y()),
+  };
+
+  int error = 0;
+
+  const auto step = Eigen::Vector2i{
+      source.x() < target.x() ? 1 : -1,
+      source.y() < target.y() ? 1 : -1,
+  };
+
+  auto current = source;
+
+  do {
+    if (steep) {
+      if (auto opt = map.data_at(current.y(), current.x()); opt.has_value()) {
+        if (opt.value()) {
+          break;
+        }
+      } else {
+        return std::nullopt;
+      }
+    } else {
+      if (auto opt = map.data_at(current.x(), current.y()); opt.has_value()) {
+        if (opt.value()) {
+          break;
+        }
+      } else {
+        return std::nullopt;
+      }
+    }
+
+    current.x() += step.x();
+    error += delta.y();
+    if (delta.x() <= 2 * error) {
+      current.y() += step.y();
+      error -= delta.x();
+    }
+
+    if (current.x() == (target.x() + step.x())) {
+      return std::nullopt;
+    }
+  } while (true);
+
+  return (current - source).cast<double>().norm() * map.resolution();
+}
+
+}  // namespace beluga::testing
+
+#endif

beluga/test/benchmark/benchmark_raycasting.cpp

@@ -17,6 +17,7 @@
 #include <benchmark/benchmark.h>
 
 #include <beluga/algorithm/raycasting.hpp>
+#include <beluga/test/raycasting.hpp>
 #include <beluga/test/static_occupancy_grid.hpp>
 
 #include <range/v3/range/conversion.hpp>
@@ -50,23 +51,124 @@ BENCHMARK_CAPTURE(BM_Bresenham2i, Modified, beluga::Bresenham2i::kModified)
 
 using beluga::testing::StaticOccupancyGrid;
 
+template <std::size_t Rows, std::size_t Cols>
+class BaselineGrid : public beluga::BaseOccupancyGrid2<BaselineGrid<Rows, Cols>> {
+ public:
+  struct ValueTraits {
+    [[nodiscard]] bool is_free(bool value) const { return !value; }
+    [[nodiscard]] bool is_unknown(bool) const { return false; }
+    [[nodiscard]] bool is_occupied(bool value) const { return value; }
+  };
+
+  explicit BaselineGrid(std::initializer_list<bool>, double resolution) : resolution_{resolution} {
+    std::fill(std::begin(data()), std::end(data()), false);
+  }
+
+  [[nodiscard]] const Sophus::SE2d& origin() const { return origin_; }
+
+  [[nodiscard]] auto& data() { return grid_; }
+  [[nodiscard]] const auto& data() const { return grid_; }
+  [[nodiscard]] std::size_t size() const { return grid_.size(); }
+
+  [[nodiscard]] std::size_t width() const { return Cols; }
+  [[nodiscard]] std::size_t height() const { return Rows; }
+  [[nodiscard]] double resolution() const { return resolution_; }
+
+  [[nodiscard]] auto value_traits() const { return ValueTraits{}; }
+
+ private:
+  double resolution_;
+  Sophus::SE2d origin_;
+  std::array<bool, Rows * Cols> grid_;
+  static constexpr std::size_t kWidth = Cols;
+  static constexpr std::size_t kHeight = Rows;
+};
+
+const auto kBearingAngles = std::array{
+    0.,                                    // Horizontal
+    Sophus::Constants<double>::pi() / 2.,  // Vertical
+    Sophus::Constants<double>::pi() / 4.,  // Diagonal
+};
+
+const auto kBearingLabels = std::array{
+    "Horizontal",
+    "Vertical",
+    "Diagonal",
+};
+
+template <template <std::size_t, std::size_t> class Grid>
+void BM_RayCasting2d_BaselineRaycast(benchmark::State& state) {
+  constexpr double kMaxRange = 100.0;
+  constexpr double kResolution = 0.05;
+
+  const auto bearing_index = static_cast<std::size_t>(state.range(0));
+  const auto n = static_cast<int>(state.range(1));
+  Grid<1280, 1280> grid{{}, kResolution};
+  grid.data()[grid.index_at(n, n)] = true;
+  grid.data()[grid.index_at(0, n)] = true;
+  grid.data()[grid.index_at(n, 0)] = true;
+
+  const auto source_pose = Eigen::Vector2d{0., 0.};
+  const auto beam_bearing = Sophus::SO2d{kBearingAngles.at(bearing_index)};
+
+  const auto source = grid.cell_near(source_pose);
+  const auto target = grid.cell_near(source_pose + kMaxRange * beam_bearing.unit_complex());
+
+  for (auto _ : state) {
+    benchmark::DoNotOptimize(beluga::testing::raycast(grid, source, target));
+  }
+  state.SetComplexityN(n);
+  state.SetLabel(kBearingLabels.at(bearing_index));
+}
+
+BENCHMARK_TEMPLATE(BM_RayCasting2d_BaselineRaycast, BaselineGrid)
+    ->ArgsProduct({
+        {0, 1, 2},
+        {128, 256, 512, 1024},
+    })
+    ->Complexity();
+
+BENCHMARK_TEMPLATE(BM_RayCasting2d_BaselineRaycast, StaticOccupancyGrid)
+    ->ArgsProduct({
+        {0, 1, 2},
+        {128, 256, 512, 1024},
+    })
+    ->Complexity();
+
+template <template <std::size_t, std::size_t> class Grid>
 void BM_RayCasting2d(benchmark::State& state) {
   constexpr double kMaxRange = 100.0;
   constexpr double kResolution = 0.05;
 
-  const auto n = static_cast<int>(state.range(0));
-  auto grid = StaticOccupancyGrid<1280, 1280>{{}, kResolution};
+  const auto bearing_index = static_cast<std::size_t>(state.range(0));
+  const auto n = static_cast<int>(state.range(1));
+  auto grid = Grid<1280, 1280>{{}, kResolution};
   grid.data()[grid.index_at(n, n)] = true;
+  grid.data()[grid.index_at(0, n)] = true;
+  grid.data()[grid.index_at(n, 0)] = true;
 
-  const auto source_pose = Sophus::SE2d{0., Eigen::Vector2d{1., 1.}};
-  const auto beam_bearing = Sophus::SO2d{Sophus::Constants<double>::pi() / 4.};
+  const auto source_pose = Sophus::SE2d{0., Eigen::Vector2d{0., 0.}};
+  const auto beam_bearing = Sophus::SO2d{kBearingAngles.at(bearing_index)};
   const auto beam = beluga::Ray2d{grid, source_pose, kMaxRange};
   for (auto _ : state) {
     benchmark::DoNotOptimize(beam.cast(beam_bearing));
   }
   state.SetComplexityN(n);
+  state.SetLabel(kBearingLabels.at(bearing_index));
 }
 
-BENCHMARK(BM_RayCasting2d)->RangeMultiplier(2)->Range(128, 1024)->Complexity();
+BENCHMARK_TEMPLATE(BM_RayCasting2d, BaselineGrid)
+    ->ArgsProduct({
+        {0, 1, 2},
+        {128, 256, 512, 1024},
+    })
+    ->Complexity();
+
+BENCHMARK_TEMPLATE(BM_RayCasting2d, StaticOccupancyGrid)
+    ->ArgsProduct({
+        {0, 1, 2},
+        {128, 256, 512, 1024},
+    })
+    ->Complexity();
 
 }  // namespace