perf(behavior_velocity_planner): add faster PolygonIterator (#943)

* Add grid_map_utils pkg with faster implementation of PolygonIterator

Signed-off-by: Maxime CLEMENT <[email protected]>

common/grid_map_utils/CMakeLists.txt

@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 3.5)
+project(grid_map_utils)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+find_package(ament_cmake REQUIRED)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  DIRECTORY src
+)
+
+target_link_libraries(${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(test_${PROJECT_NAME}
+    test/test_polygon_iterator.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}
+  )
+
+  find_package(OpenCV REQUIRED)
+  add_executable(benchmark test/benchmark.cpp)
+  target_link_libraries(benchmark
+    ${PROJECT_NAME}
+    ${OpenCV_LIBS}
+  )
+endif()
+
+ament_auto_package()

common/grid_map_utils/README.md

@@ -0,0 +1,52 @@
+# Grid Map Utils
+
+## Overview
+
+This packages contains a re-implementation of the `grid_map::PolygonIterator` used to iterate over
+all cells of a grid map contained inside some polygon.
+
+## Algorithm
+
+This implementation uses the [scan line algorithm](https://en.wikipedia.org/wiki/Scanline_rendering),
+a common algorithm used to draw polygons on a rasterized image.
+The main idea of the algorithm adapted to a grid map is as follow:
+
+- calculate intersections between rows of the grid map and the edges of the polygon edges;
+- calculate for each row the column between each pair of intersections;
+- the resulting `(row, column)` indexes are inside of the polygon.
+
+More details on the scan line algorithm can be found in the References.
+
+## API
+
+The `grid_map_utils::PolygonIterator` follows the same API as the original [`grid_map::PolygonIterator`](https://docs.ros.org/en/kinetic/api/grid_map_core/html/classgrid__map_1_1PolygonIterator.html).
+
+## Assumptions
+
+The behavior of the `grid_map_utils::PolygonIterator` is only guaranteed to match the `grid_map::PolygonIterator` if edges of the polygon do not _exactly_ cross any cell center.
+In such a case, whether the crossed cell is considered inside or outside of the polygon can vary due to floating precision error.
+
+## Performances
+
+Benchmarking code is implemented in `test/benchmarking.cpp` and is also used to validate that the `grid_map_utils::PolygonIterator` behaves exactly like the `grid_map::PolygonIterator`.
+
+The following figure shows a comparison of the runtime between the implementation of this package (`grid_map_utils`) and the original implementation (`grid_map`).
+The time measured includes the construction of the iterator and the iteration over all indexes and is shown using a logarithmic scale.
+Results were obtained varying the side size of a square grid map with `100 <= n <= 1000` (size=`n` means a grid of `n x n` cells),
+random polygons with a number of vertices `3 <= m <= 100` and with each parameter `(n,m)` repeated 10 times.
+
+![Runtime comparison](media/runtime_comparison.png)
+
+## Future improvements
+
+There exists variations of the scan line algorithm for multiple polygons.
+These can be implemented if we want to iterate over the cells contained in at least one of multiple polygons.
+
+The current implementation imitate the behavior of the original `grid_map::PolygonIterator` where a cell is selected if its center position is inside the polygon.
+This behavior could be changed for example to only return all cells overlapped by the polygon.
+
+## References
+
+- <https://en.wikipedia.org/wiki/Scanline_rendering>
+- <https://web.cs.ucdavis.edu/~ma/ECS175_S00/Notes/0411_b.pdf>
+- <https://www.techfak.uni-bielefeld.de/ags/wbski/lehre/digiSA/WS0607/3DVRCG/Vorlesung/13.RT3DCGVR-vertex-2-fragment.pdf>

common/grid_map_utils/include/grid_map_utils/polygon_iterator.hpp

@@ -0,0 +1,129 @@
+// Copyright 2022 Tier IV, 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 GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_
+#define GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_
+
+#include "grid_map_core/TypeDefs.hpp"
+
+#include <grid_map_core/GridMap.hpp>
+#include <grid_map_core/GridMapMath.hpp>
+#include <grid_map_core/Polygon.hpp>
+
+#include <utility>
+#include <vector>
+
+namespace grid_map_utils
+{
+
+/// @brief Representation of a polygon edge made of 2 vertices
+struct Edge
+{
+  grid_map::Position first;
+  grid_map::Position second;
+
+  Edge(grid_map::Position f, grid_map::Position s) : first(std::move(f)), second(std::move(s)) {}
+
+  /// @brief Sorting operator resulting in edges sorted from highest to lowest x values
+  bool operator<(const Edge & e)
+  {
+    return first.x() > e.first.x() || (first.x() == e.first.x() && second.x() > e.second.x());
+  }
+};
+
+/** @brief A polygon iterator for grid_map::GridMap based on the scan line algorithm.
+    @details This iterator allows to iterate over all cells whose center is inside a polygon. \
+             This reproduces the behavior of the original grid_map::PolygonIterator which uses\
+             a "point in polygon" check for each cell of the gridmap, making it very expensive\
+             to run on large maps. In comparison, the scan line algorithm implemented here is \
+             much more scalable.
+*/
+class PolygonIterator
+{
+public:
+  /// @brief Constructor.
+  /// @details Calculate the indexes of the gridmap that are inside the polygon using the scan line
+  /// algorithm.
+  /// @param grid_map the grid map to iterate on.
+  /// @param polygon the polygonal area to iterate on.
+  PolygonIterator(const grid_map::GridMap & grid_map, const grid_map::Polygon & polygon);
+  /// @brief Compare to another iterator.
+  /// @param other other iterator.
+  /// @return whether the current iterator points to a different address than the other one.
+  bool operator!=(const PolygonIterator & other) const;
+  /// @brief Dereference the iterator with const.
+  /// @return the value to which the iterator is pointing.
+  const grid_map::Index & operator*() const;
+  /// @brief Increase the iterator to the next element.
+  /// @return a reference to the updated iterator.
+  PolygonIterator & operator++();
+  /// @brief Indicates if iterator is past end.
+  /// @return true if iterator is out of scope, false if end has not been reached.
+  [[nodiscard]] bool isPastEnd() const;
+
+private:
+  /** @brief Calculate sorted edges of the given polygon.
+      @details Vertices in an edge are ordered from higher to lower x.
+              Edges are sorted in reverse lexicographical order of x.
+      @param polygon Polygon for which edges are calculated.
+      @return Sorted edges of the polygon.
+  */
+  static std::vector<Edge> calculateSortedEdges(const grid_map::Polygon & polygon);
+
+  /// @brief Calculates intersections between lines (i.e., center of rows) and the polygon edges.
+  /// @param edges Edges of the polygon.
+  /// @param from_to_row ranges of lines to use for intersection.
+  /// @param origin Position of the top-left cell in the grid map.
+  /// @param grid_map grid map.
+  /// @return for each row the vector of y values with an intersection.
+  static std::vector<std::vector<double>> calculateIntersectionsPerLine(
+    const std::vector<Edge> & edges, const std::pair<int, int> from_to_row,
+    const grid_map::Position & origin, const grid_map::GridMap & grid_map);
+
+  /// @brief Calculates the range of rows covering the given edges.
+  /// @details The rows are calculated without any shift that might exist in the grid map.
+  /// @param edges Edges of the polygon.
+  /// @param origin Position of the top-left cell in the grid map.
+  /// @param grid_map grid map.
+  /// @return the range of rows as a pair {first row, last row}.
+  static std::pair<int, int> calculateRowRange(
+    const std::vector<Edge> & edges, const grid_map::Position & origin,
+    const grid_map::GridMap & grid_map);
+
+  // Helper functions
+  /// @brief Increment the current_line_ to the line with intersections
+  void goToNextLine();
+  /// @brief Calculate the initial current_col_ and the current_to_col_ for the current intersection
+  void calculateColumnIndexes();
+  /// @brief Calculate the current_index_ from the current_line_ and current_col_
+  void calculateIndex();
+
+  /// Gridmap info
+  int row_of_first_line_;
+  grid_map::Index map_start_idx_;
+  grid_map::Size map_size_;
+  double map_resolution_;
+  double map_origin_y_;
+  /// Intersections between scan lines and the polygon
+  std::vector<std::vector<double>> intersections_per_line_;
+  std::vector<double>::const_iterator intersection_iter_;
+  /// current indexes
+  grid_map::Index current_index_;
+  size_t current_line_;
+  int current_col_;
+  int current_to_col_;
+};
+}  // namespace grid_map_utils
+
+#endif  // GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_

common/grid_map_utils/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>grid_map_utils</name>
+  <version>0.0.0</version>
+  <description>Utilities for the grid_map library</description>
+  <maintainer email="[email protected]">mclement</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+
+  <depend>grid_map_core</depend>
+  <depend>grid_map_cv</depend>
+  <depend>libopencv-dev</depend>
+  <depend>tier4_autoware_utils</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>