diff --git a/common/grid_map_utils/CMakeLists.txt b/common/grid_map_utils/CMakeLists.txt new file mode 100644 index 0000000000000..0fbb34e94e688 --- /dev/null +++ b/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() diff --git a/common/grid_map_utils/README.md b/common/grid_map_utils/README.md new file mode 100644 index 0000000000000..ca37cf8919078 --- /dev/null +++ b/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 + +- +- +- diff --git a/common/grid_map_utils/include/grid_map_utils/polygon_iterator.hpp b/common/grid_map_utils/include/grid_map_utils/polygon_iterator.hpp new file mode 100644 index 0000000000000..4f2e149a50f72 --- /dev/null +++ b/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 +#include +#include + +#include +#include + +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 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> calculateIntersectionsPerLine( + const std::vector & edges, const std::pair 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 calculateRowRange( + const std::vector & 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> intersections_per_line_; + std::vector::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_ diff --git a/common/grid_map_utils/media/runtime_comparison.png b/common/grid_map_utils/media/runtime_comparison.png new file mode 100644 index 0000000000000..8c249c0831393 Binary files /dev/null and b/common/grid_map_utils/media/runtime_comparison.png differ diff --git a/common/grid_map_utils/package.xml b/common/grid_map_utils/package.xml new file mode 100644 index 0000000000000..68550d4fd0a15 --- /dev/null +++ b/common/grid_map_utils/package.xml @@ -0,0 +1,25 @@ + + + + grid_map_utils + 0.0.0 + Utilities for the grid_map library + mclement + Apache License 2.0 + + autoware_cmake + eigen3_cmake_module + + grid_map_core + grid_map_cv + libopencv-dev + tier4_autoware_utils + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/grid_map_utils/src/polygon_iterator.cpp b/common/grid_map_utils/src/polygon_iterator.cpp new file mode 100644 index 0000000000000..3ab96bd0612da --- /dev/null +++ b/common/grid_map_utils/src/polygon_iterator.cpp @@ -0,0 +1,214 @@ +// 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. + +#include "grid_map_utils/polygon_iterator.hpp" + +#include "grid_map_core/GridMap.hpp" +#include "grid_map_core/Polygon.hpp" +#include "grid_map_core/TypeDefs.hpp" + +#include +#include +#include + +namespace grid_map_utils +{ + +std::vector PolygonIterator::calculateSortedEdges(const grid_map::Polygon & polygon) +{ + std::vector edges; + edges.reserve(polygon.nVertices()); + const auto & vertices = polygon.getVertices(); + for (auto vertex = vertices.cbegin(); std::next(vertex) != vertices.cend(); ++vertex) { + // order pair by decreasing x and ignore horizontal edges (when x is equal) + if (vertex->x() > std::next(vertex)->x()) + edges.emplace_back(*vertex, *std::next(vertex)); + else if (vertex->x() < std::next(vertex)->x()) + edges.emplace_back(*std::next(vertex), *vertex); + } + std::sort(edges.begin(), edges.end()); + edges.shrink_to_fit(); + return edges; +} + +std::vector> PolygonIterator::calculateIntersectionsPerLine( + const std::vector & edges, const std::pair from_to_row, + const grid_map::Position & origin, const grid_map::GridMap & grid_map) +{ + const auto from_row = from_to_row.first; + const auto to_row = from_to_row.second; + // calculate for each line the y value intersecting with the polygon in decreasing order + std::vector> y_intersections_per_line; + y_intersections_per_line.reserve(to_row - from_row + 1); + for (auto row = from_row; row <= to_row; ++row) { + std::vector y_intersections; + const auto line_x = origin.x() - grid_map.getResolution() * row; + for (const auto & edge : edges) { + // special case when exactly touching a vertex: only count edge for its lowest x + // up-down edge (\/) case: count the vertex twice + // down-down edge case: count the vertex only once + if (edge.second.x() == line_x) { + y_intersections.push_back(edge.second.y()); + } else if (edge.first.x() >= line_x && edge.second.x() < line_x) { + const auto diff = edge.first - edge.second; + const auto y = edge.second.y() + (line_x - edge.second.x()) * diff.y() / diff.x(); + y_intersections.push_back(y); + } else if (edge.first.x() < line_x) { // edge below the line + break; + } + } + std::sort(y_intersections.begin(), y_intersections.end(), std::greater()); + // remove pairs outside of map + auto iter = y_intersections.cbegin(); + while (iter != y_intersections.cend() && std::next(iter) != y_intersections.cend() && + *iter >= origin.y() && *std::next(iter) >= origin.y()) { + iter = y_intersections.erase(iter); + iter = y_intersections.erase(iter); + } + iter = std::lower_bound( + y_intersections.cbegin(), y_intersections.cend(), + origin.y() - (grid_map.getSize()(1) - 1) * grid_map.getResolution(), std::greater()); + while (iter != y_intersections.cend() && std::next(iter) != y_intersections.cend()) { + iter = y_intersections.erase(iter); + iter = y_intersections.erase(iter); + } + y_intersections_per_line.push_back(y_intersections); + } + return y_intersections_per_line; +} + +std::pair PolygonIterator::calculateRowRange( + const std::vector & edges, const grid_map::Position & origin, + const grid_map::GridMap & grid_map) +{ + const auto min_vertex_x = + std::min_element(edges.cbegin(), edges.cend(), [](const Edge & e1, const Edge & e2) { + return e1.second.x() < e2.second.x(); + })->second.x(); + const auto max_vertex_x = edges.front().first.x(); + + const auto dist_min_to_origin = origin.x() - min_vertex_x + grid_map.getResolution(); + const auto dist_max_to_origin = origin.x() - max_vertex_x + grid_map.getResolution(); + const auto min_row = std::clamp( + static_cast(dist_max_to_origin / grid_map.getResolution()), 0, grid_map.getSize()(0) - 1); + const auto max_row = std::clamp( + static_cast(dist_min_to_origin / grid_map.getResolution()), 0, grid_map.getSize()(0) - 1); + + return {min_row, max_row}; +} + +PolygonIterator::PolygonIterator( + const grid_map::GridMap & grid_map, const grid_map::Polygon & polygon) +{ + auto poly = polygon; + if (poly.nVertices() < 3) return; + // repeat the first vertex to get the last edge [last vertex, first vertex] + if (poly.getVertex(0) != poly.getVertex(poly.nVertices() - 1)) poly.addVertex(poly.getVertex(0)); + + map_start_idx_ = grid_map.getStartIndex(); + map_resolution_ = grid_map.getResolution(); + map_size_ = grid_map.getSize(); + const auto origin = [&]() { + grid_map::Position origin; + grid_map.getPosition(map_start_idx_, origin); + return origin; + }(); + map_origin_y_ = origin.y(); + + // We make line scan left -> right / up -> down *in the index frame* (idx[0,0] is pos[up, left]). + // In the position frame, this corresponds to high -> low Y values and high -> low X values. + const std::vector edges = calculateSortedEdges(poly); + if (edges.empty()) return; + const auto from_to_row = calculateRowRange(edges, origin, grid_map); + intersections_per_line_ = calculateIntersectionsPerLine(edges, from_to_row, origin, grid_map); + + row_of_first_line_ = from_to_row.first; + current_col_ = 0; + current_to_col_ = -1; + current_line_ = -1; // goToNextLine() increments the line so assign -1 to start from 0 + + // Initialize iterator to the first (row,column) inside the Polygon + if (!intersections_per_line_.empty()) { + goToNextLine(); + if (!isPastEnd()) { + calculateColumnIndexes(); + calculateIndex(); + } + } +} + +bool PolygonIterator::operator!=(const PolygonIterator & other) const +{ + return current_line_ != other.current_line_ || current_col_ != other.current_col_; +} + +const grid_map::Index & PolygonIterator::operator*() const { return current_index_; } + +void PolygonIterator::goToNextLine() +{ + ++current_line_; + while (current_line_ < intersections_per_line_.size() && + intersections_per_line_[current_line_].size() < 2) + ++current_line_; + if (!isPastEnd()) intersection_iter_ = intersections_per_line_[current_line_].cbegin(); +} + +void PolygonIterator::calculateColumnIndexes() +{ + const auto dist_from_origin = map_origin_y_ - *intersection_iter_ + map_resolution_; + current_col_ = + std::clamp(static_cast(dist_from_origin / map_resolution_), 0, map_size_(1) - 1); + ++intersection_iter_; + const auto dist_to_origin = map_origin_y_ - *intersection_iter_; + current_to_col_ = + std::clamp(static_cast(dist_to_origin / map_resolution_), 0, map_size_(1) - 1); + // Case where intersections do not encompass the center of a cell: iterate again + if (current_to_col_ < current_col_) { + operator++(); + } +} + +void PolygonIterator::calculateIndex() +{ + current_index_(0) = map_start_idx_(0) + row_of_first_line_ + static_cast(current_line_); + grid_map::wrapIndexToRange(current_index_(0), map_size_(0)); + current_index_(1) = map_start_idx_(1) + current_col_; + grid_map::wrapIndexToRange(current_index_(1), map_size_(1)); +} + +PolygonIterator & PolygonIterator::operator++() +{ + ++current_col_; + if (current_col_ > current_to_col_) { + ++intersection_iter_; + if ( + intersection_iter_ == intersections_per_line_[current_line_].cend() || + std::next(intersection_iter_) == intersections_per_line_[current_line_].cend()) { + goToNextLine(); + } + if (!isPastEnd()) { + calculateColumnIndexes(); + } + } + if (!isPastEnd()) { + calculateIndex(); + } + return *this; +} + +[[nodiscard]] bool PolygonIterator::isPastEnd() const +{ + return current_line_ >= intersections_per_line_.size(); +} +} // namespace grid_map_utils diff --git a/common/grid_map_utils/test/benchmark.cpp b/common/grid_map_utils/test/benchmark.cpp new file mode 100644 index 0000000000000..c8e352ce5d185 --- /dev/null +++ b/common/grid_map_utils/test/benchmark.cpp @@ -0,0 +1,182 @@ +// 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. + +#include "grid_map_core/TypeDefs.hpp" +#include "grid_map_cv/GridMapCvConverter.hpp" +#include "grid_map_cv/GridMapCvProcessing.hpp" +#include "grid_map_utils/polygon_iterator.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +int main(int argc, char * argv[]) +{ + bool visualize = false; + for (int i = 1; i < argc; ++i) { + const auto arg = std::string(argv[i]); + if (arg == "-v" || arg == "--visualize") { + visualize = true; + } + } + std::ofstream result_file; + result_file.open("benchmark_results.csv"); + result_file + << "#Size PolygonVertices PolygonIndexes grid_map_utils_constructor grid_map_utils_iteration " + "grid_map_constructor grid_map_iteration\n"; + tier4_autoware_utils::StopWatch stopwatch; + + constexpr auto nb_iterations = 10; + constexpr auto polygon_side_vertices = + 25; // number of vertex per side of the square base_polygon + const auto grid_map_length = grid_map::Length(10.0, 10.0); + std::random_device r; + std::default_random_engine engine(0); + // TODO(Maxime CLEMENT): moving breaks the polygon visualization + std::uniform_real_distribution move_dist(-2.0, 2.0); + std::uniform_real_distribution poly_x_offset(-2.0, 2.0); + std::uniform_real_distribution poly_y_offset(-2.0, 2.0); + + grid_map::Polygon base_polygon; + const auto top_left = grid_map::Position(-grid_map_length.x() / 2, grid_map_length.y() / 2); + const auto top_right = grid_map::Position(grid_map_length.x() / 2, grid_map_length.y() / 2); + const auto bot_right = grid_map::Position(grid_map_length.x() / 2, -grid_map_length.y() / 2); + const auto bot_left = grid_map::Position(-grid_map_length.x() / 2, -grid_map_length.y() / 2); + const auto top_vector = top_right - top_left; + for (double i = 0; i < polygon_side_vertices; ++i) { + const auto factor = i / polygon_side_vertices; + base_polygon.addVertex(top_left + factor * top_vector); + } + const auto right_vector = bot_right - top_right; + for (double i = 0; i < polygon_side_vertices; ++i) { + const auto factor = i / polygon_side_vertices; + base_polygon.addVertex(top_right + factor * right_vector); + } + const auto bot_vector = bot_left - bot_right; + for (double i = 0; i < polygon_side_vertices; ++i) { + const auto factor = i / polygon_side_vertices; + base_polygon.addVertex(bot_right + factor * bot_vector); + } + const auto left_vector = top_left - bot_left; + for (double i = 0; i < polygon_side_vertices; ++i) { + const auto factor = i / polygon_side_vertices; + base_polygon.addVertex(bot_left + factor * left_vector); + } + + for (auto grid_map_size = 100; grid_map_size <= 1000; grid_map_size += 100) { + std::cout << "Map of size " << grid_map_size << " by " << grid_map_size << std::endl; + const auto resolution = grid_map_length(0) / grid_map_size; + grid_map::GridMap map({"layer"}); + map.setGeometry(grid_map_length, resolution); + for (auto vertices = 3ul; vertices <= base_polygon.nVertices(); ++vertices) { + auto polygon_indexes = 0.0; + std::cout << "\tPolygon with " << vertices << " vertices" << std::endl; + + double grid_map_utils_constructor_duration{}; + double grid_map_constructor_duration{}; + double grid_map_utils_iteration_duration{}; + double grid_map_iteration_duration{}; + + for (auto iteration = 0; iteration < nb_iterations; ++iteration) { + map.setGeometry(grid_map::Length(10.0, 10.0), resolution, grid_map::Position(0.0, 0.0)); + const auto move = grid_map::Position(move_dist(engine), move_dist(engine)); + map.move(move); + + // generate random sub-polygon of base_polygon with some noise + grid_map::Polygon polygon; + std::vector indexes(base_polygon.nVertices()); + for (size_t i = 0; i <= base_polygon.nVertices(); ++i) indexes[i] = i; + std::shuffle(indexes.begin(), indexes.end(), std::default_random_engine(iteration)); + indexes.resize(vertices); + std::sort(indexes.begin(), indexes.end()); + for (const auto idx : indexes) { + const auto offset = grid_map::Position(poly_x_offset(engine), poly_y_offset(engine)); + polygon.addVertex(base_polygon.getVertex(idx) + offset); + } + stopwatch.tic("gmu_ctor"); + grid_map_utils::PolygonIterator grid_map_utils_iterator(map, polygon); + grid_map_utils_constructor_duration += stopwatch.toc("gmu_ctor"); + stopwatch.tic("gm_ctor"); + grid_map::PolygonIterator grid_map_iterator(map, polygon); + grid_map_constructor_duration += stopwatch.toc("gm_ctor"); + bool diff = false; + while (!grid_map_utils_iterator.isPastEnd() && !grid_map_iterator.isPastEnd()) { + stopwatch.tic("gmu_iter"); + const auto gmu_idx = *grid_map_utils_iterator; + ++grid_map_utils_iterator; + grid_map_utils_iteration_duration += stopwatch.toc("gmu_iter"); + stopwatch.tic("gm_iter"); + const auto gm_idx = *grid_map_iterator; + ++grid_map_iterator; + grid_map_iteration_duration += stopwatch.toc("gm_iter"); + ++polygon_indexes; + + if (gmu_idx.x() != gm_idx.x() || gmu_idx.y() != gm_idx.y()) { + diff = true; + } + } + if (grid_map_iterator.isPastEnd() != grid_map_utils_iterator.isPastEnd()) { + diff = true; + } + if (diff || visualize) { + // Prepare images of the cells selected by the two PolygonIterators + auto gridmap = map; + for (grid_map_utils::PolygonIterator iterator(map, polygon); !iterator.isPastEnd(); + ++iterator) + map.at("layer", *iterator) = 100; + for (grid_map::PolygonIterator iterator(gridmap, polygon); !iterator.isPastEnd(); + ++iterator) + gridmap.at("layer", *iterator) = 100; + + cv::Mat img; + cv::Mat custom_img; + cv::Mat gm_img; + cv::Mat diff_img; + + grid_map::GridMapCvConverter::toImage( + map, "layer", CV_8UC1, 0.0, 100, img); + cv::resize(img, custom_img, cv::Size(500, 500), cv::INTER_LINEAR); + grid_map::GridMapCvConverter::toImage( + gridmap, "layer", CV_8UC1, 0.0, 100, img); + cv::resize(img, gm_img, cv::Size(500, 500), cv::INTER_LINEAR); + cv::compare(custom_img, gm_img, diff_img, cv::CMP_EQ); + + cv::imshow("custom", custom_img); + cv::imshow("grid_map", gm_img); + cv::imshow("diff", diff_img); + cv::waitKey(0); + cv::destroyAllWindows(); + } + } + // print results to file + result_file << grid_map_size << " " << vertices << " " << polygon_indexes / nb_iterations + << " " << grid_map_utils_constructor_duration / nb_iterations << " " + << grid_map_utils_iteration_duration / nb_iterations << " " + << grid_map_constructor_duration / nb_iterations << " " + << grid_map_iteration_duration / nb_iterations << "\n"; + } + } + result_file.close(); +} diff --git a/common/grid_map_utils/test/test_polygon_iterator.cpp b/common/grid_map_utils/test/test_polygon_iterator.cpp new file mode 100644 index 0000000000000..71a7e1db8d4f6 --- /dev/null +++ b/common/grid_map_utils/test/test_polygon_iterator.cpp @@ -0,0 +1,280 @@ +// 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. + +#include "grid_map_utils/polygon_iterator.hpp" + +#include +#include + +// gtest +#include + +// Vector +#include +#include +#include + +using grid_map::GridMap; +using grid_map::Index; +using grid_map::Length; +using grid_map::Polygon; +using grid_map::Position; + +// Copied from grid_map::PolygonIterator +TEST(PolygonIterator, FullCover) +{ + std::vector types; + types.emplace_back("type"); + GridMap map(types); + map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + + Polygon polygon; + polygon.addVertex(Position(-100.0, 100.0)); + polygon.addVertex(Position(100.0, 100.0)); + polygon.addVertex(Position(100.0, -100.0)); + polygon.addVertex(Position(-100.0, -100.0)); + + grid_map_utils::PolygonIterator iterator(map, polygon); + + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(0, (*iterator)(0)); + EXPECT_EQ(0, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(0, (*iterator)(0)); + EXPECT_EQ(1, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(0, (*iterator)(0)); + EXPECT_EQ(2, (*iterator)(1)); + + for (int i = 0; i < 37; ++i) ++iterator; + + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(7, (*iterator)(0)); + EXPECT_EQ(4, (*iterator)(1)); + + ++iterator; + EXPECT_TRUE(iterator.isPastEnd()); +} + +// Copied from grid_map::PolygonIterator +TEST(PolygonIterator, Outside) +{ + GridMap map({"types"}); + map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + + Polygon polygon; + polygon.addVertex(Position(99.0, 101.0)); + polygon.addVertex(Position(101.0, 101.0)); + polygon.addVertex(Position(101.0, 99.0)); + polygon.addVertex(Position(99.0, 99.0)); + + grid_map_utils::PolygonIterator iterator(map, polygon); + + EXPECT_TRUE(iterator.isPastEnd()); +} + +// Copied from grid_map::PolygonIterator +TEST(PolygonIterator, Square) +{ + GridMap map({"types"}); + map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + + Polygon polygon; + polygon.addVertex(Position(-1.0, 1.5)); + polygon.addVertex(Position(1.0, 1.5)); + polygon.addVertex(Position(1.0, -1.5)); + polygon.addVertex(Position(-1.0, -1.5)); + + grid_map_utils::PolygonIterator iterator(map, polygon); + + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(3, (*iterator)(0)); + EXPECT_EQ(1, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(3, (*iterator)(0)); + EXPECT_EQ(2, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(3, (*iterator)(0)); + EXPECT_EQ(3, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(4, (*iterator)(0)); + EXPECT_EQ(1, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(4, (*iterator)(0)); + EXPECT_EQ(2, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(4, (*iterator)(0)); + EXPECT_EQ(3, (*iterator)(1)); + + ++iterator; + EXPECT_TRUE(iterator.isPastEnd()); +} + +// Copied from grid_map::PolygonIterator +TEST(PolygonIterator, TopLeftTriangle) +{ + GridMap map({"types"}); + map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + + Polygon polygon; + polygon.addVertex(Position(-40.1, 20.6)); + polygon.addVertex(Position(40.1, 20.4)); + polygon.addVertex(Position(-40.1, -20.6)); + + grid_map_utils::PolygonIterator iterator(map, polygon); + + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(0, (*iterator)(0)); + EXPECT_EQ(0, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(1, (*iterator)(0)); + EXPECT_EQ(0, (*iterator)(1)); +} + +// Copied from grid_map::PolygonIterator +TEST(PolygonIterator, MoveMap) +{ + GridMap map({"layer"}); + map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + map.move(Position(2.0, 0.0)); + + Polygon polygon; + polygon.addVertex(Position(6.1, 1.6)); + polygon.addVertex(Position(0.9, 1.6)); + polygon.addVertex(Position(0.9, -1.6)); + polygon.addVertex(Position(6.1, -1.6)); + grid_map_utils::PolygonIterator iterator(map, polygon); + + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(6, (*iterator)(0)); + EXPECT_EQ(1, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(6, (*iterator)(0)); + EXPECT_EQ(2, (*iterator)(1)); + + for (int i = 0; i < 4; ++i) ++iterator; + + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(7, (*iterator)(0)); + EXPECT_EQ(3, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(0, (*iterator)(0)); + EXPECT_EQ(1, (*iterator)(1)); + + for (int i = 0; i < 8; ++i) ++iterator; + + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(2, (*iterator)(0)); + EXPECT_EQ(3, (*iterator)(1)); + + ++iterator; + EXPECT_TRUE(iterator.isPastEnd()); +} + +// This test shows a difference when an edge passes exactly through the center of a cell +TEST(PolygonIterator, Difference) +{ + GridMap map({"layer"}); + map.setGeometry(Length(5.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + + // Triangle where the hypothenus is an exact diagonal of the map: difference. + Polygon polygon; + polygon.addVertex(Position(2.5, 2.5)); + polygon.addVertex(Position(-2.5, 2.5)); + polygon.addVertex(Position(-2.5, -2.5)); + grid_map_utils::PolygonIterator iterator(map, polygon); + grid_map::PolygonIterator gm_iterator(map, polygon); + bool diff = false; + while (!iterator.isPastEnd() && !gm_iterator.isPastEnd()) { + if ((*gm_iterator)(0) != (*iterator)(0) || (*gm_iterator)(1) != (*iterator)(1)) diff = true; + ++iterator; + ++gm_iterator; + } + if (iterator.isPastEnd() != gm_iterator.isPastEnd()) { + diff = true; + } + EXPECT_TRUE(diff); + + // Triangle where the hypothenus does not cross any cell center: no difference. + polygon.removeVertices(); + polygon.addVertex(Position(2.5, 2.1)); + polygon.addVertex(Position(-2.5, 2.5)); + polygon.addVertex(Position(-2.5, -2.9)); + iterator = grid_map_utils::PolygonIterator(map, polygon); + gm_iterator = grid_map::PolygonIterator(map, polygon); + diff = false; + while (!iterator.isPastEnd() && !gm_iterator.isPastEnd()) { + if ((*gm_iterator)(0) != (*iterator)(0) || (*gm_iterator)(1) != (*iterator)(1)) diff = true; + ++iterator; + ++gm_iterator; + } + if (iterator.isPastEnd() != gm_iterator.isPastEnd()) { + diff = true; + } + EXPECT_FALSE(diff); +} + +TEST(PolygonIterator, SelfCrossingPolygon) +{ + GridMap map({"layer"}); + map.setGeometry(Length(5.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + + // Hour-glass shape + Polygon polygon; + polygon.addVertex(Position(2.5, 2.9)); + polygon.addVertex(Position(2.5, -2.9)); + polygon.addVertex(Position(-2.5, 2.5)); + polygon.addVertex(Position(-2.5, -2.5)); + grid_map_utils::PolygonIterator iterator(map, polygon); + grid_map::PolygonIterator gm_iterator(map, polygon); + + const std::vector expected_indexes = { + Index(0, 0), Index(0, 1), Index(0, 2), Index(0, 3), Index(0, 4), Index(1, 1), Index(1, 2), + Index(1, 3), Index(2, 2), Index(3, 2), Index(4, 1), Index(4, 2), Index(4, 3)}; + bool diff = false; + size_t i = 0; + while (!iterator.isPastEnd() && !gm_iterator.isPastEnd()) { + if ((*gm_iterator)(0) != (*iterator)(0) || (*gm_iterator)(1) != (*iterator)(1)) diff = true; + ASSERT_TRUE(i < expected_indexes.size()); + EXPECT_EQ((*iterator)(0), expected_indexes[i](0)); + EXPECT_EQ((*iterator)(1), expected_indexes[i](1)); + ++i; + ++iterator; + ++gm_iterator; + } + if (iterator.isPastEnd() != gm_iterator.isPastEnd()) { + diff = true; + } + EXPECT_FALSE(diff); +}