From 2efc87456c60de6a1918033c3c0d9e0e7b104f62 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Thu, 26 May 2022 11:41:07 +0900 Subject: [PATCH] Remove PolygonIterator and use implementation from grid_map_utils Signed-off-by: Maxime CLEMENT --- .../CMakeLists.txt | 1 - .../occupancy_grid_utils.hpp | 11 - .../polygon_iterator.hpp | 175 --------- .../package.xml | 1 + .../src/occupancy_grid_utils.cpp | 9 +- .../test/test_polygon_iterator.cpp | 345 ------------------ 6 files changed, 7 insertions(+), 535 deletions(-) delete mode 100644 planning/apparent_safe_velocity_limiter/include/apparent_safe_velocity_limiter/polygon_iterator.hpp delete mode 100644 planning/apparent_safe_velocity_limiter/test/test_polygon_iterator.cpp diff --git a/planning/apparent_safe_velocity_limiter/CMakeLists.txt b/planning/apparent_safe_velocity_limiter/CMakeLists.txt index b50dc78b7b9c9..7ffccf2bff35d 100644 --- a/planning/apparent_safe_velocity_limiter/CMakeLists.txt +++ b/planning/apparent_safe_velocity_limiter/CMakeLists.txt @@ -31,7 +31,6 @@ if(BUILD_TESTING) ament_add_gtest(test_${PROJECT_NAME} test/test_collision_distance.cpp test/test_occupancy_grid_utils.cpp - test/test_polygon_iterator.cpp ) target_link_libraries(test_${PROJECT_NAME} apparent_safe_velocity_limiter_node diff --git a/planning/apparent_safe_velocity_limiter/include/apparent_safe_velocity_limiter/occupancy_grid_utils.hpp b/planning/apparent_safe_velocity_limiter/include/apparent_safe_velocity_limiter/occupancy_grid_utils.hpp index 796d0a30b11aa..a7c16d6753db0 100644 --- a/planning/apparent_safe_velocity_limiter/include/apparent_safe_velocity_limiter/occupancy_grid_utils.hpp +++ b/planning/apparent_safe_velocity_limiter/include/apparent_safe_velocity_limiter/occupancy_grid_utils.hpp @@ -16,26 +16,15 @@ #define APPARENT_SAFE_VELOCITY_LIMITER__OCCUPANCY_GRID_UTILS_HPP_ #include "apparent_safe_velocity_limiter/collision_distance.hpp" -#include "grid_map_core/Polygon.hpp" #include "grid_map_core/TypeDefs.hpp" -#include "grid_map_core/iterators/GridMapIterator.hpp" #include -#include -#include -#include -#include #include #include #include -#include #include -#include - -#include - namespace apparent_safe_velocity_limiter { diff --git a/planning/apparent_safe_velocity_limiter/include/apparent_safe_velocity_limiter/polygon_iterator.hpp b/planning/apparent_safe_velocity_limiter/include/apparent_safe_velocity_limiter/polygon_iterator.hpp deleted file mode 100644 index 5dd5e02c491ae..0000000000000 --- a/planning/apparent_safe_velocity_limiter/include/apparent_safe_velocity_limiter/polygon_iterator.hpp +++ /dev/null @@ -1,175 +0,0 @@ -// 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 APPARENT_SAFE_VELOCITY_LIMITER__POLYGON_ITERATOR_HPP_ -#define APPARENT_SAFE_VELOCITY_LIMITER__POLYGON_ITERATOR_HPP_ - -#include "grid_map_core/TypeDefs.hpp" - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace apparent_safe_velocity_limiter -{ - -/// @brief More efficient polygon iterator using scan line algorithm -class PolygonIterator -{ - grid_map::Polygon polygon_; - std::vector polygon_indexes_; - size_t current_index = 0; - -public: - PolygonIterator(const grid_map::GridMap & grid_map, const grid_map::Polygon & polygon) - : polygon_(polygon) - { - if (polygon_.nVertices() < 3) return; - // if needed repeat the first vertex to naturally get the last edge - if (polygon_.getVertex(0) != polygon_.getVertex(polygon_.nVertices() - 1)) - polygon_.addVertex(polygon_.getVertex(0)); - - const auto resolution = grid_map.getResolution(); - const auto & map_pose = grid_map.getPosition(); - 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. - - // get edges sorted from highest to lowest X values - - 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)) {} - }; - std::vector edges; - 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(), [](const Edge & e1, const Edge & e2) - { - return e1.first.x() > e2.first.x() || - (e1.first.x() == e2.first.x() && e1.second.x() > e2.second.x()); - } - ); - // get min/max x edge values - const auto max_vertex_x = edges.front().first.x(); - const auto min_vertex_x = edges.back().second.x(); - // get min/max x values truncated to grid cell centers - const auto max_line_x = std::clamp( - min_x + resolution * std::floor((max_vertex_x - min_x) / resolution), min_x, max_x); - const auto min_line_x = std::clamp( - min_x + resolution * std::floor((min_vertex_x - min_x) / resolution), min_x, max_x); - // calculate for each line the y value intersecting with the polygon in decreasing order - std::vector> y_intersections_per_line; - const auto nb_x_lines = static_cast((max_line_x - min_line_x) / resolution) + 1; - y_intersections_per_line.reserve(nb_x_lines); - const auto epsilon = resolution / 2.0; - for (auto line_x = max_line_x; line_x >= min_line_x - epsilon; line_x -= resolution) { - std::list y_intersections; - 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; - } - } - y_intersections.sort(std::greater()); - // remove pairs outside of map - auto iter = y_intersections.begin(); - while(iter != y_intersections.end() && std::next(iter) != y_intersections.end() - && *iter >= max_y && *std::next(iter) >= max_y) { - iter = y_intersections.erase(iter); - iter = y_intersections.erase(iter); - } - iter = std::lower_bound(y_intersections.begin(), y_intersections.end(), min_y, std::greater()); - while(iter != y_intersections.end() && std::next(iter) != y_intersections.end()) { - iter = y_intersections.erase(iter); - iter = y_intersections.erase(iter); - } - y_intersections_per_line.push_back(y_intersections); - } - // calculate map indexes between pairs of intersections Y values on each X line - polygon_indexes_.reserve(nb_x_lines * static_cast((max_y - min_y) / resolution + 1)); - const auto & start_idx = grid_map.getStartIndex(); - grid_map::Index idx; - grid_map::getIndexFromPosition( - idx, grid_map::Position(max_line_x, map_pose.y()), grid_map.getLength(), map_pose, resolution, - map_size, start_idx); - const auto from_row = idx(0); - for (size_t i = 0; i < y_intersections_per_line.size(); ++i) { - const auto & y_intersections = y_intersections_per_line[i]; - auto row = static_cast(from_row + i); - grid_map::wrapIndexToRange(row, map_size(0)); - for (auto y_iter = y_intersections.cbegin(); y_iter != y_intersections.cend(); ++y_iter) { - const auto from_y = std::clamp(*y_iter, min_y, max_y + resolution); - const auto from_col = static_cast(std::abs(max_y + resolution - from_y) / resolution); - - ++y_iter; - const auto to_y = std::clamp(*y_iter, min_y, max_y); - const auto to_col = static_cast(std::abs(max_y - to_y) / resolution); - for (auto col = from_col; col <= to_col; ++col) { - auto wrapped_col = start_idx(1) + col; - grid_map::wrapIndexToRange(wrapped_col, map_size(1)); - polygon_indexes_.emplace_back(row, wrapped_col); - } - } - } - } - - bool operator!=(const PolygonIterator & other) const - { - return current_index != other.current_index; - } - - const grid_map::Index & operator*() const { return polygon_indexes_[current_index]; } - - PolygonIterator & operator++() - { - ++current_index; - return *this; - } - - [[nodiscard]] bool isPastEnd() const { return current_index >= polygon_indexes_.size(); } -}; -} // namespace apparent_safe_velocity_limiter - -#endif // APPARENT_SAFE_VELOCITY_LIMITER__POLYGON_ITERATOR_HPP_ diff --git a/planning/apparent_safe_velocity_limiter/package.xml b/planning/apparent_safe_velocity_limiter/package.xml index cfa5dc22dd936..eed2a690988e0 100644 --- a/planning/apparent_safe_velocity_limiter/package.xml +++ b/planning/apparent_safe_velocity_limiter/package.xml @@ -16,6 +16,7 @@ eigen grid_map_msgs grid_map_ros + grid_map_utils libboost-dev nav_msgs pcl_ros diff --git a/planning/apparent_safe_velocity_limiter/src/occupancy_grid_utils.cpp b/planning/apparent_safe_velocity_limiter/src/occupancy_grid_utils.cpp index 2affc8889747b..fd9a6b9fe37af 100644 --- a/planning/apparent_safe_velocity_limiter/src/occupancy_grid_utils.cpp +++ b/planning/apparent_safe_velocity_limiter/src/occupancy_grid_utils.cpp @@ -13,7 +13,11 @@ // limitations under the License. #include -#include +#include +#include +#include +#include +#include namespace apparent_safe_velocity_limiter { @@ -37,8 +41,7 @@ void maskPolygons( in_masks.reserve(polygon_in_masks.size()); for (const auto & poly : polygon_in_masks) in_masks.push_back(convert(poly)); grid_map::Position position; - for (apparent_safe_velocity_limiter::PolygonIterator iterator( - grid_map, convert(polygon_out_mask)); + for (grid_map_utils::PolygonIterator iterator(grid_map, convert(polygon_out_mask)); !iterator.isPastEnd(); ++iterator) { grid_map.getPosition(*iterator, position); if (std::find_if(in_masks.begin(), in_masks.end(), [&](const auto & mask) { diff --git a/planning/apparent_safe_velocity_limiter/test/test_polygon_iterator.cpp b/planning/apparent_safe_velocity_limiter/test/test_polygon_iterator.cpp deleted file mode 100644 index b5b1ec82dbb70..0000000000000 --- a/planning/apparent_safe_velocity_limiter/test/test_polygon_iterator.cpp +++ /dev/null @@ -1,345 +0,0 @@ -// 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 "apparent_safe_velocity_limiter/polygon_iterator.hpp" -#include "grid_map_core/GridMap.hpp" -#include "grid_map_core/TypeDefs.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; - -TEST(PolygonIterator, Dummy) -{ - std::vector types; - types.emplace_back("type"); - GridMap map(types); - map.setGeometry(Length(2.0, 2.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) - - Polygon polygon; - polygon.addVertex(Position(-1.0, 1.0)); - polygon.addVertex(Position(1.0, 1.0)); - polygon.addVertex(Position(1.0, -1.0)); - polygon.addVertex(Position(-1.0, -1.0)); - apparent_safe_velocity_limiter::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(1, (*iterator)(0)); - EXPECT_EQ(0, (*iterator)(1)); - - ++iterator; - EXPECT_FALSE(iterator.isPastEnd()); - EXPECT_EQ(1, (*iterator)(0)); - EXPECT_EQ(1, (*iterator)(1)); - - ++iterator; - EXPECT_TRUE(iterator.isPastEnd()); -} -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)); - - apparent_safe_velocity_limiter::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()); -} - -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)); - - apparent_safe_velocity_limiter::PolygonIterator iterator(map, polygon); - - EXPECT_TRUE(iterator.isPastEnd()); -} - -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)); - - apparent_safe_velocity_limiter::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()); -} - -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)); - - apparent_safe_velocity_limiter::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)); -} - -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)); - apparent_safe_velocity_limiter::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()); -} - -TEST(PolygonIterator, Bench) -{ - tier4_autoware_utils::StopWatch stopwatch; - double ctor_duration{}; - double iter_duration{}; - - GridMap map({"layer"}); - - Polygon base_polygon; - base_polygon.addVertex(Position(-5.0, 5.0)); - base_polygon.addVertex(Position(0.0, 5.0)); - base_polygon.addVertex(Position(5.0, 5.0)); - base_polygon.addVertex(Position(5.0, 0.0)); - base_polygon.addVertex(Position(5.0, -5.0)); - base_polygon.addVertex(Position(0.0, -5.0)); - base_polygon.addVertex(Position(-5.0, -5.0)); - base_polygon.addVertex(Position(-5.0, 0.0)); - - for (double resolution = 0.001; 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 < 1; ++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))); - } - stopwatch.tic("ctor"); - apparent_safe_velocity_limiter::PolygonIterator iterator(map, polygon); - ctor_duration += stopwatch.toc("ctor"); - - while (!iterator.isPastEnd()) { - stopwatch.tic("iter"); - ++iterator; - iter_duration += stopwatch.toc("iter"); - } - } - } - std::printf( - "Total Runtime (constructor/increment): %2.2fms, %2.2fms\n", ctor_duration, iter_duration); -} - -TEST(PolygonIterator, BenchCompare) -{ - tier4_autoware_utils::StopWatch stopwatch; - double ctor_duration{}; - double gm_ctor_duration{}; - double iter_duration{}; - double gm_iter_duration{}; - - GridMap map({"layer"}); - - Polygon base_polygon; - base_polygon.addVertex(Position(-5.0, 5.0)); - base_polygon.addVertex(Position(0.0, 5.0)); - base_polygon.addVertex(Position(5.0, 5.0)); - base_polygon.addVertex(Position(5.0, 0.0)); - base_polygon.addVertex(Position(5.0, -5.0)); - base_polygon.addVertex(Position(0.0, -5.0)); - base_polygon.addVertex(Position(-5.0, -5.0)); - base_polygon.addVertex(Position(-5.0, 0.0)); - - 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) - 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)) + move); - } - stopwatch.tic("ctor"); - apparent_safe_velocity_limiter::PolygonIterator iterator(map, polygon); - ctor_duration += stopwatch.toc("ctor"); - stopwatch.tic("gm_ctor"); - grid_map::PolygonIterator iterator_gridmap(map, polygon); - gm_ctor_duration += stopwatch.toc("gm_ctor"); - - while (!iterator.isPastEnd() && !iterator_gridmap.isPastEnd()) { - // std::cout << (*iterator).transpose() << " " << (*iterator_gridmap).transpose() << - // std::endl; - ASSERT_EQ((*iterator).x(), (*iterator_gridmap).x()); - EXPECT_EQ((*iterator).y(), (*iterator_gridmap).y()); - stopwatch.tic("iter"); - ++iterator; - iter_duration += stopwatch.toc("iter"); - stopwatch.tic("gm_iter"); - ++iterator_gridmap; - gm_iter_duration += stopwatch.toc("gm_iter"); - } - EXPECT_EQ(iterator.isPastEnd(), iterator_gridmap.isPastEnd()); - while (!iterator_gridmap.isPastEnd()) - std::cout << "MISSING " << (*++iterator_gridmap).transpose() << std::endl; - while (!iterator.isPastEnd()) - std::cout << (*++iterator).transpose() << " MISSING" << std::endl; - } - } - std::printf( - "Total Runtimes (constructor/increment):\nCustom: %2.2fms, %2.2fms\nGridMap: %2.2fms, " - "%2.2fms\n", - ctor_duration, gm_ctor_duration, iter_duration, gm_iter_duration); -}