From b20c054b9ff0371f1ac7228806dee21b6fed8a2b Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 31 Mar 2023 16:53:07 +0900 Subject: [PATCH] feat(elevation_map_loader): large size pointcloud map (backport #2571, #2885, #943) (#337) * feat(elevation_map_loader): reduce memory usage of elevation_map_loader (#2571) * feat: reduce memory usage of elevation_map_loader Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * chore: remove unnecessary comment Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * fix: modify variables' name Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * feat(elevation_map_loader): use polygon iterator to speed up (#2885) * use grid_map::PolygonIterator instead of grid_map::GridMapIterator Signed-off-by: Shunsuke Miura * formatting Signed-off-by: Shunsuke Miura * use use_lane_filter option Signed-off-by: Shunsuke Miura * delete unused use-lane-filter option Signed-off-by: Shunsuke Miura * change use_lane_filter to True, clarify the scope Signed-off-by: Shunsuke Miura * change to use grid_map_utils::PolygonIterator Signed-off-by: Shunsuke Miura * Add lane margin parameter Signed-off-by: Shunsuke Miura * use boost geometry buffer to expand lanes Signed-off-by: Shunsuke Miura * Change use_lane_filter param default to false Signed-off-by: Shunsuke Miura * update README Signed-off-by: Shunsuke Miura --------- Signed-off-by: Shunsuke Miura * perf(behavior_velocity_planner): add faster PolygonIterator (#943) * Add grid_map_utils pkg with faster implementation of PolygonIterator Signed-off-by: Maxime CLEMENT * suppress error --------- Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Signed-off-by: Shunsuke Miura Signed-off-by: Maxime CLEMENT Co-authored-by: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- common/grid_map_utils/CMakeLists.txt | 42 +++ common/grid_map_utils/README.md | 52 ++++ .../grid_map_utils/polygon_iterator.hpp | 129 ++++++++ .../media/runtime_comparison.png | Bin 0 -> 18307 bytes common/grid_map_utils/package.xml | 25 ++ .../grid_map_utils/src/polygon_iterator.cpp | 214 +++++++++++++ common/grid_map_utils/test/benchmark.cpp | 182 ++++++++++++ .../test/test_polygon_iterator.cpp | 280 ++++++++++++++++++ .../ground_segmentation.launch.py | 1 + .../elevation_map_loader/CMakeLists.txt | 2 +- perception/elevation_map_loader/README.md | 26 +- .../elevation_map_loader_node.hpp | 23 +- .../launch/elevation_map_loader.launch.xml | 12 - perception/elevation_map_loader/package.xml | 1 + .../src/elevation_map_loader_node.cpp | 223 ++++---------- 15 files changed, 1001 insertions(+), 211 deletions(-) create mode 100644 common/grid_map_utils/CMakeLists.txt create mode 100644 common/grid_map_utils/README.md create mode 100644 common/grid_map_utils/include/grid_map_utils/polygon_iterator.hpp create mode 100644 common/grid_map_utils/media/runtime_comparison.png create mode 100644 common/grid_map_utils/package.xml create mode 100644 common/grid_map_utils/src/polygon_iterator.cpp create mode 100644 common/grid_map_utils/test/benchmark.cpp create mode 100644 common/grid_map_utils/test/test_polygon_iterator.cpp 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 0000000000000000000000000000000000000000..8c249c083139351c0bf25098994f67ef36d1eaf2 GIT binary patch literal 18307 zcmc({by!r<_b)t@2#6>WiXccSt)zg2g3{eFgft_dbPZqtB1$(>(m8Ywh>C>LF*HgK zL&p$9ynFQf``!0G_x^LA=e^I%f6O^&pS9Osd&Osc_6gHeSD++kAcsI8luC;7+7Jk# zAp}C;cJTuEWp2@{8Uj&JR+4|D>ut6^O_oG6(s2Hg8R=N&yFVk(Y}MRw)R9#GaE9GC zw%}zr`Camh*Zb{hJ~YYCM^F%6prxVLmA`uB5#6{AEzJ{>alI=vSLLxb*wXxifb-pS zB@wCoMwoA=U!&a;&L@5M9yGtG(e^qzWBHRW20{RV$UbGVZ~VkmDy%2OaL^BkqvUpNkmyJ$0WZZlNc+G&iFml>u{RpH7$BOHf~ zPZqLE!Kh5#sTl8=L}U*XfSJjGG1wwy&g%WQ+DP+eHxE)CaL;gufm z2QFWjFbh2O7}`{vDhrf8{QXAIV^w!3pLq|vt?q_>X z$y`HB2FQdWr+rFa^|wSE%=jUCrNerpX97x5mdx3()tFQNrJhuJ8R=^>^bkl&N(phv z+}yKWf}(7u-h!|*um(+7Njg4b>)M_)NwF3+wBkEw^GfS5AEfJ|@~$yaatK26F8gGi zOC)K7&Ci7PNDt{R9=E+LEiL_R__9nadN;lCv>4GMh$}Ng zPJ9@BuxYuO+Pj}S=P`}NM zK!G{_6%lF5`f-)}W$KN~h`7$-N{5!b z-B2vmuo`q zgkG?)Yl>LqnB4d;HOv#J)V zlAoNqMh{l(Bg%-XBE5`Rm!;O*$GYem#1@XVUt-lf!D19zzxol7=Gb&9F@n`aS#Ksf z`f&#QHQXCihP}t1jwects^&T#V5;0xG!;zt%EZ!GRZld%My1euy6{g2ine^PAPd$TNpO#kB1f7Z7^?rJr%^pxUv9tIS^IYS{ zS(HW&MOBT4LM5X~?ej3^*aybBxs2|q$SWrKbn6H9U@4zv2t}8{23xwjyAMU7=hn=-vpGoJNGs=)Ss1!X-FFWAu^;;EmbtRQMLDeSeuI6E9apaLu2R1C=v8Cmu1GV z&4DP4O1rcjYd&XEf^0vgf8ViB8drk(JMrG(G<7s*iuk}@FCL;w=w3Ouum}6k!r>_Y_U(O@g0qs#wg5TVQQEOiu=DG~j{(EmHZ634|@=wu)AHEZGm zgi`(sA@-3T3n!b*4W!ksFgq4D&eBC?jItOoACRYS-XGU9|%>1>k8+eHP<)QfE(|&iL=Dp*njq zAHB%|=DdxH0Pz4lm>d4deNaizd8)oz=B-C7*)`5ihx|0#iDsW&>$cYz?qn&y+9MXvmAJHxYZr;F@i~Q@hvC&1scw&|>2zk*^mZ&<=a})YzAFm*ZqTf?e z^yI;&%OvV2Q%@z|1Yy^n%!x7ty`3b%bP!FJ%{_pSxweDl~Fay zhao)Nq6>QIUKjhEuH=b$nPAeR!g}UV@^9zoWv>&%EZyg5>Q_aE<$k7kKSJJGoErnw^ zALQth{^c~U^cz|Ouqv6ip_iGsTTd1R7DH}2-vg^`SFz8P|A{qQA+pS^chb2@Enqv` zJPEOW;OdVZw;L?d<8ixur^H5Hm^;^Nc8y%ea|jM}@IQp%MnrTWt+YdV*N5N?$_my` zKU}|U0QHEUj!#^p+FW8WHsj1am|h~++kT-pl#dgp39OU-DF?jMs~`C!=E1or`Bqb* zF}sw2(n$t}Qg`2{3U2NLwPoQiH)bXbj{hVT*=QH-j&CNH9`vOcsuEJn#8!4}#LmGa zlm2=v*t;dH7X=Ia|5}yte^ASaU*+S8?w=T$%aBql3Ta_qL~bU2Ri{^JYj4$PI$G|j z)JUbrS4+Y=1D^@Q)+V?rij(#<^Vg2_AA=vpEhk@RB&}E_>r`Hh=W9#tI~Dr- zWXA2EG|@n(duiWMn^<;)tBwu!=l)TVGu#@|$jKZ@E`!I`d{!@&|9Tszy+a7gkHe8##nQ>FzH8f_v5jf;0&#rM1 zoT*LTxjRF@430^NuVCm+q3*+J6g-{XDX=B+IlkuWSB;Yy|4IAUw3O4DBvGK2>SK8D zYwDTmM}!nIG_!|V+S|Y8wT};O{<4q8n39ziU~IIpvQ&ls+YW{457l3*&?{Nv^l^I* z_aAMS-GpvNk8xi(tFrA#g4@rq;D^8TR?J`LTKd^6uAac?m2@bzX@z`IzFJgw zTc7f{oYzR2zUmsc~;Kv!TP0foe7)k zGtEyx1)O=~E(Vt=$?GG`bYpPeIR4FX>XYrZ_6$X9jBT{xu(Sx406#n@XQfj(vGkE_ zQ3z#1rNTzSXBqQ3a?Smk{?)#k(&r*i$8=q zZ@7*19qFB+PlRfI#3wl2MV@==lvkZp;M!%p zY}FklAADSXJjX@|i62Zb4E%Jx@;xf3+&` zNV4I1Lw055x*9ysy{cD-S2h$Ri4>6jo$t?Vr(?^I+>OVs20=TH zZNbcAc0{Il@s{}L;*y~*#raQjNXWPDPgnuZmIetT&C@|y4`1=)@?3l~(mSt&E4%MyE2^9%n8snP5 z8K(?~)55J|k35K7`>uvkS7~7UYn8K=%a0iH8oi=-0wR;B$(Phyij;Bg{L$+sJ@a;J zZXBZd@amt>5x-Geul{+_{e5@yD#Weq4%;_AD?3H;z)gIAxO8ER{_hCD z1&jJzR~~e4R98rJk5f`ebsttz$XGj7g}Ei%s2LpKVQL>7D1rNMGDoTWePUpFz2U1? zQNlB1oHJ@)x3R4I;cxF#iN57i0yPq_JmnhI#IJeo`3NJwX>9HdNuKTX${>mEjSS!J zAI3)&$~>=WT9g|qZThpH=XgFr3Lk!%%oqyxlKmDnutjyB9#!VW57k_}+pmpx|NO?N zZW)-PQ|9XGW!)zRwb3z?2rnm%k?!-O@>4q5DtuJ|9LG=d`$12S;h&zd!R=a>`7(Z^UZSQb!djMB9&W zUY3x)RmLx+0K!ro6;X;5kgXaLT{??;%W+2&x=Q}IvFD*#o-H#%HeR*5vF;2dT6fH`1b7 zPEB-mraBd2CqIyEAxK&UQT{EsjAfzv>(Ac!--A!EmMhF9;ORFRoJ?*ZhV|}3R_CeacUQ|;mZf{KDmE$BGhKWE}6pSle3Bp4Y zp>~nJKU9Hh^%J`$)Fau)qLS`>xbP|DC>ZRHk0lGW7}HmUE&A^Ji|6FJn^y{gID`IP zDA3_C2vQz5KFw;gxdvId1#E&r$r`2ZmY>0I;YCtNqjo^jzXukC#pJ6cS*bCXo}9Td zCR(t|7!9u*w(EF~8bXS>TX|6NwsA9@GxqmRB2?j)&a>mj#`j4qm;eb1irgVoD>KCklHrJ*3{sWw~uZI^p^ z-0PuGXq>^7dtPtkZ#5IO^cE%$JgX364bvC_IrbU=|z&G+oQ@XyJ>pK&2bqj6C0q-nl)^e6AW=^27pcIkT7~y+ca03)0j7BwCk%=5 z`1f<<+i%+at5jty@kQv@9*|C2YT}^gd@9Yd^oOHU9QL5PECrUoHUyE)26oh`QW0cM zxH}tP8M>i-*4*N2JW_yK*CG6Ok-4IPSjL2tR^y;lCtJC=DDl8Yw#ZIB`l}~>{nxnZ z_hrx7D&D)|)*$`%z>Hn_Po^&vG91VFFseP%3NEh_!9UbjEm#_9*4MzeVgfY^UKg3o&`kKza-fWGpOb54O?k@kNf!L?@Xjy-5O>**|T~dNNDegzlHT=U80b2=QWK-Lv=5W1t=lk z2Vx~ZJRqYaEuP*g)De8t${$#nNSw$IU;wZcGuuHX z%%@tNV|Y@PuQc-Rb{^wnknx3)yumsv1*1yiyAw&3wU$Nw@*yT^T)79c{AUe9980_7 zp&&s4&er(7;qXK0r<*e>rpz{ZyNza?ubwcED_oC;O12-2zTn#Mv`Mo9j>Oo1UO}na zA`*yH?G50M;Qo%jv9=<0m`C2>Y^oFXqn`e|z)@Y`43-)nm8h5$`tOsFxtO}7F+rk0 z(o_w3MY7pIq!*Ldcf#X$=)7y^Ke@Q9(jFuY0`c&0H6dgev{$Mj98!%FA`=M z@v$|RDm|QtpyuPR)*yn|2hqhdHhN$@tW4%W<7VkLX zGMI9-Mfe&ZkTDrPZVRTD->*`iq2z3cUIj6Fj#-Z0s+p*P-LXsaNMQ6B)^hC?U+zrc zGg*TMao(4XhoL*d(m*R^V4RbGn!hp%GI5d3BET#O&g?1U^2^ zu8GUbKqtq<2Z@08&(WelAeY$Gi#LCJUemD`Qm31|e*Q`p>cL)9fAEM?BmFsZie7%2 zXUg;0mT7;a2S#7~Xd$*#YRa8Pg@`o@v1j?Sg)Cd`s^%SDT64c(kPrU$C04gMYx2Ol zr9^lJqY%Ya0w901;jn-Lh;Tpe3+Tltf$qY3kI_iB0u>sC$k5f(Q`($C=K39MVEKO_ z0`_@?L<;^m4t##%y4xc**#r@%=i`qlvJ_CU45c`@kZV#F|0Q&{nd0?RR$Hfpz!}~l z)>N@^KL`Y?qfe;cVqEbo_>=csm@8zjWo0G#n zL%LuLYSlvTvqFiH(f!`m-|soFWso4oF2^s&E7_5%nOY3a0Vng!sV{__CWK)ckbN@5 zxp6lRE8KTUxUFbO1712w06F3SEz9$>&2#I)9HqJ4qlK++RAS*X`Krm!9@7Qwt#rmk zXo_d+Nx0^+8A`;EKsq&yC=Dgr=>Q`5*b5o1xyd{1p$2y6t$_j~m0VcSTwBq%VB4Pf zG9)2np|0ctFQv9z$ffAsG|8Vw%NZ`UADLy$)MZZHfjr8a$Hf75pBpIp$@G0u88aLO zUKE6olo0TcokTf6&$Kl)Eo#y84#0|S*U74%1E0P9@y6{ZX_MdI=)F9U2?s8Z_0PnF z6X=1l6%$Yd)!DZ>1FzUjQ`?lOuEt&Zo&SEMHYk9bL9Xh`a&qdWz3pz!Rll(2T|Q`c z!4b|4ksV4L3Cg9N_IR%eQyr(Y4|@zVPy(U#K$^ZB@5ikL$O7xyW@sV(Z*4@-n!Dr$ zw7?B>Au&IYUc_deWx6d7HCdaQGrR#@l2XY~zW1j$K0Y36o7we#ZN?#Q8@Tys!!Isg zDB8>6fg7q^XC^4{pr<2;p3CKUd!s_67%1Ywils<;mF=xh8oUn=U+2$MBI7sb<)|~g zMy1?)R~MuyjbF7(Chm;Hb|?>wByLjb2RaA<$OOgQ4T1DL)+ZX>Ll3F-@c5n5!pi&O zbTt^Oyn1wF^!=Skg+}Q4_@eQJHh^X3;T&S8Pfj*_M5zofK&lyy)Fc#`;nhC65&ABq zDW88Y|2&Y!^<~pS$BoG+IUo8TZKQJ4TW7%~AhHzEBSHJw%%_Uv6cl)Wq~*IjYih}D z1KaoG+Jv_nWq=}e{NMcX`*Cf&O5s^W(lq6Q2-IXzM8v*sN1AFZ9|Eb?1m=TnZSeM{5Sjjdx@QiCAbq?T6Se|r2AG=pEL<1Gq_v%%kgf;Ij^LbFTLhe3%tpF zb+~Kh(n6%6?t6(kPXN{Sh@Gk7E`j2j8?-?|1Jrdc>`uzQ!R~Tj;^)t-Zg>8GbPA7v z+v3h~f#({W>YwbH$rmW0C1zuV@jQwgu;9cDfq`evD-g&qc$;=W>nyLU%TV4&Mz>%0 zmny_NZuyl|J5B0`P~NmXe$}kID??=5r<`%k{_Bv6|Z;H~fiy}Y)IhW62)gGsbUcY6;D^p12Ql}akV z&H&d9j~NNlGc)6-dbkzz>BUzv*UsCaNltbd@LGS+iXTmvohS3ZXFx7#rYq?<(oh4S zqc1a{(R>OX8>z5TxFP2JNj`hvaQ3O#{3j$71o;) zu?T*A`v@T33-5W#%DqS`S(l1q-oHAX%ho@ZUFvk+?07%`DQ^XjfKKK@S<^;L#_5_J z-Zyq{cQ6ao&G;76&*S{oif|VV0}0lkx8o>tltG*D5wK1y`v!TLA#3pZIgPoBO&!>7-^CmY$n=gI zLCz*(iGhL21tKiIL0p~jx-IjQYd2RJ*Q0ziQ6{2ci2-*{Sm@8DsNx@+<%Y+4gQKge z_*6ZalNgdI-s$~CIqo?z1)tpbsaO3Q-9ap=jyw&(-rf?w{V?QVKvH@Z#jeWX(o&}| z37lf4(oYZ^Bs_|esjKtJZj3w5yv`8t3cicbzn>Wi2fVG6>Jm;SN`s+1wJNdYUmyWo z%!yTxk-ID|-}_+xl2MC)15(BG<;x1jz>{sPZ4Mq_vqyui6978lDu8o@6u0}|W~={I z0Dq`n&$ick^eyjVN2@~y;a^RW&4Ey!*m{1txn$XQB&&cRr+_ijfM%$z`2Mg?sgxs= z-&%#>v+oxPM+5F6H$_25X@Xze1oR9hLx~{$IW{7X#xP~B$P1WvY!Oi}9z1WkOibuH zW*KVaH#~v#t2+`hK#em3OouJlsn?a1yI7C%=5uCpu&Suf)ZZVGGSQq%(1mva7FJR} z29&204Q}}*rVTN+HI#i1wQhbkZjg%~5|#3gEHSN^^56woWtjrGL6Ex1+3o?X47F z5{B#Rkh-K=tqTIWS@qv}=xXr&&b~#n`G;X*6m#&Er5LL)g3-l^iex7Ew z)m2uB??2cN6KiQ|veZqvhBt#Qsi#5q%0PDL9tPDnH9pql<$|6;HX&7*h)Y5S+E+o2 zl=MRHXi&%#_=kOR1^F@`j-$@_>08Oa>!|u}eoSZam(DoZ3YDoA6MIj~qu&{JC*6M4 z4b2p6O#d^QSCwNjWC1`{_+|u|flcHTA9c@WmI=hZw?*JxcevXd*PmOC4YuVP8_dgz zWGZsnOuuHYGIz|mu;J@>sOb=S_wKHr;~sHVu!6|%%o}8(b2c}Wog1FwJr)&2mdyvI zrD!a*c$saYv1K9F#dB%)9Aa>6W!vb|Q~Peu@6PW=X9RY;abgv6#92)6)66SZ?OVL` zW6~al_8~ZozTDg+S)O7id+7#bt@!KvB>!V4`5BXIqL%Cl8AR+#YG!nqO9_~f~*qbKNOa~ETYe)TfIZR7SPX%Fs~5p4pAd8pdmK|u_DxeEwNPy3`uvXP48b2t|y!F-FfjD>QP5viW<*zgL??Fbp z&ew5o->K;)B<0d+&<57nH=C29)nRHN#-iTST5Zx`GMh8$0h`jq556)=MMU8BI84Zz z-(BSqtQhZlv7KFh-DON#d`%7Cu9iOGDA@Y~_}(&zTHSb_gigM?wzf7alcZ-H;O-f= z;5H<2SA=gzGfn0cxM3z-F9}cqWFu3_Cs1u{zb&l@4fQdr{vLGgUCYd$%kuN*wC_Sj zh*>FlCVV3fQlr@@?=#p|LyxO*NTKOl1wOMsE~?;IHLvmR+dVUPd*&CQ{zaM>Va;K| z6ma-Q+td{Jjtg&W*)f6V2Z4Y9XjXxh*U>SF_|q3>1-X(R{zW8bh4fkIvcGFr5O;%* zVa*348ftTvI_6=tx2vyjHivps;BzOIMz=VbTki&;xZ(ah8I@%z-w!3Co5inwk=cR5 zhd@c8(pSTA>>#@Di9~z^Z^WrEyMgh@6e6%@Vk#(V6VTn(jRv5HP5ZSe_%w@WC|VUvOS>8Wz#^HK^v`d4irtL2d>xW z7I+HZGl!XR4A~R#bSo;vDu`czh?eF!1$s}a(W@pN3GTM7P>ra!!5c(a!$%bsrbjHV z8JeZg$K9DNgyxiD3f;G>;7rMmRR$yKx#%re^tvO=4`swH@-^Zqe5)Q}e zpy0e7FyY6JcdKLCI&&YHeI{>XGg+QUnEgN@GxDbMLPt%uD#Pzjg%8sBtNKjL zho~vUE*CQ?G2F}cd{s*~Hyq7|3}6!R&0a#T&1;KLh9`%;AHQ1ErPiga_CQTxEah=m z*b)m*R|h|TN=X*_qW?WF7x54digjlQUtGlXapvEJ6E;L}!FtXq`_6%Kw|UG6=gAmu z1m!NF@of|;l7$%vtI=_B37{6vQboNI53dd{9*lfMJ91yge<%Q9t;Q@CyXmerH8o-> z^aO2MS9Wzs@Y3wMrD$PAl>6^6*Gsb4b?%Q{F_bkrW|QxCaYULwFW9HCLWgoN#cPmC zfl}bx-I+70V)p&CAN3}oS?GpPF7n%5S0&aCm-L=_ziympl zbX$I&&Ehsk_kGCt{(4!#)M;au!$WanfFEg!9HZxlMi6EX!{7Xp&zf?o^x8YPs#vwiQ9t~ zBSLA+P%rH502yM*d!_njyhR-);+cFQv(Vm?0Jc6+<2tBxr|WhoBdgt~mm$37EmJe? zTN5q-^X9{rDS(dqL4ws-{mjONC;~A`l|VcHsgm zQ2RZBH*O~OG@bFbiT``p4vhQb!;Qf#xzM*rlMDEj%*qzSc*^%o+<`ur>GxGt@vxd%<74DjH&?VkXz5t z(7t7+*{AB0?`k2y?wYZfq5Q~)XNU9KD85Ia2=x1TUp3Xji^xYIzsEMKj7Ik}MzwFf@DGLluLJ4JPIXkmmzzam5W`K)_Qz3k|kYH(A7m;mlHAjV10pr(>sFHM(~@O5mhNX1#n z&J3}}d0wnYRaaZ5fUcAdIlJix_Yl3psZk__&UyN`oN3*K%9iO^_w`;*f)vypW4^kV zQacZ=?*|L!qtkpLDe1QKQ=Z zfs6>c8BZCT5b}$G2PZLcsX&lQa@4Myz&<4f`epTBj;K*oRFngV#DhpbFF*QkuvYWa zRRdc`&2J;0pM)=f^g;bWrO(#KTTjI=2#Y6DmB_IBh*0uRn-_~dQp;z!BS4XCH|=oa z)pP0P-tZ7H=yinNw~SP#dsKY+x?y1#KX2XEI$A0dA=Qp{knVQP& z6$x&rMvbG2bq0k4;g=R*%oH=K;vp(r9YH5qk*^0r*H-NZGriupo*M>E^tJCgf=E+4 zOAcY6Bq$~cdp@fy9zs_(OfM9vsN^r2HtLvC%gX+TcSg2ov&qv$-WIN9!nRtW6wqL4 zU*q$loaw`-yuLp}QNwnwvLW#6aSg@ab~K4bEs&xacC!SXOoaLrTKSA}2tSyp_4W{r zj}lLqIl7|XBty#}Jts%Dk?VP)Hze*J$Gyd5rlz=pqS%gXbz}+)#|w$_7T*(LLP&I_G2q2 zuf6&QJZBACxfak*zD0(xP|7j97iNT}-D6ECamZ4mG+}5I_IJ2AX?X32aGXe-{&Sc8 zIGog{JOWkvG9p?2tL4U!?)$PR(eX#@dM1Zz-)TBdd=+$;;5S5v+$7ZUhdd;l>^7OH zL{E!8^+m%TLjb79Sp^kV zS%>Bv8#md(T_@i;?iO6XdX1jpQU+C zIQyrG_yW8K+zc!`530BF-4_@=>2S0daR8D?>~RgdZTSHnitOd0jW`hJvx|rMGC=% z2aS9JeyMIa!K!{;0t`UZ)cq_k=M^0(5>_p9^yU?av!!fBYF)%gj0Wtu=|RNyrj8GU5;nW`AtfrOU91V2@bGZOPuloe?Vfsz%}qA#KeMe(hkNk+QGWpm z2G1Ug3{GB?wSBR?_VoX+@2^l z-n~l|v76fQeGCU1>RutTBeDKrS0j@5d4UUJFM82X%`naNl3aRSQ#66`weHJL3n!dh z4i^R9#D*4@35=+>Ba!Uv=FcPEMZQjh^p}A571;YE)0odT)G~gvI?#%^O|eJN<*BKh z??AhU>o{KAr-G@lulPV9d=v!5WbcDt$647EgRl(!(^ZUqyst(=423`lco4yW3+8|A zsmcgOAPiJ>w#fJiuslqQ*9AM7sgIuP$j_Xx+ds{8A826u+?n_% zg+OX98v5kTjR$PV#^=OO1whx|prhi>?5p=)U0_dZ#O<;iOv8a&hMQ&3Z4WPRqG__! z`gXS_15g!VYHyP12uL7b9)0O%lMpfNU*a`LQXY9+tUr-&U$J*ppjZ?oZJW33-CBKf z<+5%X?6H1E7eR6}uXRyDzKIt%Bkhe7vwLXwEa_Q|pWuiK=$*zZyv_(%BiAxhV~!l6 zv8UYy75>iLRHl(^S>I&%+H#NQap(24AAU{&njre`bg!UB-e%<6U9j#gDu2#1(evQU&bH3T<){3rc>fX{sP#MS%U*+h^X zG9Zn8TR&j$;K}TK0JsGL$rUF}X5BR!QuulJniW?2)OuY-qp%dPb1n_lg1(mu~) zW>rO2JLz@YAVL05zjO=fv4sHccBMXcOJ+@OH$-~x!8q%#F5mcZA-g;to$5K|#! z?&-s5@r0IrOsVF@tH=p#<-cI112M1l_dF@B-FG+mmK;b6j#FkxE<$A89)I~NUA%{) zo2mYJf9gSr@v-W5K=X$oPy*BGn-V}$ZW7Wl$baj9uXa@@Lm6Wz{z&QP-4#|3IPu3S z^CH%4bOBCfq7B!rWG*XOiGAt&F7k}06 zsXBu|6mNorfI>kYZ$F{4Se4@ZReot^tfgix{q8#k&~qgL*C)y)x!=rEDCg5-)d|c5 zGd=$_=vzh#8iGv~aND8=w4?4PH?p$VT|!8b6yVPU5*$Nl@pqX-f#^vgxW9 ztR6P^eXu|)f}@_D55bxYiD4=7aK-1L>!3TP#MuUrnk;Z(i0hQ_G+%=({*H%&r6X2%(AD^?;zjc?NKVd#nW@d&LhANAXRhLk@EWU6>eEx%w_0i6v!~Wvex%TkI zy4KUu6I|k*yLj}|{ET&EWMo?!0oz}N@g|?G?XLbLA=Hm3 zHE;dfyhep)u8x+bUb=oy7PO#p?E2CNYhbAAW7j_ht;g};5bU>;j#EJ5=mJM>Zf@>2 z>LXfA`=btj+;mX{2Pm5l=fXLy#s5w+k-z>oA!;)LRfCuHOx*)d5PAM@R?UD&u>+z#MRjF;5Me+xn-2^VHyK74o#k?*LGDD$NC%b|^!?En#F?SVEaR4UORqwE#swn%)ec4_yB{*PQd; z*wqo0viiCAhoXO_qh&%4I^4y1{ro$NENGLbEzNd8c7z}SIIPR}?`1#Z&yNx3F?TZj z2mJ+(XFKxLQo&If6lNq*g-ZG`vw5eR$D`Kgug!h(n(-0z!#;Q8b6fgVD73oq_qCa= z8f_N+{!m?A-Q_@MB(Pk>ee3zEK48F8vo~B>#|4b$CPMzhu>oS|F|$hSZT!dZsbx7he{4Ob^RZ~X^|ng&DPlu zA699yLKH&(TKoGms!ol-W1U?j`_5mD8`eHgHY(0Ohpz-;!Z05H*;ohm0IRHoq4?3m zg*p?!h)_vuff;eAnswM;+gOmVG_TB#ls0~x6?J4hRG+u&e(Z_{-Pt(5O=Ml~KkbS@ z`?hA!PiOzj40~7ND+BBPz5(@lpnwsal>g5q9tn2$4xU8{z zX3Yn0r(3w$^O+B(;;?dRh4OzwiaJeLx6q%$eg6d0ukynFD(r{#IWDKU%*h<*+IE_D z`%cVhOq%C}LnR#Nxm%;&l;*$F1-#08vyP8qGxh-$*Zyj-41Jzm-4)H-KZ-LSHk79weIiIyCqFHPfE6>3>Vt-=yQ=s3~)ip3j(FY}ViHKi8jZIu1%c z-w%{^ci#hNIeruWNo+mIRZSID zQ&v&Q1qYMWHV(Sa4wQVLlP%cA*EmTzb#@$w2_PRe#G9ZrJCUb$bG+8cLpTVygrEh_QTpeFJ~JUpLYcb-0=6lHSo3YjsL##tMuE6LelE?0 zfK8%WOkV`rfn%>@0)VUdf9G=#{|5)L{$JmrSja8Z$?t9>W#%Pid=bRxyOJg7GV{}JNX#_z9%)=<>i|EjVr9;AQa$9cw9sY&dlPj zIrs)@c zWypeNk!+me!3{y{cX0^`Kg=bao423dc<5bD#&PUXw2Q_9`b}!Npu-^hwb5hjIuD>n z3EFhiTnk|Y4XXF5=?{%S-1>0+{#WA4@4Kdfr~BIa`kbJP_4;JpB{2MeV{jKOqQH7} zH{edX_p@e!aS_L$VT{)KVTa5Do-VQ}<2?1i=l3hp5+uRGwS&L`NR*Z5m?GnV(p16& z?YOagHdXbliE4*fjDN&;H*gktx3RvyUOiASOS991@22SUd*+yJd`d764>PWFc6yq% z)R!S3oq;a!@MDoFDZ;QJ*)ng^4*L{5iPq-i_MrlQGBV zLP8_;u8ZB?=5t5x^6RfD`0weYU52h8a1Ye_2v`C+&J9Q*uAOG*IiL1bE+g)_12Sl; zY)-B7^y&o4(&fMx4n8lEf^HOdTS_XFkhE~ZlN?JoaYGqr!ZV2<)ORUwik9>$8i3J_ z-L~i2w^7QicbLS9`i0s5j;x51K||lFcMi&2+vL_Yvs{Ug+=#ME_xVxM)(FxmC{AK+ z-3;OJvLWIDdN{kRqLN5I@|M)E&LQxL6CMx@?V1G!3iHz)>Aqp^8Wepgz!YyZZMGbG zc9ho5a2M;B#(-drKw?vjUQqqlWPTZ0m$k{At6nnb=DCD^wyPq59R6?x4SkJmx$M`3 zf + + + 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); +} diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 654af3d9bfd0b..e9ae7c3c1cd80 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -327,6 +327,7 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic): "use_lane_filter": False, "use_inpaint": True, "inpaint_radius": 1.0, + "lane_margin": 2.0, "param_file_path": PathJoinSubstitution( [ FindPackageShare("tier4_perception_launch"), diff --git a/perception/elevation_map_loader/CMakeLists.txt b/perception/elevation_map_loader/CMakeLists.txt index 8f5faa9a38513..4c221e7af0e80 100644 --- a/perception/elevation_map_loader/CMakeLists.txt +++ b/perception/elevation_map_loader/CMakeLists.txt @@ -8,7 +8,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) + add_compile_options(-Wall -Wextra -Wpedantic) endif() find_package(PCL REQUIRED COMPONENTS io) diff --git a/perception/elevation_map_loader/README.md b/perception/elevation_map_loader/README.md index 7bfdf5729127c..d527ebe0754e9 100644 --- a/perception/elevation_map_loader/README.md +++ b/perception/elevation_map_loader/README.md @@ -36,21 +36,17 @@ Cells with No elevation value can be inpainted using the values of neighboring c ### Node parameters -| Name | Type | Description | Default value | -| :-------------------------------- | :---------- | :--------------------------------------------------------------------------------------------------------- | :------------ | -| map_layer_name | std::string | elevation_map layer name | elevation | -| param_file_path | std::string | GridMap parameters config | path_default | -| elevation_map_file_path | std::string | elevation_map file (bag2) | path_default | -| map_frame | std::string | map_frame when loading elevation_map file | map | -| use_inpaint | bool | Whether to inpaint empty cells | true | -| inpaint_radius | float | Radius of a circular neighborhood of each point inpainted that is considered by the algorithm [m] | 0.3 | -| use_elevation_map_cloud_publisher | bool | Whether to publish `output/elevation_map_cloud` | false | -| use_lane_filter | bool | Whether to filter elevation_map with vector_map | false | -| lane_margin | float | Value of how much to expand the range of vector_map [m] | 0.5 | -| lane_height_diff_thresh | float | Only point clouds in the height range of this value from vector_map are used to generate elevation_map [m] | 1.0 | -| lane_filter_voxel_size_x | float | Voxel size x for calculating point clouds in vector_map [m] | 0.04 | -| lane_filter_voxel_size_y | float | Voxel size y for calculating point clouds in vector_map [m] | 0.04 | -| lane_filter_voxel_size_z | float | Voxel size z for calculating point clouds in vector_map [m] | 0.04 | +| Name | Type | Description | Default value | +| :-------------------------------- | :---------- | :-------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| map_layer_name | std::string | elevation_map layer name | elevation | +| param_file_path | std::string | GridMap parameters config | path_default | +| elevation_map_directory | std::string | elevation_map file (bag2) | path_default | +| map_frame | std::string | map_frame when loading elevation_map file | map | +| use_inpaint | bool | Whether to inpaint empty cells | true | +| inpaint_radius | float | Radius of a circular neighborhood of each point inpainted that is considered by the algorithm [m] | 0.3 | +| use_elevation_map_cloud_publisher | bool | Whether to publish `output/elevation_map_cloud` | false | +| use_lane_filter | bool | Whether to filter elevation_map with vector_map | false | +| lane_margin | float | Margin distance from the lane polygon of the area to be included in the inpainting mask [m]. Used only when use_lane_filter=True. | 0.0 | ### GridMap parameters diff --git a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp index 308e18ddce374..57f7a2429c68c 100644 --- a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp +++ b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp @@ -73,22 +73,11 @@ class ElevationMapLoaderNode : public rclcpp::Node void publish(); void createElevationMap(); void setVerbosityLevelToDebugIfFlagSet(); - void createElevationMapFromPointcloud(); - tier4_autoware_utils::LinearRing2d getConvexHull( - const pcl::PointCloud::Ptr & input_cloud); - lanelet::ConstLanelets getIntersectedLanelets( - const tier4_autoware_utils::LinearRing2d & convex_hull, - const lanelet::ConstLanelets & road_lanelets_); - pcl::PointCloud::Ptr getLaneFilteredPointCloud( - const lanelet::ConstLanelets & joint_lanelets, - const pcl::PointCloud::Ptr & cloud); - bool checkPointWithinLanelets( - const pcl::PointXYZ & point, const lanelet::ConstLanelets & joint_lanelets); + void createElevationMapFromPointcloud( + const pcl::shared_ptr & grid_map_pcl_loader); void inpaintElevationMap(const float radius); pcl::PointCloud::Ptr createPointcloudFromElevationMap(); void saveElevationMap(); - float calculateDistancePointFromPlane( - const pcl::PointXYZ & point, const lanelet::ConstLanelet & lanelet); grid_map::GridMap elevation_map_; std::string layer_name_; @@ -97,17 +86,13 @@ class ElevationMapLoaderNode : public rclcpp::Node bool use_inpaint_; float inpaint_radius_; bool use_elevation_map_cloud_publisher_; - pcl::shared_ptr grid_map_pcl_loader_; + std::string param_file_path_; DataManager data_manager_; struct LaneFilter { - float voxel_size_x_; - float voxel_size_y_; - float voxel_size_z_; - float lane_margin_; - float lane_height_diff_thresh_; lanelet::ConstLanelets road_lanelets_; + float lane_margin_; bool use_lane_filter_; }; LaneFilter lane_filter_; diff --git a/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml b/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml index ddb99f7ce9e1a..7b415f5b2e770 100644 --- a/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml +++ b/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml @@ -5,13 +5,6 @@ - - - - - - - @@ -20,10 +13,5 @@ - - - - - diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml index 1a727f591ffcd..9643d9e2b3c92 100644 --- a/perception/elevation_map_loader/package.xml +++ b/perception/elevation_map_loader/package.xml @@ -14,6 +14,7 @@ grid_map_cv grid_map_pcl grid_map_ros + grid_map_utils lanelet2_extension libpcl-all-dev pcl_conversions diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index 57f95eb7e1e89..7d0f0d2f7a7d3 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -19,10 +19,12 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -50,7 +52,7 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio : Node("elevation_map_loader", options) { layer_name_ = this->declare_parameter("map_layer_name", std::string("elevation")); - std::string param_file_path = this->declare_parameter("param_file_path", "path_default"); + param_file_path_ = this->declare_parameter("param_file_path", "path_default"); map_frame_ = this->declare_parameter("map_frame", "map"); use_inpaint_ = this->declare_parameter("use_inpaint", true); inpaint_radius_ = this->declare_parameter("inpaint_radius", 0.3); @@ -61,16 +63,7 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio data_manager_.use_lane_filter_ = use_lane_filter; lane_filter_.use_lane_filter_ = use_lane_filter; - lane_filter_.lane_margin_ = this->declare_parameter("lane_margin", 0.5); - lane_filter_.lane_height_diff_thresh_ = this->declare_parameter("lane_height_diff_thresh", 1.0); - lane_filter_.voxel_size_x_ = declare_parameter("lane_filter_voxel_size_x", 0.04); - lane_filter_.voxel_size_y_ = declare_parameter("lane_filter_voxel_size_y", 0.04); - lane_filter_.voxel_size_z_ = declare_parameter("lane_filter_voxel_size_z", 0.04); - - auto grid_map_logger = rclcpp::get_logger("grid_map_logger"); - grid_map_logger.set_level(rclcpp::Logger::Level::Error); - grid_map_pcl_loader_ = pcl::make_shared(grid_map_logger); - grid_map_pcl_loader_->loadParameters(param_file_path); + lane_filter_.lane_margin_ = this->declare_parameter("lane_margin", 0.0); rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); @@ -153,9 +146,11 @@ void ElevationMapLoaderNode::onPointcloudMap( const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_map) { RCLCPP_INFO(this->get_logger(), "subscribe pointcloud_map"); - pcl::PointCloud map_pcl; - pcl::fromROSMsg(*pointcloud_map, map_pcl); - data_manager_.map_pcl_ptr_ = pcl::make_shared>(map_pcl); + { + pcl::PointCloud map_pcl; + pcl::fromROSMsg(*pointcloud_map, map_pcl); + data_manager_.map_pcl_ptr_ = pcl::make_shared>(map_pcl); + } if (data_manager_.isInitialized()) { publish(); } @@ -177,30 +172,29 @@ void ElevationMapLoaderNode::onVectorMap( void ElevationMapLoaderNode::createElevationMap() { - if (lane_filter_.use_lane_filter_) { - const auto convex_hull = getConvexHull(data_manager_.map_pcl_ptr_); - lanelet::ConstLanelets intersected_lanelets = - getIntersectedLanelets(convex_hull, lane_filter_.road_lanelets_); - pcl::PointCloud::Ptr lane_filtered_map_pcl_ptr = - getLaneFilteredPointCloud(intersected_lanelets, data_manager_.map_pcl_ptr_); - grid_map_pcl_loader_->setInputCloud(lane_filtered_map_pcl_ptr); - } else { - grid_map_pcl_loader_->setInputCloud(data_manager_.map_pcl_ptr_); + auto grid_map_logger = rclcpp::get_logger("grid_map_logger"); + grid_map_logger.set_level(rclcpp::Logger::Level::Error); + { + pcl::shared_ptr grid_map_pcl_loader = + pcl::make_shared(grid_map_logger); + grid_map_pcl_loader->loadParameters(param_file_path_); + grid_map_pcl_loader->setInputCloud(data_manager_.map_pcl_ptr_); + createElevationMapFromPointcloud(grid_map_pcl_loader); + elevation_map_ = grid_map_pcl_loader->getGridMap(); } - createElevationMapFromPointcloud(); - elevation_map_ = grid_map_pcl_loader_->getGridMap(); if (use_inpaint_) { inpaintElevationMap(inpaint_radius_); } saveElevationMap(); } -void ElevationMapLoaderNode::createElevationMapFromPointcloud() +void ElevationMapLoaderNode::createElevationMapFromPointcloud( + const pcl::shared_ptr & grid_map_pcl_loader) { const auto start = std::chrono::high_resolution_clock::now(); - grid_map_pcl_loader_->preProcessInputCloud(); - grid_map_pcl_loader_->initializeGridMapGeometryFromInputCloud(); - grid_map_pcl_loader_->addLayerFromInputCloud(layer_name_); + grid_map_pcl_loader->preProcessInputCloud(); + grid_map_pcl_loader->initializeGridMapGeometryFromInputCloud(); + grid_map_pcl_loader->addLayerFromInputCloud(layer_name_); grid_map::grid_map_pcl::printTimeElapsedToRosInfoStream( start, "Finish creating elevation map. Total time: ", this->get_logger()); } @@ -209,12 +203,45 @@ void ElevationMapLoaderNode::inpaintElevationMap(const float radius) { // Convert elevation layer to OpenCV image to fill in holes. // Get the inpaint mask (nonzero pixels indicate where values need to be filled in). + namespace bg = boost::geometry; + using tier4_autoware_utils::Point2d; + elevation_map_.add("inpaint_mask", 0.0); elevation_map_.setBasicLayers(std::vector()); - for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { - if (!elevation_map_.isValid(*iterator, layer_name_)) { - elevation_map_.at("inpaint_mask", *iterator) = 1.0; + if (lane_filter_.use_lane_filter_) { + for (const auto & lanelet : lane_filter_.road_lanelets_) { + auto lane_polygon = lanelet.polygon2d().basicPolygon(); + grid_map::Polygon polygon; + + if (lane_filter_.lane_margin_ > 0) { + lanelet::BasicPolygons2d out; + bg::strategy::buffer::distance_symmetric distance_strategy( + lane_filter_.lane_margin_); + bg::strategy::buffer::join_miter join_strategy; + bg::strategy::buffer::end_flat end_strategy; + bg::strategy::buffer::point_square point_strategy; + bg::strategy::buffer::side_straight side_strategy; + bg::buffer( + lane_polygon, out, distance_strategy, side_strategy, join_strategy, end_strategy, + point_strategy); + lane_polygon = out.front(); + } + for (const auto & p : lane_polygon) { + polygon.addVertex(grid_map::Position(p[0], p[1])); + } + for (grid_map_utils::PolygonIterator iterator(elevation_map_, polygon); !iterator.isPastEnd(); + ++iterator) { + if (!elevation_map_.isValid(*iterator, layer_name_)) { + elevation_map_.at("inpaint_mask", *iterator) = 1.0; + } + } + } + } else { + for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { + if (!elevation_map_.isValid(*iterator, layer_name_)) { + elevation_map_.at("inpaint_mask", *iterator) = 1.0; + } } } cv::Mat original_image; @@ -236,138 +263,6 @@ void ElevationMapLoaderNode::inpaintElevationMap(const float radius) elevation_map_.erase("inpaint_mask"); } -tier4_autoware_utils::LinearRing2d ElevationMapLoaderNode::getConvexHull( - const pcl::PointCloud::Ptr & input_cloud) -{ - // downsample pointcloud to reduce convex hull calculation cost - pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); - downsampled_cloud->points.reserve(input_cloud->points.size()); - pcl::VoxelGrid filter; - filter.setInputCloud(input_cloud); - filter.setLeafSize(0.5, 0.5, 100.0); - filter.filter(*downsampled_cloud); - - tier4_autoware_utils::MultiPoint2d candidate_points; - for (const auto & p : downsampled_cloud->points) { - candidate_points.emplace_back(p.x, p.y); - } - - tier4_autoware_utils::LinearRing2d convex_hull; - boost::geometry::convex_hull(candidate_points, convex_hull); - - return convex_hull; -} - -lanelet::ConstLanelets ElevationMapLoaderNode::getIntersectedLanelets( - const tier4_autoware_utils::LinearRing2d & convex_hull, - const lanelet::ConstLanelets & road_lanelets) -{ - lanelet::ConstLanelets intersected_lanelets; - for (const auto & road_lanelet : road_lanelets) { - if (boost::geometry::intersects(convex_hull, road_lanelet.polygon2d().basicPolygon())) { - intersected_lanelets.push_back(road_lanelet); - } - } - return intersected_lanelets; -} - -bool ElevationMapLoaderNode::checkPointWithinLanelets( - const pcl::PointXYZ & point, const lanelet::ConstLanelets & intersected_lanelets) -{ - tier4_autoware_utils::Point2d point2d(point.x, point.y); - for (const auto & lanelet : intersected_lanelets) { - if (lane_filter_.lane_margin_ > 0) { - if ( - boost::geometry::distance(point2d, lanelet.polygon2d().basicPolygon()) > - lane_filter_.lane_margin_) { - continue; - } - } else { - if (!boost::geometry::within(point2d, lanelet.polygon2d().basicPolygon())) { - continue; - } - } - - if (lane_filter_.lane_height_diff_thresh_ > 0) { - float distance = calculateDistancePointFromPlane(point, lanelet); - if (distance < lane_filter_.lane_height_diff_thresh_) { - return true; - } - } else { - return true; - } - } - return false; -} - -float ElevationMapLoaderNode::calculateDistancePointFromPlane( - const pcl::PointXYZ & point, const lanelet::ConstLanelet & lanelet) -{ - const Eigen::Vector3d point_3d(point.x, point.y, point.z); - const Eigen::Vector2d point_2d(point.x, point.y); - - const float distance_3d = boost::geometry::distance(point_3d, lanelet.centerline3d()); - const float distance_2d = boost::geometry::distance(point_2d, lanelet.centerline2d()); - const float distance = std::sqrt(distance_3d * distance_3d - distance_2d * distance_2d); - - return distance; -} - -pcl::PointCloud::Ptr ElevationMapLoaderNode::getLaneFilteredPointCloud( - const lanelet::ConstLanelets & intersected_lanelets, - const pcl::PointCloud::Ptr & cloud) -{ - pcl::PointCloud filtered_cloud; - filtered_cloud.header = cloud->header; - - pcl::PointCloud::Ptr centralized_cloud(new pcl::PointCloud); - centralized_cloud->reserve(cloud->size()); - - // The coordinates of the point cloud are too large, resulting in calculation errors, - // so offset them to the center. - // https://github.com/PointCloudLibrary/pcl/issues/4895 - Eigen::Vector4f centroid; - pcl::compute3DCentroid(*cloud, centroid); - for (const auto & p : cloud->points) { - centralized_cloud->points.push_back( - pcl::PointXYZ(p.x - centroid[0], p.y - centroid[1], p.z - centroid[2])); - } - - pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); - pcl::VoxelGrid voxel_grid; - voxel_grid.setLeafSize(lane_filter_.voxel_size_x_, lane_filter_.voxel_size_y_, 100000.0); - voxel_grid.setInputCloud(centralized_cloud); - voxel_grid.setSaveLeafLayout(true); - voxel_grid.filter(*downsampled_cloud); - - std::unordered_map> downsampled2original_map; - for (const auto & p : centralized_cloud->points) { - if (std::isnan(p.x) || std::isnan(p.y) || std::isnan(p.z)) { - continue; - } - const size_t index = voxel_grid.getCentroidIndex(p); - downsampled2original_map[index].points.push_back(p); - } - - for (auto & point : downsampled_cloud->points) { - if (checkPointWithinLanelets( - pcl::PointXYZ(point.x + centroid[0], point.y + centroid[1], point.z + centroid[2]), - intersected_lanelets)) { - const size_t index = voxel_grid.getCentroidIndex(point); - for (auto & original_point : downsampled2original_map[index].points) { - original_point.x += centroid[0]; - original_point.y += centroid[1]; - original_point.z += centroid[2]; - filtered_cloud.points.push_back(original_point); - } - } - } - - pcl::PointCloud::Ptr filtered_cloud_ptr; - filtered_cloud_ptr = pcl::make_shared>(filtered_cloud); - return filtered_cloud_ptr; -} - pcl::PointCloud::Ptr ElevationMapLoaderNode::createPointcloudFromElevationMap() { pcl::PointCloud output_cloud;