Minor cleanup of PolygonIterator

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

planning/apparent_safe_velocity_limiter/include/apparent_safe_velocity_limiter/polygon_iterator.hpp

@@ -15,17 +15,12 @@
 #ifndef APPARENT_SAFE_VELOCITY_LIMITER__POLYGON_ITERATOR_HPP_
 #define APPARENT_SAFE_VELOCITY_LIMITER__POLYGON_ITERATOR_HPP_
 
-#include "grid_map_core/Polygon.hpp"
-#include "grid_map_core/TypeDefs.hpp"
-#include "grid_map_core/iterators/GridMapIterator.hpp"
-
 #include <grid_map_core/GridMap.hpp>
-#include <tier4_autoware_utils/system/stop_watch.hpp>
+#include <grid_map_core/GridMapMath.hpp>
+#include <grid_map_core/Polygon.hpp>
 
 #include <algorithm>
-#include <cmath>
 #include <functional>
-#include <iostream>
 #include <limits>
 #include <set>
 #include <utility>
@@ -52,10 +47,13 @@ class PolygonIterator
 
     const auto resolution = grid_map.getResolution();
     const auto & map_pose = grid_map.getPosition();
-    const auto min_map_x = map_pose.x() - grid_map.getLength().x() / 2.0 + resolution / 2;
-    const auto max_map_x = map_pose.x() + grid_map.getLength().x() / 2.0 - resolution / 2;
-    const auto min_map_y = map_pose.y() - grid_map.getLength().y() / 2.0 + resolution / 2;
-    const auto max_map_y = map_pose.y() + grid_map.getLength().y() / 2.0 - resolution / 2;
+    const auto & map_size = grid_map.getSize();
+    // A cell is selected when its center point is inside the polygon
+    // Thus we restrict the range of x/y values to the center of the top left and bottom right cells
+    const auto min_x = map_pose.x() - grid_map.getLength().x() / 2.0 + resolution / 2;
+    const auto max_x = map_pose.x() + grid_map.getLength().x() / 2.0 - resolution / 2;
+    const auto min_y = map_pose.y() - grid_map.getLength().y() / 2.0 + resolution / 2;
+    const auto max_y = map_pose.y() + grid_map.getLength().y() / 2.0 - resolution / 2;
 
     // We make line scan from left to right / up to down *in the index frame*.
     // In the position frame, this corresponds to high to low Y values and high to low X values.
@@ -73,12 +71,6 @@ class PolygonIterator
       }
     };
 
-    tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stopwatch;
-    double edges_d{};
-    double inter_d{};
-    double index_d{};
-
-    stopwatch.tic("edges");
     std::multiset<Edge, Comp> edges;
     const auto & vertices = polygon_.getVertices();
     for (auto vertex = vertices.cbegin(); std::next(vertex) != vertices.cend(); ++vertex) {
@@ -89,19 +81,15 @@ class PolygonIterator
         edges.insert(edge);
       }
     }
-    edges_d += stopwatch.toc("edges");
     // get min/max x edge values
-    const auto max_x = edges.cbegin()->first.x();
-    const auto min_x = std::prev(edges.cend())->second.x();
+    const auto max_vertex_x = edges.cbegin()->first.x();
+    const auto min_vertex_x = std::prev(edges.cend())->second.x();
     // get min/max x values truncated to grid cell centers
     const auto max_line_x = std::clamp(
-      min_map_x + resolution * static_cast<int>((max_x - min_map_x) / resolution), min_map_x,
-      max_map_x);
+      min_x + resolution * static_cast<int>((max_vertex_x - min_x) / resolution), min_x, max_x);
     const auto min_line_x = std::clamp(
-      min_map_x + resolution * static_cast<int>((min_x - min_map_x) / resolution), min_map_x,
-      max_map_x);
+      min_x + resolution * static_cast<int>((min_vertex_x - min_x) / resolution), min_x, max_x);
     // calculate for each line the y value intersecting with the polygon in decreasing order
-    stopwatch.tic("inter");
     std::vector<std::multiset<double, std::greater<>>> y_intersections_per_line;
     const auto nb_x_lines = static_cast<size_t>((max_line_x - min_line_x) / resolution) + 1;
     y_intersections_per_line.reserve(nb_x_lines);
@@ -125,38 +113,33 @@ class PolygonIterator
       }
       y_intersections_per_line.push_back(y_intersections);
     }
-    inter_d += stopwatch.toc("inter");
     // calculate map indexes between pairs of intersections Y values on each X line
-    grid_map::Position pos;
-    grid_map::Index idx;
-    stopwatch.tic("index");
-    polygon_indexes_.reserve(
-      nb_x_lines * static_cast<size_t>((max_map_y - min_map_y) / resolution + 1));
+    polygon_indexes_.reserve(nb_x_lines * static_cast<size_t>((max_y - min_y) / resolution + 1));
+    const auto & start_idx = grid_map.getStartIndex();
+    // TODO(Maxime CLEMENT): from_row can be 1 less than real value due to float precision issue
+    const auto from_row = static_cast<size_t>((max_x - max_line_x) / resolution);
     for (size_t i = 0; i < y_intersections_per_line.size(); ++i) {
       const auto y_intersections = y_intersections_per_line[i];
-      pos.x() = max_line_x - static_cast<double>(i) * resolution;
-      grid_map.getIndex(pos, idx);
+      auto row = static_cast<int>(start_idx(0) + from_row + i);
+      grid_map::wrapIndexToRange(row, map_size(0));
+      // query the index of the row at it may be shifted in the gridmap
       for (auto y_iter = y_intersections.cbegin(); y_iter != y_intersections.cend(); ++y_iter) {
-        const auto from_y = std::clamp(*y_iter, min_map_y, max_map_y + resolution);
-        const auto from_col =
-          static_cast<int>(std::abs(max_map_y + resolution - from_y) / resolution);
+        const auto from_y = std::clamp(*y_iter, min_y, max_y + resolution);
+        const auto from_col = static_cast<int>(std::abs(max_y + resolution - from_y) / resolution);
 
         ++y_iter;
-        const auto to_y = std::clamp(*y_iter, min_map_y, max_map_y);
-        const auto to_col = static_cast<int>(std::abs(max_map_y - to_y) / resolution);
-        // special case where both intersection points are outside of the map
-        if (
-          (from_y >= max_map_y && to_y >= max_map_y) || (from_y <= min_map_y && to_y <= min_map_y))
-          continue;
+        const auto to_y = std::clamp(*y_iter, min_y, max_y);
+        const auto to_col = static_cast<int>(std::abs(max_y - to_y) / resolution);
+        // TODO(Maxime CLEMENT): remove pairs outside of map directly when finding intersection
+        // points special case where both intersection points are outside of the map
+        if ((from_y >= max_y && to_y >= max_y) || (from_y <= min_y && to_y <= min_y)) continue;
         for (auto col = from_col; col <= to_col; ++col) {
-          polygon_indexes_.emplace_back(idx(0), col);
+          auto wrapped_col = start_idx(1) + col;
+          grid_map::wrapIndexToRange(wrapped_col, map_size(1));
+          polygon_indexes_.emplace_back(row, wrapped_col);
         }
       }
     }
-    index_d += stopwatch.toc("index");
-    std::cout << "Edges: " << edges_d << std::endl;
-    std::cout << "Inter: " << inter_d << std::endl;
-    std::cout << "Index: " << index_d << std::endl;
   }
 
   bool operator!=(const PolygonIterator & other) const

planning/apparent_safe_velocity_limiter/test/test_polygon_iterator.cpp

@@ -301,14 +301,16 @@ TEST(PolygonIterator, BenchCompare)
 
   for (double resolution = 0.01; resolution <= 1.00; resolution *= 10) {
     map.setGeometry(Length(10.0, 10.0), resolution, Position(0.0, 0.0));  // bufferSize(8, 5)
-    for (auto seed = 0; seed < 1000; ++seed) {
+    const auto move = grid_map::Position(2.0, 2.0);
+    map.move(move);
+    for (auto seed = 0; seed < 100; ++seed) {
       std::cout << seed << std::endl;
       std::random_device r;
       std::default_random_engine engine(seed);
       std::uniform_real_distribution uniform_dist(-2.50, 2.50);
       Polygon polygon;
       for (const auto & vertex : base_polygon.getVertices()) {
-        polygon.addVertex(vertex + Position(uniform_dist(engine), uniform_dist(engine)));
+        polygon.addVertex(vertex + Position(uniform_dist(engine), uniform_dist(engine)) + move);
       }
       stopwatch.tic("ctor");
       apparent_safe_velocity_limiter::PolygonIterator iterator(map, polygon);
@@ -320,7 +322,7 @@ TEST(PolygonIterator, BenchCompare)
       while (!iterator.isPastEnd() && !iterator_gridmap.isPastEnd()) {
         // std::cout << (*iterator).transpose() << " " << (*iterator_gridmap).transpose() <<
         // std::endl;
-        EXPECT_EQ((*iterator).x(), (*iterator_gridmap).x());
+        ASSERT_EQ((*iterator).x(), (*iterator_gridmap).x());
         EXPECT_EQ((*iterator).y(), (*iterator_gridmap).y());
         stopwatch.tic("iter");
         ++iterator;