From 01805e6e0beb3d43a05aaae3e3c7d613919e1b9e Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Tue, 18 Jul 2023 20:07:50 +0200 Subject: [PATCH 01/64] initial commit lanelet2_map_learning module --- lanelet2_map_learning/CHANGELOG.rst | 3 + lanelet2_map_learning/CMakeLists.txt | 69 ++ lanelet2_map_learning/README.md | 3 + .../lanelet2_map_learning/Exceptions.h | 23 + .../include/lanelet2_map_learning/Forward.h | 116 ++++ .../include/lanelet2_map_learning/MapGraph.h | 224 ++++++ .../lanelet2_map_learning/MapGraphContainer.h | 78 +++ .../include/lanelet2_map_learning/Types.h | 46 ++ .../lanelet2_map_learning/internal/.gitignore | 0 .../lanelet2_map_learning/internal/Graph.h | 208 ++++++ .../internal/GraphUtils.h | 399 +++++++++++ .../internal/MapGraphBuilder.h | 64 ++ .../internal/MapGraphVisualization.h | 122 ++++ lanelet2_map_learning/package.xml | 27 + lanelet2_map_learning/src/.gitignore | 0 lanelet2_map_learning/src/MapGraph.cpp | 508 ++++++++++++++ lanelet2_map_learning/src/MapGraphBuilder.cpp | 356 ++++++++++ lanelet2_map_learning/test/LaneletTestMap.xml | 1 + .../test/test_lanelet_or_area_path.cpp | 213 ++++++ lanelet2_map_learning/test/test_relations.cpp | 536 ++++++++++++++ lanelet2_map_learning/test/test_route.cpp | 383 ++++++++++ lanelet2_map_learning/test/test_routing.cpp | 516 ++++++++++++++ .../test/test_routing_graph_container.cpp | 112 +++ lanelet2_map_learning/test/test_routing_map.h | 533 ++++++++++++++ .../test/test_routing_visualization.cpp | 102 +++ .../internal/eigen_converter.h | 653 ++++++++++++++++++ 26 files changed, 5295 insertions(+) create mode 100644 lanelet2_map_learning/CHANGELOG.rst create mode 100644 lanelet2_map_learning/CMakeLists.txt create mode 100644 lanelet2_map_learning/README.md create mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/Exceptions.h create mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/Forward.h create mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h create mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/MapGraphContainer.h create mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/Types.h create mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/internal/.gitignore create mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h create mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/internal/GraphUtils.h create mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphBuilder.h create mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphVisualization.h create mode 100644 lanelet2_map_learning/package.xml create mode 100644 lanelet2_map_learning/src/.gitignore create mode 100644 lanelet2_map_learning/src/MapGraph.cpp create mode 100644 lanelet2_map_learning/src/MapGraphBuilder.cpp create mode 100644 lanelet2_map_learning/test/LaneletTestMap.xml create mode 100644 lanelet2_map_learning/test/test_lanelet_or_area_path.cpp create mode 100644 lanelet2_map_learning/test/test_relations.cpp create mode 100644 lanelet2_map_learning/test/test_route.cpp create mode 100644 lanelet2_map_learning/test/test_routing.cpp create mode 100644 lanelet2_map_learning/test/test_routing_graph_container.cpp create mode 100644 lanelet2_map_learning/test/test_routing_map.h create mode 100644 lanelet2_map_learning/test/test_routing_visualization.cpp create mode 100644 lanelet2_python/include/lanelet2_python/internal/eigen_converter.h diff --git a/lanelet2_map_learning/CHANGELOG.rst b/lanelet2_map_learning/CHANGELOG.rst new file mode 100644 index 00000000..c8962696 --- /dev/null +++ b/lanelet2_map_learning/CHANGELOG.rst @@ -0,0 +1,3 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package lanelet2_map_learning +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ \ No newline at end of file diff --git a/lanelet2_map_learning/CMakeLists.txt b/lanelet2_map_learning/CMakeLists.txt new file mode 100644 index 00000000..06562477 --- /dev/null +++ b/lanelet2_map_learning/CMakeLists.txt @@ -0,0 +1,69 @@ +set(MRT_PKG_VERSION 4.0.0) +# Modify only if you know what you are doing! +cmake_minimum_required(VERSION 3.5.1) +project(lanelet2_map_learning) + +################### +## Find packages ## +################### +find_package(mrt_cmake_modules REQUIRED) +include(UseMrtStdCompilerFlags) +include(GatherDeps) + +# You can add a custom.cmake in order to add special handling for this package. E.g. you can do: +# list(REMOVE_ITEM DEPENDEND_PACKAGES ...) +# To remove libs which cannot be found automatically. You can also "find_package" other, custom dependencies there. +# You can also set PROJECT_INSTALL_FILES to install files that are not installed by default. +if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/custom.cmake") + include("${CMAKE_CURRENT_SOURCE_DIR}/custom.cmake") +endif() + +find_package(AutoDeps REQUIRED COMPONENTS ${DEPENDEND_PACKAGES}) + +mrt_parse_package_xml() + +######################## +## Add python modules ## +######################## +# This adds a python module if located under src/{PROJECT_NAME) +mrt_python_module_setup() + +mrt_glob_files(PROJECT_PYTHON_SOURCE_FILES_SRC "python_api/*.cpp") +if (PROJECT_PYTHON_SOURCE_FILES_SRC) + # Add a cpp-python api library. Make sure there are no name collisions with python modules in this project + mrt_add_python_api( ${PROJECT_NAME} + FILES ${PROJECT_PYTHON_SOURCE_FILES_SRC} + ) +endif() + +############################ +## Read source code files ## +############################ +mrt_glob_files_recurse(PROJECT_HEADER_FILES_INC "include/*.h" "include/*.hpp" "include/*.cuh") +mrt_glob_files(PROJECT_SOURCE_FILES_INC "src/*.h" "src/*.hpp" "src/*.cuh") +mrt_glob_files(PROJECT_SOURCE_FILES_SRC "src/*.cpp" "src/*.cu") + +########### +## Build ## +########### +# Declare a cpp library +mrt_add_library(${PROJECT_NAME} + INCLUDES ${PROJECT_HEADER_FILES_INC} ${PROJECT_SOURCE_FILES_INC} + SOURCES ${PROJECT_SOURCE_FILES_SRC} + ) + +############# +## Install ## +############# +# Install all targets, headers by default and scripts and other files if specified (folders or files). +# This command also exports libraries and config files for dependent packages and this supersedes catkin_package. +mrt_install(PROGRAMS scripts FILES res data ${PROJECT_INSTALL_FILES}) + +############# +## Testing ## +############# +# Add test targets for cpp and python tests +if (CATKIN_ENABLE_TESTING) + mrt_add_tests(test) + mrt_add_nosetests(test) +endif() diff --git a/lanelet2_map_learning/README.md b/lanelet2_map_learning/README.md new file mode 100644 index 00000000..8b84889c --- /dev/null +++ b/lanelet2_map_learning/README.md @@ -0,0 +1,3 @@ +# Lanelet2 Map Learning + +The map learning module for lanelet2. \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Exceptions.h b/lanelet2_map_learning/include/lanelet2_map_learning/Exceptions.h new file mode 100644 index 00000000..8a86912c --- /dev/null +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Exceptions.h @@ -0,0 +1,23 @@ +#pragma once +#include +#include + +#include + +namespace lanelet { +/** + * @brief Thrown when an export to the provided file(name) cannot be done. + */ +class ExportError : public LaneletError { + using LaneletError::LaneletError; +}; + +/** + * @brief Thrown when an there's an error in the graph. + * It will feature further information. + */ +class MapGraphError : public LaneletError { + using LaneletError::LaneletError; +}; + +} // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h new file mode 100644 index 00000000..1c2a239b --- /dev/null +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h @@ -0,0 +1,116 @@ +#pragma once + +#include + +#include +#include +#include +#include + +namespace lanelet { +namespace map_learning { +namespace internal { + +using IdPair = std::pair; + +template +class Graph; + +class MapGraphGraph; +class RouteGraph; +} // namespace internal + +using LaneId = uint16_t; +class MapGraph; +using MapGraphPtr = std::shared_ptr; +using MapGraphUPtr = std::unique_ptr; +using MapGraphConstPtr = std::shared_ptr; + +class MapGraphContainer; +using MapGraphContainerUPtr = std::unique_ptr; + +class Route; +struct LaneletRelation; +using LaneletRelations = std::vector; + +class LaneletPath; +using LaneletPaths = std::vector; +class LaneletOrAreaPath; +using LaneletOrAreaPaths = std::vector; + +//! This enum expresses the types of relations lanelet2 distiguishes internally. Between two lanelets a and b (in this +//! order), exactly one of these relation exists. +//! +//! @note The relation between b and a is different than between a and b. There is also no obvious +//! symmetry. When a is left of b, b can be either right or adjacent right to b. +enum class RelationType : uint8_t { + None = 0, //!< No relation + Successor = 0b1, //!< A (the only) direct, reachable successor. Not merging and not diverging. + Left = 0b10, //!< (the only) directly adjacent, reachable left neighbour + Right = 0b100, //!< (the only) directly adjacent, reachable right neighbour + AdjacentLeft = 0b1000, //!< directly adjacent, unreachable left neighbor + AdjacentRight = 0b10000, //!< directly adjacent, unreachable right neighbor + Conflicting = 0b100000, //!< Unreachable but with overlapping shape + Area = 0b1000000 //!< Adjacent to a reachable area +}; + +using RelationUnderlyingType = std::underlying_type_t; + +constexpr RelationType allRelations() { return static_cast(0b1111111); } +static_assert(allRelations() > RelationType::Area, "allRelations is wrong!"); + +constexpr RelationType operator~(RelationType r) { return RelationType(~RelationUnderlyingType(r)); } +constexpr RelationType operator&(RelationType r1, RelationType r2) { + return RelationType(RelationUnderlyingType(r1) & RelationUnderlyingType(r2)); +} +constexpr RelationType operator&=(RelationType& r1, RelationType r2) { return r1 = r1 & r2; } +constexpr RelationType operator|(RelationType r1, RelationType r2) { + return RelationType(std::underlying_type_t(r1) | RelationUnderlyingType(r2)); +} +constexpr RelationType operator|=(RelationType& r1, RelationType r2) { return r1 = r1 | r2; } + +// Used for graph export +inline std::string relationToString(RelationType type) { + switch (type) { + case RelationType::None: + return "None"; + case RelationType::Successor: + return "Successor"; + case RelationType::Left: + return "Left"; + case RelationType::Right: + return "Right"; + case RelationType::AdjacentLeft: + return "AdjacentLeft"; + case RelationType::AdjacentRight: + return "AdjacentRight"; + case RelationType::Conflicting: + return "Conflicting"; + case RelationType::Area: + return "Area"; + } + return ""; // some compilers need that +} + +inline std::string relationToColor(RelationType type) { + switch (type) { + case RelationType::None: + return ""; + case RelationType::Successor: + return "green"; + case RelationType::Left: + return "blue"; + case RelationType::Right: + return "magenta"; + case RelationType::Conflicting: + return "red"; + case RelationType::AdjacentLeft: + case RelationType::AdjacentRight: + return "black"; + case RelationType::Area: + return "yellow"; + } + return ""; // some compilers need that +} +} // namespace map_learning +} // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h new file mode 100644 index 00000000..531a8385 --- /dev/null +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h @@ -0,0 +1,224 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +#include + +#include "lanelet2_map_learning/Forward.h" +#include "lanelet2_map_learning/Types.h" + +namespace lanelet { +namespace map_learning { + +/** @brief Main class of the map learning module that holds the graph information and can be queried. + * The MapGraph class is the central object of this module and is initialized with a LaneletMap and TrafficRules. + * A graph with all interesting relations will be created for the traffic participant featured in the provided + * TrafficRules module. The graph can answer queries like "left", "following", "conflicting" lanelets. + * + * @note The direction of lanelets matters! 'lanelet' and 'lanelet.invert()' are differentiated since this matters when + * lanelets are passable in both directions. + * @note 'adjacent_left' and 'adjacent_right' means that there is a passable lanelet left/right of another passable + * lanelet, but a lane change is not allowed. */ +class MapGraph { + public: + using Errors = std::vector; ///< For the checkValidity function + using Configuration = std::map; ///< Used to provide a configuration + //! Defined configuration attributes + static constexpr const char ParticipantHeight[] = "participant_height"; + + /** @brief Main constructor with optional configuration. + * @param laneletMap Map that should be used to build the graph + * @param trafficRules Traffic rules that apply to find passable lanelets + * @param config Optional configuration */ + static MapGraphUPtr build(const LaneletMap& laneletMap, const traffic_rules::TrafficRules& trafficRules, + const Configuration& config = Configuration()); + + //! Similar to the above but for a LaneletSubmap + static MapGraphUPtr build(const LaneletSubmap& laneletSubmap, const traffic_rules::TrafficRules& trafficRules, + const Configuration& config = Configuration()); + + //! The graph can not be copied, only moved + MapGraph() = delete; + MapGraph(const MapGraph&) = delete; + MapGraph& operator=(const MapGraph&) = delete; + MapGraph(MapGraph&& /*other*/) noexcept; + MapGraph& operator=(MapGraph&& /*other*/) noexcept; + ~MapGraph(); + + /** @brief Determines the relation between two lanelets + * @param from Start lanelet + * @param to Goal lanelet + * @param includeConflicting if false, conflicting lanelets are not considered as relations + * @return Relation between the lanelets or nothing if no relation exists. Nothing if at least one of the lanelets + * is not passable. */ + Optional routingRelation(const ConstLanelet& from, const ConstLanelet& to, + bool includeConflicting = false) const; + + /** @brief Returns the lanelets that can be reached from this lanelet. + * @param lanelet Start lanelet + * @param withLaneChanges Include left and right lanes or not + * @return Lanelets that can be directly reached + * @see followingRelations */ + ConstLanelets following(const ConstLanelet& lanelet, bool withLaneChanges = false) const; + + /** @brief Returns the lanelets that can be reached from this lanelet and the relation. + * @param lanelet Start lanelet + * @param withLaneChanges Include left and right lanes or not + * @return Lanelets can be directly reached + * @see following */ + LaneletRelations followingRelations(const ConstLanelet& lanelet, bool withLaneChanges = false) const; + + /** @brief Returns the possible previous lanelets of this lanelet. + * @param lanelet Start lanelet + * @param withLaneChanges Include left and right lanes or not + * @return All previous lanelets + * @see previousRelations */ + ConstLanelets previous(const ConstLanelet& lanelet, bool withLaneChanges = false) const; + + /** @brief Returns the possible previous lanelets of this lanelet and the relation. + * @param lanelet Start lanelet + * @param withLaneChanges Include left and right lanes or not + * @return Lanelets that could be used to reach this lanelet + * @see previous */ + LaneletRelations previousRelations(const ConstLanelet& lanelet, bool withLaneChanges = false) const; + + /** @brief Retrieve all reachable left and right lanelets + * @param lanelet Start lanelet + * @return All left and right lanelets that can be reached, including lanelet, ordered left to right. */ + ConstLanelets besides(const ConstLanelet& lanelet) const; + + /** @brief Get left (routable) lanelet of a given lanelet if it exists. + * @param lanelet Start lanelet + * @return Left lanelet if it exists. Nothing if it doesn't. + * @see adjacentLeft, lefts, adjacentLefts */ + Optional left(const ConstLanelet& lanelet) const; + + /** @brief Get adjacent left (non-routable) lanelet of a given lanelet if it exists. + * @param lanelet Start lanelet + * @return Adjacent left lanelet if it exists. Nothing if it doesn't. + * @see left, lefts, adjacentLefts */ + Optional adjacentLeft(const ConstLanelet& lanelet) const; + + /** @brief Get right (routable) lanelet of a given lanelet if it exists. + * @param lanelet Start lanelet + * @return Right lanelet if it exists. Nothing if it doesn't. + * @see adjacentRight, rights, adjacentRights */ + Optional right(const ConstLanelet& lanelet) const; + + /** @brief Get adjacent right (non-routable) lanelet of a given lanelet if it exists. + * @param lanelet Start lanelet + * @return Adjacent right lanelet if it exists. Nothing if it doesn't. + * @see right, rights, adjacentRights */ + Optional adjacentRight(const ConstLanelet& lanelet) const; + + /** @brief Get all left (routable) lanelets of a given lanelet if they exist. + * @param lanelet Start lanelet + * @return Left lanelets if they exists. Empty if they don't. + * @see adjacentLeft, left, adjacentLefts */ + ConstLanelets lefts(const ConstLanelet& lanelet) const; + + /** @brief Get all adjacent left (non-routable) lanelets of a given lanelet if they exist. + * @param lanelet Start lanelet + * @return Adjacent left lanelets if they exists. Empty if they don't. + * @see adjacentLeft, left, lefts */ + ConstLanelets adjacentLefts(const ConstLanelet& lanelet) const; + + /** @brief Retrieve all lanelets and relations left of a given lanelet. + * @param lanelet Start lanelet + * @return All lanelets and relations left of a given lanelet. + * @see lefts, adjacentLefts */ + LaneletRelations leftRelations(const ConstLanelet& lanelet) const; + + /** @brief Get all right (routable) lanelets of a given lanelet if they exist. + * @param lanelet Start lanelet + * @return Right lanelets if they exists. Empty if they don't. + * @see adjacentRight, right, adjacentRights */ + ConstLanelets rights(const ConstLanelet& lanelet) const; + + /** @brief Get all adjacent right (non-routable) lanelets of a given lanelet if they exist. + * @param lanelet Start lanelet + * @return Adjacent right lanelets if they exists. Empty if they don't. + * @see adjacentRight, right, rights */ + ConstLanelets adjacentRights(const ConstLanelet& lanelet) const; + + /** @brief Retrieve all lanelets and relations right of a given lanelet. + * @param lanelet Start lanelet + * @return All lanelets and relations right of a given lanelet. + * @see rights, adjacentRights */ + LaneletRelations rightRelations(const ConstLanelet& lanelet) const; + + /** @brief Retrieve all lanelets that are conflicting with the given lanelet. + * + * Conflicting means that their bounding boxes overlap and the height clearance is smaller than the specified + * "participant_height". + * @param laneletOrArea Lanelet/Area to get conflicting lanelets for. + * @return All conflicting lanelets. */ + ConstLaneletOrAreas conflicting(const ConstLaneletOrArea& laneletOrArea) const; + + /** @brief Export the internal graph to graphML (xml-based) file format. + * @param filename Fully qualified file name - ideally with extension (.graphml) + * @param edgeTypesToExclude Exclude the specified relations. E.g. conflicting. Combine them with "|". + @see exportGraphViz */ + void exportGraphML(const std::string& filename, const RelationType& edgeTypesToExclude = RelationType::None) const; + + /** @brief Export the internal graph to graphViz (DOT) file format. + * This format includes coloring of the edges in the graph and bears little more information than graphML export. + * @param filename Fully qualified file name - ideally with extension (.gv) + * @param edgeTypesToExclude Exclude the specified relations. E.g. conflicting. Combine them with "|". */ + void exportGraphViz(const std::string& filename, const RelationType& edgeTypesToExclude = RelationType::None) const; + + /** @brief An abstract lanelet map holding the information of the graph. + * A good way to view the graph since it can be exported using the lanelet2_io module and there can be + * viewed in tools like JOSM. Each lanelet is represented by a point at the center of gravity of the lanelet. + * Relations are linestrings between points representing lanelets. + * @param includeAdjacent Also include adjacent (non-routable) relations + * @param includeConflicting Also include conflicting relations + * @return LaneletMap with the requested information */ + LaneletMapPtr getDebugLaneletMap(bool includeAdjacent = false, bool includeConflicting = false) const; + + /** + * @brief Returns a submap that contains all lanelets and areas within this graph, and nothing else. + * You can obtain a full map of the graph by calling passabelSubmap()->laneletMap(), which ist a potentially + * costly operation. + */ + inline LaneletSubmapConstPtr passableSubmap() const noexcept { return passableLaneletSubmap_; } + + /** @brief LaneletSubmap that includes all passable lanelets and areas. + * This map contains all passable lanelets and areas with all primitives (linestrings, points), including regulatory + * elements and lanelets referenced by them. It can be used to perform spacial queries e.g. for localization. When + * selecting a lanelet from this map please be aware that the graph may also contain the inverted lanelet. + * @return LaneletMap with all passable lanelets and areas + * + * This function is deprecated because it was misleading that the map also contained lanelets referenced by regulatory + * elements and not only the lanelets from the graph. + */ + [[deprecated("Use passableSubmap to obtain the lanelets and areas within the graph!")]] inline LaneletMapConstPtr + passableMap() const noexcept { + return passableSubmap()->laneletMap(); + } + + /** @brief Performs some basic sanity checks. + * It is recommended to call this function after the graph has been generated since it can point out some + * mapping errors. + * @throws MapGraphError if an error is found an 'throwOnError' is true + * @param throwOnError Decide wheter to throw an exception or just return the errors + * @return Possible errors if 'throwOnError' is false. */ + Errors checkValidity(bool throwOnError = true) const; + + /** + * Constructs the graph. Don't call this directly, use MapGraph::build instead. + */ + MapGraph(std::unique_ptr&& graph, lanelet::LaneletSubmapConstPtr&& passableMap); + + private: + //! Documentation to be found in the cpp file. + std::unique_ptr graph_; ///< Wrapper of the graph + LaneletSubmapConstPtr passableLaneletSubmap_; ///< Lanelet map of all passable lanelets +}; + +} // namespace map_learning +} // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphContainer.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphContainer.h new file mode 100644 index 00000000..2053c774 --- /dev/null +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphContainer.h @@ -0,0 +1,78 @@ +#pragma once + +#include +#include +#include + +#include +#include + +#include "lanelet2_map_learning/MapGraph.h" + +namespace lanelet { +namespace map_learning { + +/** @brief Container to associate multiple graphs to allow queries on multiple graphs + * @note We cannot use the 'conflicting' relations that have been determined when creating the individual graphs + * because they used their respective height in 3D (e.g. 2m for a pedestrian), but the participant we want to query for + * could be taller (e.g. 4m truck). Therefore we can't rely on that. */ +class MapGraphContainer { + public: + using ConflictingInGraph = std::pair; //!< id of conflicing graph, lanelets in conflict there + using ConflictingInGraphs = std::vector; + + /** @brief Constructor of graph container + * @param MapGraphs The graphs that should be used in the container */ + explicit MapGraphContainer(std::vector MapGraphs) : graphs_{std::move(MapGraphs)} {} + + /** @brief Constructor of graph container + * @param MapGraphs The graphs that should be used in the container */ + explicit MapGraphContainer(const std::vector& MapGraphs) + : graphs_{utils::transform(MapGraphs, [](auto& g) { return MapGraphConstPtr(g); })} {} + + /** @brief Find the conflicting lanelets of a given lanelet within a specified graph + * @param lanelet Find conflicting ones for this lanelet + * @param MapGraphId ID/position in vector of the graph + * @param participantHeight Optional height of the participant + * @return Conflicting lanelets within that graph + * @throws InvalidInputError if the MapGraphId is too high */ + ConstLanelets conflictingInGraph(const ConstLanelet& lanelet, size_t MapGraphId, + double participantHeight = .0) const { + if (MapGraphId >= graphs_.size()) { + throw InvalidInputError("Graph ID is higher than the number of graphs."); + } + auto overlaps = [lanelet, participantHeight](const ConstLanelet& ll) { + return participantHeight != .0 ? !geometry::overlaps3d(lanelet, ll, participantHeight) + : !geometry::overlaps2d(lanelet, ll); + }; + const auto map{graphs_[MapGraphId]->passableSubmap()}; + ConstLanelets conflicting{map->laneletLayer.search(geometry::boundingBox2d(lanelet))}; + auto begin = conflicting.begin(); + auto end = conflicting.end(); + end = std::remove(begin, end, lanelet); + end = std::remove_if(begin, end, overlaps); + conflicting.erase(end, conflicting.end()); + return conflicting; + } + + /** @brief Find the conflicting lanelets of a given lanelet within all graphs + * @param lanelet Find conflicting ones for this lanelet + * @param participantHeight Optional height of the participant + * @return Conflicting lanelets within the graphs */ + ConflictingInGraphs conflictingInGraphs(const ConstLanelet& lanelet, double participantHeight = .0) const { + ConflictingInGraphs result; + for (size_t it = 0; it < graphs_.size(); it++) { + result.emplace_back(std::make_pair(it, conflictingInGraph(lanelet, it, participantHeight))); + } + return result; + } + + //! Returns the graphs stored in the container + const std::vector& MapGraphs() const { return graphs_; } + + private: + std::vector graphs_; ///< Map graphs of the container. +}; + +} // namespace map_learning +} // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h new file mode 100644 index 00000000..21257394 --- /dev/null +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h @@ -0,0 +1,46 @@ +#pragma once +#include +#include + +#include + +#include "lanelet2_map_learning/Forward.h" + +namespace lanelet { +namespace map_learning { + +//! This object carries the required information for the graph neighbourhood search +struct LaneletOrAreaVisitInformation { + ConstLaneletOrArea laneletOrArea; //!< The lanelet or area that is currently visited + ConstLaneletOrArea predecessor; //!< Its predecessor on the shortest path + double cost{}; //!< The accumulated cost from the start along shortest path + size_t length{}; //!< Number of elements from start to here along shortest path (including this) + size_t numLaneChanges{}; //!< Number of lane changes from start to here along shortest path +}; + +//! This object carries the required information for the graph neighbourhood search +struct LaneletVisitInformation { + ConstLanelet lanelet; //!< The lanelet or area that is currently visited + ConstLanelet predecessor; //!< Its predecessor on the shortest path + double cost{}; //!< The accumulated cost from the start along shortest path + size_t length{}; //!< The number of lanelets from start to here along shortest path (including this) + size_t numLaneChanges{}; //!< Number of lane changes from start to here along shortest path +}; + +using LaneletVisitFunction = std::function; +using LaneletOrAreaVisitFunction = std::function; + +//! Represents the relation of a lanelet to another lanelet +struct LaneletRelation { + ConstLanelet lanelet; //!< the lanelet this relation refers to + RelationType relationType; //!< the type of relation to that +}; +inline bool operator==(const LaneletRelation& lhs, const LaneletRelation& rhs) { + return lhs.lanelet == rhs.lanelet && lhs.relationType == rhs.relationType; +} +inline bool operator!=(const LaneletRelation& rhs, const LaneletRelation& lhs) { return !(rhs == lhs); } + +using LaneletRelations = std::vector; + +} // namespace map_learning +} // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/.gitignore b/lanelet2_map_learning/include/lanelet2_map_learning/internal/.gitignore new file mode 100644 index 00000000..e69de29b diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h b/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h new file mode 100644 index 00000000..8c749eb1 --- /dev/null +++ b/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h @@ -0,0 +1,208 @@ +#pragma once + +#include + +#include +#include +#include +#include + +#include "lanelet2_map_learning/Exceptions.h" +#include "lanelet2_map_learning/Forward.h" + +namespace lanelet { +namespace map_learning { +namespace internal { +/** @brief Internal information of a vertex in the graph + * If A* search is adapted, this could hold information about longitude and latitude. */ +struct VertexInfo { + // careful. You must be sure that this is indeed a lanelet + const ConstLanelet& lanelet() const { return static_cast(laneletOrArea); } + const ConstArea& area() const { return static_cast(laneletOrArea); } + const ConstLaneletOrArea& get() const { return laneletOrArea; } + ConstLaneletOrArea laneletOrArea; +}; + +/** @brief Internal information of a vertex in the route graph */ +struct MapVertexInfo { + const ConstLanelet& get() const { return lanelet; } + + ConstLanelet lanelet; + LaneId laneId{}; + ConstLaneletOrAreas conflictingInMap; +}; + +/** @brief Internal information of an edge in the graph */ +struct EdgeInfo { + RelationType relation; ///< Relation between the two lanelets. E.g. SUCCESSOR or CONFLICTING. +}; + +/// General graph type definitions +using GraphType = boost::adjacency_list; +using RouteGraphType = boost::adjacency_list; +using GraphTraits = boost::graph_traits; + +template +struct EdgeFilter; + +/// Filtered graphs provide a filtered view on a graph. Here we can filter out types of relations (e.g. CONFLICTING) +template +using FilteredGraphT = boost::filtered_graph>; + +template +using FilteredGraphTraits = boost::graph_traits>; + +using FilteredMapGraph = FilteredGraphT; +using FilteredRouteGraph = FilteredGraphT; + +/** @brief An internal edge filter to get a filtered view on the graph. */ +template +struct EdgeFilter { + /// Needed to be able to iterate edges + EdgeFilter() = default; + + /// @brief Constructor for a filter that includes all relation types. + /// @param graph Base graph + EdgeFilter(const GraphT& graph) : pmRelation_{boost::get(&EdgeInfo::relation, graph)} {} + + /// @brief Constructor for a filter that includes all allowed relations in 'relation' + /// @param graph Base graph + /// @param relation Relation that will pass the filter + EdgeFilter(const GraphT& graph, const RelationType& relation) + : relations_{relation}, pmRelation_{boost::get(&EdgeInfo::relation, graph)} {} + + /// @brief Operator that determines wheter an edge should pass the filter or not. + template + inline bool operator()(const Edge& e) const { + return relations_ == allRelations() || bool(relations_ & boost::get(pmRelation_, e)); + } + + private: + RelationType relations_{allRelations()}; ///< Relations that pass the filter + typename boost::property_map::const_type + pmRelation_; ///< Property map to the relations of edges in the graph +}; + +using LaneletOrAreaToVertex = std::unordered_map; +using FilteredGraphDesc = std::pair; + +/// @brief Manages the actual graph and provieds different views on the edges (lazily computed) +template +class Graph { + public: + using FilteredGraph = FilteredGraphT; + using CostFilter = EdgeFilter; + using Vertex = typename boost::graph_traits::vertex_descriptor; + using Edge = typename boost::graph_traits::edge_descriptor; + + explicit Graph() {} + + inline const BaseGraphT& get() const noexcept { return graph_; } + inline BaseGraphT& get() noexcept { return graph_; } + inline const LaneletOrAreaToVertex& vertexLookup() const noexcept { return laneletOrAreaToVertex_; } + + FilteredGraph withLaneChanges() const { + return getFilteredGraph(RelationType::Successor | RelationType::Left | RelationType::Right); + } + + FilteredGraph withoutLaneChanges() const { return getFilteredGraph(RelationType::Successor); } + + FilteredGraph withAreasAndLaneChanges() const { + return getFilteredGraph(RelationType::Successor | RelationType::Left | RelationType::Right | RelationType::Area); + } + + FilteredGraph withAreasWithoutLaneChanges() const { + return getFilteredGraph(RelationType::Successor | RelationType::Area); + } + + FilteredGraph left() const { return getFilteredGraph(RelationType::Left); } + FilteredGraph somehowLeft() const { return getFilteredGraph(RelationType::Left | RelationType::AdjacentLeft); } + + FilteredGraph right() const { return getFilteredGraph(RelationType::Right); } + FilteredGraph somehowRight() const { return getFilteredGraph(RelationType::Right | RelationType::AdjacentRight); } + + FilteredGraph adjacentLeft() const { return getFilteredGraph(RelationType::AdjacentLeft); } + + FilteredGraph adjacentRight() const { return getFilteredGraph(RelationType::AdjacentRight); } + + FilteredGraph conflicting() const { return getFilteredGraph(RelationType::Conflicting); } + + FilteredGraph withoutConflicting() const { return getFilteredGraph(allRelations() | ~RelationType::Conflicting); } + + inline bool empty() const noexcept { return laneletOrAreaToVertex_.empty(); } + + //! add new lanelet to graph + inline Vertex addVertex(const typename BaseGraphT::vertex_property_type& property) { + GraphType::vertex_descriptor vd = 0; + vd = boost::add_vertex(graph_); + graph_[vd] = property; + laneletOrAreaToVertex_.emplace(property.get(), vd); + return vd; + } + + void addEdge(const ConstLaneletOrArea& from, const ConstLaneletOrArea& to, const EdgeInfo& edgeInfo) { + auto fromVertex = getVertex(from); + auto toVertex = getVertex(to); + if (!fromVertex || !toVertex) { + assert(false && "Lanelet/Area is not part of the graph."); // NOLINT + return; + } + addEdge(*fromVertex, *toVertex, edgeInfo); + } + + void addEdge(Vertex from, Vertex to, const EdgeInfo& edgeInfo) { + auto edge = boost::add_edge(from, to, graph_); + assert(edge.second && "Edge could not be added to the graph."); + graph_[edge.first] = edgeInfo; + } + + /** @brief Received the edgeInfo between two given vertices + * @return EdgeInfo or nothing if there's no edge */ + Optional getEdgeInfo(const ConstLanelet& from, const ConstLanelet& to) const noexcept { + return getEdgeInfoFor(from, to, graph_); + } + + /** @brief Received the edgeInfo between two given vertices and a given (filtered)graph + * @return EdgeInfo or nothing if there's no edge */ + template + Optional getEdgeInfoFor(const ConstLanelet& from, const ConstLanelet& to, const Graph& g) const noexcept { + auto fromVertex = getVertex(from); + auto toVertex = getVertex(to); + if (!fromVertex || !toVertex) { + return {}; + } + auto edgeToNext{boost::edge(*fromVertex, *toVertex, g)}; + if (edgeToNext.second) { + return g[edgeToNext.first]; + } + return {}; + } + + //! Helper function to determine the graph vertex of a given lanelet + Optional::vertex_descriptor> getVertex( + const ConstLaneletOrArea& lanelet) const noexcept { + try { + return laneletOrAreaToVertex_.at(lanelet); + } catch (std::out_of_range&) { + return {}; + } + } + + private: + FilteredGraph getFilteredGraph(RelationType relations) const { + return FilteredGraph(graph_, CostFilter(graph_, relations)); + } + BaseGraphT graph_; //!< The actual graph object + LaneletOrAreaToVertex laneletOrAreaToVertex_; //!< Mapping of lanelets/areas to vertices of the graph +}; + +class MapGraphGraph : public Graph { + using Graph::Graph; +}; +class RouteGraph : public Graph { + using Graph::Graph; +}; + +} // namespace internal +} // namespace map_learning +} // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/GraphUtils.h b/lanelet2_map_learning/include/lanelet2_map_learning/internal/GraphUtils.h new file mode 100644 index 00000000..9506415b --- /dev/null +++ b/lanelet2_map_learning/include/lanelet2_map_learning/internal/GraphUtils.h @@ -0,0 +1,399 @@ +#pragma once +#include + +#include +#include + +#include "lanelet2_map_learning/internal/Graph.h" + +namespace lanelet { +namespace map_learning { +namespace internal { +using LaneletVertexId = GraphTraits::vertex_descriptor; +using LaneletVertexIds = std::vector; +using RouteLanelets = std::set; + +template +inline bool has(const ContainerT& c, const T& t) { + return std::find(c.begin(), c.end(), t) != c.end(); +} +template +inline bool has(const std::set& c, const T& t) { + return c.find(t) != c.end(); +} +template +inline bool hasRelation(const GraphT& g, EdgeT e) { + return (g[e].relation & R) != RelationType::None; +} +template +Optional getNext(LaneletVertexId ofVertex, const Graph& g) { + auto edges = boost::out_edges(ofVertex, g); + auto it = std::find_if(edges.first, edges.second, [&](auto e) { return hasRelation(g, e); }); + if (it != edges.second) { + return boost::target(*it, g); + } + return {}; +} + +// Class that selects between in_edges and out_edges (i wish I had c++17...) +template +struct GetEdges {}; +template <> +struct GetEdges { + template + inline auto operator()(Id id, Graph& g) { + return in_edges(id, g); + } +}; +template <> +struct GetEdges { + template + inline auto operator()(Id id, Graph& g) { + return out_edges(id, g); + } +}; + +// Class that selects between in_edges and out_edges (i wish I had c++17...) +template +struct GetTarget {}; +template <> +struct GetTarget { + using T = FilteredMapGraph::vertex_descriptor; + template + inline T operator()(Id id, Graph& g) { + return boost::source(id, g); + } +}; +template <> +struct GetTarget { + using T = FilteredMapGraph::vertex_descriptor; + template + inline T operator()(Id id, Graph& g) { + return boost::target(id, g); + } +}; + +template +bool anySidewayNeighbourIs(Vertex v, const Graph& g, Func&& f) { + Optional currVertex = v; + while (!!currVertex && !f(*currVertex)) { + currVertex = getNext(*currVertex, g); + } + if (!!currVertex) { + return true; + } + currVertex = getNext(v, g); + while (!!currVertex && !f(*currVertex)) { + currVertex = getNext(*currVertex, g); + } + return !!currVertex; +} + +template +std::set getAllNeighbourLanelets(LaneletVertexId ofVertex, const Graph& ofRoute) { + std::set result{ofVertex}; + Optional currVertex = ofVertex; + while (!!currVertex) { + result.insert(*currVertex); + currVertex = getNext(*currVertex, ofRoute); + } + currVertex = ofVertex; + while (!!currVertex) { + result.insert(*currVertex); + currVertex = getNext(*currVertex, ofRoute); + } + return result; +} + +//! Filter that reduces the original graph by edges that belong to different cost types or lane changes +class OriginalGraphFilter { + public: + OriginalGraphFilter() = default; + OriginalGraphFilter(const GraphType& g, bool withLaneChange) + : g_{&g}, filterMask_{RelationType::Successor | RelationType::Conflicting} { + if (withLaneChange) { + filterMask_ |= + RelationType::Left | RelationType::Right | RelationType::AdjacentLeft | RelationType::AdjacentRight; + } + } + bool operator()(const GraphTraits::edge_descriptor& v) const { + const auto& edge = (*g_)[v]; + return (edge.relation & filterMask_) != RelationType::None; + } + + private: + const GraphType* g_; + RelationType filterMask_; +}; +using OriginalGraph = boost::filtered_graph; + +//! Reduces the graph to a set of desired vertices +class OnRouteFilter { + public: + OnRouteFilter() = default; + explicit OnRouteFilter(const RouteLanelets& onRoute) : onRoute_{&onRoute} {} + + bool operator()(LaneletVertexId vertexId) const { return onRoute_->find(vertexId) != onRoute_->end(); } + + private: + const RouteLanelets* onRoute_{}; +}; + +template +class EdgeRelationFilter { + public: + EdgeRelationFilter() = default; + explicit EdgeRelationFilter(const GraphType& graph) : graph_{&graph} {} + bool operator()(FilteredMapGraph::edge_descriptor e) const { + auto type = (*graph_)[e].relation; + return (type & Relation) != RelationType::None; + } + + private: + const GraphType* graph_{}; +}; + +//! Removes edges from the graph that are not drivable (e.g. adjacent or conflicing) +using OnlyDrivableEdgesFilter = + EdgeRelationFilter; +using OnlyConflictingFilter = EdgeRelationFilter; + +//! Removes conflicting edges from the graph +class NoConflictingFilter { + public: + NoConflictingFilter() = default; + explicit NoConflictingFilter(const OriginalGraph& originalGraph) : originalGraph_{&originalGraph} {} + bool operator()(FilteredMapGraph::edge_descriptor e) const { + auto type = (*originalGraph_)[e].relation; + return type != RelationType::Conflicting; + } + + private: + const OriginalGraph* originalGraph_{}; +}; + +//! Removes vertices from the graph that are not adjacent to a set of vertices. Adjacent can also mean conflicting! +class NextToRouteFilter { + public: + NextToRouteFilter() = default; + NextToRouteFilter(const RouteLanelets& onRoute, const OriginalGraph& originalGraph) + : onRoute_{&onRoute}, originalGraph_{&originalGraph} {} + + bool operator()(LaneletVertexId vertexId) const { + // at least one out edge must be connected to lanelets on the route. This includes conflicting and adjacent! + if (onRoute_->find(vertexId) != onRoute_->end()) { + return true; + } + auto outEdges = boost::out_edges(vertexId, *originalGraph_); + auto connectedToRoute = std::any_of(outEdges.first, outEdges.second, [&](OriginalGraph::edge_descriptor e) { + auto dest = boost::target(e, *originalGraph_); + return onRoute_->find(dest) != onRoute_->end() || + onRoute_->find(boost::source(e, *originalGraph_)) != onRoute_->end(); + }); + return connectedToRoute; + } + + private: + const RouteLanelets* onRoute_{}; + const OriginalGraph* originalGraph_{}; +}; + +//! Removes edges from the graph that are not drivable (e.g. adjacent or conflicing) OR leave the route at its end +class OnlyDrivableEdgesWithinFilter { + using SuccessorFilter = EdgeRelationFilter; + + public: + OnlyDrivableEdgesWithinFilter() = default; + explicit OnlyDrivableEdgesWithinFilter(RouteLanelets withinLanelets, const OriginalGraph& originalGraph) + : drivableEdge_{originalGraph}, successorEdge_{originalGraph}, withinLanelets_{std::move(withinLanelets)} {} + bool operator()(FilteredMapGraph::edge_descriptor e) const { + return drivableEdge_(e) && (!successorEdge_(e) || withinLanelets_.find(e.m_source) == withinLanelets_.end()); + } + + private: + OnlyDrivableEdgesFilter drivableEdge_; + SuccessorFilter successorEdge_; + RouteLanelets withinLanelets_; +}; + +//! Finds vertices that are in conflict or adjacent to some vertices (not reachable though bidirectional lane changes) +class ConflictingSectionFilter { + public: + ConflictingSectionFilter() = default; + explicit ConflictingSectionFilter(const OriginalGraph& g, const RouteLanelets& onRoute) + : g_{&g}, onRoute_{&onRoute} {} + + bool operator()(LaneletVertexId vertexId) const { + // conflicting if it is not yet part of the route but in conflict with a route lanelet + if (has(*onRoute_, vertexId)) { + return false; + } + auto outEdges = boost::out_edges(vertexId, *g_); + bool isNeighbour = false; + bool isConflicting = false; + std::for_each(outEdges.first, outEdges.second, [&](auto edge) { + if (onRoute_->find(boost::target(edge, *g_)) == onRoute_->end()) { + return; + } + auto type = (*g_)[edge].relation; + auto neighbourTypes = RelationType::Left | RelationType::Right; + auto conflictTypes = RelationType::Conflicting | RelationType::AdjacentLeft | RelationType::AdjacentRight; + if ((type & (neighbourTypes)) != RelationType::None) { + auto outEdge = boost::edge(boost::target(edge, *g_), boost::source(edge, *g_), *g_); + auto reverseIsNeigh = outEdge.second && ((*g_)[outEdge.first].relation & neighbourTypes) != RelationType::None; + isNeighbour |= reverseIsNeigh; + isConflicting |= !isNeighbour; + } + isConflicting |= (type & (conflictTypes)) != RelationType::None; + }); + return isConflicting && !isNeighbour; + } + + private: + const OriginalGraph* g_{}; + const RouteLanelets* onRoute_{}; +}; + +//! Finds vertices within a set of vertices that are in conflict with another set of vertices +class OnRouteAndConflictFilter { + public: + OnRouteAndConflictFilter() = default; + explicit OnRouteAndConflictFilter(const RouteLanelets& onRoute, const std::vector& conflictWith, + const OriginalGraph& g) + : onRoute_{&onRoute}, conflictWith_{&conflictWith}, g_{&g} {} + + bool operator()(LaneletVertexId vertexId) const { + auto isOk = anySidewayNeighbourIs(vertexId, *g_, [&](auto v) { + if (!has(*onRoute_, vertexId)) { + return false; + } + auto outEdges = boost::out_edges(v, *g_); + auto isConflicting = [&](OriginalGraph::edge_descriptor e) { + auto conflictTypes = RelationType::Conflicting | RelationType::AdjacentLeft | RelationType::AdjacentRight; + auto type = (*g_)[e].relation; + return (type & conflictTypes) != RelationType::None && has(*conflictWith_, boost::target(e, *g_)); + }; + return v == start_ || v == end_ || std::any_of(outEdges.first, outEdges.second, isConflicting); + }); + return isOk; + } + + void setConflicting(const LaneletVertexIds& conflictWith) { conflictWith_ = &conflictWith; } + void setStart(LaneletVertexId start) { start_ = start; } + void setEnd(LaneletVertexId end) { end_ = end; } + + private: + const RouteLanelets* onRoute_{}; + const LaneletVertexIds* conflictWith_{}; + LaneletVertexId start_{}; + LaneletVertexId end_{}; + const OriginalGraph* g_{}; +}; + +//! For graph queries, we implement our own color map because boost's color does not perform well on sparse graphs +struct SparseColorMap { + using key_type = LaneletVertexId; // NOLINT + using value_type = boost::two_bit_color_type; // NOLINT + using reference = void; // NOLINT + using category = boost::read_write_property_map_tag; // NOLINT + using MapT = std::map; + std::shared_ptr data{std::make_shared()}; +}; + +inline SparseColorMap::value_type get(const SparseColorMap& map, LaneletVertexId key) { + auto val = map.data->find(key); + if (val != map.data->end()) { + return static_cast(val->second); + } + return SparseColorMap::value_type::two_bit_white; +} + +inline void put(SparseColorMap& map, LaneletVertexId key, SparseColorMap::value_type value) { + (*map.data)[key] = static_cast(value); +} + +//! An iterator that finds paths from a start vertex to all reachable destinations. +template +class ConnectedPathIterator { + public: + using Vertices = std::vector; + + private: + template + class PathVisitor : public boost::default_dfs_visitor { + public: + PathVisitor(Func& f, Vertices& path) : path_{&path}, f_{&f} {} + void discover_vertex(typename GraphT::vertex_descriptor v, const GraphT& /*g*/) { // NOLINT + path_->push_back(v); + movingForward_ = true; + } + void finish_vertex(typename GraphT::vertex_descriptor v, const GraphT& /*g*/) { // NOLINT + if (movingForward_) { + (*f_)(*path_); + } + movingForward_ = false; + assert(!path_->empty()); + assert(path_->back() == v); + path_->pop_back(); + } + + private: + bool movingForward_{true}; + Vertices* path_; + const std::decay_t* f_; + }; + + public: + ConnectedPathIterator() = default; + explicit ConnectedPathIterator(const GraphT& g) : g_{g} {} + + //! Calls a function for all full paths starting from start. A full path is a path from start to either a leaf or the + //! last unvisited vertex in loops. + template + void forEachPath(LaneletVertexId start, Func&& f) { + assert(g_.m_vertex_pred(start)); + path_.clear(); + PathVisitor vis(f, path_); + SparseColorMap cm; + boost::depth_first_visit(g_, start, vis, cm); + } + + //! Returns whether a path exists in the graph that connects from and to + bool hasPathFromTo(LaneletVertexId from, LaneletVertexId to) { + class PathFound {}; + auto destinationReached = [&](const auto& path) { + return std::any_of(std::make_reverse_iterator(path.end()), std::make_reverse_iterator(path.begin()), + [&](auto p) { return p == to; }); + }; + try { + forEachPath(from, [&](const auto& path) { + if (destinationReached(path)) { + throw PathFound{}; + } + }); + } catch (PathFound) { // NOLINT(misc-throw-by-value-catch-by-reference) + return true; + } + return false; + } + + GraphT& graph() { return g_; } + + private: + GraphT g_; + Vertices path_; +}; + +// Aliases for some graphs needed by us +using OnRouteGraph = boost::filtered_graph; +using DrivableGraph = boost::filtered_graph; +using NoConflictingGraph = boost::filtered_graph; +using OnlyConflictingGraph = boost::filtered_graph; +using NextToRouteGraph = boost::filtered_graph; +using ConflictOrAdjacentToRouteGraph = + boost::filtered_graph; +using ConflictsWithPathGraph = boost::filtered_graph; + +} // namespace internal +} // namespace map_learning +} // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphBuilder.h b/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphBuilder.h new file mode 100644 index 00000000..f9ee94a3 --- /dev/null +++ b/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphBuilder.h @@ -0,0 +1,64 @@ +#pragma once +#include + +#include "lanelet2_map_learning/MapGraph.h" +#include "lanelet2_map_learning/internal/Graph.h" + +namespace lanelet { +class LaneletLayer; + +namespace map_learning { +namespace internal { + +class LaneChangeLaneletsCollector; + +class MapGraphBuilder { + public: + MapGraphBuilder(const traffic_rules::TrafficRules& trafficRules, const MapGraph::Configuration& config); + + MapGraphUPtr build(const LaneletMapLayers& laneletMapLayers); + + private: + using PointsLaneletMap = std::multimap; + using PointsLaneletMapIt = PointsLaneletMap::iterator; + using PointsLaneletMapResult = std::pair; + + static ConstLanelets getPassableLanelets(const LaneletLayer& lanelets, + const traffic_rules::TrafficRules& trafficRules); + static ConstAreas getPassableAreas(const AreaLayer& areas, const traffic_rules::TrafficRules& trafficRules); + void appendBidirectionalLanelets(ConstLanelets& llts); + void addLaneletsToGraph(ConstLanelets& llts); + void addAreasToGraph(ConstAreas& areas); + void addEdges(const ConstLanelets& lanelets, const LaneletLayer& passableLanelets); + void addEdges(const ConstAreas& areas, const LaneletLayer& passableLanelets, const AreaLayer& passableAreas); + void addFollowingEdges(const ConstLanelet& ll); + void addSidewayEdge(LaneChangeLaneletsCollector& laneChangeLanelets, const ConstLanelet& ll, + const ConstLineString3d& bound, const RelationType& relation); + void addConflictingEdge(const ConstLanelet& ll, const LaneletLayer& passableLanelets); + void addLaneChangeEdges(LaneChangeLaneletsCollector& laneChanges, const RelationType& relation); + void addAreaEdge(const ConstArea& area, const LaneletLayer& passableLanelets); + void addAreaEdge(const ConstArea& area, const AreaLayer& passableAreas); + + //! Helper function to read the participant height from the configuration + Optional participantHeight() const; + + //! Adds the first and last points of a lanelet to the search index + void addPointsToSearchIndex(const ConstLanelet& ll); + bool hasEdge(const ConstLanelet& from, const ConstLanelet& to); + + void assignLaneChange(ConstLanelets froms, ConstLanelets tos, const RelationType& relation); + /** @brief Assigns edge to a relation between two lanelets + * @param from Start lanelet + * @param to Goal lanelet + * @param relation Relation between the two lanelets */ + void assignEdge(const ConstLaneletOrArea& from, const ConstLaneletOrArea& to, const RelationType& relation); + + std::unique_ptr graph_; + PointsLaneletMap pointsToLanelets_; ///< A map of tuples (first or last left and right boundary points) to lanelets + std::set bothWaysLaneletIds_; + const traffic_rules::TrafficRules& trafficRules_; + const MapGraph::Configuration& config_; +}; +} // namespace internal +} // namespace map_learning +} // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphVisualization.h b/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphVisualization.h new file mode 100644 index 00000000..ba6ee963 --- /dev/null +++ b/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphVisualization.h @@ -0,0 +1,122 @@ +#pragma once + +#include +#include +#include + +#include "lanelet2_map_learning/Exceptions.h" +#include "lanelet2_map_learning/Forward.h" +#include "lanelet2_map_learning/internal/Graph.h" + +namespace lanelet { + +// This one needs to be in this namepace. Otherwise it's not found. +inline std::istream& operator>>(std::istream& is, ConstLaneletOrArea& /*r*/) { return is; } + +namespace map_learning { +inline std::ostream& operator<<(std::ostream& os, const RelationType& r) { return os << relationToString(r); } +inline std::istream& operator>>(std::istream& is, const RelationType& /*r*/) { return is; } + +namespace internal { + +/** @brief Internal vertex writer for graphViz file export. */ +template +class VertexWriterGraphViz { + public: + explicit VertexWriterGraphViz(const Graph* g) : graph_(g) {} + template + void operator()(std::ostream& out, const VertexOrEdge& v) const { + const Id id{(*graph_)[v].laneletOrArea.id()}; + out << "[label=\"" << id << "\" lanelet=\"" << id << "\"]"; + } + + private: + const Graph* graph_; +}; + +/** @brief Internal edge writer for graphViz file export. + Includes label and color. */ +template +class EdgeWriterGraphViz { + public: + explicit EdgeWriterGraphViz(const Graph* g) : graph_(g) {} + template + void operator()(std::ostream& out, const VertexOrEdge& v) const { + const RelationType relation{(*graph_)[v].relation}; + out << "[label=\"" << relationToString(relation) << "\" color=\"" << relationToColor(relation); + } + + private: + const Graph* graph_; +}; + +/** @brief Implementation of graphViz export function + * @param filename Fully qualified file name - ideally with extension (.gv) + * @param g Graph to export + * @param edgeFilter Edge filter that will be used to create a filtered graph + * @param vertexFilter Vertex filter. Not used yet */ +template +inline void exportGraphVizImpl(const std::string& filename, const G& g, E edgeFilter = boost::keep_all(), + V vertexFilter = boost::keep_all()) { + std::ofstream file; + file.open(filename); + if (!file.is_open()) { + throw lanelet::ExportError("Could not open file at " + filename + "."); + } + + VertexWriterGraphViz vertexWriter(&g); + EdgeWriterGraphViz edgeWriter(&g); + boost::filtered_graph fg(g, edgeFilter, vertexFilter); + boost::write_graphviz(file, fg, vertexWriter, edgeWriter); + + file.close(); +} + +/** @brief GraphViz export function + * @param filename Fully qualified file name - ideally with extension (.gv) + * @param g Graph to export + * @param relationTypes Relations that will be included in the export */ +template +inline void exportGraphVizImpl(const std::string& filename, const G& g, const RelationType& relationTypes) { + auto edgeFilter = EdgeFilter(g, relationTypes); + exportGraphVizImpl(filename, g, edgeFilter); +} + +/** @brief Implementation of graphML export function + * @param filename Fully qualified file name - ideally with extension (.graphml) + * @param g Graph to export + * @param eFilter Edge filter that will be used to create a filtered graph + * @param vFilter Vertex filter. Not used yet */ +template +inline void exportGraphMLImpl(const std::string& filename, const G& g, E eFilter = boost::keep_all(), + V vFilter = boost::keep_all()) { + std::ofstream file; + file.open(filename); + if (!file.is_open()) { + throw lanelet::ExportError("Could not open file at " + filename + "."); + } + + boost::filtered_graph> filteredGraph(g, eFilter, vFilter); + + auto pmId = boost::get(&VertexInfo::laneletOrArea, filteredGraph); // NOLINT + auto pmRelation = boost::get(&EdgeInfo::relation, filteredGraph); + + boost::dynamic_properties dp; + dp.property("info", pmId); + dp.property("relation", pmRelation); + + boost::write_graphml(file, filteredGraph, dp, false); +} + +/** @brief GraphML export function + * @param filename Fully qualified file name - ideally with extension (.graphml) + * @param g Graph to export + * @param relationTypes Relations that will be included in the export */ +template +inline void exportGraphMLImpl(const std::string& filename, const G& g, const RelationType& relationTypes) { + auto edgeFilter = EdgeFilter(g, relationTypes); + exportGraphMLImpl(filename, g, edgeFilter); +} +} // namespace internal +} // namespace map_learning +} // namespace lanelet diff --git a/lanelet2_map_learning/package.xml b/lanelet2_map_learning/package.xml new file mode 100644 index 00000000..2644da55 --- /dev/null +++ b/lanelet2_map_learning/package.xml @@ -0,0 +1,27 @@ + + + + lanelet2_map_learning + 1.2.1 + Map learning module for lanelet2 + + BSD + Fabian Immel + Fabian Immel + https://github.com/fzi-forschungszentrum-informatik/lanelet2.git + + catkin + ament_cmake_core + mrt_cmake_modules + mrt_cmake_modules + gtest + boost + lanelet2_core + lanelet2_traffic_rules + + + catkin + ament_cmake + + + diff --git a/lanelet2_map_learning/src/.gitignore b/lanelet2_map_learning/src/.gitignore new file mode 100644 index 00000000..e69de29b diff --git a/lanelet2_map_learning/src/MapGraph.cpp b/lanelet2_map_learning/src/MapGraph.cpp new file mode 100644 index 00000000..b0e0acbf --- /dev/null +++ b/lanelet2_map_learning/src/MapGraph.cpp @@ -0,0 +1,508 @@ +#include "lanelet2_map_learning/MapGraph.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include // Asserts +#include +#include + +#include "lanelet2_map_learning/Exceptions.h" +#include "lanelet2_map_learning/Forward.h" +#include "lanelet2_map_learning/internal/Graph.h" +#include "lanelet2_map_learning/internal/GraphUtils.h" +#include "lanelet2_map_learning/internal/MapGraphBuilder.h" +#include "lanelet2_map_learning/internal/MapGraphVisualization.h" + +namespace lanelet { +namespace map_learning { + +#if __cplusplus < 201703L +constexpr const char MapGraph::ParticipantHeight[]; +#endif + +using internal::FilteredMapGraph; +using internal::GraphType; +using internal::LaneletVertexId; +using internal::MapGraphGraph; + +template +T reservedVector(size_t size) { + T t; + t.reserve(size); + return t; +} + +//! Helper function to create a new point that represents a lanelet. +template +PointT createPoint(const ConstLaneletOrArea& ll) { + PointT p; + p.setId(ll.id()); + p.setAttribute("id", Attribute(ll.id())); + if (ll.isLanelet()) { + boost::geometry::centroid(utils::toHybrid(ll.lanelet()->polygon2d()), p); + } + if (ll.isArea()) { + boost::geometry::centroid(utils::toHybrid(utils::to2D(ll.area()->outerBoundPolygon())), p); + } + return p; +} + +/** @brief Implementation function to retrieve a neighboring vertex + * @throws MapGraphError if 'throwOnError' is true and there is more than one neighboring lanelet + * @param vertex Start vertex + * @param graph Filtered graph with the allowed type of edge + * @param throwOnError Decides wheter to throw or ignore an error + * @return Neighboring lanelet */ +Optional neighboringImpl(const GraphType::vertex_descriptor vertex, const FilteredMapGraph& graph, + bool throwOnError = false) { + auto outEdges = boost::out_edges(vertex, graph); + if (throwOnError && std::distance(outEdges.first, outEdges.second) > 1) { + std::string ids; + std::for_each(outEdges.first, outEdges.second, [&graph, &ids](const auto& edge) { + ids += " " + std::to_string(graph[boost::target(edge, graph)].laneletOrArea.id()); + }); + throw MapGraphError("More than one neighboring lanelet to " + std::to_string(graph[vertex].laneletOrArea.id()) + + " with this relation:" + ids); + } + if (outEdges.first != outEdges.second) { + return graph[boost::target(*(outEdges.first), graph)].laneletOrArea; + } + return {}; +} + +Optional neighboringLaneletImpl(const GraphType::vertex_descriptor vertex, const FilteredMapGraph& graph, + bool throwOnError = false) { + auto value = neighboringImpl(vertex, graph, throwOnError); + if (!!value && value->isLanelet()) { + return value->lanelet(); + } + return {}; +} + +template +Optional ifInGraph(const MapGraphGraph& g, const ConstLaneletOrArea& llt, Func f) { + auto vertex = g.getVertex(llt); + if (!vertex) { + return {}; + } + return f(*vertex); +} + +template +Optional ifLaneletInGraph(const MapGraphGraph& g, const ConstLanelet& llt, Func f) { + auto laneletVertex = g.getVertex(llt); + if (!laneletVertex) { + return {}; + } + return f(*laneletVertex); +} + +template +ConstLanelets getUntilEnd(const ConstLanelet& start, Func next) { + auto result = reservedVector(3); + Optional current = start; + while (!!(current = next(*current))) { + result.emplace_back(*current); + } + return result; +} + +ConstLaneletOrAreas getAllEdgesFromGraph(const MapGraphGraph& graph, const FilteredMapGraph& subgraph, + const ConstLaneletOrArea& laneletOrArea, bool edgesOut) { + ConstLaneletOrAreas result; + auto laneletVertex = graph.getVertex(laneletOrArea); + if (!laneletVertex) { + return result; + } + auto processEdges = [&](auto edgeRange) { + result.reserve(size_t(std::distance(edgeRange.first, edgeRange.second))); + for (; edgeRange.first != edgeRange.second; edgeRange.first++) { + auto node = + edgesOut ? boost::target(*edgeRange.first, graph.get()) : boost::source(*edgeRange.first, graph.get()); + result.emplace_back(graph.get()[node].laneletOrArea); + } + return result; + }; + return edgesOut ? processEdges(boost::out_edges(*laneletVertex, subgraph)) + : processEdges(boost::in_edges(*laneletVertex, subgraph)); +} + +ConstLanelets getLaneletEdgesFromGraph(const MapGraphGraph& graph, const FilteredMapGraph& subgraph, + const ConstLanelet& lanelet, bool edgesOut) { + ConstLanelets result; + auto allEdges = getAllEdgesFromGraph(graph, subgraph, lanelet, edgesOut); + result = reservedVector(allEdges.size()); + for (auto& edge : allEdges) { + if (edge.isLanelet()) { + result.push_back(*edge.lanelet()); + } + } + return result; +} + +MapGraph::MapGraph(MapGraph&& /*other*/) noexcept = default; +MapGraph& MapGraph::operator=(MapGraph&& /*other*/) noexcept = default; +MapGraph::~MapGraph() = default; + +MapGraphUPtr MapGraph::build(const LaneletMap& laneletMap, const traffic_rules::TrafficRules& trafficRules, + const MapGraph::Configuration& config) { + return internal::MapGraphBuilder(trafficRules, config).build(laneletMap); +} + +MapGraphUPtr MapGraph::build(const LaneletSubmap& laneletSubmap, const traffic_rules::TrafficRules& trafficRules, + const MapGraph::Configuration& config) { + return internal::MapGraphBuilder(trafficRules, config).build(laneletSubmap); +} + +Optional MapGraph::routingRelation(const ConstLanelet& from, const ConstLanelet& to, + bool includeConflicting) const { + auto edgeInfo = includeConflicting ? graph_->getEdgeInfo(from, to) + : graph_->getEdgeInfoFor(from, to, graph_->withoutConflicting()); + if (!!edgeInfo) { + return edgeInfo->relation; + } + return {}; +} + +ConstLanelets MapGraph::following(const ConstLanelet& lanelet, bool withLaneChanges) const { + auto subgraph = withLaneChanges ? graph_->withLaneChanges() : graph_->withoutLaneChanges(); + return getLaneletEdgesFromGraph(*graph_, subgraph, lanelet, true); +} + +LaneletRelations MapGraph::followingRelations(const ConstLanelet& lanelet, bool withLaneChanges) const { + ConstLanelets foll{following(lanelet, withLaneChanges)}; + LaneletRelations result; + for (auto const& it : foll) { + result.emplace_back(LaneletRelation{it, *routingRelation(lanelet, it)}); + } + return result; +} // namespace map_learning + +ConstLanelets MapGraph::previous(const ConstLanelet& lanelet, bool withLaneChanges) const { + auto subgraph = withLaneChanges ? graph_->withLaneChanges() : graph_->withoutLaneChanges(); + return getLaneletEdgesFromGraph(*graph_, subgraph, lanelet, false); +} + +LaneletRelations MapGraph::previousRelations(const ConstLanelet& lanelet, bool withLaneChanges) const { + ConstLanelets prev{previous(lanelet, withLaneChanges)}; + LaneletRelations result; + result.reserve(prev.size()); + for (auto const& it : prev) { + Optional relation{routingRelation(it, lanelet)}; + if (!!relation) { + result.emplace_back(LaneletRelation{it, *relation}); + } else { + assert(false && "Two Lanelets in a route are not connected. This shouldn't happen."); // NOLINT + } + } + return result; +} + +ConstLanelets MapGraph::besides(const ConstLanelet& lanelet) const { + auto move = [](auto it) { return std::make_move_iterator(it); }; + ConstLanelets left{lefts(lanelet)}; + ConstLanelets right{rights(lanelet)}; + ConstLanelets result; + result.reserve(left.size() + right.size() + 1); + result.insert(std::end(result), move(left.rbegin()), move(left.rend())); + result.push_back(lanelet); + result.insert(std::end(result), move(std::begin(right)), move(std::end(right))); + return result; +} + +Optional MapGraph::left(const ConstLanelet& lanelet) const { + return ifLaneletInGraph(*graph_, lanelet, + [&](auto& vertex) { return neighboringLaneletImpl(vertex, graph_->left()); }); +} + +Optional MapGraph::adjacentLeft(const ConstLanelet& lanelet) const { + return ifLaneletInGraph(*graph_, lanelet, + [&](auto& vertex) { return neighboringLaneletImpl(vertex, graph_->adjacentLeft()); }); +} + +Optional MapGraph::right(const ConstLanelet& lanelet) const { + return ifLaneletInGraph(*graph_, lanelet, + [&](auto& vertex) { return neighboringLaneletImpl(vertex, graph_->right()); }); +} + +Optional MapGraph::adjacentRight(const ConstLanelet& lanelet) const { + return ifLaneletInGraph(*graph_, lanelet, + [&](auto& vertex) { return neighboringLaneletImpl(vertex, graph_->adjacentRight()); }); +} + +ConstLanelets MapGraph::lefts(const ConstLanelet& lanelet) const { + return getUntilEnd(lanelet, [&](const ConstLanelet& llt) { return left(llt); }); +} + +ConstLanelets MapGraph::adjacentLefts(const ConstLanelet& lanelet) const { + return getUntilEnd(lanelet, [&](const ConstLanelet& llt) { return adjacentLeft(llt); }); +} + +LaneletRelations MapGraph::leftRelations(const ConstLanelet& lanelet) const { + bool leftReached{false}; + ConstLanelet current = lanelet; + LaneletRelations result; + while (!leftReached) { + const ConstLanelets leftOf{lefts(current)}; + for (auto const& it : leftOf) { + result.emplace_back(LaneletRelation{it, RelationType::Left}); + current = it; + } + const ConstLanelets adjacentLeftOf{adjacentLefts(current)}; + for (auto const& it : adjacentLeftOf) { + result.emplace_back(LaneletRelation{it, RelationType::AdjacentLeft}); + current = it; + } + leftReached = (leftOf.empty() && adjacentLeftOf.empty()); + } + return result; +} + +ConstLanelets MapGraph::rights(const ConstLanelet& lanelet) const { + return getUntilEnd(lanelet, [&](const ConstLanelet& llt) { return right(llt); }); +} + +ConstLanelets MapGraph::adjacentRights(const ConstLanelet& lanelet) const { + return getUntilEnd(lanelet, [&](const ConstLanelet& llt) { return adjacentRight(llt); }); +} + +LaneletRelations MapGraph::rightRelations(const ConstLanelet& lanelet) const { + bool rightReached{false}; + ConstLanelet current = lanelet; + auto result = reservedVector(3); + while (!rightReached) { + const ConstLanelets rightOf{rights(current)}; + for (auto const& it : rightOf) { + result.emplace_back(LaneletRelation{it, RelationType::Right}); + current = it; + } + const ConstLanelets adjacentRightOf{adjacentRights(current)}; + for (auto const& it : adjacentRightOf) { + result.emplace_back(LaneletRelation{it, RelationType::AdjacentRight}); + current = it; + } + rightReached = (rightOf.empty() && adjacentRightOf.empty()); + } + return result; +} + +ConstLaneletOrAreas MapGraph::conflicting(const ConstLaneletOrArea& laneletOrArea) const { + return getAllEdgesFromGraph(*graph_, graph_->conflicting(), laneletOrArea, true); +} + +void MapGraph::exportGraphML(const std::string& filename, const RelationType& edgeTypesToExclude) const { + if (filename.empty()) { + throw InvalidInputError("No filename passed"); + } + RelationType relations = allRelations() & ~edgeTypesToExclude; + internal::exportGraphMLImpl(filename, graph_->get(), relations); +} + +void MapGraph::exportGraphViz(const std::string& filename, const RelationType& edgeTypesToExclude) const { + if (filename.empty()) { + throw InvalidInputError("No filename passed"); + } + RelationType relations = allRelations() & ~edgeTypesToExclude; + internal::exportGraphVizImpl(filename, graph_->get(), relations); +} + +//! Helper function to slim down getDebugLaneletMap +RelationType allowedRelationsfromConfiguration(bool includeAdjacent, bool includeConflicting) { + RelationType allowedRelations{RelationType::Successor | RelationType::Left | RelationType::Right | + RelationType::Area}; + if (includeAdjacent) { + allowedRelations |= RelationType::AdjacentLeft; + allowedRelations |= RelationType::AdjacentRight; + } + if (includeConflicting) { + allowedRelations |= RelationType::Conflicting; + } + return allowedRelations; +} + +LineString3d createLineString(const Point2d& from, const Point2d& to, RelationType relation) { + LineString2d lineString(utils::getId()); + lineString.push_back(from); + lineString.push_back(to); + LineString3d lineString3d(lineString); + lineString3d.setAttribute("relation", relationToString(relation)); + return lineString3d; +} + +class DebugMapBuilder { + public: + using LaneletOrAreaPair = std::pair; + explicit DebugMapBuilder(const FilteredMapGraph& graph) : graph_{graph} {} + LaneletMapPtr run(const internal::LaneletOrAreaToVertex& loa) { + LaneletMapPtr output = std::make_shared(); + for (const auto& vertex : loa) { + visitVertex(vertex); + } + auto lineStrings = utils::transform(lineStringMap_, [](auto& mapLs) { return mapLs.second; }); + auto map = utils::createMap(lineStrings); + for (auto& p : pointMap_) { + map->add(utils::to3D(p.second)); + } + return map; + } + + private: + void visitVertex(const internal::LaneletOrAreaToVertex::value_type& vertex) { + addPoint(vertex.first); + auto edges = boost::out_edges(vertex.second, graph_); + for (auto edge = edges.first; edge != edges.second; ++edge) { + const auto& target = graph_[boost::target(*edge, graph_)].laneletOrArea; + addPoint(target); + const auto& edgeInfo = graph_[*edge]; + addEdge(vertex.first, target, edgeInfo); + } + } + + static LaneletOrAreaPair getPair(const ConstLaneletOrArea& first, const ConstLaneletOrArea& second) { + return first.id() < second.id() ? LaneletOrAreaPair(first, second) : LaneletOrAreaPair(second, first); + } + + void addPoint(const ConstLaneletOrArea& point) { + auto inMap = pointMap_.find(point); + if (inMap == pointMap_.end()) { + pointMap_.emplace(point, createPoint(point)); + } + } + + void addEdge(const ConstLaneletOrArea& from, const ConstLaneletOrArea& to, internal::EdgeInfo edge) { + auto pair = getPair(from, to); + auto inMap = lineStringMap_.find(pair); + if (inMap != lineStringMap_.end()) { + inMap->second.setAttribute("relation_reverse", relationToString(edge.relation)); + + } else { + auto pFrom = pointMap_.at(from); + auto pTo = pointMap_.at(to); + LineString3d lineString3d{createLineString(pFrom, pTo, edge.relation)}; + lineStringMap_.emplace(pair, lineString3d); + } + } + + FilteredMapGraph graph_; + std::unordered_map lineStringMap_; // Stores all relations + std::unordered_map pointMap_; // Stores all 'edges' +}; + +LaneletMapPtr MapGraph::getDebugLaneletMap(bool includeAdjacent, bool includeConflicting) const { + internal::EdgeFilter edgeFilter(graph_->get(), + allowedRelationsfromConfiguration(includeAdjacent, includeConflicting)); + FilteredMapGraph filteredGraph(graph_->get(), edgeFilter); + return DebugMapBuilder(filteredGraph).run(graph_->vertexLookup()); +} + +MapGraph::Errors MapGraph::checkValidity(bool throwOnError) const { + Errors errors; + for (const auto& laWithVertex : graph_->vertexLookup()) { + const auto& la = laWithVertex.first; + auto ll = laWithVertex.first.lanelet(); + const auto& vertex = laWithVertex.second; + auto id = la.id(); + // Check left relation + Optional left; + try { + left = neighboringLaneletImpl(vertex, graph_->left(), true); + + } catch (MapGraphError& e) { + errors.emplace_back(std::string("Left: ") + e.what()); + } + Optional adjacentLeft; + try { + adjacentLeft = neighboringLaneletImpl(vertex, graph_->adjacentLeft(), true); + } catch (MapGraphError& e) { + errors.emplace_back(std::string("Adjacent left: ") + e.what()); + } + if (left && adjacentLeft) { + errors.emplace_back("Lanelet " + std::to_string(id) + " has both 'left' (id: " + std::to_string(left->id()) + + ") and 'adjancent_left' (id: " + std::to_string(adjacentLeft->id()) + ") lanelet"); + } + if (left) { + LaneletRelations rel{rightRelations(*left)}; + if (rel.empty()) { + errors.emplace_back("There is a 'left' relation from " + std::to_string(id) + " to " + + std::to_string(left->id()) + " but no relation back"); + } else if (rel.front().lanelet != ll) { + errors.emplace_back("There is a 'left' relation from " + std::to_string(id) + " to " + + std::to_string(left->id()) + ", but " + std::to_string(id) + + " isn't the closest lanelet the other way round"); + } + } + if (adjacentLeft) { + LaneletRelations rel{rightRelations(*adjacentLeft)}; + if (rel.empty()) { + errors.emplace_back("There is a 'adjacentLeft' relation from " + std::to_string(id) + " to " + + std::to_string(adjacentLeft->id()) + " but no relation back"); + } else if (rel.front().lanelet != ll) { + errors.emplace_back("There is a 'adjacentLeft' relation from " + std::to_string(id) + " to " + + std::to_string(adjacentLeft->id()) + ", but " + std::to_string(id) + + " isn't the closest lanelet the other way round"); + } + } + // Check right + Optional right; + try { + right = neighboringLaneletImpl(vertex, graph_->right(), true); + } catch (MapGraphError& e) { + errors.emplace_back(std::string("Right: ") + e.what()); + } + Optional adjacentRight; + try { + adjacentRight = neighboringLaneletImpl(vertex, graph_->adjacentRight(), true); + } catch (MapGraphError& e) { + errors.emplace_back(std::string("Adjacent right: ") + e.what()); + } + if (right && adjacentRight) { + errors.emplace_back("Lanelet " + std::to_string(id) + " has both 'right' (id: " + std::to_string(right->id()) + + ") and 'adjancent_right' (id: " + std::to_string(adjacentRight->id()) + ") lanelet"); + } + if (right) { + LaneletRelations rel{leftRelations(*right)}; + if (rel.empty()) { + errors.emplace_back("There is a 'right' relation from " + std::to_string(id) + " to " + + std::to_string(right->id()) + " but no relation back"); + } else if (rel.front().lanelet != ll) { + errors.emplace_back("There is a 'right' relation from " + std::to_string(id) + " to " + + std::to_string(right->id()) + ", but " + std::to_string(id) + + " isn't the closest lanelet the other way round"); + } + } + if (adjacentRight) { + LaneletRelations rel{leftRelations(*adjacentRight)}; + if (rel.empty()) { + errors.emplace_back("There is a 'adjacentRight' relation from " + std::to_string(id) + " to " + + std::to_string(adjacentRight->id()) + " but no relation back"); + } else if (rel.front().lanelet != ll) { + errors.emplace_back("There is a 'adjacentRight' relation from " + std::to_string(id) + " to " + + std::to_string(adjacentRight->id()) + ", but " + std::to_string(id) + + " isn't the closest lanelet the other way round"); + } + } + } + if (throwOnError && !errors.empty()) { + std::stringstream ss; + ss << "Errors found in graph:"; + for (const auto& err : errors) { + ss << "\n\t- " << err; + } + throw MapGraphError(ss.str()); + } + return errors; +} + +MapGraph::MapGraph(std::unique_ptr&& graph, LaneletSubmapConstPtr&& passableMap) + : graph_{std::move(graph)}, passableLaneletSubmap_{std::move(passableMap)} {} + +} // namespace map_learning +} // namespace lanelet diff --git a/lanelet2_map_learning/src/MapGraphBuilder.cpp b/lanelet2_map_learning/src/MapGraphBuilder.cpp new file mode 100644 index 00000000..cd8a51fd --- /dev/null +++ b/lanelet2_map_learning/src/MapGraphBuilder.cpp @@ -0,0 +1,356 @@ +#include "lanelet2_map_learning/internal/MapGraphBuilder.h" + +#include +#include +#include + +#include + +#include "lanelet2_map_learning/Exceptions.h" +#include "lanelet2_map_learning/MapGraph.h" +#include "lanelet2_map_learning/internal/Graph.h" + +namespace lanelet { +namespace map_learning { +namespace internal { +namespace { +inline IdPair orderedIdPair(const Id id1, const Id id2) { return (id1 < id2) ? IdPair(id1, id2) : IdPair(id2, id1); } +} // namespace + +//! This class collects lane changable lanelets and combines them to a sequence of adjacent lanechangable lanelets +class LaneChangeLaneletsCollector { + struct LaneChangeInfo { + ConstLanelet target; + bool visited; + }; + using LaneChangeMap = std::unordered_map; + + public: + using LaneChangeLanelets = std::pair; + + LaneChangeLaneletsCollector() = default; + void add(ConstLanelet from, ConstLanelet to) { + laneChanges_.emplace(std::move(from), LaneChangeInfo{std::move(to), false}); + currPos_ = laneChanges_.begin(); + } + + template + Optional getNextChangeLanelets(Func1&& prev, Func2&& next) { + for (; currPos_ != laneChanges_.end() && currPos_->second.visited; ++currPos_) { + } + if (currPos_ == laneChanges_.end()) { + return {}; + } + return getLaneChangeLanelets(currPos_, std::forward(prev), std::forward(next)); + } + + private: + template + LaneChangeLanelets getLaneChangeLanelets(LaneChangeMap::iterator iter, Func1&& prev, Func2&& next) { + iter->second.visited = true; + auto followers = getAdjacentLaneChangeLanelets(iter, std::forward(next)); + auto predecessors = getAdjacentLaneChangeLanelets(iter, std::forward(prev)); + std::reverse(predecessors.first.begin(), predecessors.first.end()); + std::reverse(predecessors.second.begin(), predecessors.second.end()); + std::pair result; + result.first = utils::concatenate({predecessors.first, ConstLanelets{iter->first}, followers.first}); + result.second = utils::concatenate({predecessors.second, ConstLanelets{iter->second.target}, followers.second}); + return result; + } + + template + LaneChangeLanelets getAdjacentLaneChangeLanelets(LaneChangeMap::iterator iter, Func1&& adjacent) { + std::pair successors; + while (true) { + auto nextSourceLlts = adjacent(iter->first); + auto nextTargetLlts = adjacent(iter->second.target); + if (nextSourceLlts.size() != 1 || nextTargetLlts.size() != 1) { + break; + } + ConstLanelet& nextSourceLlt = nextSourceLlts.front(); + ConstLanelet& nextTargetLlt = nextTargetLlts.front(); + iter = laneChanges_.find(nextSourceLlt); + if (iter == laneChanges_.end() || iter->second.visited || iter->second.target != nextTargetLlt) { + break; + } + iter->second.visited = true; + successors.first.push_back(nextSourceLlt); + successors.second.push_back(nextTargetLlt); + } + return successors; + } + + LaneChangeMap laneChanges_; + LaneChangeMap::iterator currPos_{laneChanges_.end()}; +}; + +MapGraphBuilder::MapGraphBuilder(const traffic_rules::TrafficRules& trafficRules, const MapGraph::Configuration& config) + : graph_{std::make_unique()}, trafficRules_{trafficRules}, config_{config} {} + +MapGraphUPtr MapGraphBuilder::build(const LaneletMapLayers& laneletMapLayers) { + auto passableLanelets = getPassableLanelets(laneletMapLayers.laneletLayer, trafficRules_); + auto passableAreas = getPassableAreas(laneletMapLayers.areaLayer, trafficRules_); + auto passableMap = utils::createConstSubmap(passableLanelets, passableAreas); + appendBidirectionalLanelets(passableLanelets); + addLaneletsToGraph(passableLanelets); + addAreasToGraph(passableAreas); + addEdges(passableLanelets, passableMap->laneletLayer); + addEdges(passableAreas, passableMap->laneletLayer, passableMap->areaLayer); + return std::make_unique(std::move(graph_), std::move(passableMap)); +} + +ConstLanelets MapGraphBuilder::getPassableLanelets(const LaneletLayer& lanelets, + const traffic_rules::TrafficRules& trafficRules) { + ConstLanelets llts; + llts.reserve(lanelets.size()); + std::copy_if(lanelets.begin(), lanelets.end(), std::back_inserter(llts), + [&trafficRules](const ConstLanelet& llt) { return trafficRules.canPass(llt); }); + return llts; +} + +ConstAreas MapGraphBuilder::getPassableAreas(const AreaLayer& areas, const traffic_rules::TrafficRules& trafficRules) { + ConstAreas ars; + ars.reserve(areas.size()); + std::copy_if(areas.begin(), areas.end(), std::back_inserter(ars), + [&trafficRules](const ConstArea& area) { return trafficRules.canPass(area); }); + return ars; +} + +void MapGraphBuilder::appendBidirectionalLanelets(ConstLanelets& llts) { + std::deque invLanelets; + for (auto& ll : llts) { + if (trafficRules_.canPass(ll.invert())) { + invLanelets.push_back(ll.invert()); + bothWaysLaneletIds_.emplace(ll.id()); + } + } + llts.insert(llts.end(), invLanelets.begin(), invLanelets.end()); +} + +void MapGraphBuilder::addLaneletsToGraph(ConstLanelets& llts) { + for (auto& ll : llts) { + graph_->addVertex(VertexInfo{ll}); + addPointsToSearchIndex(ll); + } +} + +void MapGraphBuilder::addAreasToGraph(ConstAreas& areas) { + for (auto& ar : areas) { + graph_->addVertex(VertexInfo{ar}); + } +} + +void MapGraphBuilder::addEdges(const ConstLanelets& lanelets, const LaneletLayer& passableLanelets) { + LaneChangeLaneletsCollector leftToRight; + LaneChangeLaneletsCollector rightToLeft; + // Check relations between lanelets + for (auto const& ll : lanelets) { + addFollowingEdges(ll); + addSidewayEdge(rightToLeft, ll, ll.leftBound(), RelationType::AdjacentLeft); + addSidewayEdge(leftToRight, ll, ll.rightBound(), RelationType::AdjacentRight); + addConflictingEdge(ll, passableLanelets); + } + + // now process the lane changes + addLaneChangeEdges(rightToLeft, RelationType::Left); + addLaneChangeEdges(leftToRight, RelationType::Right); +} + +void MapGraphBuilder::addEdges(const ConstAreas& areas, const LaneletLayer& passableLanelets, + const AreaLayer& passableAreas) { + for (const auto& area : areas) { + addAreaEdge(area, passableLanelets); + addAreaEdge(area, passableAreas); + } +} + +void MapGraphBuilder::addFollowingEdges(const ConstLanelet& ll) { + auto endPointsLanelets = + pointsToLanelets_.equal_range(orderedIdPair(ll.leftBound().back().id(), ll.rightBound().back().id())); + // Following + ConstLanelets following; + std::for_each(endPointsLanelets.first, endPointsLanelets.second, [&ll, this, &following](auto it) { + if (geometry::follows(ll, it.second) && this->trafficRules_.canPass(ll, it.second)) { + following.push_back(it.second); + } + }); + if (following.empty()) { + return; + } + // find out if there are several previous merging lanelets + ConstLanelets merging; + std::for_each(endPointsLanelets.first, endPointsLanelets.second, [&following, this, &merging](auto it) { + if (geometry::follows(it.second, following.front()) && this->trafficRules_.canPass(it.second, following.front())) { + merging.push_back(it.second); + } + }); +} + +void MapGraphBuilder::addSidewayEdge(LaneChangeLaneletsCollector& laneChangeLanelets, const ConstLanelet& ll, + const ConstLineString3d& bound, const RelationType& relation) { + auto directlySideway = [&relation, &ll](const ConstLanelet& sideLl) { + return relation == RelationType::AdjacentLeft ? geometry::leftOf(sideLl, ll) : geometry::rightOf(sideLl, ll); + }; + auto sideOf = pointsToLanelets_.equal_range(orderedIdPair(bound.front().id(), bound.back().id())); + for (auto it = sideOf.first; it != sideOf.second; ++it) { + if (ll != it->second && !hasEdge(ll, it->second) && directlySideway(it->second)) { + if (trafficRules_.canChangeLane(ll, it->second)) { + // we process lane changes later, when we know all lanelets that can participate in lane change + laneChangeLanelets.add(ll, it->second); + } else { + assignEdge(ll, it->second, relation); + } + } + } +} + +void MapGraphBuilder::addConflictingEdge(const ConstLanelet& ll, const LaneletLayer& passableLanelets) { + // Conflicting + ConstLanelets results = passableLanelets.search(geometry::boundingBox2d(ll)); + ConstLanelet other; + for (auto& result : results) { + if (bothWaysLaneletIds_.find(ll.id()) != bothWaysLaneletIds_.end() && result == ll) { + other = result.invert(); + assignEdge(ll, other, RelationType::Conflicting); + assignEdge(other, ll, RelationType::Conflicting); + continue; + } + other = result; + if (hasEdge(ll, result)) { + continue; + } + auto vertex = graph_->getVertex(other); + if (!vertex || result == ll) { + continue; + } + auto maxHeight = participantHeight(); + if ((maxHeight && geometry::overlaps3d(ll, other, *maxHeight)) || (!maxHeight && geometry::overlaps2d(ll, other))) { + assignEdge(ll, other, RelationType::Conflicting); + assignEdge(other, ll, RelationType::Conflicting); + } + } +} + +void MapGraphBuilder::addLaneChangeEdges(LaneChangeLaneletsCollector& laneChanges, const RelationType& relation) { + auto getSuccessors = [this](auto beginEdgeIt, auto endEdgeIt, auto getVertex) { + ConstLanelets nexts; + for (; beginEdgeIt != endEdgeIt; ++beginEdgeIt) { + auto& edgeInfo = graph_->get()[*beginEdgeIt]; + if (edgeInfo.relation == RelationType::Successor) { + nexts.push_back(graph_->get()[getVertex(*beginEdgeIt, graph_->get())].lanelet()); + } + } + return nexts; + }; + auto next = [this, &getSuccessors](const ConstLanelet& llt) { + auto edges = boost::out_edges(*graph_->getVertex(llt), graph_->get()); + return getSuccessors(edges.first, edges.second, [](auto edge, const auto& g) { return boost::target(edge, g); }); + }; + auto prev = [this, &getSuccessors](const ConstLanelet& llt) { + auto edges = boost::in_edges(*graph_->getVertex(llt), graph_->get()); + return getSuccessors(edges.first, edges.second, [](auto edge, const auto& g) { return boost::source(edge, g); }); + }; + Optional laneChangeLanelets; + while (!!(laneChangeLanelets = laneChanges.getNextChangeLanelets(prev, next))) { + assignLaneChange(std::move(laneChangeLanelets->first), std::move(laneChangeLanelets->second), relation); + } +} + +void MapGraphBuilder::addAreaEdge(const ConstArea& area, const LaneletLayer& passableLanelets) { + auto candidates = passableLanelets.search(geometry::boundingBox2d(area)); + for (auto& candidate : candidates) { + bool canPass = false; + if (trafficRules_.canPass(area, candidate)) { + canPass = true; + assignEdge(area, candidate, RelationType::Area); + } + if (trafficRules_.canPass(area, candidate.invert())) { + canPass = true; + assignEdge(area, candidate.invert(), RelationType::Area); + } + if (trafficRules_.canPass(candidate, area)) { + canPass = true; + assignEdge(candidate, area, RelationType::Area); + } + if (trafficRules_.canPass(candidate.invert(), area)) { + canPass = true; + assignEdge(candidate.invert(), area, RelationType::Area); + } + if (canPass) { + continue; + } + auto maxHeight = participantHeight(); + if ((maxHeight && geometry::overlaps3d(area, candidate, *maxHeight)) || + (!maxHeight && geometry::overlaps2d(area, candidate))) { + assignEdge(candidate, area, RelationType::Conflicting); + } + } +} + +void MapGraphBuilder::addAreaEdge(const ConstArea& area, const AreaLayer& passableAreas) { + auto candidates = passableAreas.search(geometry::boundingBox2d(area)); + for (auto& candidate : candidates) { + if (candidate == area) { + continue; + } + if (trafficRules_.canPass(area, candidate)) { + assignEdge(area, candidate, RelationType::Area); + continue; + } + auto maxHeight = participantHeight(); + if ((maxHeight && geometry::overlaps3d(ConstArea(area), candidate, *maxHeight)) || + (!maxHeight && geometry::overlaps2d(ConstArea(area), candidate))) { + assignEdge(candidate, area, RelationType::Conflicting); + } + } +} + +Optional MapGraphBuilder::participantHeight() const { + auto height = config_.find(MapGraph::ParticipantHeight); + if (height != config_.end()) { + return height->second.asDouble(); + } + return {}; +} + +void MapGraphBuilder::addPointsToSearchIndex(const ConstLanelet& ll) { + using PointLaneletPair = std::pair; + pointsToLanelets_.insert( + PointLaneletPair(orderedIdPair(ll.leftBound().front().id(), ll.rightBound().front().id()), ll)); + pointsToLanelets_.insert( + PointLaneletPair(orderedIdPair(ll.leftBound().back().id(), ll.rightBound().back().id()), ll)); + pointsToLanelets_.insert( + PointLaneletPair(orderedIdPair(ll.leftBound().front().id(), ll.leftBound().back().id()), ll)); + pointsToLanelets_.insert( + PointLaneletPair(orderedIdPair(ll.rightBound().front().id(), ll.rightBound().back().id()), ll)); +} + +bool MapGraphBuilder::hasEdge(const ConstLanelet& from, const ConstLanelet& to) { + return !!graph_->getEdgeInfo(from, to); +} + +void MapGraphBuilder::assignLaneChange(ConstLanelets froms, ConstLanelets tos, const RelationType& relation) { + assert(relation == RelationType::Left || relation == RelationType::Right); + assert(froms.size() == tos.size()); + for (; !froms.empty(); froms.erase(froms.begin()), tos.erase(tos.begin())) { + auto adjacent = relation == RelationType::Left ? RelationType::AdjacentLeft : RelationType::AdjacentRight; + graph_->addEdge(froms.front(), tos.front(), EdgeInfo{adjacent}); + } +} + +void MapGraphBuilder::assignEdge(const ConstLaneletOrArea& from, const ConstLaneletOrArea& to, + const RelationType& relation) { + EdgeInfo edgeInfo{}; + edgeInfo.relation = relation; + if (relation == RelationType::Successor || relation == RelationType::Area || relation == RelationType::Left || + relation == RelationType::Right || relation == RelationType::AdjacentLeft || + relation == RelationType::AdjacentRight || relation == RelationType::Conflicting) { + assert(false && "Trying to add edge with wrong relation type to graph."); // NOLINT + return; + } + graph_->addEdge(from, to, edgeInfo); +} + +} // namespace internal +} // namespace map_learning +} // namespace lanelet diff --git a/lanelet2_map_learning/test/LaneletTestMap.xml b/lanelet2_map_learning/test/LaneletTestMap.xml new file mode 100644 index 00000000..2210728f --- /dev/null +++ b/lanelet2_map_learning/test/LaneletTestMap.xml @@ -0,0 +1 @@ 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 \ No newline at end of file diff --git a/lanelet2_map_learning/test/test_lanelet_or_area_path.cpp b/lanelet2_map_learning/test/test_lanelet_or_area_path.cpp new file mode 100644 index 00000000..da7c4728 --- /dev/null +++ b/lanelet2_map_learning/test/test_lanelet_or_area_path.cpp @@ -0,0 +1,213 @@ +#include +#include + +#include + +#include "lanelet2_map_learning/LaneletPath.h" + +using namespace lanelet; +using namespace lanelet::map_learning; + +/* + * 6 l8 7 l7 8 l6 9 l5 10 + * X<------X<------X<------X<------X + * ^ ^ ^ ^ + * |l9 |l10 |l11 |l12 + * | a1 | a2 | ll1 | ll2 + * | | ll3 | ll4 | + * X------>X------>X------>X------>X + * 1 l1 2 l2 3 l3 4 l4 5 + * | a51 | WE COME + * | V IN PEACE + * X<------X + * 12 11 + */ + +class LaneletOrAreaTest : public ::testing::Test { + private: + void SetUp() override { + Id id(1000); + p1 = Point3d(++id, 0, 0, 0); + p2 = Point3d(++id, 1, 0, 0); + p3 = Point3d(++id, 2, 0, 0); + p4 = Point3d(++id, 3, 0, 0); + p5 = Point3d(++id, 4, 0, 0); + p6 = Point3d(++id, 0, 1, 0); + p7 = Point3d(++id, 1, 1, 0); + p8 = Point3d(++id, 2, 1, 0); + p9 = Point3d(++id, 3, 1, 0); + p10 = Point3d(++id, 4, 1, 0); + p11 = Point3d(++id, 4, -1, 0); + p12 = Point3d(++id, 3, -1, 0); + + ls1 = LineString3d(++id, {p1, p2}); + ls2 = LineString3d(++id, {p2, p3}); + ls3 = LineString3d(++id, {p3, p4}); + ls4 = LineString3d(++id, {p4, p5}); + ls5 = LineString3d(++id, {p10, p9}); + ls6 = LineString3d(++id, {p9, p8}); + ls7 = LineString3d(++id, {p8, p7}); + ls8 = LineString3d(++id, {p7, p6}); + ls9 = LineString3d(++id, {p1, p6}); + ls10 = LineString3d(++id, {p2, p7}); + ls11 = LineString3d(++id, {p3, p8}); + ls12 = LineString3d(++id, {p4, p9}); + ls13 = LineString3d(++id, {p5, p11, p12, p4}); + + l1 = Lanelet(++id, ls6.invert(), ls3); + l2 = Lanelet(++id, ls5.invert(), ls4); + l3 = Lanelet(++id, ls10, ls11); + l4 = Lanelet(++id, ls11, ls12); + + a1 = Area(++id, {ls9, ls8.invert(), ls10.invert(), ls1.invert()}); + a2 = Area(++id, {ls10, ls7.invert(), ls11.invert(), ls2.invert()}); + a51 = Area(++id, {ls4, ls13}); + + areaPath = LaneletOrAreaPath({ConstLaneletOrArea(a1), ConstLaneletOrArea(a2)}); + laneletPath = LaneletOrAreaPath({ConstLaneletOrArea(l1), ConstLaneletOrArea(l2)}); + bothPath = LaneletOrAreaPath({ConstLaneletOrArea(a2), ConstLaneletOrArea(l1)}); + invAreaPath = LaneletOrAreaPath({ConstLaneletOrArea(l1.invert()), ConstLaneletOrArea(a2)}); + invLLPath = LaneletOrAreaPath({ConstLaneletOrArea(l2.invert()), ConstLaneletOrArea(l1.invert())}); + longPath = LaneletOrAreaPath( + {ConstLaneletOrArea(a1), ConstLaneletOrArea(a2), ConstLaneletOrArea(l1), ConstLaneletOrArea(l2)}); + longInvPath = LaneletOrAreaPath({ConstLaneletOrArea(l2.invert()), ConstLaneletOrArea(l1.invert()), + ConstLaneletOrArea(a2), ConstLaneletOrArea(a1)}); + sidePath = LaneletOrAreaPath({ConstLaneletOrArea(a1), ConstLaneletOrArea(l3), ConstLaneletOrArea(l4)}); + sideInvPath = LaneletOrAreaPath({ConstLaneletOrArea(l3), ConstLaneletOrArea(a1)}); + cornerPath = LaneletOrAreaPath({ConstLaneletOrArea(l1), ConstLaneletOrArea(l2), ConstLaneletOrArea(a51)}); + cornerPathInv = + LaneletOrAreaPath({ConstLaneletOrArea(a51), ConstLaneletOrArea(l2.invert()), ConstLaneletOrArea(l1.invert())}); + } + + public: + Point3d p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11, p12; + LineString3d ls1, ls2, ls3, ls4, ls5, ls6, ls7, ls8, ls9, ls10, ls11, ls12, ls13; + Lanelet l1, l2, l3, l4; + Area a1, a2, a51; + LaneletOrAreaPath areaPath, laneletPath, bothPath, invAreaPath, invLLPath, longPath, longInvPath, sidePath, + sideInvPath, cornerPath, cornerPathInv; +}; +namespace { +void checkIdentical(const BasicPolygon3d& lhs, const BasicPolygon3d& rhs) { + auto to2D = [](const BasicPolygon3d& p3d) { + BasicPolygon2d p2d(p3d.size()); + for (size_t i = 0; i < p3d.size(); ++i) { + p2d[i] = p3d.at(i).head<2>(); + } + return p2d; + }; + BasicPolygon2d lhs2d = to2D(lhs); + BasicPolygon2d rhs2d = to2D(rhs); + // equals is buggy in older boost versions +#if BOOST_VERSION > 106500 + EXPECT_TRUE(boost::geometry::equals(lhs2d, rhs2d)); +#endif +} + +void checkEvenlySpaced(const BasicPolygon3d& poly, const double dist = 1.) { + for (size_t i = 0; i + 1 < poly.size(); ++i) { + EXPECT_DOUBLE_EQ(boost::geometry::distance(poly.at(i), poly.at(i + 1)), dist); + } + EXPECT_DOUBLE_EQ(boost::geometry::distance(poly.back(), poly.front()), dist); +} + +} // namespace + +TEST_F(LaneletOrAreaTest, enclosingPolygonAreas) { // NOLINT + BasicPolygons2d intersect; + BasicPolygon3d joined = getEnclosingPolygon3d(areaPath); + BasicPolygon3d expected{p1, p6, p8, p3}; + checkIdentical(joined, expected); + checkEvenlySpaced(joined); + EXPECT_EQ(joined.size(), 6ul); +} + +TEST_F(LaneletOrAreaTest, enclosingPolygonLanelets) { // NOLINT + BasicPolygons2d intersect; + BasicPolygon3d joined = getEnclosingPolygon3d(laneletPath); + BasicPolygon3d expected{p3, p8, p10, p5}; + checkIdentical(joined, expected); + checkEvenlySpaced(joined); + EXPECT_EQ(joined.size(), 6ul); +} + +TEST_F(LaneletOrAreaTest, enclosingPolygonLaneletsInverted) { // NOLINT + BasicPolygons2d intersect; + BasicPolygon3d joined = getEnclosingPolygon3d(invLLPath); + BasicPolygon3d expected{p3, p8, p10, p5}; + checkIdentical(joined, expected); + checkEvenlySpaced(joined); + EXPECT_EQ(joined.size(), 6ul); +} + +TEST_F(LaneletOrAreaTest, enclosingPolygonMixed) { // NOLINT + BasicPolygons2d intersect; + BasicPolygon3d joined = getEnclosingPolygon3d(bothPath); + BasicPolygon3d expected{p2, p7, p9, p4}; + checkIdentical(joined, expected); + checkEvenlySpaced(joined); + EXPECT_EQ(joined.size(), 6ul); +} + +TEST_F(LaneletOrAreaTest, enclosingPolygonAreaInverted) { // NOLINT + BasicPolygons2d intersect; + BasicPolygon3d joined = getEnclosingPolygon3d(invAreaPath); + BasicPolygon3d expected{p2, p7, p9, p4}; + checkIdentical(joined, expected); + checkEvenlySpaced(joined); + EXPECT_EQ(joined.size(), 6ul); +} + +TEST_F(LaneletOrAreaTest, enclosingPolygonLong) { // NOLINT + BasicPolygons2d intersect; + BasicPolygon3d joined = getEnclosingPolygon3d(longPath); + BasicPolygon3d expected{p1, p6, p10, p5}; + checkIdentical(joined, expected); + checkEvenlySpaced(joined); + EXPECT_EQ(joined.size(), 10ul); +} + +TEST_F(LaneletOrAreaTest, enclosingPolygonLongInverted) { // NOLINT + BasicPolygons2d intersect; + BasicPolygon3d joined = getEnclosingPolygon3d(longInvPath); + BasicPolygon3d expected{p1, p6, p10, p5}; + checkIdentical(joined, expected); + checkEvenlySpaced(joined); + EXPECT_EQ(joined.size(), 10ul); +} + +TEST_F(LaneletOrAreaTest, enclosingPolygonSideways) { // NOLINT + BasicPolygons2d intersect; + BasicPolygon3d joined = getEnclosingPolygon3d(sidePath); + BasicPolygon3d expected{p1, p6, p9, p4}; + checkIdentical(joined, expected); + checkEvenlySpaced(joined); + EXPECT_EQ(joined.size(), 8ul); +} + +TEST_F(LaneletOrAreaTest, enclosingPolygonSidewaysInvertrd) { // NOLINT + BasicPolygons2d intersect; + BasicPolygon3d joined = getEnclosingPolygon3d(sideInvPath); + BasicPolygon3d expected{p1, p6, p8, p3}; + checkIdentical(joined, expected); + checkEvenlySpaced(joined); + EXPECT_EQ(joined.size(), 6ul); +} + +TEST_F(LaneletOrAreaTest, enclosingPolygonCorner) { // NOLINT + BasicPolygons2d intersect; + BasicPolygon3d joined = getEnclosingPolygon3d(cornerPath); + BasicPolygon3d expected{p8, p10, p11, p12, p4, p3}; + checkIdentical(joined, expected); + checkEvenlySpaced(joined); + EXPECT_EQ(joined.size(), 8ul); +} + +TEST_F(LaneletOrAreaTest, enclosingPolygonCornerInv) { // NOLINT + BasicPolygons2d intersect; + BasicPolygon3d joined = getEnclosingPolygon3d(cornerPathInv); + BasicPolygon3d expected{p8, p10, p11, p12, p4, p3}; + checkIdentical(joined, expected); + checkEvenlySpaced(joined); + EXPECT_EQ(joined.size(), 8ul); +} diff --git a/lanelet2_map_learning/test/test_relations.cpp b/lanelet2_map_learning/test/test_relations.cpp new file mode 100644 index 00000000..9ed52fae --- /dev/null +++ b/lanelet2_map_learning/test/test_relations.cpp @@ -0,0 +1,536 @@ +#include + +#include + +#include "lanelet2_map_learning/MapGraph.h" +#include "test_routing_map.h" + +using namespace lanelet; +using namespace lanelet::routing::tests; + +TEST_F(GermanVehicleGraph, LaneChangesLeft) { // NOLINT + // Multiple1 + Optional left = graph->left(lanelets.at(2001)); + EXPECT_TRUE((!!left)); + EXPECT_TRUE(*left == lanelets.at(2003)); + + left = graph->left(lanelets.at(2002)); + EXPECT_FALSE((!!left)); +} + +TEST_F(GermanVehicleGraph, LaneChangesLeftMerging) { // NOLINT + Optional left = graph->left(lanelets.at(2005)); + EXPECT_FALSE((!!left)); + + left = graph->left(lanelets.at(2006)); + EXPECT_FALSE((!!left)); +} + +TEST_F(GermanVehicleGraph, LaneChangesLeftDiverging) { // NOLINT + Optional left = graph->left(lanelets.at(2008)); + EXPECT_FALSE((!!left)); + + left = graph->left(lanelets.at(2009)); + EXPECT_FALSE((!!left)); +} + +TEST_F(GermanVehicleGraph, LaneChangesLeftMaxHose) { // NOLINT + Optional left = graph->left(lanelets.at(2016)); + EXPECT_FALSE((!!left)); + + left = graph->left(lanelets.at(2020)); + EXPECT_FALSE((!!left)); +} + +TEST_F(GermanVehicleGraph, LaneChangesLeftInvalid) { // NOLINT + ConstLanelet invalidLanelet; + Optional left = graph->left(invalidLanelet); + EXPECT_FALSE((!!left)); +} + +TEST_F(GermanVehicleGraph, LaneChangesRight) { // NOLINT + // Multiple1 + Optional right = graph->right(lanelets.at(2003)); + EXPECT_TRUE((!!right)); + EXPECT_TRUE(*right == lanelets.at(2001)); + + right = graph->right(lanelets.at(2004)); + EXPECT_FALSE((!!right)); + + right = graph->right(lanelets.at(2001)); + EXPECT_FALSE((!!right)); + + right = graph->right(lanelets.at(2002)); + EXPECT_FALSE((!!right)); +} + +TEST_F(GermanVehicleGraph, LaneChangesRightMerging) { // NOLINT + Optional right = graph->right(lanelets.at(2006)); + EXPECT_FALSE((!!right)); + + right = graph->right(lanelets.at(2005)); + EXPECT_FALSE((!!right)); +} + +TEST_F(GermanVehicleGraph, LaneChangesRightDiverging) { // NOLINT + Optional right = graph->right(lanelets.at(2008)); + EXPECT_FALSE((!!right)); + + right = graph->right(lanelets.at(2009)); + EXPECT_FALSE((!!right)); +} + +TEST_F(GermanVehicleGraph, LaneChangesRightMaxHose) { // NOLINT + Optional right = graph->right(lanelets.at(2016)); + EXPECT_FALSE((!!right)); + + right = graph->right(lanelets.at(2020)); + EXPECT_FALSE((!!right)); +} + +TEST_F(GermanVehicleGraph, LaneChangesRightInvalid) { // NOLINT + // Invalid + ConstLanelet invalidLanelet; + Optional right = graph->right(invalidLanelet); + EXPECT_FALSE((!!right)); +} + +TEST_F(GermanVehicleGraph, AllLaneChangesLeft) { // NOLINT + // Multiple2 + ConstLanelets left; + left = graph->lefts(lanelets.at(2012)); + EXPECT_EQ(left.size(), 1ul); + EXPECT_TRUE(std::find(left.begin(), left.end(), lanelets.at(2011)) != left.end()); + + left = graph->lefts(lanelets.at(2011)); + EXPECT_TRUE(left.empty()); + + left = graph->lefts(lanelets.at(2014)); + EXPECT_EQ(left.size(), 1ul); + EXPECT_TRUE(std::find(left.begin(), left.end(), lanelets.at(2013)) != left.end()); + + left = graph->lefts(lanelets.at(2015)); + EXPECT_EQ(left.size(), 2ul); + EXPECT_TRUE(std::find(left.begin(), left.end(), lanelets.at(2014)) != left.end()); + EXPECT_TRUE(std::find(left.begin(), left.end(), lanelets.at(2013)) != left.end()); +} + +TEST_F(GermanVehicleGraph, AllLaneChangesRight) { // NOLINT + // Multiple2 + ConstLanelets right; + right = graph->rights(lanelets.at(2011)); + EXPECT_EQ(right.size(), 1ul); + EXPECT_TRUE(std::find(right.begin(), right.end(), lanelets.at(2012)) != right.end()); + + right = graph->rights(lanelets.at(2012)); + EXPECT_TRUE(right.empty()); + + right = graph->rights(lanelets.at(2013)); + EXPECT_EQ(right.size(), 2ul); + EXPECT_TRUE(std::find(right.begin(), right.end(), lanelets.at(2014)) != right.end()); + EXPECT_TRUE(std::find(right.begin(), right.end(), lanelets.at(2015)) != right.end()); + + right = graph->rights(lanelets.at(2014)); + EXPECT_EQ(right.size(), 1ul); + EXPECT_TRUE(std::find(right.begin(), right.end(), lanelets.at(2015)) != right.end()); +} + +TEST_F(GermanVehicleGraph, FollowingWithoutLaneChange) { // NOLINT + // Multiple1 + ConstLanelets following = graph->following(lanelets.at(2001), false); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2002)) != following.end()); + + following = graph->following(lanelets.at(2003), false); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2004)) != following.end()); + + following = graph->following(lanelets.at(2002), false); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2006)) != following.end()); + + following = graph->following(lanelets.at(2004), false); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2005)) != following.end()); +} + +TEST_F(GermanVehicleGraph, FollowingWithoutLaneChangeMerging) { // NOLINT + ConstLanelets following = graph->following(lanelets.at(2005), false); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2007)) != following.end()); +} + +TEST_F(GermanVehicleGraph, FollowingWithoutLaneChangeDiverging) { // NOLINT + // Single Lane -> Diverging + ConstLanelets following = graph->following(lanelets.at(2007), false); + EXPECT_EQ(following.size(), 2ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2008)) != following.end()); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2009)) != following.end()); +} + +TEST_F(GermanVehicleGraph, FollowingWithoutLaneChangeMaxHose) { // NOLINT + // Max Hose Problem + ConstLanelets following = graph->following(lanelets.at(2018), false); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2016)) != following.end()); + + following = graph->following(lanelets.at(2019), false); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2020)) != following.end()); + + following = graph->following(lanelets.at(2020), false); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2022)) != following.end()); + + following = graph->following(lanelets.at(2022), false); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2024)) != following.end()); + + following = graph->following(lanelets.at(2021), false); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2020).invert()) != following.end()); + + following = graph->following(lanelets.at(2020).invert(), false); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2018)) != following.end()); +} + +TEST_F(GermanVehicleGraph, FollowingWithoutLaneChangeInvalid) { // NOLINT + // Invalid + ConstLanelet invalidLanelet; + ConstLanelets following = graph->following(invalidLanelet, false); + EXPECT_TRUE(following.empty()); +} + +TEST_F(GermanVehicleGraph, FollowingWithLaneChange) { // NOLINT + // Multiple1 + ConstLanelets following = graph->following(lanelets.at(2001), true); + EXPECT_EQ(following.size(), 2ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2002)) != following.end()); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2003)) != following.end()); + + following = graph->following(lanelets.at(2003), true); + EXPECT_EQ(following.size(), 2ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2001)) != following.end()); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2004)) != following.end()); + + following = graph->following(lanelets.at(2002), true); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2006)) != following.end()); + + following = graph->following(lanelets.at(2004), true); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2005)) != following.end()); +} + +TEST_F(GermanVehicleGraph, FollowingWithLaneChangeMerging) { // NOLINT + ConstLanelets following = graph->following(lanelets.at(2005), true); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2007)) != following.end()); + + following = graph->following(lanelets.at(2006), true); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2007)) != following.end()); +} + +TEST_F(GermanVehicleGraph, FollowingWithLaneChangeDiverging) { // NOLINT + // Single Lane -> Diverging + ConstLanelets following = graph->following(lanelets.at(2007), true); + EXPECT_EQ(following.size(), 2ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2008)) != following.end()); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2009)) != following.end()); +} + +TEST_F(GermanVehicleGraph, FollowingWithLaneChangeMaxHose) { // NOLINT + // Max Hose Problem + ConstLanelets following = graph->following(lanelets.at(2018), true); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2016)) != following.end()); + + following = graph->following(lanelets.at(2019), true); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2020)) != following.end()); + + following = graph->following(lanelets.at(2020), true); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2022)) != following.end()); + + following = graph->following(lanelets.at(2022), true); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2024)) != following.end()); + + following = graph->following(lanelets.at(2021), true); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2020).invert()) != following.end()); + + following = graph->following(lanelets.at(2020).invert(), true); + EXPECT_EQ(following.size(), 1ul); + EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2018)) != following.end()); +} + +TEST_F(GermanVehicleGraph, PreviousWithoutLaneChange) { // NOLINT + // Multiple1 + ConstLanelets previous; + previous = graph->previous(lanelets.at(2001), false); + EXPECT_EQ(previous.size(), 0ul); + + previous = graph->previous(lanelets.at(2003), false); + EXPECT_EQ(previous.size(), 0ul); + + previous = graph->previous(lanelets.at(2002), false); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2001)) != previous.end()); + + previous = graph->previous(lanelets.at(2004), false); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2003)) != previous.end()); +} + +TEST_F(GermanVehicleGraph, PreviousWithoutLaneChangeMerging) { // NOLINT + ConstLanelets previous = graph->previous(lanelets.at(2005), false); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2004)) != previous.end()); + + previous = graph->previous(lanelets.at(2006), false); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2002)) != previous.end()); +} + +TEST_F(GermanVehicleGraph, PreviousWithoutLaneChangeSingleLane) { // NOLINT + ConstLanelets previous = graph->previous(lanelets.at(2007), false); + EXPECT_EQ(previous.size(), 2ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2005)) != previous.end()); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2006)) != previous.end()); +} + +TEST_F(GermanVehicleGraph, PreviousWithoutLaneChangeDiverging) { // NOLINT + ConstLanelets previous = graph->previous(lanelets.at(2008), false); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2007)) != previous.end()); + + previous = graph->previous(lanelets.at(2009), false); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2007)) != previous.end()); +} + +TEST_F(GermanVehicleGraph, PreviousWithoutLaneChangeMaxHose) { // NOLINT + // Max Hose Problem + ConstLanelets previous = graph->previous(lanelets.at(2019), false); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2017)) != previous.end()); + + previous = graph->previous(lanelets.at(2020), false); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2019)) != previous.end()); + + previous = graph->previous(lanelets.at(2022), false); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2020)) != previous.end()); + + previous = graph->previous(lanelets.at(2024), false); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2022)) != previous.end()); + + previous = graph->previous(lanelets.at(2021), false); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2023)) != previous.end()); + + previous = graph->previous(lanelets.at(2020).invert(), false); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2021)) != previous.end()); + + previous = graph->previous(lanelets.at(2018), false); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2020).invert()) != previous.end()); + + previous = graph->previous(lanelets.at(2016), false); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2018)) != previous.end()); +} + +TEST_F(GermanVehicleGraph, PreviousWithoutLaneChangeInvalid) { // NOLINT + ConstLanelet invalidLanelet; + ConstLanelets previous = graph->previous(invalidLanelet, false); + EXPECT_TRUE(previous.empty()); +} + +TEST_F(GermanVehicleGraph, PreviousWithLaneChange) { // NOLINT + // Multiple1 + ConstLanelets previous = graph->previous(lanelets.at(2001), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2003)) != previous.end()); + + previous = graph->previous(lanelets.at(2003), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2001)) != previous.end()); + + previous = graph->previous(lanelets.at(2002), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2001)) != previous.end()); + + previous = graph->previous(lanelets.at(2004), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2003)) != previous.end()); +} + +TEST_F(GermanVehicleGraph, PreviousWithLaneChangeMerging) { // NOLINT + ConstLanelets previous = graph->previous(lanelets.at(2005), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2004)) != previous.end()); + + previous = graph->previous(lanelets.at(2006), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2002)) != previous.end()); +} + +TEST_F(GermanVehicleGraph, PreviousWithLaneChangeSingleLane) { // NOLINT + ConstLanelets previous = graph->previous(lanelets.at(2007), true); + EXPECT_EQ(previous.size(), 2ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2005)) != previous.end()); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2006)) != previous.end()); +} + +TEST_F(GermanVehicleGraph, PreviousWithLaneChangeDiverging) { // NOLINT + ConstLanelets previous = graph->previous(lanelets.at(2008), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2007)) != previous.end()); + + previous = graph->previous(lanelets.at(2009), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2007)) != previous.end()); +} + +TEST_F(GermanVehicleGraph, PreviousWithLaneChangeMaxHose) { // NOLINT + ConstLanelets previous = graph->previous(lanelets.at(2019), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2017)) != previous.end()); + + previous = graph->previous(lanelets.at(2020), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2019)) != previous.end()); + + previous = graph->previous(lanelets.at(2022), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2020)) != previous.end()); + + previous = graph->previous(lanelets.at(2024), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2022)) != previous.end()); + + previous = graph->previous(lanelets.at(2021), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2023)) != previous.end()); + + previous = graph->previous(lanelets.at(2020).invert(), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2021)) != previous.end()); + + previous = graph->previous(lanelets.at(2018), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2020).invert()) != previous.end()); + + previous = graph->previous(lanelets.at(2016), true); + EXPECT_EQ(previous.size(), 1ul); + EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2018)) != previous.end()); +} + +TEST_F(GermanVehicleGraph, PreviousWithLaneChangeInvalid) { // NOLINT + ConstLanelet invalidLanelet; + ConstLanelets previous = graph->previous(invalidLanelet, true); + EXPECT_TRUE(previous.empty()); +} + +TEST_F(GermanVehicleGraph, GetBesides) { // NOLINT + // Diverging2 + ConstLanelets laneletSet; + laneletSet = graph->besides(lanelets.at(2010)); + ASSERT_EQ(1ul, laneletSet.size()); + EXPECT_EQ(laneletSet[0], lanelets.at(2010)); + + laneletSet = graph->besides(lanelets.at(2011)); + ASSERT_EQ(laneletSet.size(), 2ul); + EXPECT_EQ(laneletSet[0], lanelets.at(2011)); + EXPECT_EQ(laneletSet[1], lanelets.at(2012)); + + // Multiple2 + laneletSet = graph->besides(lanelets.at(2015)); + ASSERT_EQ(laneletSet.size(), 3ul); + EXPECT_EQ(laneletSet[0], lanelets.at(2013)); + EXPECT_EQ(laneletSet[1], lanelets.at(2014)); + EXPECT_EQ(laneletSet[2], lanelets.at(2015)); + + laneletSet = graph->besides(lanelets.at(2014)); + ASSERT_EQ(laneletSet.size(), 3ul); + EXPECT_EQ(laneletSet[0], lanelets.at(2013)); + EXPECT_EQ(laneletSet[1], lanelets.at(2014)); + EXPECT_EQ(laneletSet[2], lanelets.at(2015)); + + laneletSet = graph->besides(lanelets.at(2013)); + ASSERT_EQ(laneletSet.size(), 3ul); + EXPECT_EQ(laneletSet[0], lanelets.at(2013)); + EXPECT_EQ(laneletSet[1], lanelets.at(2014)); + EXPECT_EQ(laneletSet[2], lanelets.at(2015)); +} + +TEST_F(GermanVehicleGraph, conflicting) { // NOLINT + // Normal Lanelets + auto result = graph->conflicting(lanelets.at(2007)); + EXPECT_EQ(result.size(), 0ul); + + result = graph->conflicting(lanelets.at(2012)); + EXPECT_EQ(result.size(), 0ul); + + result = graph->conflicting(lanelets.at(2001)); + EXPECT_EQ(result.size(), 0ul); + + result = graph->conflicting(lanelets.at(2005)); + EXPECT_EQ(result.size(), 1ul); +} + +TEST_F(GermanVehicleGraph, conflicting3d) { // NOLINT + // Bridge lanelet + auto result = graph->conflicting(lanelets.at(2005)); + ASSERT_EQ(result.size(), 1ul); + EXPECT_TRUE(result.front() == lanelets.at(2006)); + + result = graph->conflicting(lanelets.at(2006)); + ASSERT_EQ(result.size(), 1ul); + EXPECT_TRUE(result.front() == lanelets.at(2005)); + + result = graph->conflicting(lanelets.at(2032)); + EXPECT_EQ(result.size(), 0ul); +} + +TEST_F(GermanVehicleGraph, conflictingBothWays) { // NOLINT + // Both ways Lanelet + auto result = graph->conflicting(lanelets.at(2020)); + ASSERT_EQ(result.size(), 1ul); + EXPECT_NE(result.front().lanelet()->inverted(), lanelets.at(2020).inverted()); + + result = graph->conflicting(lanelets.at(2020).invert()); + ASSERT_EQ(result.size(), 1ul); + EXPECT_EQ(result.front().lanelet()->inverted(), lanelets.at(2020).inverted()); +} + +TEST_F(GermanVehicleGraph, conflictingMerging) { // NOLINT + auto result = graph->conflicting(lanelets.at(2005)); + ASSERT_EQ(result.size(), 1ul); + EXPECT_EQ(result.front(), lanelets.at(2006)); +} + +TEST_F(GermanVehicleGraph, conflictingDiverging) { // NOLINT + auto result = graph->conflicting(lanelets.at(2008)); + ASSERT_EQ(result.size(), 1ul); + EXPECT_EQ(result.front(), lanelets.at(2009)); +} + +TEST_F(MapGraphTest, routingCostOnLaneChange) { // NOLINT + // tests that all lane changes are not possible when the space for a lane change is too small according to the routing + // cost object + auto graph = setUpGermanVehicleGraph(*testData.laneletMap, 2, 2, 3); + auto llt = [&](auto id) -> ConstLanelet { return lanelets.at(id); }; + EXPECT_EQ(graph->left(lanelets.at(2012)), llt(2011)); + EXPECT_EQ(graph->adjacentLeft(lanelets.at(2015)), llt(2014)); + EXPECT_EQ(graph->right(lanelets.at(2011)), llt(2012)); + EXPECT_EQ(graph->adjacentRight(lanelets.at(2014)), llt(2015)); +} diff --git a/lanelet2_map_learning/test/test_route.cpp b/lanelet2_map_learning/test/test_route.cpp new file mode 100644 index 00000000..40314891 --- /dev/null +++ b/lanelet2_map_learning/test/test_route.cpp @@ -0,0 +1,383 @@ +#include + +#include "lanelet2_map_learning/Route.h" +#include "lanelet2_map_learning/MapGraph.h" +#include "test_routing_map.h" + +using namespace lanelet; +using namespace lanelet::routing; +using namespace lanelet::routing::tests; + +template +class TestRoute : public GermanVehicleGraph { + public: + explicit TestRoute(uint16_t routingCostId = 0, bool withLaneChanges = true) { + Ids viaIds{ViaIds...}; + start = lanelets.at(From); + end = lanelets.at(To); + via = utils::transform(viaIds, [&](auto id) -> ConstLanelet { return lanelets.at(id); }); + auto optRoute = via.empty() ? graph->getRoute(start, end, routingCostId, withLaneChanges) + : graph->getRouteVia(start, via, end, routingCostId, withLaneChanges); + EXPECT_TRUE(!!optRoute); + route = std::move(*optRoute); + } + ConstLanelet start; + ConstLanelet end; + ConstLanelets via; + Route route; + // these would better be defined as static constexpr, but c++14 doesnt support it well + const Id From{FromId}; // NOLINT + const Id To{ToId}; // NOLINT +}; + +class Route1 : public TestRoute<2001, 2014> {}; +class Route1NoLc : public TestRoute<2001, 2014> { + public: + Route1NoLc() : TestRoute(0, false) {} +}; +class Route2 : public TestRoute<2001, 2004> {}; +class Route3 : public TestRoute<2003, 2002> {}; +class Route4 : public TestRoute<2003, 2013> {}; +class Route5 : public TestRoute<2066, 2064> {}; +class RouteMaxHoseLeftRight : public TestRoute<2017, 2024> {}; +class RouteMaxHoseRightLeft : public TestRoute<2023, 2016> {}; +class RouteMaxHoseLeftRightDashedSolid : public TestRoute<2017, 2025> {}; +class RouteMaxHoseLeftRightDashedSolidFurther : public TestRoute<2017, 2027> {}; +class RouteSolidDashed : public TestRoute<2024, 2026> {}; +class RouteSolidDashedWithAdjacent : public TestRoute<2024, 2029> {}; +class RouteSplittedDiverging : public TestRoute<2029, 2038> {}; +class RouteSplittedDivergingAndMerging : public TestRoute<2041, 2049> {}; +class RouteViaSimple : public TestRoute<2003, 2015, 2009> {}; +class RouteMissingLanelet : public TestRoute<2003, 2015> {}; +class RouteInCircle : public TestRoute<2029, 2068> {}; +class RouteCircular : public TestRoute<2037, 2037, 2065> {}; +class RouteCircularNoLc : public TestRoute<2037, 2037, 2065> { + public: + RouteCircularNoLc() : TestRoute(0, false) {} +}; + +template +class AllRoutesTest : public T {}; + +using AllRoutes = + testing::Types; + +TYPED_TEST_SUITE(AllRoutesTest, AllRoutes); + +TYPED_TEST(AllRoutesTest, CheckValidity) { EXPECT_NO_THROW(this->route.checkValidity()); } // NOLINT + +TYPED_TEST(AllRoutesTest, ShortestMapInDebugMap) { + const auto map{this->route.getDebugLaneletMap()}; + for (const auto& el : this->route.shortestPath()) { + ASSERT_NE(map->pointLayer.find(el.id()), map->pointLayer.end()); + const Point3d point{*map->pointLayer.find(el.id())}; + EXPECT_TRUE(*point.attribute("shortest_path").asBool()); + } +} + +TYPED_TEST(AllRoutesTest, DebugMapCompleteness) { + const LaneletMapPtr debugMap{this->route.getDebugLaneletMap()}; + const LaneletSubmapConstPtr laneletMap{this->route.laneletSubmap()}; + + for (const auto& it : laneletMap->laneletLayer) { + ASSERT_NE(debugMap->pointLayer.find(it.id()), debugMap->pointLayer.end()); + Point3d point{*debugMap->pointLayer.find(it.id())}; + EXPECT_NO_THROW(point.attribute("id")); // NOLINT + EXPECT_NO_THROW(point.attribute("lane_id")); // NOLINT + } + + for (const auto& it : debugMap->lineStringLayer) { + EXPECT_NO_THROW(it.attribute("relation_1")); // NOLINT + } +} + +TEST_F(Route1, CreateRoute1) { // NOLINT + EXPECT_EQ(route.size(), 15ul); // NOLINT + EXPECT_GT(route.length2d(), 10.); + EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); // NOLINT + EXPECT_EQ(route.remainingShortestPath(start), route.shortestPath()); // NOLINT + EXPECT_TRUE(route.remainingShortestPath(lanelets.at(2003)).empty()); // NOLINT + EXPECT_NE(route.getDebugLaneletMap(), nullptr); + EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size()); // NOLINT + EXPECT_NE(route.laneletSubmap(), nullptr); + EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size()); // NOLINT +} + +bool hasRelation(const LaneletRelations& relations, const ConstLanelet& llt, RelationType relationType) { + LaneletRelation rel{llt, relationType}; + return utils::anyOf(relations, [&rel](auto& relation) { return rel == relation; }); +} + +template +bool hasLanelet(const ContainerT& llts, const ConstLanelet& llt) { + return utils::anyOf(llts, [&llt](auto& curr) { return curr == llt; }); +} + +TEST_F(Route1, Relations) { // NOLINT + LaneletRelations previous{route.previousRelations(lanelets.at(2007))}; + EXPECT_EQ(previous.size(), 2ul); // NOLINT + EXPECT_TRUE(hasRelation(previous, ConstLanelet(lanelets.at(2005)), RelationType::Successor)); + EXPECT_TRUE(hasRelation(previous, ConstLanelet(lanelets.at(2006)), RelationType::Successor)); + + LaneletRelations following{route.followingRelations(lanelets.at(2007))}; + EXPECT_EQ(following.size(), 2ul); // NOLINT + EXPECT_TRUE(hasRelation(following, ConstLanelet(lanelets.at(2008)), RelationType::Successor)); + EXPECT_TRUE(hasRelation(following, ConstLanelet(lanelets.at(2009)), RelationType::Successor)); +} + +TEST_F(Route1, Conflicting) { // NOLINT + auto conflictingInMap{route.conflictingInMap(lanelets.at(2009))}; + EXPECT_EQ(conflictingInMap.size(), 1ul); // NOLINT + EXPECT_TRUE(hasLanelet(conflictingInMap, lanelets.at(2008))); + + ConstLanelets conflictingInRoute{route.conflictingInRoute(lanelets.at(2009))}; + EXPECT_EQ(conflictingInRoute.size(), 1ul); // NOLINT + EXPECT_TRUE(hasLanelet(conflictingInRoute, lanelets.at(2008))); + + EXPECT_TRUE(route.conflictingInRoute(lanelets.at(2007)).empty()); + EXPECT_TRUE(route.conflictingInMap(lanelets.at(2007)).empty()); +} + +TEST_F(Route1, forEachSuccessor) { // NOLINT + double cLast = 0.; + route.forEachSuccessor(lanelets.at(2001), [&](const LaneletVisitInformation& i) { + EXPECT_TRUE(route.contains(i.lanelet)); + EXPECT_LE(cLast, i.cost); + cLast = i.cost; + return true; + }); +} + +TEST_F(Route1, Lanes) { // NOLINT + auto remainingLane{route.remainingLane(lanelets.at(2002))}; + EXPECT_EQ(remainingLane.size(), 2ul); // NOLINT + EXPECT_TRUE(hasLanelet(remainingLane, lanelets.at(2002))); + + auto fullLane{route.fullLane(lanelets.at(2001))}; + EXPECT_EQ(fullLane.size(), 3ul); // NOLINT + EXPECT_TRUE(hasLanelet(fullLane, lanelets.at(2002))); + EXPECT_EQ(route.numLanes(), 7ul); +} + +TEST_F(Route1NoLc, CreateRoute) { // NOLINT + EXPECT_EQ(route.size(), 7ul); // NOLINT + EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0, false)); // NOLINT + EXPECT_NE(route.getDebugLaneletMap(), nullptr); + EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size()); // NOLINT + EXPECT_NE(route.laneletSubmap(), nullptr); + EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size()); // NOLINT +} +TEST_F(Route1NoLc, Lanes) { // NOLINT + auto remainingLane{route.remainingLane(lanelets.at(2002))}; + EXPECT_EQ(remainingLane.size(), 6ul); // NOLINT + EXPECT_TRUE(hasLanelet(remainingLane, lanelets.at(2002))); + + auto fullLane{route.fullLane(lanelets.at(2002))}; + EXPECT_EQ(fullLane.size(), 7ul); // NOLINT + + EXPECT_TRUE(route.remainingLane(lanelets.at(2010)).empty()); + EXPECT_EQ(route.numLanes(), 1ul); +} + +TEST_F(Route1NoLc, forEachPredecessor) { // NOLINT + route.forEachPredecessor(lanelets.at(2001), [&](const LaneletVisitInformation& i) { + EXPECT_EQ(i.lanelet, lanelets.at(2001)); + return true; + }); +} + +TEST_F(Route2, CreateRoute2) { // NOLINT + EXPECT_EQ(route.size(), 3ul); // NOLINT + EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); // NOLINT + EXPECT_NE(route.getDebugLaneletMap(), nullptr); + EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size()); // NOLINT + EXPECT_NE(route.laneletSubmap(), nullptr); + EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size()); // NOLINT +} + +TEST_F(Route3, CreateRoute3) { // NOLINT + EXPECT_EQ(route.size(), 3ul); // NOLINT + EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); // NOLINT + EXPECT_NE(route.getDebugLaneletMap(), nullptr); + EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size()); // NOLINT + EXPECT_NE(route.laneletSubmap(), nullptr); + EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size()); // NOLINT +} + +TEST_F(Route4, CreateRoute4) { // NOLINT + EXPECT_EQ(route.size(), 15ul); // NOLINT + EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); // NOLINT + EXPECT_NE(route.getDebugLaneletMap(), nullptr); + EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size()); // NOLINT + EXPECT_NE(route.laneletSubmap(), nullptr); + EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size()); // NOLINT +} + +TEST_F(Route5, NoCircle) { + EXPECT_EQ(route.size(), 9); + EXPECT_FALSE(hasLanelet(route.laneletSubmap()->laneletLayer, lanelets.at(2065))); +} + +TEST_F(RouteMaxHoseLeftRight, CreateRouteMaxHose1) { // NOLINT + EXPECT_EQ(route.size(), 5ul); // NOLINT + EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); // NOLINT + EXPECT_NE(route.getDebugLaneletMap(), nullptr); + EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size()); // NOLINT + EXPECT_NE(route.laneletSubmap(), nullptr); + EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size()); // NOLINT +} + +TEST_F(RouteMaxHoseLeftRight, Lanes) { // NOLINT + auto remainingLane{route.remainingLane(lanelets.at(2020))}; + EXPECT_EQ(remainingLane.size(), 3ul); // NOLINT + EXPECT_TRUE(std::find(std::begin(remainingLane), std::end(remainingLane), ConstLanelet(lanelets.at(2022))) != + std::end(remainingLane)); + + auto fullLane{route.fullLane(lanelets.at(2020))}; + EXPECT_EQ(fullLane.size(), 5ul); // NOLINT + EXPECT_TRUE(std::find(std::begin(fullLane), std::end(fullLane), ConstLanelet(lanelets.at(2024))) != + std::end(fullLane)); + EXPECT_EQ(route.numLanes(), 1ul); +} + +TEST_F(RouteMaxHoseLeftRight, InvalidLane) { // NOLINT + auto remainingLane{route.remainingLane(lanelets.at(2020).invert())}; + EXPECT_TRUE(remainingLane.empty()); +} + +TEST_F(RouteMaxHoseRightLeft, CreateRouteMaxHose2) { // NOLINT + EXPECT_EQ(route.size(), 5ul); // NOLINT + EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); // NOLINT + EXPECT_NE(route.getDebugLaneletMap(), nullptr); + EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size()); // NOLINT + EXPECT_NE(route.laneletSubmap(), nullptr); + EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size()); // NOLINT +} + +TEST_F(RouteMaxHoseRightLeft, Lanes) { // NOLINT + auto remainingLane{route.remainingLane(lanelets.at(2020).invert())}; + EXPECT_EQ(remainingLane.size(), 3ul); // NOLINT + EXPECT_TRUE(std::find(std::begin(remainingLane), std::end(remainingLane), ConstLanelet(lanelets.at(2018))) != + std::end(remainingLane)); + + auto fullLane{route.fullLane(lanelets.at(2020).invert())}; + EXPECT_EQ(fullLane.size(), 5ul); // NOLINT + EXPECT_TRUE(std::find(std::begin(fullLane), std::end(fullLane), ConstLanelet(lanelets.at(2021))) != + std::end(fullLane)); + EXPECT_EQ(route.numLanes(), 1ul); // NOLINT +} + +TEST_F(RouteMaxHoseLeftRightDashedSolid, Lanes) { // NOLINT + auto remainingLane{route.remainingLane(lanelets.at(2020))}; + EXPECT_EQ(remainingLane.size(), 4ul); // NOLINT + EXPECT_FALSE(std::find(std::begin(remainingLane), std::end(remainingLane), ConstLanelet(lanelets.at(2018))) != + std::end(remainingLane)); + + auto fullLane{route.fullLane(lanelets.at(2020))}; + EXPECT_EQ(fullLane.size(), 6ul); // NOLINT + EXPECT_TRUE(std::find(std::begin(fullLane), std::end(fullLane), ConstLanelet(lanelets.at(2021))) == + std::end(fullLane)); + EXPECT_EQ(route.numLanes(), 1ul); // NOLINT +} + +TEST_F(RouteMaxHoseLeftRightDashedSolid, DashedSolidLineRegarded) { // NOLINT + EXPECT_FALSE(!!route.rightRelation(lanelets.at(2025))); +} + +TEST_F(RouteMaxHoseLeftRightDashedSolidFurther, Lanes) { // NOLINT + EXPECT_EQ(route.numLanes(), 1ul); // NOLINT + EXPECT_TRUE(route.remainingLane(lanelets.at(2026)).empty()); +} + +TEST_F(RouteMaxHoseLeftRightDashedSolidFurther, Elements) { // NOLINT + EXPECT_EQ(route.size(), 7ul); // NOLINT +} + +TEST_F(RouteMaxHoseLeftRightDashedSolidFurther, DashedSolidLineRegarded) { // NOLINT + EXPECT_FALSE(!!route.rightRelation(lanelets.at(2025))); +} + +TEST_F(RouteSolidDashed, Lanes) { // NOLINT + EXPECT_EQ(route.numLanes(), 2ul); // NOLINT +} + +TEST_F(RouteSolidDashedWithAdjacent, Lanes) { // NOLINT + EXPECT_EQ(route.numLanes(), 2ul); // NOLINT +} + +TEST_F(RouteSolidDashedWithAdjacent, AdjacentLaneletInRoute) { // NOLINT + ASSERT_TRUE(!!route.rightRelation(lanelets.at(2025))); + EXPECT_EQ(*route.rightRelation(lanelets.at(2025)), // NOLINT + (LaneletRelation{lanelets.at(2026), RelationType::Right})); + ASSERT_TRUE(!!route.leftRelation(lanelets.at(2026))); + EXPECT_EQ(*route.leftRelation(lanelets.at(2026)), // NOLINT + (LaneletRelation{lanelets.at(2025), RelationType::AdjacentLeft})); + ASSERT_TRUE(!!route.rightRelation(lanelets.at(2027))); + EXPECT_EQ(*route.rightRelation(lanelets.at(2027)), // NOLINT + (LaneletRelation{lanelets.at(2028), RelationType::AdjacentRight})); + ASSERT_TRUE(!!route.leftRelation(lanelets.at(2028))); + EXPECT_EQ(*route.leftRelation(lanelets.at(2028)), // NOLINT + (LaneletRelation{lanelets.at(2027), RelationType::AdjacentLeft})); + ASSERT_TRUE(!!route.rightRelation(lanelets.at(2029))); + EXPECT_EQ(*route.rightRelation(lanelets.at(2029)), // NOLINT + (LaneletRelation{lanelets.at(2030), RelationType::AdjacentRight})); + ASSERT_TRUE(!!route.leftRelation(lanelets.at(2030))); + EXPECT_EQ(*route.leftRelation(lanelets.at(2030)), // NOLINT + (LaneletRelation{lanelets.at(2029), RelationType::Left})); +} + +TEST_F(RouteViaSimple, create) { // NOLINT + EXPECT_EQ(route.size(), 15ul); + EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); +} + +TEST_F(RouteMissingLanelet, create) { // NOLINT + EXPECT_EQ(route.size(), 15ul); // NOLINT + EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); // NOLINT +} + +TEST_F(GermanVehicleGraph, CannotCreateRoute) { // NOLINT + Optional route{graph->getRoute(lanelets.at(2014), lanelets.at(2007), 0)}; + EXPECT_FALSE(!!route); + // NOLINTNEXTLINE + EXPECT_THROW(graph->getRoute(lanelets.at(2001), lanelets.at(2004), numCostModules), lanelet::InvalidInputError); +} + +TEST_F(RouteSplittedDiverging, Completeness) { // NOLINT + EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), 7ul); // NOLINT + EXPECT_EQ(route.numLanes(), 3ul); // NOLINT +} + +TEST_F(RouteSplittedDivergingAndMerging, Completeness) { // NOLINT + EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), 11ul); // NOLINT + EXPECT_EQ(route.numLanes(), 5ul); // NOLINT +} + +TEST_F(RouteInCircle, create) { // NOLINT + EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), 11ul); // NOLINT + EXPECT_EQ(route.numLanes(), 3ul); // NOLINT + EXPECT_EQ(route.remainingLane(lanelets.at(2067)).size(), 0ul); // NOLINT +} + +TEST_F(RouteCircular, Circularity) { // NOLINT + EXPECT_EQ(route.size(), 10ul); + // at any point of the circular route, the remaining lane should cross the circle + EXPECT_EQ(route.remainingLane(start).size(), 6ul); + EXPECT_EQ(*route.remainingLane(start).begin(), start); + EXPECT_EQ(route.remainingLane(lanelets.at(2036)).size(), 7ul); + EXPECT_EQ(route.remainingShortestPath(lanelets.at(2067)).size(), 7ul); + EXPECT_EQ(route.remainingLane(lanelets.at(2067)).size(), 1ul); + EXPECT_EQ(route.remainingShortestPath(lanelets.at(2067)).front(), lanelets.at(2067)); + EXPECT_EQ(route.remainingLane(lanelets.at(2033)).size(), 3ul); +} + +TEST_F(RouteCircularNoLc, Circularity) { // NOLINT + EXPECT_EQ(route.size(), 7ul); + EXPECT_EQ(route.numLanes(), 1ul); + EXPECT_EQ(route.remainingLane(start).size(), 7ul); + EXPECT_EQ(route.remainingShortestPath(lanelets.at(2036)).size(), 7ul); + EXPECT_EQ(route.remainingLane(lanelets.at(2067)).size(), 7ul); +} diff --git a/lanelet2_map_learning/test/test_routing.cpp b/lanelet2_map_learning/test/test_routing.cpp new file mode 100644 index 00000000..894536a7 --- /dev/null +++ b/lanelet2_map_learning/test/test_routing.cpp @@ -0,0 +1,516 @@ +#include +#include + +#include + +#include "lanelet2_map_learning/MapGraph.h" +#include "lanelet2_map_learning/internal/Graph.h" +#include "lanelet2_map_learning/internal/ShortestPath.h" +#include "test_routing_map.h" + +using namespace lanelet; +using namespace lanelet::routing; +using namespace lanelet::routing::internal; +using namespace lanelet::routing::tests; + +GraphType getSimpleGraph() { + /* 3 + * (1)---(3) + * /1 \1 / \3 + * (0) X (5) + * \2 /1 \ /1 + * (2)----(4) + * 3 + */ + GraphType g; + auto v0 = boost::add_vertex(g); + auto v1 = boost::add_vertex(g); + auto v2 = boost::add_vertex(g); + auto v3 = boost::add_vertex(g); + auto v4 = boost::add_vertex(g); + auto v5 = boost::add_vertex(g); + auto addEdge = [](auto v0, auto v1, auto& g, double c) { + auto e = boost::add_edge(v0, v1, g); + g[e.first].routingCost = c; + }; + addEdge(v0, v1, g, 1); + addEdge(v0, v2, g, 2); + addEdge(v1, v3, g, 3); + addEdge(v1, v4, g, 1); + addEdge(v2, v3, g, 1); + addEdge(v2, v4, g, 3); + addEdge(v3, v5, g, 3); + addEdge(v4, v5, g, 1); + return g; +} + +TEST(DijkstraSearch, onSimpleGraph) { + auto g = getSimpleGraph(); + DijkstraStyleSearch searcher(g); + std::vector expCost{0, 1, 2, 3, 2, 6}; + std::vector length{1, 2, 2, 3, 3, 4}; + std::vector predecessors{0, 0, 0, 2, 1, 3}; + searcher.query(0, [&](const VertexVisitInformation& v) -> bool { + EXPECT_DOUBLE_EQ(expCost[v.vertex], v.cost) << v.vertex; + EXPECT_EQ(length[v.vertex], v.length) << v.vertex; + EXPECT_EQ(predecessors[v.vertex], v.predecessor) << v.vertex; + EXPECT_EQ(v.length, v.numLaneChanges + 1); + return v.vertex != 4; + }); + EXPECT_EQ(searcher.getMap().size(), boost::num_vertices(g)); + for (const auto& v : searcher.getMap()) { + EXPECT_EQ(v.second.predicate, v.first != 4) << v.first; + EXPECT_EQ(v.second.isLeaf, v.first == 5 || v.first == 4) << v.first; + } +} + +TEST_F(GermanPedestrianGraph, NumberOfLanelets) { // NOLINT + EXPECT_EQ(graph->passableSubmap()->laneletLayer.size(), 5ul); + EXPECT_TRUE(graph->passableSubmap()->laneletLayer.exists(2031)); + EXPECT_TRUE(graph->passableSubmap()->laneletLayer.exists(2050)); + EXPECT_EQ(graph->passableSubmap()->areaLayer.size(), 3ul); + EXPECT_TRUE(graph->passableSubmap()->areaLayer.exists(3000)); + EXPECT_TRUE(graph->passableSubmap()->areaLayer.exists(3001)); + EXPECT_TRUE(graph->passableSubmap()->areaLayer.exists(3002)); +} + +TEST_F(GermanBicycleGraph, NumberOfLanelets) { // NOLINT + EXPECT_TRUE(graph->passableSubmap()->laneletLayer.exists(2013)); + EXPECT_FALSE(graph->passableSubmap()->laneletLayer.exists(2022)); +} + +TEST_F(GermanVehicleGraph, GetShortestPath) { // NOLINT + // Multiple1 + auto shortestPath = graph->shortestPath(lanelets.at(2001), lanelets.at(2004), 0); + ASSERT_TRUE(!!shortestPath); + EXPECT_EQ(shortestPath->size(), 3ul); + EXPECT_EQ((*shortestPath)[0], lanelets.at(2001)); + EXPECT_EQ((*shortestPath)[1], lanelets.at(2003)); + EXPECT_EQ((*shortestPath)[2], lanelets.at(2004)); + + shortestPath = graph->shortestPath(lanelets.at(2003), lanelets.at(2002), 0); + ASSERT_TRUE(!!shortestPath); + EXPECT_EQ(shortestPath->size(), 3ul); + EXPECT_EQ((*shortestPath)[0], lanelets.at(2003)); + EXPECT_EQ((*shortestPath)[1], lanelets.at(2001)); + EXPECT_EQ((*shortestPath)[2], lanelets.at(2002)); + + shortestPath = graph->shortestPath(lanelets.at(2003), lanelets.at(2001), 0); + ASSERT_TRUE(!!shortestPath); + EXPECT_EQ(shortestPath->size(), 2ul); + EXPECT_EQ((*shortestPath)[0], lanelets.at(2003)); + EXPECT_EQ((*shortestPath)[1], lanelets.at(2001)); + + shortestPath = graph->shortestPath(lanelets.at(2003), lanelets.at(2004), 0); + ASSERT_TRUE(!!shortestPath); + EXPECT_EQ(shortestPath->size(), 2ul); + EXPECT_EQ((*shortestPath)[0], lanelets.at(2003)); + EXPECT_EQ((*shortestPath)[1], lanelets.at(2004)); + + shortestPath = graph->shortestPath(lanelets.at(2001), lanelets.at(2002), 0); + ASSERT_TRUE(!!shortestPath); + EXPECT_EQ(shortestPath->size(), 2ul); + EXPECT_EQ((*shortestPath)[0], lanelets.at(2001)); + EXPECT_EQ((*shortestPath)[1], lanelets.at(2002)); + + shortestPath = graph->shortestPath(lanelets.at(2004), lanelets.at(2002), 0); + EXPECT_FALSE(!!shortestPath); + + shortestPath = graph->shortestPath(lanelets.at(2002), lanelets.at(2004), 0); + EXPECT_FALSE(!!shortestPath); +} + +TEST_F(GermanVehicleGraph, GetShortestPathMaxHose) { // NOLINT + auto shortestPath = graph->shortestPath(lanelets.at(2019), lanelets.at(2022), 0); + ASSERT_TRUE(!!shortestPath); + EXPECT_EQ(shortestPath->size(), 3ul); + EXPECT_EQ((*shortestPath)[0], lanelets.at(2019)); + EXPECT_EQ((*shortestPath)[1], lanelets.at(2020)); + EXPECT_EQ((*shortestPath)[2], lanelets.at(2022)); + + shortestPath = graph->shortestPath(lanelets.at(2021), lanelets.at(2018), 0); + ASSERT_TRUE(!!shortestPath); + EXPECT_EQ(shortestPath->size(), 3ul); + EXPECT_EQ((*shortestPath)[0], lanelets.at(2021)); + EXPECT_EQ((*shortestPath)[1], lanelets.at(2020).invert()); + EXPECT_EQ((*shortestPath)[2], lanelets.at(2018)); + + shortestPath = graph->shortestPath(lanelets.at(2019), lanelets.at(2018), 0); + EXPECT_FALSE(!!shortestPath); +} + +TEST_F(GermanVehicleGraph, GetShortestPathInvalid) { // NOLINT + ConstLanelet invalidLanelet; + auto shortestPath = graph->shortestPath(invalidLanelet, lanelets.at(2004), 0); + EXPECT_FALSE(!!shortestPath); + + EXPECT_THROW(graph->shortestPath(lanelets.at(2001), lanelets.at(2004), numCostModules), InvalidInputError); // NOLINT +} + +TEST_F(GermanVehicleGraph, GetShortestPathVia1) { // NOLINT + // Multiple1 + ConstLanelets interm; + interm.push_back(static_cast(lanelets.at(2003))); + auto shortestPath = graph->shortestPathVia(lanelets.at(2001), interm, lanelets.at(2004), 0); + ASSERT_TRUE(!!shortestPath); + EXPECT_EQ(shortestPath->size(), 3ul); + EXPECT_EQ((*shortestPath)[0], lanelets.at(2001)); + EXPECT_EQ((*shortestPath)[1], lanelets.at(2003)); + EXPECT_EQ((*shortestPath)[2], lanelets.at(2004)); + + shortestPath = graph->shortestPathVia(lanelets.at(2001), interm, lanelets.at(2002), 0); + ASSERT_TRUE(!!shortestPath); + EXPECT_EQ(shortestPath->size(), 4ul); + EXPECT_EQ((*shortestPath)[0], lanelets.at(2001)); + EXPECT_EQ((*shortestPath)[1], lanelets.at(2003)); + EXPECT_EQ((*shortestPath)[2], lanelets.at(2001)); + EXPECT_EQ((*shortestPath)[3], lanelets.at(2002)); + + shortestPath = graph->shortestPathVia(lanelets.at(2004), interm, lanelets.at(2002), 0); + ASSERT_FALSE(!!shortestPath); + + shortestPath = graph->shortestPathVia(lanelets.at(2002), interm, lanelets.at(2004), 0); + ASSERT_FALSE(!!shortestPath); +} + +TEST_F(GermanVehicleGraph, GetShortestPathVia2) { // NOLINT + // Multiple1 -> Multiple2 + ConstLanelets interm; + interm.push_back(static_cast(lanelets.at(2003))); + interm.push_back(static_cast(lanelets.at(2007))); + auto shortestPath = graph->shortestPathVia(lanelets.at(2001), interm, lanelets.at(2013), 0); + ASSERT_TRUE(!!shortestPath); + EXPECT_EQ(shortestPath->size(), 8ul); + auto pathIt = shortestPath->begin(); + EXPECT_EQ(*pathIt, lanelets.at(2001)); + EXPECT_EQ(*++pathIt, lanelets.at(2003)); + EXPECT_EQ(*++pathIt, lanelets.at(2004)); + EXPECT_EQ(*++pathIt, lanelets.at(2005)); + EXPECT_EQ(*++pathIt, lanelets.at(2007)); + EXPECT_EQ(*++pathIt, lanelets.at(2008)); + EXPECT_EQ(*++pathIt, lanelets.at(2010)); + EXPECT_EQ(*++pathIt, lanelets.at(2013)); +} + +TEST_F(GermanVehicleGraph, GetShortestPathViaInvalid) { // NOLINT + ConstLanelets interm; + interm.push_back(static_cast(lanelets.at(2003))); + interm.push_back(static_cast(lanelets.at(2007))); + // NOLINTNEXTLINE + EXPECT_THROW(graph->shortestPathVia(lanelets.at(2001), interm, lanelets.at(2002), numCostModules), InvalidInputError); +} + +template +bool containsLanelet(const LaneletsT& reachableSet, Id lltId) { + return !!utils::findIf(reachableSet, [lltId](auto& llt) { return llt.id() == lltId; }); +} + +TEST_F(GermanVehicleGraph, reachableSet) { // NOLINT + auto reachable = graph->reachableSet(lanelets.at(2001), 0., 0); + EXPECT_EQ(reachable.size(), 1ul); + EXPECT_TRUE(containsLanelet(reachable, 2001)); + + reachable = graph->reachableSet(lanelets.at(2001), 2.1, 0); + EXPECT_EQ(reachable.size(), 3ul); + EXPECT_TRUE(containsLanelet(reachable, 2002)); + EXPECT_TRUE(containsLanelet(reachable, 2003)); + + reachable = graph->reachableSet(lanelets.at(2001), 100, 0); + EXPECT_EQ(reachable.size(), 15ul); // Will fail if people extend the map + + reachable = graph->reachableSet(lanelets.at(2002), 100, 0); + EXPECT_EQ(reachable.size(), 11ul); // Will fail if people extend the map +} + +TEST_F(GermanVehicleGraph, reachableSetMaxHose) { // NOLINT + auto reachable = graph->reachableSet(lanelets.at(2017), 100, 0); + EXPECT_EQ(reachable.size(), 22ul); // Will fail if people extend the map + + reachable = graph->reachableSet(lanelets.at(2021), 100, 0); + EXPECT_EQ(reachable.size(), 4ul); +} + +TEST_F(GermanVehicleGraph, reachableSetInvalid) { // NOLINT + EXPECT_THROW(graph->reachableSet(lanelets.at(2021), 0.0, numCostModules), InvalidInputError); // NOLINT + ConstLanelet invalid; + auto reachable = graph->reachableSet(invalid, 0, 0); + EXPECT_TRUE(reachable.empty()); +} + +TEST_F(GermanPedestrianGraph, reachableSetCrossingWithArea) { // NOLINT + auto reachable = graph->reachableSetIncludingAreas(lanelets.at(2050), 100); + EXPECT_EQ(reachable.size(), 6ul); + EXPECT_TRUE(containsLanelet(reachable, 2050)); + EXPECT_TRUE(containsLanelet(reachable, 2053)); + EXPECT_TRUE(containsLanelet(reachable, 2052)); + EXPECT_TRUE(containsLanelet(reachable, 3000)); + EXPECT_TRUE(containsLanelet(reachable, 3001)); + EXPECT_TRUE(containsLanelet(reachable, 3002)); +} +TEST_F(GermanPedestrianGraph, reachableSetStartingFromArea) { // NOLINT + auto reachable = graph->reachableSetIncludingAreas(areas.at(3000), 100); + EXPECT_EQ(reachable.size(), 5ul); +} +TEST_F(GermanPedestrianGraph, reachableSetWithAreaFromTwoWayLanelet) { // NOLINT + auto reachable = graph->reachableSetIncludingAreas(lanelets.at(2053).invert(), 100); + EXPECT_TRUE(containsLanelet(reachable, 2053)); + EXPECT_EQ(reachable.size(), 6ul); +} +TEST_F(GermanPedestrianGraph, reachableSetWithAreaFromUnconnectedLanelet) { // NOLINT + auto reachable = graph->reachableSetIncludingAreas(lanelets.at(2051), 100); + EXPECT_EQ(reachable.size(), 1ul); +} + +TEST_F(GermanPedestrianGraph, possiblePathsWithAreaFromLanelet) { // NOLINT + auto reachable = graph->possiblePathsIncludingAreas(lanelets.at(2050), 10, 0, false); + ASSERT_EQ(reachable.size(), 3ul); + EXPECT_EQ(reachable[0].size(), 3ul); + EXPECT_EQ(reachable[1].size(), 3ul); + EXPECT_EQ(reachable[2].size(), 3ul); +} + +TEST_F(GermanPedestrianGraph, possiblePathsWithAreaFromUnconnectedLanelet) { // NOLINT + auto reachable = graph->possiblePathsIncludingAreas(lanelets.at(2050), 3, false); + ASSERT_EQ(reachable.size(), 3ul); + EXPECT_EQ(reachable[0].size(), 3ul); + EXPECT_EQ(reachable[1].size(), 3ul); + EXPECT_EQ(reachable[2].size(), 3ul); +} + +TEST_F(GermanVehicleGraph, possiblePathsMinRoutingCosts) { // NOLINT + // MIN ROUTING COST - With lane changes + // Multiple 1 + auto routes = graph->possiblePaths(lanelets.at(2001), 2.2, 0, true); + EXPECT_EQ(routes.size(), 2ul); + + routes = graph->possiblePaths(lanelets.at(2002), 4.0, 0, true); + ASSERT_EQ(routes.size(), 1ul); + auto& firstRoute = *routes.begin(); + EXPECT_EQ(firstRoute.size(), 3ul); + EXPECT_TRUE(containsLanelet(firstRoute, 2006)); +} + +TEST_F(GermanVehicleGraph, possiblePathsIncludeShorterLc) { // NOLINT + auto routes = graph->possiblePaths(lanelets.at(2041), PossiblePathsParams{1000, {}, 0, true, true}); + EXPECT_EQ(routes.size(), 3); + auto lastLLts = utils::transform(routes, [](auto& route) { return route.back(); }); + EXPECT_TRUE(has(lastLLts, lanelets.at(2062))); + EXPECT_TRUE(has(lastLLts, lanelets.at(2049))); + EXPECT_TRUE(has(lastLLts, lanelets.at(2048))); +} + +TEST_F(GermanVehicleGraph, possiblePathsIncludeShorterAllLimitsLc) { // NOLINT + auto routes = graph->possiblePaths(lanelets.at(2041), PossiblePathsParams{1000, 100, 0, true, true}); + EXPECT_EQ(routes.size(), 3); + auto lastLLts = utils::transform(routes, [](auto& route) { return route.back(); }); + EXPECT_TRUE(has(lastLLts, lanelets.at(2062))); + EXPECT_TRUE(has(lastLLts, lanelets.at(2049))); + EXPECT_TRUE(has(lastLLts, lanelets.at(2048))); +} + +TEST_F(GermanVehicleGraph, possiblePathsIncludeShorterNoLc) { // NOLINT + auto routes = graph->possiblePaths(lanelets.at(2041), PossiblePathsParams{1000, {}, 0, false, true}); + EXPECT_EQ(routes.size(), 3); + auto lastLLts = utils::transform(routes, [](auto& route) { return route.back(); }); + EXPECT_TRUE(has(lastLLts, lanelets.at(2063))); + EXPECT_TRUE(has(lastLLts, lanelets.at(2049))); + EXPECT_TRUE(has(lastLLts, lanelets.at(2047))); +} + +TEST_F(GermanVehicleGraph, possiblePathsIncludeShorterAllLimitsNoLc) { // NOLINT + auto routes = graph->possiblePaths(lanelets.at(2041), PossiblePathsParams{1000, 100, 0, false, true}); + EXPECT_EQ(routes.size(), 3); + auto lastLLts = utils::transform(routes, [](auto& route) { return route.back(); }); + EXPECT_TRUE(has(lastLLts, lanelets.at(2063))); + EXPECT_TRUE(has(lastLLts, lanelets.at(2049))); + EXPECT_TRUE(has(lastLLts, lanelets.at(2047))); +} + +TEST_F(GermanVehicleGraph, possiblePathsLimitLengthNoLc) { // NOLINT + auto routes = graph->possiblePaths(lanelets.at(2041), PossiblePathsParams{2.5, 3, 0, false, false}); + EXPECT_TRUE(std::all_of(routes.begin(), routes.end(), [](auto& r) { return r.size() <= 3; })); + EXPECT_EQ(routes.size(), 3); + auto lastLLts = utils::transform(routes, [](auto& route) { return route.back(); }); + EXPECT_TRUE(has(lastLLts, lanelets.at(2042))); +} + +TEST_F(GermanVehicleGraph, possiblePathsMaxHose) { // NOLINT + // Max Hose Problem + auto routes = graph->possiblePaths(lanelets.at(2017), 10.0, 0, true); + ASSERT_EQ(routes.size(), 1ul); + auto& firstRoute = *routes.begin(); + EXPECT_EQ(firstRoute.size(), 5ul); + EXPECT_TRUE(containsLanelet(firstRoute, 2024)); +} + +TEST_F(GermanVehicleGraph, possiblePathsMinLanelets) { // NOLINT + // MIN NUMBER LANELETS - With lane changes + auto routes = graph->possiblePaths(lanelets.at(2001), 2, false); + EXPECT_EQ(routes.size(), 1ul); + auto& firstRoute = *routes.begin(); + ASSERT_EQ(firstRoute.size(), 2ul); + EXPECT_TRUE(containsLanelet(firstRoute, 2002)); + EXPECT_EQ(firstRoute.getRemainingLane(firstRoute.begin()).size(), firstRoute.size()); + + routes = graph->possiblePaths(lanelets.at(2001), 30, false); + EXPECT_EQ(routes.size(), 0ul); + + routes = graph->possiblePaths(lanelets.at(2001), 10, true); + EXPECT_EQ(routes.size(), 0ul); + + routes = graph->possiblePaths(lanelets.at(2001), 7, true); + EXPECT_EQ(routes.size(), 3ul); +} + +TEST_F(GermanVehicleGraph, possiblePathsInvalid) { // NOLINT + // Invalid num costs length + EXPECT_THROW(graph->possiblePaths(lanelets.at(2002), 0., numCostModules, true), InvalidInputError); // NOLINT + auto routes = graph->possiblePaths(lanelets.at(2002), 0.); + ASSERT_EQ(routes.size(), 1ul); + EXPECT_EQ(routes[0].size(), 1ul); + ConstLanelet invalid; + routes = graph->possiblePaths(invalid, 10.0, 0, true); + EXPECT_EQ(routes.size(), 0ul); + + // Invalid min number lanelets + EXPECT_THROW(graph->possiblePaths(lanelets.at(2002), 1, numCostModules, true), InvalidInputError); // NOLINT + routes = graph->possiblePaths(invalid, 10, 0, true); + EXPECT_EQ(routes.size(), 0ul); +} + +TEST_F(GermanVehicleGraph, possiblePathsTowardsWithoutLc) { // NOLINT + auto routes = graph->possiblePathsTowards(lanelets.at(2024), 9, 0, false); + ASSERT_EQ(routes.size(), 1UL); + EXPECT_EQ(routes[0].front().id(), 2017); + EXPECT_EQ(routes[0].back().id(), 2024); +} + +TEST_F(GermanVehicleGraph, possiblePathsTowardsWithLc) { // NOLINT + auto routes = graph->possiblePathsTowards(lanelets.at(2015), 7, 0, true); + ASSERT_EQ(routes.size(), 2UL); + EXPECT_TRUE(containsLanelet(routes[0], 2009) || containsLanelet(routes[0], 2008)); + EXPECT_TRUE(containsLanelet(routes[1], 2009) || containsLanelet(routes[1], 2008)); +} + +TEST_F(GermanVehicleGraph, possiblePathsTowardsMinLanelets) { // NOLINT + auto routes = graph->possiblePathsTowards(lanelets.at(2015), 5, true); + ASSERT_EQ(routes.size(), 2UL); + EXPECT_TRUE(containsLanelet(routes[0], 2009) || containsLanelet(routes[0], 2008)); + EXPECT_TRUE(containsLanelet(routes[1], 2009) || containsLanelet(routes[1], 2008)); +} + +TEST_F(GermanVehicleGraph, forEachSuccessorIsMonotonic) { // NOLINT + double lastVal = 0.; + bool lanelet2010Seen = false; + bool lanelet2013Seen = false; + graph->forEachSuccessor(lanelets.at(2007), [&](const LaneletVisitInformation& i) { + if (i.cost == 0.) { + EXPECT_EQ(i.lanelet.id(), 2007) << "First lanelet must be 2007"; + } + EXPECT_LE(lastVal, i.cost); + lastVal = i.cost; + lanelet2010Seen |= i.lanelet.id() == 2010; + lanelet2013Seen |= i.lanelet.id() == 2013; + return i.lanelet.id() != 2010 && i.lanelet.id() != 2014; + }); + EXPECT_TRUE(lanelet2010Seen); + EXPECT_FALSE(lanelet2013Seen); +} + +TEST_F(GermanPedestrianGraph, forEachSuccessorIncludingAreasReachesLanelet) { // NOLINT + class TargetFound {}; + auto throwIfTarget = [&](const LaneletOrAreaVisitInformation& i) { + if (i.laneletOrArea.id() == 2053) { + throw TargetFound{}; + } + return true; + }; + EXPECT_THROW(graph->forEachSuccessorIncludingAreas(lanelets.at(2050), throwIfTarget), TargetFound); // NOLINT +} + +TEST_F(GermanPedestrianGraph, forEachPredecessorIncludingAreasReachesLanelet) { // NOLINT + class TargetFound {}; + auto throwIfTarget = [&](const LaneletOrAreaVisitInformation& i) { + if (i.laneletOrArea.id() == 2050) { + throw TargetFound{}; + } + return true; + }; + EXPECT_THROW(graph->forEachPredecessorIncludingAreas(lanelets.at(2053), throwIfTarget), TargetFound); // NOLINT +} + +TEST_F(GermanVehicleGraph, forEachPredecessorIncludingAreasReachesLanelet) { // NOLINT + class TargetFound {}; + auto throwIfTarget = [&](const LaneletOrAreaVisitInformation& i) { + if (i.laneletOrArea.id() == 2004) { + throw TargetFound{}; + } + return true; + }; + EXPECT_THROW(graph->forEachPredecessorIncludingAreas(lanelets.at(2007), throwIfTarget), TargetFound); // NOLINT +} + +TEST_F(GermanVehicleGraph, forEachPredecessorReachesLanelet) { // NOLINT + class TargetFound {}; + auto throwIfTarget = [&](const LaneletVisitInformation& i) { + if (i.lanelet.id() == 2004) { + throw TargetFound{}; + } + return true; + }; + EXPECT_THROW(graph->forEachPredecessor(lanelets.at(2007), throwIfTarget), TargetFound); // NOLINT +} + +TEST_F(GermanPedestrianGraph, shortestPathIncludingAreasFromArea) { // NOLINT + auto path = + graph->shortestPathIncludingAreas(ConstLaneletOrArea(areas.at(3001)), ConstLaneletOrArea(lanelets.at(2053))); + ASSERT_TRUE(!!path); + EXPECT_EQ(path->size(), 2ul); +} + +TEST_F(GermanPedestrianGraph, shortestPathIncludingAreasThroughAreas) { + auto path = + graph->shortestPathIncludingAreas(ConstLaneletOrArea(lanelets.at(2050)), ConstLaneletOrArea(lanelets.at(2053))); + ASSERT_TRUE(!!path); + EXPECT_EQ(path->size(), 4ul); +} + +TEST_F(GermanPedestrianGraph, shortestPathIncludingAreasViaThroughAreas) { + auto path = + graph->shortestPathIncludingAreasVia(ConstLaneletOrArea(lanelets.at(2050)), {ConstLaneletOrArea(areas.at(3002))}, + ConstLaneletOrArea(lanelets.at(2053))); + ASSERT_TRUE(!!path); + EXPECT_EQ(path->size(), 6ul); +} + +TEST(RoutingCostInitialization, NegativeLaneChangeCost) { // NOLINT + EXPECT_NO_THROW(RoutingCostDistance(1)); // NOLINT + EXPECT_THROW(RoutingCostDistance(-1), InvalidInputError); // NOLINT +} + +class ConfigurationInitialization : public ::testing::Test { + public: + LaneletMapPtr emptyMap = std::make_shared(); + + // Initialize traffic rules and routing cost calculation module + traffic_rules::TrafficRulesPtr trafficRules{traffic_rules::TrafficRulesFactory::create( + Locations::Germany, Participants::Vehicle, traffic_rules::TrafficRules::Configuration())}; + + RoutingCostPtrs costPtrs{std::make_shared(RoutingCostDistance{2.})}; + + // Optional: Initialize config for routing graph: + MapGraph::Configuration MapGraphConf; +}; + +TEST_F(ConfigurationInitialization, EmptyConfig) { // NOLINT + EXPECT_NO_THROW(MapGraph::build(*emptyMap, *trafficRules, costPtrs, MapGraphConf)); // NOLINT +} + +TEST_F(ConfigurationInitialization, Config) { // NOLINT + MapGraphConf.emplace(std::make_pair(MapGraph::ParticipantHeight, Attribute("2."))); // NOLINT + EXPECT_NO_THROW(MapGraph::build(*emptyMap, *trafficRules, costPtrs, MapGraphConf)); // NOLINT +} + +TEST_F(ConfigurationInitialization, NoConfig) { // NOLINT + EXPECT_NO_THROW(MapGraph::build(*emptyMap, *trafficRules, costPtrs)); // NOLINT +} diff --git a/lanelet2_map_learning/test/test_routing_graph_container.cpp b/lanelet2_map_learning/test/test_routing_graph_container.cpp new file mode 100644 index 00000000..774d7909 --- /dev/null +++ b/lanelet2_map_learning/test/test_routing_graph_container.cpp @@ -0,0 +1,112 @@ +#include +#include + +#include "lanelet2_map_learning/Forward.h" +#include "lanelet2_map_learning/MapGraph.h" +#include "lanelet2_map_learning/MapGraphContainer.h" +#include "test_routing_map.h" + +using namespace lanelet; +using namespace lanelet::routing; +using namespace lanelet::routing::tests; + +class MapGraphContainerTest : public MapGraphTest { + public: + MapGraphContainerTest() { + std::vector graphs{testData.vehicleGraph, testData.pedestrianGraph}; + container = std::make_unique(graphs); + } + + MapGraphContainerUPtr container; +}; + +class RouteMapGraphContainerTest : public MapGraphContainerTest { + public: + void getAndCheckRoute(const MapGraphConstPtr& graph, Id from, Id to, routing::RoutingCostId routingCostId = 0) { + ASSERT_NE(graph->passableSubmap()->laneletLayer.find(from), graph->passableSubmap()->laneletLayer.end()); + ConstLanelet fromLanelet{*graph->passableSubmap()->laneletLayer.find(from)}; + ASSERT_NE(graph->passableSubmap()->laneletLayer.find(to), graph->passableSubmap()->laneletLayer.end()); + ConstLanelet toLanelet{*graph->passableSubmap()->laneletLayer.find(to)}; + Optional tempRoute = graph->getRoute(fromLanelet, toLanelet, routingCostId); + ASSERT_TRUE(!!tempRoute); + route = std::make_unique(std::move(*tempRoute)); + } + + std::unique_ptr route; +}; + +TEST_F(MapGraphContainerTest, ConflictingInGraph) { // NOLINT + ConstLanelet pedestrianLanelet{*laneletMap->laneletLayer.find(2031)}; + ConstLanelets conflictingVehicle{container->conflictingInGraph(pedestrianLanelet, 0)}; + ASSERT_EQ(conflictingVehicle.size(), 1ul); + EXPECT_EQ(conflictingVehicle[0], *laneletMap->laneletLayer.find(2020)); + + ConstLanelets conflictingPedestrian{container->conflictingInGraph(pedestrianLanelet, 1)}; + EXPECT_EQ(conflictingPedestrian.size(), 0ul); +} + +TEST_F(MapGraphContainerTest, ConflictingInGraphs) { // NOLINT + ConstLanelet pedestrianLanelet{*laneletMap->laneletLayer.find(2031)}; + MapGraphContainer::ConflictingInGraphs result{container->conflictingInGraphs(pedestrianLanelet)}; + ASSERT_EQ(result.size(), 2ul); + + ConstLanelets conflictingVehicle{result[0].second}; + ASSERT_EQ(conflictingVehicle.size(), 1ul); + EXPECT_EQ(conflictingVehicle[0], *laneletMap->laneletLayer.find(2020)); + + ConstLanelets conflictingPedestrian{result[1].second}; + EXPECT_EQ(conflictingPedestrian.size(), 0ul); +} + +TEST_F(RouteMapGraphContainerTest, ConflictingOfRouteInGraph) { // NOLINT + getAndCheckRoute(container->MapGraphs()[0], 2017, 2024); + ConstLanelets conflictingVehicle{container->conflictingOfRouteInGraph(route.get(), 0)}; + EXPECT_EQ(conflictingVehicle.size(), 2ul); + + ConstLanelets conflictingPedestrian{container->conflictingOfRouteInGraph(route.get(), 1)}; + EXPECT_EQ(conflictingPedestrian.size(), 1ul); +} + +TEST_F(RouteMapGraphContainerTest, ConflictingOfRouteInGraphs) { // NOLINT + getAndCheckRoute(container->MapGraphs()[0], 2017, 2024); + MapGraphContainer::ConflictingInGraphs result{container->conflictingOfRouteInGraphs(route.get(), 0)}; + ASSERT_EQ(result.size(), 2ul); + + ConstLanelets conflictingVehicle{result[0].second}; + EXPECT_EQ(conflictingVehicle.size(), 2ul); + + ConstLanelets conflictingPedestrian{result[1].second}; + EXPECT_EQ(conflictingPedestrian.size(), 1ul); +} + +TEST_F(MapGraphContainerTest, ConflictingInGraph3dFits) { // NOLINT + ConstLanelet bridgeLanelet{*laneletMap->laneletLayer.find(2032)}; + ConstLanelets conflictingVehicle{container->conflictingInGraph(bridgeLanelet, 0, 2.)}; + EXPECT_EQ(conflictingVehicle.size(), 0ul); + + conflictingVehicle = container->conflictingInGraph(lanelets.find(2005)->second, 0, 2.); + ASSERT_EQ(conflictingVehicle.size(), 1ul); + EXPECT_TRUE(conflictingVehicle.front() == lanelets.find(2006)->second); + + conflictingVehicle = container->conflictingInGraph(lanelets.find(2006)->second, 0, 2.); + ASSERT_EQ(conflictingVehicle.size(), 1ul); + EXPECT_TRUE(conflictingVehicle.front() == lanelets.find(2005)->second); + + ConstLanelets conflictingPedestrian{container->conflictingInGraph(bridgeLanelet, 1, 2.)}; + EXPECT_EQ(conflictingPedestrian.size(), 0ul); +} + +TEST_F(MapGraphContainerTest, ConflictingInGraph3dDoesntFit) { // NOLINT + ConstLanelet bridgeLanelet{*laneletMap->laneletLayer.find(2032)}; + ConstLanelets conflictingVehicle{container->conflictingInGraph(bridgeLanelet, 0, 4.)}; + EXPECT_EQ(conflictingVehicle.size(), 2ul); + + conflictingVehicle = container->conflictingInGraph(lanelets.find(2005)->second, 0, 4.); + EXPECT_EQ(conflictingVehicle.size(), 2ul); + + conflictingVehicle = container->conflictingInGraph(lanelets.find(2006)->second, 0, 4.); + EXPECT_EQ(conflictingVehicle.size(), 2ul); + + ConstLanelets conflictingPedestrian{container->conflictingInGraph(bridgeLanelet, 1, 4.)}; + EXPECT_EQ(conflictingPedestrian.size(), 0ul); +} diff --git a/lanelet2_map_learning/test/test_routing_map.h b/lanelet2_map_learning/test/test_routing_map.h new file mode 100644 index 00000000..469fe71f --- /dev/null +++ b/lanelet2_map_learning/test/test_routing_map.h @@ -0,0 +1,533 @@ +#include +#include +#include +#include +#include + +#include +#include + +#include "lanelet2_map_learning/Forward.h" +#include "lanelet2_map_learning/MapGraph.h" + +/// The coordinates and relations for this test can be found in "LaneletTestMap.xml" which can be viewed in +/// https://www.draw.io +namespace lanelet { +namespace routing { +namespace tests { + +inline MapGraphPtr setUpGermanVehicleGraph(LaneletMap& map, double laneChangeCost = 2., + double participantHeight = 2., double minLaneChangeLength = 0.) { + traffic_rules::TrafficRulesPtr trafficRules{traffic_rules::TrafficRulesFactory::create( + Locations::Germany, Participants::Vehicle, traffic_rules::TrafficRules::Configuration())}; + RoutingCostPtrs costPtrs{std::make_shared(laneChangeCost, minLaneChangeLength), + std::make_shared(laneChangeCost)}; + MapGraph::Configuration configuration; + configuration.insert(std::make_pair(MapGraph::ParticipantHeight, participantHeight)); + return MapGraph::build(map, *trafficRules, costPtrs, configuration); +} + +inline MapGraphPtr setUpGermanPedestrianGraph(LaneletMap& map, double laneChangeCost = 2.) { + traffic_rules::TrafficRulesPtr trafficRules{ + traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Pedestrian)}; + RoutingCostPtrs costPtrs{std::make_shared(laneChangeCost)}; + return MapGraph::build(map, *trafficRules, costPtrs); +} + +inline MapGraphPtr setUpGermanBicycleGraph(LaneletMap& map, double laneChangeCost = 2.) { + traffic_rules::TrafficRulesPtr trafficRules{ + traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Bicycle)}; + RoutingCostPtrs costPtrs{std::make_shared(laneChangeCost)}; + return MapGraph::build(map, *trafficRules, costPtrs); +} + +class MapGraphTestData { + public: + MapGraphTestData() { + initPoints(); + initLineStrings(); + initLanelets(); + initAreas(); + laneletMap = std::make_shared(lanelets, areas, std::unordered_map(), + std::unordered_map(), lines, points); + vehicleGraph = setUpGermanVehicleGraph(*laneletMap, laneChangeCost); + pedestrianGraph = setUpGermanPedestrianGraph(*laneletMap, laneChangeCost); + bicycleGraph = setUpGermanBicycleGraph(*laneletMap, laneChangeCost); + } + + void addPoint(double x, double y, double z) { + pointId++; + points.insert(std::pair(pointId, Point3d(pointId, x, y, z))); + } + + void addLine(const Points3d& points) { + lineId++; + lines.insert(std::pair(lineId, LineString3d(lineId, points))); + } + + void addLaneletVehicle(const LineString3d& left, const LineString3d& right) { + laneletId++; + Lanelet ll{laneletId, left, right}; + ll.setAttribute(AttributeName::Subtype, AttributeValueString::Road); + lanelets.insert(std::make_pair(laneletId, ll)); + } + + void addLaneletPedestrian(const LineString3d& left, const LineString3d& right) { + laneletId++; + Lanelet ll{laneletId, left, right}; + ll.setAttribute(AttributeName::Subtype, AttributeValueString::Crosswalk); + lanelets.insert(std::make_pair(laneletId, ll)); + } + + void addAreaPedestrian(const LineStrings3d& outerBound) { + Area area(areaId, outerBound); + area.setAttribute(AttributeName::Subtype, AttributeValueString::Crosswalk); + areas.emplace(areaId, area); + areaId++; + } + + Id pointId{0}; + Id lineId{1000}; + Id laneletId{2000}; + Id areaId{3000}; + std::unordered_map lanelets; + std::unordered_map points; + std::unordered_map lines; + std::unordered_map areas; + const double laneChangeCost{2.}; + MapGraphPtr vehicleGraph, pedestrianGraph, bicycleGraph; + LaneletMapPtr laneletMap; + + private: + void initPoints() { + points.clear(); + addPoint(0., 1., 0.); // p1 + addPoint(1., 1., 0.); + addPoint(2., 1.5, 0.); + addPoint(0., 0., 0.); + addPoint(1., 0., 0.); + addPoint(2., 0.5, 0.); + addPoint(0., 2., 0.); + addPoint(1., 2., 0.); + addPoint(2., 2.5, 0.); // p9 + addPoint(5., 3.5, 0.); + addPoint(5., 1.5, 0.); // p11 + addPoint(7., 3.5, 0.); + addPoint(7., 1.5, 0.); // p13 + addPoint(10., 5., 0.); + addPoint(10., 3., 0.); + addPoint(10., 1., 0.); // p16 + addPoint(13., 7., 0.); + addPoint(13., 5., 0.); + addPoint(13., 3., 0.); + addPoint(13., 1., 0.); // p20 + addPoint(15., 7., 0.); + addPoint(15., 5., 0.); + addPoint(15., 3., 0.); + addPoint(15., 1., 0.); // p24 + addPoint(5., 14., 0.); + addPoint(5., 12., 0.); + addPoint(5., 10., 0.); + addPoint(7., 14., 0.); + addPoint(7., 12., 0.); + addPoint(7., 10., 0.); + addPoint(10., 13., 0.); // p31 + addPoint(10., 11., 0.); + addPoint(12., 13., 0.); + addPoint(12., 11., 0.); + addPoint(15., 14., 0.); + addPoint(15., 12., 0.); + addPoint(15., 10., 0.); + addPoint(17., 14., 0.); + addPoint(17., 12., 0.); + addPoint(17., 10., 0.); // p40 + addPoint(17., 8., 0.); + addPoint(19., 12., 0.); + addPoint(19., 10., 0.); + addPoint(19., 8., 0.); // p44 + addPoint(21., 12., 0.); + addPoint(21., 10., 0.); // p46 + addPoint(21., 8., 0.); + addPoint(23., 12., 0.); // p48 + addPoint(23., 10., 0.); // p49 + addPoint(23., 8., 0.); // p50 + addPoint(10.5, 14., 0.); // p51 + addPoint(11.5, 14., 0.); // p52 + addPoint(10.5, 10., 0.); // p53 + addPoint(11.5, 10., 0.); // p54 + addPoint(3., 5., 3.); // p55 + addPoint(4., 5., 3.); // p56 + addPoint(3., -1., 3.); // p57 + addPoint(4., -1., 3.); // p58 + addPoint(26., 10., 0.); // p59 + addPoint(28., 10., 0.); // p60 + addPoint(30., 10., 0.); // p61 + addPoint(25., 11., 0.); // p62 + addPoint(26., 12., 0.); // p63 + addPoint(28., 12., 0.); // p64 + addPoint(30., 12., 0.); // p65 + addPoint(25., 13., 0.); // p66 + addPoint(28., 14., 0.); // p67 + addPoint(30., 14., 0.); // p68 + addPoint(32., 10., 0.); // p69 + addPoint(33., 9., 0.); // p70 + addPoint(32., 8., 0.); // p71 + addPoint(33., 7., 0.); // p72 + addPoint(28., 8., 0.); // p73 + addPoint(30., 8., 0.); // p74 + addPoint(35., 10., 0.); // p75 + addPoint(35., 8., 0.); // p76 + addPoint(35., 6., 0.); // p77 + addPoint(37., 9., 0.); // p78 + addPoint(37., 7., 0.); // p79 + addPoint(37., 5., 0.); // p80 + addPoint(38., 10., 0.); // p81 + addPoint(38., 8., 0.); // p82 + addPoint(41., 10., 0.); // p83 + addPoint(41., 8., 0.); // p84 + addPoint(41., 6., 0.); // p85 + addPoint(43., 10., 0.); // p86 + addPoint(43., 8., 0.); // p87 + + // points around areas + pointId = 88; + addPoint(24, 4, 0); // p89 + addPoint(26, 4, 0); // p90 + addPoint(24, 2, 0); // p91 + addPoint(26, 2, 0); // p92 + addPoint(27, 5, 0); // p93 + addPoint(28, 5, 0); // p94 + addPoint(29, 4, 0); // p95 + addPoint(29, 2, 0); // p96 + addPoint(28, 1, 0); // p97 + addPoint(27, 1, 0); // p98 + addPoint(27, 0, 0); // p99 + addPoint(28, 0, 0); // p100 + addPoint(27, 6, 0); // p101 + addPoint(28, 6, 0); // p102 + addPoint(31, 4, 0); // p103 + addPoint(33, 4, 0); // p104 + addPoint(31, 2, 0); // p105 + addPoint(33, 2, 0); // p106 + addPoint(31, 0, 0); // p107 + + // points on the conflicting and circular section + pointId = 119; + addPoint(33, 11, 0); // p120 + addPoint(37, 16, 0); // p121 + addPoint(37, 14, 0); // p122 + addPoint(37, 12, 0); // p123 + addPoint(41, 12, 0); // p124 + addPoint(32, 16, 0); // p125 + addPoint(34, 16, 0); // p126 + addPoint(28, 18, 0); // p127 + addPoint(28, 20, 0); // p128 + addPoint(21, 14, 0); // p129 + addPoint(23, 14, 0); // p130 + addPoint(19, 14, 0); // p131 + } + + void initLineStrings() { + lines.clear(); + addLine(Points3d{points.at(1), points.at(2)}); // l1001 + lines.at(1001).setAttribute(AttributeNamesString::LaneChange, true); + addLine(Points3d{points.at(4), points.at(5)}); + addLine(Points3d{points.at(2), points.at(3)}); + addLine(Points3d{points.at(5), points.at(6)}); + addLine(Points3d{points.at(7), points.at(8)}); + addLine(Points3d{points.at(8), points.at(9)}); // ls1006 + addLine(Points3d{points.at(9), points.at(10)}); + addLine(Points3d{points.at(3), points.at(11)}); + lines.at(1008).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine(Points3d{points.at(3), points.at(10)}); + lines.at(1009).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine(Points3d{points.at(6), points.at(11)}); // ls1010 + addLine(Points3d{points.at(10), points.at(12)}); + addLine(Points3d{points.at(11), points.at(13)}); // ls1012 + addLine(Points3d{points.at(12), points.at(14)}); + addLine(Points3d{points.at(12), points.at(15)}); + addLine(Points3d{points.at(13), points.at(15)}); + lines.at(1015).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine(Points3d{points.at(13), points.at(16)}); // ls1016 + addLine(Points3d{points.at(14), points.at(17)}); + addLine(Points3d{points.at(14), points.at(18)}); + addLine(Points3d{points.at(15), points.at(18)}); + lines.at(1019).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine(Points3d{points.at(15), points.at(19)}); + lines.at(1020).setAttribute(AttributeNamesString::LaneChange, true); + addLine(Points3d{points.at(16), points.at(20)}); // ls1021 + addLine(Points3d{points.at(17), points.at(21)}); + addLine(Points3d{points.at(18), points.at(22)}); + lines.at(1023).setAttribute(AttributeNamesString::LaneChange, true); + addLine(Points3d{points.at(19), points.at(23)}); + lines.at(1024).setAttribute(AttributeNamesString::LaneChange, true); + addLine(Points3d{points.at(20), points.at(24)}); + addLine(Points3d{points.at(25), points.at(28)}); // ls1026 + addLine(Points3d{points.at(26), points.at(29)}); + addLine(Points3d{points.at(27), points.at(30)}); + addLine(Points3d{points.at(28), points.at(31)}); + addLine(Points3d{points.at(29), points.at(31)}); // ls1030 + lines.at(1030).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine(Points3d{points.at(29), points.at(32)}); + lines.at(1031).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine(Points3d{points.at(30), points.at(32)}); + addLine(Points3d{points.at(31), points.at(33)}); + addLine(Points3d{points.at(32), points.at(34)}); + addLine(Points3d{points.at(33), points.at(35)}); + addLine(Points3d{points.at(33), points.at(36)}); + lines.at(1036).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine(Points3d{points.at(34), points.at(36)}); + lines.at(1037).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine(Points3d{points.at(34), points.at(37)}); + addLine(Points3d{points.at(35), points.at(38)}); + addLine(Points3d{points.at(36), points.at(39)}); + addLine(Points3d{points.at(37), points.at(40)}); // ls1041 + addLine(Points3d{points.at(39), points.at(42)}); + addLine(Points3d{points.at(40), points.at(43)}); + lines.at(1043).setAttribute(AttributeName::Type, AttributeValueString::LineThin); + lines.at(1043).setAttribute(AttributeName::Subtype, AttributeValueString::DashedSolid); + addLine(Points3d{points.at(41), points.at(44)}); // ls1044 + addLine(Points3d{points.at(42), points.at(45)}); + addLine(Points3d{points.at(43), points.at(46)}); // ls1046 + addLine(Points3d{points.at(44), points.at(47)}); + addLine(Points3d{points.at(45), points.at(48)}); + addLine(Points3d{points.at(46), points.at(49)}); + lines.at(1049).setAttribute(AttributeName::Type, AttributeValueString::LineThin); + lines.at(1049).setAttribute(AttributeName::Subtype, AttributeValueString::SolidDashed); + addLine(Points3d{points.at(47), points.at(50)}); // ls1050 + addLine(Points3d{points.at(51), points.at(53)}); // ls1051 + addLine(Points3d{points.at(52), points.at(54)}); // ls1052 + addLine(Points3d{points.at(55), points.at(57)}); // ls1053 + addLine(Points3d{points.at(56), points.at(58)}); // ls1054 + addLine(Points3d{points.at(49), points.at(59)}); // ls1055 + addLine(Points3d{points.at(59), points.at(60)}); // ls1056 + addLine(Points3d{points.at(60), points.at(61)}); // ls1057 + lines.at(1057).setAttribute(AttributeName::Type, AttributeValueString::LineThin); + lines.at(1057).setAttribute(AttributeName::Subtype, AttributeValueString::Solid); + addLine(Points3d{points.at(49), points.at(62)}); // ls1058 + lines.at(1058).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine(Points3d{points.at(62), points.at(64)}); // ls1059 + lines.at(1059).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine(Points3d{points.at(48), points.at(63)}); // ls1060 + lines.at(1060).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine(Points3d{points.at(63), points.at(64)}); // ls1061 + lines.at(1061).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine(Points3d{points.at(64), points.at(65)}); // ls1062 + lines.at(1062).setAttribute(AttributeNamesString::LaneChange, true); + addLine(Points3d{points.at(48), points.at(66)}); // ls1063 + addLine(Points3d{points.at(66), points.at(67)}); // ls1064 + addLine(Points3d{points.at(67), points.at(68)}); // ls1065 + addLine(Points3d{points.at(61), points.at(69)}); // ls1066 + addLine(Points3d{points.at(61), points.at(70)}); // ls1067 + addLine(Points3d{points.at(74), points.at(71)}); // ls1068 + addLine(Points3d{points.at(74), points.at(72)}); // ls1069 + addLine(Points3d{points.at(73), points.at(74)}); // ls1070 + addLine(Points3d{points.at(69), points.at(75)}); // ls1071 + addLine(Points3d{points.at(70), points.at(76)}); // ls1072 + addLine(Points3d{points.at(71), points.at(76)}); // ls1073 + addLine(Points3d{points.at(72), points.at(77)}); // ls1074 + addLine(Points3d{points.at(75), points.at(81)}); // ls1075 + addLine(Points3d{points.at(76), points.at(78)}); // ls1076 + addLine(Points3d{points.at(76), points.at(82)}); // ls1077 + addLine(Points3d{points.at(77), points.at(79)}); // ls1078 + addLine(Points3d{points.at(81), points.at(83)}); // ls1079 + addLine(Points3d{points.at(78), points.at(83)}); // ls1080 + addLine(Points3d{points.at(82), points.at(84)}); // ls1081 + addLine(Points3d{points.at(79), points.at(84)}); // ls1082 + lines.at(1082).setAttribute(AttributeName::Type, AttributeValueString::LineThin); + lines.at(1082).setAttribute(AttributeName::Subtype, AttributeValueString::Dashed); + addLine(Points3d{points.at(80), points.at(85)}); // ls1083 + addLine(Points3d{points.at(83), points.at(86)}); // ls1084 + addLine(Points3d{points.at(84), points.at(87)}); // ls1085 + + // linestrings around areas + addLine({points.at(89), points.at(90)}); // ls1086 + addLine({points.at(91), points.at(92)}); // ls1087 + addLine({points.at(90), points.at(93)}); // ls1088 + addLine({points.at(93), points.at(94)}); // ls1089 + addLine({points.at(94), points.at(95)}); // ls1090 + addLine({points.at(95), points.at(96)}); // ls1091 + lines.at(1091).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine({points.at(96), points.at(97)}); // ls1092 + lines.at(1092).setAttribute(AttributeName::Type, AttributeValueString::Curbstone); + lines.at(1092).setAttribute(AttributeName::Subtype, AttributeValueString::Low); + addLine({points.at(97), points.at(98)}); // ls1093 + lines.at(1093).setAttribute(AttributeName::Type, AttributeValueString::Curbstone); + lines.at(1093).setAttribute(AttributeName::Subtype, AttributeValueString::Low); + addLine({points.at(98), points.at(92)}); // ls1094 + addLine({points.at(95), points.at(103)}); // ls1095 + addLine({points.at(96), points.at(105)}); // ls1096 + lines.at(1096).setAttribute(AttributeName::Type, AttributeValueString::Wall); + addLine({points.at(103), points.at(105)}); // ls1097 + lines.at(1097).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine({points.at(103), points.at(104)}); // ls1098 + addLine({points.at(105), points.at(106)}); // ls1099 + addLine({points.at(99), points.at(100)}); // ls1100 + addLine({points.at(101), points.at(102)}); // ls1101 + addLine({points.at(92), points.at(90)}); // ls1102 + lines.at(1102).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine({points.at(97), points.at(107)}); // ls1103 + addLine({points.at(107), points.at(105)}); // ls1104 + + // lines on the conflicting and circular section + lineId = 1199; + addLine({points.at(61), points.at(120)}); // ls1200 + addLine({points.at(74), points.at(70)}); // ls1201 + addLine({points.at(120), points.at(122)}); // ls1202 + addLine({points.at(70), points.at(123)}); // ls1203 + addLine({points.at(121), points.at(124)}); // ls1204 + addLine({points.at(122), points.at(83)}); // ls1205 + addLine({points.at(123), points.at(84)}); // ls1206 + addLine({points.at(68), points.at(125)}); // ls1207 + addLine({points.at(65), points.at(126)}); // ls1208 + addLine({points.at(125), points.at(127)}); // ls1209 + addLine({points.at(126), points.at(128)}); // ls1210 + addLine({points.at(127), points.at(130)}); // ls1211 + addLine({points.at(128), points.at(129)}); // ls1212 + addLine({points.at(130), points.at(48)}); // ls1213 + addLine({points.at(129), points.at(49)}); // ls1214 + addLine({points.at(129), points.at(131)}); // ls1215 + addLine({points.at(130), points.at(42)}); // ls1216 + lines.at(1205).setAttribute(AttributeName::Type, AttributeValueString::LineThin); + lines.at(1205).setAttribute(AttributeName::Subtype, AttributeValueString::Dashed); + } + void initLanelets() { + lanelets.clear(); + addLaneletVehicle(lines.at(1001), lines.at(1002)); // ll2001 + addLaneletVehicle(lines.at(1003), lines.at(1004)); + addLaneletVehicle(lines.at(1005), lines.at(1001)); + addLaneletVehicle(lines.at(1006), lines.at(1003)); // ll2004 + addLaneletVehicle(lines.at(1007), lines.at(1008)); + addLaneletVehicle(lines.at(1009), lines.at(1010)); // ll2006 + addLaneletVehicle(lines.at(1011), lines.at(1012)); // ll2007 + addLaneletVehicle(lines.at(1013), lines.at(1015)); // ll2008 + addLaneletVehicle(lines.at(1014), lines.at(1016)); // ll2009 + addLaneletVehicle(lines.at(1017), lines.at(1019)); // ll2010 + addLaneletVehicle(lines.at(1018), lines.at(1020)); // ll2011 + addLaneletVehicle(lines.at(1020), lines.at(1021)); // ll2012 + addLaneletVehicle(lines.at(1022), lines.at(1023)); // ll2013 + addLaneletVehicle(lines.at(1023), lines.at(1024)); // ll2014 + addLaneletVehicle(lines.at(1024), lines.at(1025)); // ll2015 + addLaneletVehicle(lines.at(1027).invert(), lines.at(1026).invert()); + addLaneletVehicle(lines.at(1027), lines.at(1028)); + addLaneletVehicle(lines.at(1031).invert(), lines.at(1029).invert()); + addLaneletVehicle(lines.at(1030), lines.at(1032)); + addLaneletVehicle(lines.at(1033), lines.at(1034)); + lanelets.at(2020).setAttribute(AttributeName::OneWay, false); + addLaneletVehicle(lines.at(1037).invert(), lines.at(1035).invert()); + addLaneletVehicle(lines.at(1036), lines.at(1038)); + lanelets.at(2022).setAttribute(AttributeNamesString::Subtype, AttributeValueString::Highway); + addLaneletVehicle(lines.at(1040).invert(), lines.at(1039).invert()); + addLaneletVehicle(lines.at(1040), lines.at(1041)); // ll2024 + addLaneletVehicle(lines.at(1042), lines.at(1043)); + addLaneletVehicle(lines.at(1043), lines.at(1044)); + addLaneletVehicle(lines.at(1045), lines.at(1046)); // ll2027 + addLaneletVehicle(lines.at(1046), lines.at(1047)); + addLaneletVehicle(lines.at(1048), lines.at(1049)); + addLaneletVehicle(lines.at(1049), lines.at(1050)); // ll2030 + addLaneletPedestrian(lines.at(1051), lines.at(1052)); // ll2031 + addLaneletVehicle(lines.at(1053), lines.at(1054)); // ll2032 + addLaneletVehicle(lines.at(1060), lines.at(1055)); // ll2033 + addLaneletVehicle(lines.at(1061), lines.at(1056)); // ll2034 + addLaneletVehicle(lines.at(1062), lines.at(1057)); // ll2035 + addLaneletVehicle(lines.at(1063), lines.at(1058)); // ll2036 + addLaneletVehicle(lines.at(1064), lines.at(1059)); // ll2037 + addLaneletVehicle(lines.at(1065), lines.at(1062)); // ll2038 + addLaneletVehicle(lines.at(1066), lines.at(1068)); // ll2039 + addLaneletVehicle(lines.at(1067), lines.at(1069)); // ll2040 + addLaneletVehicle(lines.at(1057), lines.at(1070)); // ll2041 + addLaneletVehicle(lines.at(1071), lines.at(1073)); // ll2042 + addLaneletVehicle(lines.at(1072), lines.at(1074)); // ll2043 + addLaneletVehicle(lines.at(1075), lines.at(1077)); // ll2044 + addLaneletVehicle(lines.at(1076), lines.at(1078)); // ll2045 + addLaneletVehicle(lines.at(1079), lines.at(1081)); // ll2046 + addLaneletVehicle(lines.at(1080), lines.at(1082)); // ll2047 + addLaneletVehicle(lines.at(1082), lines.at(1083)); // ll2048 + addLaneletVehicle(lines.at(1084), lines.at(1085)); // ll2049 + + // area + addLaneletPedestrian(lines.at(1086), lines.at(1087)); // ll2050 + lanelets.at(2050).setAttribute(AttributeName::OneWay, true); + addLaneletPedestrian(lines.at(1101), lines.at(1089)); // ll2051 + addLaneletPedestrian(lines.at(1093).invert(), lines.at(1100)); // ll2052 + lanelets.at(2052).setAttribute(AttributeName::OneWay, true); + addLaneletPedestrian(lines.at(1098), lines.at(1099)); // ll2053 + + // lanelets on conflicting section + laneletId = 2059; + addLaneletVehicle(lines.at(1200), lines.at(1201)); // ll2060 + addLaneletVehicle(lines.at(1202), lines.at(1203)); // ll2061 + addLaneletVehicle(lines.at(1204), lines.at(1205)); // ll2062 + addLaneletVehicle(lines.at(1205), lines.at(1206)); // ll2063 + addLaneletVehicle(lines.at(1207), lines.at(1208)); // ll2064 + addLaneletVehicle(lines.at(1209), lines.at(1210)); // ll2065 + addLaneletVehicle(lines.at(1211), lines.at(1212)); // ll2066 + addLaneletVehicle(lines.at(1213), lines.at(1214)); // ll2067 + addLaneletVehicle(lines.at(1216), lines.at(1215)); // ll2068 + } + + void initAreas() { + addAreaPedestrian({lines.at(1102), lines.at(1088), lines.at(1089), lines.at(1090), lines.at(1091), lines.at(1092), + lines.at(1093), lines.at(1094)}); // ar3000 + addAreaPedestrian({lines.at(1095), lines.at(1097), lines.at(1096), lines.at(1091).invert()}); // ar3001 + // addAreaPedestrian({lines.at(1096).invert(), lines.at(1092), lines.at(1103), lines.at(1104)}); // ar3002 + addAreaPedestrian( + {lines.at(1096), lines.at(1104).invert(), lines.at(1103).invert(), lines.at(1092).invert()}); // ar3002 + areas.at(3002).setAttribute(AttributeName::Subtype, AttributeValueString::Walkway); + } +}; + +namespace { // NOLINT +static MapGraphTestData testData; // NOLINT +} // namespace + +class MapGraphTest : public ::testing::Test { + public: + const std::unordered_map& lanelets{testData.lanelets}; + const std::unordered_map& areas{testData.areas}; + const std::unordered_map& points{testData.points}; + const std::unordered_map& lines{testData.lines}; + const LaneletMapConstPtr laneletMap{testData.laneletMap}; +}; + +class GermanVehicleGraph : public MapGraphTest { + protected: + GermanVehicleGraph() { EXPECT_NO_THROW(graph->checkValidity()); } // NOLINT + + public: + MapGraphConstPtr graph{testData.vehicleGraph}; + uint8_t numCostModules{2}; +}; + +class GermanPedestrianGraph : public MapGraphTest { + protected: + GermanPedestrianGraph() { EXPECT_NO_THROW(graph->checkValidity()); } // NOLINT + + public: + MapGraphConstPtr graph{testData.pedestrianGraph}; + uint8_t numCostModules{2}; +}; + +class GermanBicycleGraph : public MapGraphTest { + protected: + GermanBicycleGraph() { EXPECT_NO_THROW(graph->checkValidity()); } // NOLINT + + public: + MapGraphConstPtr graph{testData.bicycleGraph}; + uint8_t numCostModules{2}; +}; + +template +class AllGraphsTest : public T {}; + +using AllGraphs = testing::Types; + +#ifndef TYPED_TEST_SUITE +// backwards compability with old gtest versions +#define TYPED_TEST_SUITE TYPED_TEST_CASE +#endif + +TYPED_TEST_SUITE(AllGraphsTest, AllGraphs); +} // namespace tests +} // namespace routing +} // namespace lanelet diff --git a/lanelet2_map_learning/test/test_routing_visualization.cpp b/lanelet2_map_learning/test/test_routing_visualization.cpp new file mode 100644 index 00000000..a58f51b1 --- /dev/null +++ b/lanelet2_map_learning/test/test_routing_visualization.cpp @@ -0,0 +1,102 @@ +#include + +#include + +#include "lanelet2_map_learning/Exceptions.h" +#include "lanelet2_map_learning/MapGraph.h" +#include "test_routing_map.h" + +using namespace lanelet; +using namespace lanelet::routing; +using namespace lanelet::routing::tests; +namespace fs = boost::filesystem; + +class Tempfile { + public: + Tempfile() { + char path[] = {"/tmp/lanelet2_unittest.XXXXXX"}; + auto* res = mkdtemp(path); + if (res == nullptr) { + throw lanelet::LaneletError("Failed to crate temporary directory"); + } + path_ = path; + } + Tempfile(const Tempfile&) = delete; + Tempfile(Tempfile&&) = delete; + Tempfile& operator=(const Tempfile&) = delete; + Tempfile& operator=(Tempfile&&) = delete; + ~Tempfile() { fs::remove_all(fs::path(path_)); } + + auto operator()(const std::string& str) const noexcept -> std::string { return (fs::path(path_) / str).string(); } + + private: + std::string path_; +}; + +static Tempfile tempfile; + +TEST_F(GermanVehicleGraph, GraphVizExport) { // NOLINT + EXPECT_NO_THROW(graph->exportGraphViz(tempfile("graphexport.dot"))); // NOLINT + RelationType excluded{RelationType::Conflicting}; + EXPECT_NO_THROW(graph->exportGraphViz(tempfile("graphexportWithExcluded1.dot"), excluded)); // NOLINT + excluded |= RelationType::AdjacentLeft; + excluded |= RelationType::AdjacentRight; + EXPECT_NO_THROW(graph->exportGraphViz(tempfile("graphexportWithExcluded2.dot"), excluded)); // NOLINT +} + +TEST_F(GermanVehicleGraph, GraphVizExportError) { // NOLINT + EXPECT_THROW(graph->exportGraphViz("", RelationType::None), lanelet::InvalidInputError); // NOLINT + EXPECT_THROW(graph->exportGraphViz("/place/that/doesnt/exist"), lanelet::ExportError); // NOLINT +} + +TEST_F(GermanVehicleGraph, GraphMLExport) { // NOLINT + EXPECT_NO_THROW(graph->exportGraphML(tempfile("graphexport.graphml"))); // NOLINT + RelationType excluded{RelationType::Conflicting}; + EXPECT_NO_THROW(graph->exportGraphML(tempfile("graphexportWithExcluded1.graphml"), excluded)); // NOLINT + excluded |= RelationType::AdjacentLeft; + excluded |= RelationType::AdjacentLeft; + excluded |= RelationType::AdjacentRight; + EXPECT_NO_THROW(graph->exportGraphML(tempfile("graphexportWithExcluded2.graphml"), excluded)); // NOLINT +} + +TEST_F(GermanVehicleGraph, GraphMLExportError) { // NOLINT + EXPECT_THROW(graph->exportGraphML("", RelationType::None), lanelet::InvalidInputError); // NOLINT + EXPECT_THROW(graph->exportGraphML("/place/that/doesnt/exist", RelationType::None), lanelet::ExportError); // NOLINT +} + +TEST_F(GermanVehicleGraph, DebugLaneletMap) { // NOLINT + LaneletMapPtr map{graph->getDebugLaneletMap()}; + EXPECT_TRUE(map->pointLayer.exists(2007)); + EXPECT_TRUE(map->pointLayer.exists(2020)); + EXPECT_TRUE(map->pointLayer.exists(2032)); + EXPECT_GT(map->lineStringLayer.size(), map->pointLayer.size()); + EXPECT_FALSE(map->pointLayer.exists(2031)); +} + +TEST_F(GermanPedestrianGraph, DebugLaneletMap) { // NOLINT + LaneletMapPtr map{graph->getDebugLaneletMap()}; + EXPECT_FALSE(map->pointLayer.exists(2007)); + EXPECT_FALSE(map->pointLayer.exists(2020)); + EXPECT_FALSE(map->pointLayer.exists(2032)); + EXPECT_TRUE(map->pointLayer.exists(2031)); +} + +TEST_F(GermanBicycleGraph, DebugLaneletMap) { // NOLINT + LaneletMapPtr map{graph->getDebugLaneletMap()}; + EXPECT_TRUE(map->pointLayer.exists(2007)); + EXPECT_TRUE(map->pointLayer.exists(2020)); + EXPECT_TRUE(map->pointLayer.exists(2032)); + EXPECT_FALSE(map->pointLayer.exists(2022)); + EXPECT_FALSE(map->pointLayer.exists(2031)); +} + +TYPED_TEST(AllGraphsTest, CheckDebugLaneletMap) { + ASSERT_NO_THROW(this->graph->getDebugLaneletMap()); // NOLINT + const LaneletMapPtr map{this->graph->getDebugLaneletMap()}; + for (const auto& it : map->pointLayer) { + EXPECT_NO_THROW(it.attribute("id")); // NOLINT + } + for (const auto& it : map->lineStringLayer) { + EXPECT_NO_THROW(it.attribute("relation")); // NOLINT + } +} diff --git a/lanelet2_python/include/lanelet2_python/internal/eigen_converter.h b/lanelet2_python/include/lanelet2_python/internal/eigen_converter.h new file mode 100644 index 00000000..9f2ed20b --- /dev/null +++ b/lanelet2_python/include/lanelet2_python/internal/eigen_converter.h @@ -0,0 +1,653 @@ +// MIT License + +// Copyright (c) 2019 Vincent SAMY + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#pragma once + +#include +#include +#include +#include +#include + +namespace py = boost::python; +namespace np = boost::python::numpy; + +namespace converters { + +/* + * List -> Eigen converters + */ + +/** Conversion from a python list to an Eigen Vector + * This convert list of type T (int, float32, float64, ...) to an Eigen::VectorType + * The template VectorType should be a type as Eigen::Matrix or Eigen::Matrix + * Note it should also work for Eigen::Array + */ +template +struct python_list_to_eigen_vector { + python_list_to_eigen_vector() { + py::converter::registry::push_back(&convertible, &construct, py::type_id()); + } + + static void* convertible(PyObject* obj_ptr) { + static_assert(VectorType::ColsAtCompileTime == 1 || VectorType::RowsAtCompileTime == 1, + "Passed a Matrix into a Vector generator"); // Only for vectors conversion + + if (!PySequence_Check(obj_ptr) || + (VectorType::ColsAtCompileTime == 1 && VectorType::RowsAtCompileTime != Eigen::Dynamic && + (PySequence_Size(obj_ptr) != VectorType::RowsAtCompileTime)) // Check Fixed-size vector + || (VectorType::RowsAtCompileTime == 1 && VectorType::ColsAtCompileTime != Eigen::Dynamic && + (PySequence_Size(obj_ptr) != VectorType::ColsAtCompileTime))) // Check Fixed-size row vector + return 0; + + if (VectorType::ColsAtCompileTime == 1 && VectorType::RowsAtCompileTime != Eigen::Dynamic && + (PySequence_Size(obj_ptr) != VectorType::RowsAtCompileTime)) + return 0; + + py::list arr = py::extract(obj_ptr); + for (long i = 0; i < py::len(arr); i++) + if (!py::extract(arr[i]).check()) return 0; + + return obj_ptr; + } + + static void construct(PyObject* obj_ptr, py::converter::rvalue_from_python_stage1_data* data) { + py::list arr = py::extract(obj_ptr); + auto len = py::len(arr); + + using storage_type = py::converter::rvalue_from_python_storage; + void* storage = reinterpret_cast(data)->storage.bytes; + + new (storage) VectorType; + VectorType& vec = *static_cast(storage); + + if (VectorType::RowsAtCompileTime == Eigen::Dynamic || VectorType::ColsAtCompileTime == Eigen::Dynamic) + vec.resize(len); + + for (long i = 0; i < len; ++i) vec(i) = py::extract(arr[i]); + + data->convertible = storage; + } +}; + +/** Conversion from a python list to an Eigen Matrix + * This convert list of list of type T (int, float32, float64, ...) to an Eigen::MatrixType + * The template MatrixType should be a type as Eigen::Matrix + * Note it should also work for Eigen::Array + */ +template +struct python_list_to_eigen_matrix { + python_list_to_eigen_matrix() { + py::converter::registry::push_back(&convertible, &construct, py::type_id()); + } + + static void* convertible(PyObject* obj_ptr) { + static_assert(MatrixType::ColsAtCompileTime != 1 && MatrixType::RowsAtCompileTime != 1, + "Passed a Vector into a Matrix generator"); // Only for matrix conversion + + auto checkNestedList = [](const py::list& list) { + py::extract nested_list(list[0]); + if (!nested_list.check()) return false; + + auto cols = py::len(nested_list()); + for (long i = 1; i < py::len(list); ++i) { + py::extract nested_list(list[i]); + if (!nested_list.check() || py::len(nested_list) != cols) // Check nested list size + return false; + for (long j = 0; j < cols; ++j) + if (!py::extract(nested_list()[j]).check()) // Check list type + return false; + } + + return true; + }; + + py::extract extract_list(obj_ptr); + py::extract extract_nested_list(extract_list()); + if (!extract_list.check() // Check list + || !checkNestedList(extract_list()) // Check nested lists + || (MatrixType::RowsAtCompileTime != Eigen::Dynamic && + MatrixType::RowsAtCompileTime != py::len(extract_list())) // Check rows are the same + || (MatrixType::ColsAtCompileTime != Eigen::Dynamic && + MatrixType::ColsAtCompileTime != py::len(extract_nested_list()))) // Check cols are the same + return 0; + + return obj_ptr; + } + + static void construct(PyObject* obj_ptr, py::converter::rvalue_from_python_stage1_data* data) { + py::list arr = py::extract(obj_ptr); + auto rows = py::len(arr); + auto cols = py::len(py::extract(arr[0])()); + + using storage_type = py::converter::rvalue_from_python_storage; + void* storage = reinterpret_cast(data)->storage.bytes; + + new (storage) MatrixType; + MatrixType& mat = *static_cast(storage); + + // Resize matrix if (half-)dynamic-sized matrix + if (MatrixType::RowsAtCompileTime == Eigen::Dynamic || MatrixType::ColsAtCompileTime == Eigen::Dynamic) + mat.resize(rows, cols); + + for (long i = 0; i < rows; ++i) + for (long j = 0; j < cols; ++j) mat(i, j) = py::extract(arr[i][j]); + + data->convertible = storage; + } +}; + +/* + * Numpy -> Eigen converters + */ + +/** Conversion from a numpy ndarray to an Eigen Vector + * This convert numpy.array of dtype T (int, float32, float64, ...) to an Eigen::VectorType + * The template VectorType should be a type as Eigen::Matrix or Eigen::Matrix + * Note it should also work for Eigen::Array + */ +template +struct numpy_array_to_eigen_vector { + numpy_array_to_eigen_vector() { + py::converter::registry::push_back(&convertible, &construct, py::type_id()); + } + + static void* convertible(PyObject* obj_ptr) { + static_assert(VectorType::RowsAtCompileTime == 1 || VectorType::ColsAtCompileTime == 1, + "Passed a Matrix into a Vector generator"); // Check that in c++ side, it is an Eigen vector + py::extract arr(obj_ptr); + if (!arr.check() // Check it is a numpy array + || arr().get_nd() != + 1 // Check array dimension (does not allow numpy array of type (1, 3), needs to ravel it first) + || arr().get_dtype() != np::dtype::get_builtin() // Check type + || (VectorType::RowsAtCompileTime == 1 && VectorType::ColsAtCompileTime != Eigen::Dynamic && + VectorType::ColsAtCompileTime != + arr().shape(0)) // Check vector size in case of fixed-size array (for a row-vector) + || (VectorType::ColsAtCompileTime == 1 && VectorType::RowsAtCompileTime != Eigen::Dynamic && + VectorType::RowsAtCompileTime != + arr().shape(0))) // Check vector size in case of fixed-size array (for a column-vector) + return 0; + + return obj_ptr; + } + + static void construct(PyObject* obj_ptr, py::converter::rvalue_from_python_stage1_data* data) { + np::ndarray arr = py::extract(obj_ptr); + + using storage_type = py::converter::rvalue_from_python_storage; + void* storage = reinterpret_cast(data)->storage.bytes; + + new (storage) VectorType; + VectorType& vec = *static_cast(storage); + // Resize for dynamic-sized matrices + if (VectorType::RowsAtCompileTime == Eigen::Dynamic || VectorType::ColsAtCompileTime == Eigen::Dynamic) + vec.resize(arr.shape(0)); + + // Extract values. The type has been check in the convertible function + for (int i = 0; i < arr.shape(0); ++i) vec(i) = py::extract(arr[i]); + + data->convertible = storage; + } +}; + +/** Conversion from a numpy ndarray to an Eigen Matrix + * This convert numpy.array of dtype T (int, float32, float64, ...) to an Eigen::MatrixType + * The template MatrixType should be a type as Eigen::Matrix + * Note it should also work for Eigen::Array + */ +template +struct numpy_array_to_eigen_matrix { + numpy_array_to_eigen_matrix() { + py::converter::registry::push_back(&convertible, &construct, py::type_id()); + } + + static void* convertible(PyObject* obj_ptr) { + static_assert(MatrixType::ColsAtCompileTime != 1 && MatrixType::RowsAtCompileTime != 1, + "Passed a Vector into a Matrix generator"); // Only for matrix conversion + + py::extract arr(obj_ptr); + if (!arr.check() // Check it is a numpy array + || arr().get_nd() != 2 // Check array dimension + || arr().get_dtype() != np::dtype::get_builtin() // Check type + || (MatrixType::RowsAtCompileTime != Eigen::Dynamic && + MatrixType::RowsAtCompileTime != arr().shape(0)) // Check rows are the same + || (MatrixType::ColsAtCompileTime != Eigen::Dynamic && + MatrixType::ColsAtCompileTime != arr().shape(1))) // Check cols are the same + return 0; + + return obj_ptr; + } + + static void construct(PyObject* obj_ptr, py::converter::rvalue_from_python_stage1_data* data) { + np::ndarray arr = py::extract(obj_ptr); + + using storage_type = py::converter::rvalue_from_python_storage; + void* storage = reinterpret_cast(data)->storage.bytes; + + new (storage) MatrixType; + MatrixType& mat = *static_cast(storage); + // Resize for dynamic-sized matrices + // For half dynamic-sized matrices, the fixed-size part has been check in the convertible function + if (MatrixType::RowsAtCompileTime == Eigen::Dynamic || MatrixType::ColsAtCompileTime == Eigen::Dynamic) + mat.resize(arr.shape(0), arr.shape(1)); + + // Extract values. The type has been check in the convertible function + for (int i = 0; i < arr.shape(0); ++i) + for (int j = 0; j < arr.shape(1); ++j) mat(i, j) = py::extract(arr[i][j]); + + data->convertible = storage; + } +}; + +/* + * Eigen -> Numpy converters + */ + +/** Conversion from an Eigen Vector to an numpy ndarray + * This convert Eigen::VectorType of type T (int, float, double, ...) to an numpy.array + * The template VectorType should be a type as Eigen::Matrix or Eigen::Matrix + * Note it should also work for Eigen::Array + */ +template +struct eigen_vector_to_numpy_array { + static PyObject* convert(const VectorType& mat) { + static_assert(VectorType::ColsAtCompileTime == 1 || VectorType::RowsAtCompileTime == 1, + "Passed a Matrix into a Vector generator"); // Ensure that it is a vector + + np::dtype dt = np::dtype::get_builtin(); + auto shape = py::make_tuple(mat.size()); + np::ndarray mOut = np::empty(shape, dt); + + for (Eigen::Index i = 0; i < mat.size(); ++i) mOut[i] = mat(i); + + return py::incref(mOut.ptr()); + } +}; + +/** Conversion from an Eigen Matrix to an numpy ndarray + * This convert Eigen::MatrixType of type T (int, float, double, ...) to an numpy.array + * The template MatrixType should be a type as Eigen::Matrix + * Note it should also work for Eigen::Array + */ +template +struct eigen_matrix_to_numpy_array { + static PyObject* convert(const MatrixType& mat) { + static_assert(MatrixType::ColsAtCompileTime != 1 && MatrixType::RowsAtCompileTime != 1, + "Passed a Vector into a Matrix generator"); // Ensure that it is not a vector + + np::dtype dt = np::dtype::get_builtin(); + auto shape = py::make_tuple(mat.rows(), mat.cols()); + np::ndarray mOut = np::empty(shape, dt); + + for (Eigen::Index i = 0; i < mat.rows(); ++i) + for (Eigen::Index j = 0; j < mat.cols(); ++j) mOut[i][j] = mat(i, j); + + return py::incref(mOut.ptr()); + } +}; + +/* + * Main converters + */ + +/** Main converters + * Simple enum struct for simplification of common Eigen global typedefs + */ +enum struct Converters : short { + None = 0, /*!< No conversion */ + Matrix = 1, /*!< Convert all global typedefs matrices */ + Vector = 1 << 1, /*!< Convert all global typedefs vectors */ + RowVector = 1 << 2, /*!< Convert all global typedefs row vectors */ + Array = 1 << 3, /*!< Convert all global typedefs arrays */ + ColumnArray = 1 << 4, /*!< Convert all global typedefs column arrays */ + RowArray = 1 << 5, /*!< Convert all global typedefs row arrays */ + NoStandard = 1 << 6, /*! Convert some non standard yet usually used bindings (Vector6, Matrix6, Array6) */ + + NoRowMatrixConversion = Matrix | Vector, /*!< Convert all global typedefs matrices and column vectors */ + AllMatrixConversion = + Matrix | Vector | RowVector, /*!< Convert all global typedefs matrices and (column and row) vectors */ + NoRowArrayConversion = Array | ColumnArray, /*!< Convert all global typedefs arrays and column arrays */ + AllArrayConversion = + Array | ColumnArray | RowArray, /*!< Convert all global typedefs arrays and (column and row) vectors */ + All = AllMatrixConversion | AllArrayConversion | NoStandard /*!< Convert everything */ +}; + +inline short operator&(Converters lhs, Converters rhs) { return static_cast(lhs) & static_cast(rhs); } + +/** Global matrix conversion + * Generate the conversion for Eigen global matrices typedefs, depending of the type T (int, float, double, ...) + * \param isListConvertible if true, generate conversion from python list to Eigen Matrix + */ +template +void convertAllMatrix(bool isListConvertible = true) { + // python -> eigen + numpy_array_to_eigen_matrix >(); // Matrix2 + numpy_array_to_eigen_matrix >(); // MatrixX2 + numpy_array_to_eigen_matrix >(); // Matrix2X + numpy_array_to_eigen_matrix >(); // Matrix3 + numpy_array_to_eigen_matrix >(); // MatrixX3 + numpy_array_to_eigen_matrix >(); // Matrix3X + numpy_array_to_eigen_matrix >(); // Matrix4 + numpy_array_to_eigen_matrix >(); // MatrixX4 + numpy_array_to_eigen_matrix >(); // Matrix4X + numpy_array_to_eigen_matrix >(); // MatrixX + + // list -> eigen + if (isListConvertible) { + python_list_to_eigen_matrix >(); // Matrix2 + python_list_to_eigen_matrix >(); // MatrixX2 + python_list_to_eigen_matrix >(); // Matrix2X + python_list_to_eigen_matrix >(); // Matrix3 + python_list_to_eigen_matrix >(); // MatrixX3 + python_list_to_eigen_matrix >(); // Matrix3X + python_list_to_eigen_matrix >(); // Matrix4 + python_list_to_eigen_matrix >(); // MatrixX4 + python_list_to_eigen_matrix >(); // Matrix4X + python_list_to_eigen_matrix >(); // MatrixX + } + + // eigen -> python + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // Matrix2 + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // MatrixX2 + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // Matrix2X + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // Matrix3 + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // MatrixX3 + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // Matrix3X + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // Matrix4 + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // MatrixX4 + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // Matrix4X + py::to_python_converter< + Eigen::Matrix, + eigen_matrix_to_numpy_array > >(); // MatrixX +} + +/** Global column vector conversion + * Generate the conversion for Eigen global column vectors typedefs, depending of the type T (int, float, double, ...) + * \param isListConvertible if true, generate conversion from python list to Eigen Vector + */ +template +void convertAllVector(bool isListConvertible = true) { + // python -> eigen + numpy_array_to_eigen_vector >(); // Vector2 + numpy_array_to_eigen_vector >(); // Vector3 + numpy_array_to_eigen_vector >(); // Vector4 + numpy_array_to_eigen_vector >(); // VectorX + + // list -> eigen + if (isListConvertible) { + python_list_to_eigen_vector >(); // Vector2 + python_list_to_eigen_vector >(); // Vector3 + python_list_to_eigen_vector >(); // Vector4 + python_list_to_eigen_vector >(); // VectorX + } + + // eigen -> python + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // Vector2 + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // Vector3 + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // Vector4 + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // VectorX +} + +/** Global row vector conversion + * Generate the conversion for Eigen global row vectors typedefs, depending of the type T (int, float, double, ...) + * \param isListConvertible if true, generate conversion from python list to Eigen RowVector + */ +template +void convertAllRowVector(bool isListConvertible = true) { + // python -> eigen + numpy_array_to_eigen_vector >(); // RowVector2 + numpy_array_to_eigen_vector >(); // RowVector3 + numpy_array_to_eigen_vector >(); // RowVector4 + numpy_array_to_eigen_vector >(); // RowVectorX + + // list -> eigen + if (isListConvertible) { + python_list_to_eigen_vector >(); // RowVector2 + python_list_to_eigen_vector >(); // RowVector3 + python_list_to_eigen_vector >(); // RowVector4 + python_list_to_eigen_vector >(); // RowVectorX + } + + // eigen -> python + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // RowVector2 + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // RowVector3 + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // RowVector4 + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // RowVectorX +} + +/** Global array conversion + * Generate the conversion for Eigen global arrays typedefs, depending of the type T (int, float, double, ...) + * \param isListConvertible if true, generate conversion from python list to Eigen Array + */ +template +void convertAllArray(bool isListConvertible = true) { + // python -> eigen + numpy_array_to_eigen_matrix >(); // Array2 + numpy_array_to_eigen_matrix >(); // ArrayX2 + numpy_array_to_eigen_matrix >(); // Array2X + numpy_array_to_eigen_matrix >(); // Array3 + numpy_array_to_eigen_matrix >(); // ArrayX3 + numpy_array_to_eigen_matrix >(); // Array3X + numpy_array_to_eigen_matrix >(); // Array4 + numpy_array_to_eigen_matrix >(); // ArrayX4 + numpy_array_to_eigen_matrix >(); // Array4X + numpy_array_to_eigen_matrix >(); // ArrayX + + // list -> eigen + if (isListConvertible) { + python_list_to_eigen_matrix >(); // Array2 + python_list_to_eigen_matrix >(); // ArrayX2 + python_list_to_eigen_matrix >(); // Array2X + python_list_to_eigen_matrix >(); // Array3 + python_list_to_eigen_matrix >(); // ArrayX3 + python_list_to_eigen_matrix >(); // Array3X + python_list_to_eigen_matrix >(); // Array4 + python_list_to_eigen_matrix >(); // ArrayX4 + python_list_to_eigen_matrix >(); // Array4X + python_list_to_eigen_matrix >(); // ArrayX + } + + // eigen -> python + py::to_python_converter, eigen_matrix_to_numpy_array > >(); // Array2 + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // ArrayX2 + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // Array2X + py::to_python_converter, eigen_matrix_to_numpy_array > >(); // Array3 + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // ArrayX3 + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // Array3X + py::to_python_converter, eigen_matrix_to_numpy_array > >(); // Array4 + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // ArrayX4 + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // Array4X + py::to_python_converter< + Eigen::Array, + eigen_matrix_to_numpy_array > >(); // ArrayX +} + +/** Global column array conversion + * Generate the conversion for Eigen global column arrays typedefs, depending of the type T (int, float, double, ...) + * \param isListConvertible if true, generate conversion from python list to Eigen Array + */ +template +void convertAllColumnArray(bool isListConvertible = true) { + // python -> eigen + numpy_array_to_eigen_vector >(); // Vector2 + numpy_array_to_eigen_vector >(); // Vector3 + numpy_array_to_eigen_vector >(); // Vector4 + numpy_array_to_eigen_vector >(); // VectorX + + // list -> eigen + if (isListConvertible) { + python_list_to_eigen_vector >(); // Vector2 + python_list_to_eigen_vector >(); // Vector3 + python_list_to_eigen_vector >(); // Vector4 + python_list_to_eigen_vector >(); // VectorX + } + + // eigen -> python + py::to_python_converter, eigen_vector_to_numpy_array > >(); // Vector2 + py::to_python_converter, eigen_vector_to_numpy_array > >(); // Vector3 + py::to_python_converter, eigen_vector_to_numpy_array > >(); // Vector4 + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // VectorX +} + +/** Global row array conversion + * Generate the conversion for Eigen global row arrays typedefs, depending of the type T (int, float, double, ...) + * \param isListConvertible if true, generate conversion from python list to Eigen Array + */ +template +void convertAllRowArray(bool isListConvertible = true) { + // python -> eigen + numpy_array_to_eigen_vector >(); // RowVector2 + numpy_array_to_eigen_vector >(); // RowVector3 + numpy_array_to_eigen_vector >(); // RowVector4 + numpy_array_to_eigen_vector >(); // RowVectorX + + // list -> eigen + if (isListConvertible) { + python_list_to_eigen_vector >(); // RowVector2 + python_list_to_eigen_vector >(); // RowVector3 + python_list_to_eigen_vector >(); // RowVector4 + python_list_to_eigen_vector >(); // RowVectorX + } + + // eigen -> python + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // RowVector2 + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // RowVector3 + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // RowVector4 + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // RowVectorX +} + +/** No standard conversion + * Generate the conversion for no standard Eigen matrix and vector, depending of the type T (int, float, double, ...) + * Those are Matrix6, Vector6, RowVector6 and Array6 + * \param isListConvertible if true, generate conversion from python list to Eigen Matrix + */ +template +void convertNoStandard(bool isListConvertible = true) { + // python -> eigen + numpy_array_to_eigen_matrix >(); // Matrix6 + numpy_array_to_eigen_vector >(); // Vector6 + numpy_array_to_eigen_vector >(); // RowVector6 + numpy_array_to_eigen_matrix >(); // Array6 + numpy_array_to_eigen_vector >(); // ColumnArray6 + numpy_array_to_eigen_vector >(); // RowArray6 + + // list -> eigen + if (isListConvertible) { + python_list_to_eigen_matrix >(); // Matrix6 + python_list_to_eigen_vector >(); // Vector6 + python_list_to_eigen_vector >(); // RowVector6 + python_list_to_eigen_matrix >(); // Array6 + python_list_to_eigen_vector >(); // ColumnArray6 + python_list_to_eigen_vector >(); // RowArray6 + } + + // eigen -> python + py::to_python_converter, + eigen_matrix_to_numpy_array > >(); // Matrix6 + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // Vector6 + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // RowVector6 + py::to_python_converter, eigen_matrix_to_numpy_array > >(); // Array6 + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // ColumnArray6 + py::to_python_converter, + eigen_vector_to_numpy_array > >(); // RowArray6 +} + +/** Eigen Matrix and Array conversion + * Generate the conversion for Eigen MatrixType (MatrixXd, Array2Xf, ...) + * \param isListConvertible if true, generate conversion from python list to Eigen MatrixType + */ +template +void convertMatrix(bool isListConvertible = true) { + // python -> eigen + numpy_array_to_eigen_matrix(); + + // list -> eigen + if (isListConvertible) python_list_to_eigen_matrix(); + + // eigen -> python + py::to_python_converter >(); // Vector2 +} + +/** Eigen Vector and Array conversion + * Generate the conversion for Eigen VectorType (VectorXd, RowVector4f, ArrayXf, ...) + * \param isListConvertible if true, generate conversion from python list to Eigen VectorType + */ +template +void convertVector(bool isListConvertible = true) { + // python -> eigen + numpy_array_to_eigen_vector(); + + // list -> eigen + if (isListConvertible) python_list_to_eigen_vector(); + + // eigen -> python + py::to_python_converter >(); +} + +/** Helper functions to generate the converters + * Generate all desired converters automatically + * \param converters A bitFlag that tells the function what to convert. \see Converters + * \param isListConvertible if true, generate conversion from python list to Eigen RowVector + */ +template +void convert(Converters converters, bool isListConvertible = true) { + if (converters & Converters::Matrix) convertAllMatrix(isListConvertible); + if (converters & Converters::Vector) convertAllVector(isListConvertible); + if (converters & Converters::RowVector) convertAllRowVector(isListConvertible); + if (converters & Converters::Array) convertAllArray(isListConvertible); + if (converters & Converters::ColumnArray) convertAllColumnArray(isListConvertible); + if (converters & Converters::RowArray) convertAllRowArray(isListConvertible); + if (converters & Converters::NoStandard) convertNoStandard(isListConvertible); +} + +} // namespace converters \ No newline at end of file From 4db0c181a4a64582a9983b157864ddfba9150d70 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Wed, 19 Jul 2023 21:40:03 +0200 Subject: [PATCH 02/64] bugfixes after cleanup --- .../include/lanelet2_map_learning/Forward.h | 6 - .../lanelet2_map_learning/internal/Graph.h | 20 +- lanelet2_map_learning/src/MapGraphBuilder.cpp | 15 +- .../test/test_lanelet_or_area_path.cpp | 213 -------- .../test/{test_routing_map.h => test_map.h} | 33 +- ...ainer.cpp => test_map_graph_container.cpp} | 42 +- ...ization.cpp => test_map_visualization.cpp} | 6 +- lanelet2_map_learning/test/test_relations.cpp | 15 +- lanelet2_map_learning/test/test_route.cpp | 383 ------------- lanelet2_map_learning/test/test_routing.cpp | 516 ------------------ lanelet2_routing/src/RoutingGraphBuilder.cpp | 3 + 11 files changed, 35 insertions(+), 1217 deletions(-) delete mode 100644 lanelet2_map_learning/test/test_lanelet_or_area_path.cpp rename lanelet2_map_learning/test/{test_routing_map.h => test_map.h} (94%) rename lanelet2_map_learning/test/{test_routing_graph_container.cpp => test_map_graph_container.cpp} (63%) rename lanelet2_map_learning/test/{test_routing_visualization.cpp => test_map_visualization.cpp} (97%) delete mode 100644 lanelet2_map_learning/test/test_route.cpp delete mode 100644 lanelet2_map_learning/test/test_routing.cpp diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h index 1c2a239b..93e55f61 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h @@ -29,15 +29,9 @@ using MapGraphConstPtr = std::shared_ptr; class MapGraphContainer; using MapGraphContainerUPtr = std::unique_ptr; -class Route; struct LaneletRelation; using LaneletRelations = std::vector; -class LaneletPath; -using LaneletPaths = std::vector; -class LaneletOrAreaPath; -using LaneletOrAreaPaths = std::vector; - //! This enum expresses the types of relations lanelet2 distiguishes internally. Between two lanelets a and b (in this //! order), exactly one of these relation exists. //! diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h b/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h index 8c749eb1..a783cbaa 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h @@ -23,15 +23,6 @@ struct VertexInfo { ConstLaneletOrArea laneletOrArea; }; -/** @brief Internal information of a vertex in the route graph */ -struct MapVertexInfo { - const ConstLanelet& get() const { return lanelet; } - - ConstLanelet lanelet; - LaneId laneId{}; - ConstLaneletOrAreas conflictingInMap; -}; - /** @brief Internal information of an edge in the graph */ struct EdgeInfo { RelationType relation; ///< Relation between the two lanelets. E.g. SUCCESSOR or CONFLICTING. @@ -39,7 +30,6 @@ struct EdgeInfo { /// General graph type definitions using GraphType = boost::adjacency_list; -using RouteGraphType = boost::adjacency_list; using GraphTraits = boost::graph_traits; template @@ -53,7 +43,6 @@ template using FilteredGraphTraits = boost::graph_traits>; using FilteredMapGraph = FilteredGraphT; -using FilteredRouteGraph = FilteredGraphT; /** @brief An internal edge filter to get a filtered view on the graph. */ template @@ -86,12 +75,12 @@ struct EdgeFilter { using LaneletOrAreaToVertex = std::unordered_map; using FilteredGraphDesc = std::pair; -/// @brief Manages the actual graph and provieds different views on the edges (lazily computed) +/// @brief Manages the actual graph and provides different views on the edges (lazily computed) template class Graph { public: using FilteredGraph = FilteredGraphT; - using CostFilter = EdgeFilter; + using Filter = EdgeFilter; using Vertex = typename boost::graph_traits::vertex_descriptor; using Edge = typename boost::graph_traits::edge_descriptor; @@ -190,7 +179,7 @@ class Graph { private: FilteredGraph getFilteredGraph(RelationType relations) const { - return FilteredGraph(graph_, CostFilter(graph_, relations)); + return FilteredGraph(graph_, Filter(graph_, relations)); } BaseGraphT graph_; //!< The actual graph object LaneletOrAreaToVertex laneletOrAreaToVertex_; //!< Mapping of lanelets/areas to vertices of the graph @@ -199,9 +188,6 @@ class Graph { class MapGraphGraph : public Graph { using Graph::Graph; }; -class RouteGraph : public Graph { - using Graph::Graph; -}; } // namespace internal } // namespace map_learning diff --git a/lanelet2_map_learning/src/MapGraphBuilder.cpp b/lanelet2_map_learning/src/MapGraphBuilder.cpp index cd8a51fd..e7f99259 100644 --- a/lanelet2_map_learning/src/MapGraphBuilder.cpp +++ b/lanelet2_map_learning/src/MapGraphBuilder.cpp @@ -184,6 +184,10 @@ void MapGraphBuilder::addFollowingEdges(const ConstLanelet& ll) { merging.push_back(it.second); } }); + RelationType relation = RelationType::Successor; + for (auto& followingIt : following) { + assignEdge(ll, followingIt, relation); + } } void MapGraphBuilder::addSidewayEdge(LaneChangeLaneletsCollector& laneChangeLanelets, const ConstLanelet& ll, @@ -333,22 +337,21 @@ void MapGraphBuilder::assignLaneChange(ConstLanelets froms, ConstLanelets tos, c assert(relation == RelationType::Left || relation == RelationType::Right); assert(froms.size() == tos.size()); for (; !froms.empty(); froms.erase(froms.begin()), tos.erase(tos.begin())) { - auto adjacent = relation == RelationType::Left ? RelationType::AdjacentLeft : RelationType::AdjacentRight; - graph_->addEdge(froms.front(), tos.front(), EdgeInfo{adjacent}); + graph_->addEdge(froms.front(), tos.front(), EdgeInfo{relation}); } } void MapGraphBuilder::assignEdge(const ConstLaneletOrArea& from, const ConstLaneletOrArea& to, const RelationType& relation) { - EdgeInfo edgeInfo{}; - edgeInfo.relation = relation; + EdgeInfo edgeInfo{relation}; if (relation == RelationType::Successor || relation == RelationType::Area || relation == RelationType::Left || relation == RelationType::Right || relation == RelationType::AdjacentLeft || relation == RelationType::AdjacentRight || relation == RelationType::Conflicting) { + graph_->addEdge(from, to, edgeInfo); + } else { assert(false && "Trying to add edge with wrong relation type to graph."); // NOLINT - return; } - graph_->addEdge(from, to, edgeInfo); + return; } } // namespace internal diff --git a/lanelet2_map_learning/test/test_lanelet_or_area_path.cpp b/lanelet2_map_learning/test/test_lanelet_or_area_path.cpp deleted file mode 100644 index da7c4728..00000000 --- a/lanelet2_map_learning/test/test_lanelet_or_area_path.cpp +++ /dev/null @@ -1,213 +0,0 @@ -#include -#include - -#include - -#include "lanelet2_map_learning/LaneletPath.h" - -using namespace lanelet; -using namespace lanelet::map_learning; - -/* - * 6 l8 7 l7 8 l6 9 l5 10 - * X<------X<------X<------X<------X - * ^ ^ ^ ^ - * |l9 |l10 |l11 |l12 - * | a1 | a2 | ll1 | ll2 - * | | ll3 | ll4 | - * X------>X------>X------>X------>X - * 1 l1 2 l2 3 l3 4 l4 5 - * | a51 | WE COME - * | V IN PEACE - * X<------X - * 12 11 - */ - -class LaneletOrAreaTest : public ::testing::Test { - private: - void SetUp() override { - Id id(1000); - p1 = Point3d(++id, 0, 0, 0); - p2 = Point3d(++id, 1, 0, 0); - p3 = Point3d(++id, 2, 0, 0); - p4 = Point3d(++id, 3, 0, 0); - p5 = Point3d(++id, 4, 0, 0); - p6 = Point3d(++id, 0, 1, 0); - p7 = Point3d(++id, 1, 1, 0); - p8 = Point3d(++id, 2, 1, 0); - p9 = Point3d(++id, 3, 1, 0); - p10 = Point3d(++id, 4, 1, 0); - p11 = Point3d(++id, 4, -1, 0); - p12 = Point3d(++id, 3, -1, 0); - - ls1 = LineString3d(++id, {p1, p2}); - ls2 = LineString3d(++id, {p2, p3}); - ls3 = LineString3d(++id, {p3, p4}); - ls4 = LineString3d(++id, {p4, p5}); - ls5 = LineString3d(++id, {p10, p9}); - ls6 = LineString3d(++id, {p9, p8}); - ls7 = LineString3d(++id, {p8, p7}); - ls8 = LineString3d(++id, {p7, p6}); - ls9 = LineString3d(++id, {p1, p6}); - ls10 = LineString3d(++id, {p2, p7}); - ls11 = LineString3d(++id, {p3, p8}); - ls12 = LineString3d(++id, {p4, p9}); - ls13 = LineString3d(++id, {p5, p11, p12, p4}); - - l1 = Lanelet(++id, ls6.invert(), ls3); - l2 = Lanelet(++id, ls5.invert(), ls4); - l3 = Lanelet(++id, ls10, ls11); - l4 = Lanelet(++id, ls11, ls12); - - a1 = Area(++id, {ls9, ls8.invert(), ls10.invert(), ls1.invert()}); - a2 = Area(++id, {ls10, ls7.invert(), ls11.invert(), ls2.invert()}); - a51 = Area(++id, {ls4, ls13}); - - areaPath = LaneletOrAreaPath({ConstLaneletOrArea(a1), ConstLaneletOrArea(a2)}); - laneletPath = LaneletOrAreaPath({ConstLaneletOrArea(l1), ConstLaneletOrArea(l2)}); - bothPath = LaneletOrAreaPath({ConstLaneletOrArea(a2), ConstLaneletOrArea(l1)}); - invAreaPath = LaneletOrAreaPath({ConstLaneletOrArea(l1.invert()), ConstLaneletOrArea(a2)}); - invLLPath = LaneletOrAreaPath({ConstLaneletOrArea(l2.invert()), ConstLaneletOrArea(l1.invert())}); - longPath = LaneletOrAreaPath( - {ConstLaneletOrArea(a1), ConstLaneletOrArea(a2), ConstLaneletOrArea(l1), ConstLaneletOrArea(l2)}); - longInvPath = LaneletOrAreaPath({ConstLaneletOrArea(l2.invert()), ConstLaneletOrArea(l1.invert()), - ConstLaneletOrArea(a2), ConstLaneletOrArea(a1)}); - sidePath = LaneletOrAreaPath({ConstLaneletOrArea(a1), ConstLaneletOrArea(l3), ConstLaneletOrArea(l4)}); - sideInvPath = LaneletOrAreaPath({ConstLaneletOrArea(l3), ConstLaneletOrArea(a1)}); - cornerPath = LaneletOrAreaPath({ConstLaneletOrArea(l1), ConstLaneletOrArea(l2), ConstLaneletOrArea(a51)}); - cornerPathInv = - LaneletOrAreaPath({ConstLaneletOrArea(a51), ConstLaneletOrArea(l2.invert()), ConstLaneletOrArea(l1.invert())}); - } - - public: - Point3d p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11, p12; - LineString3d ls1, ls2, ls3, ls4, ls5, ls6, ls7, ls8, ls9, ls10, ls11, ls12, ls13; - Lanelet l1, l2, l3, l4; - Area a1, a2, a51; - LaneletOrAreaPath areaPath, laneletPath, bothPath, invAreaPath, invLLPath, longPath, longInvPath, sidePath, - sideInvPath, cornerPath, cornerPathInv; -}; -namespace { -void checkIdentical(const BasicPolygon3d& lhs, const BasicPolygon3d& rhs) { - auto to2D = [](const BasicPolygon3d& p3d) { - BasicPolygon2d p2d(p3d.size()); - for (size_t i = 0; i < p3d.size(); ++i) { - p2d[i] = p3d.at(i).head<2>(); - } - return p2d; - }; - BasicPolygon2d lhs2d = to2D(lhs); - BasicPolygon2d rhs2d = to2D(rhs); - // equals is buggy in older boost versions -#if BOOST_VERSION > 106500 - EXPECT_TRUE(boost::geometry::equals(lhs2d, rhs2d)); -#endif -} - -void checkEvenlySpaced(const BasicPolygon3d& poly, const double dist = 1.) { - for (size_t i = 0; i + 1 < poly.size(); ++i) { - EXPECT_DOUBLE_EQ(boost::geometry::distance(poly.at(i), poly.at(i + 1)), dist); - } - EXPECT_DOUBLE_EQ(boost::geometry::distance(poly.back(), poly.front()), dist); -} - -} // namespace - -TEST_F(LaneletOrAreaTest, enclosingPolygonAreas) { // NOLINT - BasicPolygons2d intersect; - BasicPolygon3d joined = getEnclosingPolygon3d(areaPath); - BasicPolygon3d expected{p1, p6, p8, p3}; - checkIdentical(joined, expected); - checkEvenlySpaced(joined); - EXPECT_EQ(joined.size(), 6ul); -} - -TEST_F(LaneletOrAreaTest, enclosingPolygonLanelets) { // NOLINT - BasicPolygons2d intersect; - BasicPolygon3d joined = getEnclosingPolygon3d(laneletPath); - BasicPolygon3d expected{p3, p8, p10, p5}; - checkIdentical(joined, expected); - checkEvenlySpaced(joined); - EXPECT_EQ(joined.size(), 6ul); -} - -TEST_F(LaneletOrAreaTest, enclosingPolygonLaneletsInverted) { // NOLINT - BasicPolygons2d intersect; - BasicPolygon3d joined = getEnclosingPolygon3d(invLLPath); - BasicPolygon3d expected{p3, p8, p10, p5}; - checkIdentical(joined, expected); - checkEvenlySpaced(joined); - EXPECT_EQ(joined.size(), 6ul); -} - -TEST_F(LaneletOrAreaTest, enclosingPolygonMixed) { // NOLINT - BasicPolygons2d intersect; - BasicPolygon3d joined = getEnclosingPolygon3d(bothPath); - BasicPolygon3d expected{p2, p7, p9, p4}; - checkIdentical(joined, expected); - checkEvenlySpaced(joined); - EXPECT_EQ(joined.size(), 6ul); -} - -TEST_F(LaneletOrAreaTest, enclosingPolygonAreaInverted) { // NOLINT - BasicPolygons2d intersect; - BasicPolygon3d joined = getEnclosingPolygon3d(invAreaPath); - BasicPolygon3d expected{p2, p7, p9, p4}; - checkIdentical(joined, expected); - checkEvenlySpaced(joined); - EXPECT_EQ(joined.size(), 6ul); -} - -TEST_F(LaneletOrAreaTest, enclosingPolygonLong) { // NOLINT - BasicPolygons2d intersect; - BasicPolygon3d joined = getEnclosingPolygon3d(longPath); - BasicPolygon3d expected{p1, p6, p10, p5}; - checkIdentical(joined, expected); - checkEvenlySpaced(joined); - EXPECT_EQ(joined.size(), 10ul); -} - -TEST_F(LaneletOrAreaTest, enclosingPolygonLongInverted) { // NOLINT - BasicPolygons2d intersect; - BasicPolygon3d joined = getEnclosingPolygon3d(longInvPath); - BasicPolygon3d expected{p1, p6, p10, p5}; - checkIdentical(joined, expected); - checkEvenlySpaced(joined); - EXPECT_EQ(joined.size(), 10ul); -} - -TEST_F(LaneletOrAreaTest, enclosingPolygonSideways) { // NOLINT - BasicPolygons2d intersect; - BasicPolygon3d joined = getEnclosingPolygon3d(sidePath); - BasicPolygon3d expected{p1, p6, p9, p4}; - checkIdentical(joined, expected); - checkEvenlySpaced(joined); - EXPECT_EQ(joined.size(), 8ul); -} - -TEST_F(LaneletOrAreaTest, enclosingPolygonSidewaysInvertrd) { // NOLINT - BasicPolygons2d intersect; - BasicPolygon3d joined = getEnclosingPolygon3d(sideInvPath); - BasicPolygon3d expected{p1, p6, p8, p3}; - checkIdentical(joined, expected); - checkEvenlySpaced(joined); - EXPECT_EQ(joined.size(), 6ul); -} - -TEST_F(LaneletOrAreaTest, enclosingPolygonCorner) { // NOLINT - BasicPolygons2d intersect; - BasicPolygon3d joined = getEnclosingPolygon3d(cornerPath); - BasicPolygon3d expected{p8, p10, p11, p12, p4, p3}; - checkIdentical(joined, expected); - checkEvenlySpaced(joined); - EXPECT_EQ(joined.size(), 8ul); -} - -TEST_F(LaneletOrAreaTest, enclosingPolygonCornerInv) { // NOLINT - BasicPolygons2d intersect; - BasicPolygon3d joined = getEnclosingPolygon3d(cornerPathInv); - BasicPolygon3d expected{p8, p10, p11, p12, p4, p3}; - checkIdentical(joined, expected); - checkEvenlySpaced(joined); - EXPECT_EQ(joined.size(), 8ul); -} diff --git a/lanelet2_map_learning/test/test_routing_map.h b/lanelet2_map_learning/test/test_map.h similarity index 94% rename from lanelet2_map_learning/test/test_routing_map.h rename to lanelet2_map_learning/test/test_map.h index 469fe71f..faded50c 100644 --- a/lanelet2_map_learning/test/test_routing_map.h +++ b/lanelet2_map_learning/test/test_map.h @@ -13,32 +13,27 @@ /// The coordinates and relations for this test can be found in "LaneletTestMap.xml" which can be viewed in /// https://www.draw.io namespace lanelet { -namespace routing { +namespace map_learning { namespace tests { -inline MapGraphPtr setUpGermanVehicleGraph(LaneletMap& map, double laneChangeCost = 2., - double participantHeight = 2., double minLaneChangeLength = 0.) { +inline MapGraphPtr setUpGermanVehicleGraph(LaneletMap& map, double participantHeight = 2.) { traffic_rules::TrafficRulesPtr trafficRules{traffic_rules::TrafficRulesFactory::create( Locations::Germany, Participants::Vehicle, traffic_rules::TrafficRules::Configuration())}; - RoutingCostPtrs costPtrs{std::make_shared(laneChangeCost, minLaneChangeLength), - std::make_shared(laneChangeCost)}; MapGraph::Configuration configuration; configuration.insert(std::make_pair(MapGraph::ParticipantHeight, participantHeight)); - return MapGraph::build(map, *trafficRules, costPtrs, configuration); + return MapGraph::build(map, *trafficRules, configuration); } -inline MapGraphPtr setUpGermanPedestrianGraph(LaneletMap& map, double laneChangeCost = 2.) { +inline MapGraphPtr setUpGermanPedestrianGraph(LaneletMap& map) { traffic_rules::TrafficRulesPtr trafficRules{ traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Pedestrian)}; - RoutingCostPtrs costPtrs{std::make_shared(laneChangeCost)}; - return MapGraph::build(map, *trafficRules, costPtrs); + return MapGraph::build(map, *trafficRules); } -inline MapGraphPtr setUpGermanBicycleGraph(LaneletMap& map, double laneChangeCost = 2.) { +inline MapGraphPtr setUpGermanBicycleGraph(LaneletMap& map) { traffic_rules::TrafficRulesPtr trafficRules{ traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Bicycle)}; - RoutingCostPtrs costPtrs{std::make_shared(laneChangeCost)}; - return MapGraph::build(map, *trafficRules, costPtrs); + return MapGraph::build(map, *trafficRules); } class MapGraphTestData { @@ -50,9 +45,9 @@ class MapGraphTestData { initAreas(); laneletMap = std::make_shared(lanelets, areas, std::unordered_map(), std::unordered_map(), lines, points); - vehicleGraph = setUpGermanVehicleGraph(*laneletMap, laneChangeCost); - pedestrianGraph = setUpGermanPedestrianGraph(*laneletMap, laneChangeCost); - bicycleGraph = setUpGermanBicycleGraph(*laneletMap, laneChangeCost); + vehicleGraph = setUpGermanVehicleGraph(*laneletMap); + pedestrianGraph = setUpGermanPedestrianGraph(*laneletMap); + bicycleGraph = setUpGermanBicycleGraph(*laneletMap); } void addPoint(double x, double y, double z) { @@ -94,7 +89,6 @@ class MapGraphTestData { std::unordered_map points; std::unordered_map lines; std::unordered_map areas; - const double laneChangeCost{2.}; MapGraphPtr vehicleGraph, pedestrianGraph, bicycleGraph; LaneletMapPtr laneletMap; @@ -477,7 +471,7 @@ class MapGraphTestData { } }; -namespace { // NOLINT +namespace { // NOLINT static MapGraphTestData testData; // NOLINT } // namespace @@ -496,7 +490,6 @@ class GermanVehicleGraph : public MapGraphTest { public: MapGraphConstPtr graph{testData.vehicleGraph}; - uint8_t numCostModules{2}; }; class GermanPedestrianGraph : public MapGraphTest { @@ -505,7 +498,6 @@ class GermanPedestrianGraph : public MapGraphTest { public: MapGraphConstPtr graph{testData.pedestrianGraph}; - uint8_t numCostModules{2}; }; class GermanBicycleGraph : public MapGraphTest { @@ -514,7 +506,6 @@ class GermanBicycleGraph : public MapGraphTest { public: MapGraphConstPtr graph{testData.bicycleGraph}; - uint8_t numCostModules{2}; }; template @@ -529,5 +520,5 @@ using AllGraphs = testing::TypespassableSubmap()->laneletLayer.find(from), graph->passableSubmap()->laneletLayer.end()); - ConstLanelet fromLanelet{*graph->passableSubmap()->laneletLayer.find(from)}; - ASSERT_NE(graph->passableSubmap()->laneletLayer.find(to), graph->passableSubmap()->laneletLayer.end()); - ConstLanelet toLanelet{*graph->passableSubmap()->laneletLayer.find(to)}; - Optional tempRoute = graph->getRoute(fromLanelet, toLanelet, routingCostId); - ASSERT_TRUE(!!tempRoute); - route = std::make_unique(std::move(*tempRoute)); - } - - std::unique_ptr route; -}; - TEST_F(MapGraphContainerTest, ConflictingInGraph) { // NOLINT ConstLanelet pedestrianLanelet{*laneletMap->laneletLayer.find(2031)}; ConstLanelets conflictingVehicle{container->conflictingInGraph(pedestrianLanelet, 0)}; @@ -58,27 +43,6 @@ TEST_F(MapGraphContainerTest, ConflictingInGraphs) { // NOLINT EXPECT_EQ(conflictingPedestrian.size(), 0ul); } -TEST_F(RouteMapGraphContainerTest, ConflictingOfRouteInGraph) { // NOLINT - getAndCheckRoute(container->MapGraphs()[0], 2017, 2024); - ConstLanelets conflictingVehicle{container->conflictingOfRouteInGraph(route.get(), 0)}; - EXPECT_EQ(conflictingVehicle.size(), 2ul); - - ConstLanelets conflictingPedestrian{container->conflictingOfRouteInGraph(route.get(), 1)}; - EXPECT_EQ(conflictingPedestrian.size(), 1ul); -} - -TEST_F(RouteMapGraphContainerTest, ConflictingOfRouteInGraphs) { // NOLINT - getAndCheckRoute(container->MapGraphs()[0], 2017, 2024); - MapGraphContainer::ConflictingInGraphs result{container->conflictingOfRouteInGraphs(route.get(), 0)}; - ASSERT_EQ(result.size(), 2ul); - - ConstLanelets conflictingVehicle{result[0].second}; - EXPECT_EQ(conflictingVehicle.size(), 2ul); - - ConstLanelets conflictingPedestrian{result[1].second}; - EXPECT_EQ(conflictingPedestrian.size(), 1ul); -} - TEST_F(MapGraphContainerTest, ConflictingInGraph3dFits) { // NOLINT ConstLanelet bridgeLanelet{*laneletMap->laneletLayer.find(2032)}; ConstLanelets conflictingVehicle{container->conflictingInGraph(bridgeLanelet, 0, 2.)}; diff --git a/lanelet2_map_learning/test/test_routing_visualization.cpp b/lanelet2_map_learning/test/test_map_visualization.cpp similarity index 97% rename from lanelet2_map_learning/test/test_routing_visualization.cpp rename to lanelet2_map_learning/test/test_map_visualization.cpp index a58f51b1..c3932853 100644 --- a/lanelet2_map_learning/test/test_routing_visualization.cpp +++ b/lanelet2_map_learning/test/test_map_visualization.cpp @@ -4,11 +4,11 @@ #include "lanelet2_map_learning/Exceptions.h" #include "lanelet2_map_learning/MapGraph.h" -#include "test_routing_map.h" +#include "test_map.h" using namespace lanelet; -using namespace lanelet::routing; -using namespace lanelet::routing::tests; +using namespace lanelet::map_learning; +using namespace lanelet::map_learning::tests; namespace fs = boost::filesystem; class Tempfile { diff --git a/lanelet2_map_learning/test/test_relations.cpp b/lanelet2_map_learning/test/test_relations.cpp index 9ed52fae..ba7e899a 100644 --- a/lanelet2_map_learning/test/test_relations.cpp +++ b/lanelet2_map_learning/test/test_relations.cpp @@ -3,10 +3,10 @@ #include #include "lanelet2_map_learning/MapGraph.h" -#include "test_routing_map.h" +#include "test_map.h" using namespace lanelet; -using namespace lanelet::routing::tests; +using namespace lanelet::map_learning::tests; TEST_F(GermanVehicleGraph, LaneChangesLeft) { // NOLINT // Multiple1 @@ -523,14 +523,3 @@ TEST_F(GermanVehicleGraph, conflictingDiverging) { // NOLINT ASSERT_EQ(result.size(), 1ul); EXPECT_EQ(result.front(), lanelets.at(2009)); } - -TEST_F(MapGraphTest, routingCostOnLaneChange) { // NOLINT - // tests that all lane changes are not possible when the space for a lane change is too small according to the routing - // cost object - auto graph = setUpGermanVehicleGraph(*testData.laneletMap, 2, 2, 3); - auto llt = [&](auto id) -> ConstLanelet { return lanelets.at(id); }; - EXPECT_EQ(graph->left(lanelets.at(2012)), llt(2011)); - EXPECT_EQ(graph->adjacentLeft(lanelets.at(2015)), llt(2014)); - EXPECT_EQ(graph->right(lanelets.at(2011)), llt(2012)); - EXPECT_EQ(graph->adjacentRight(lanelets.at(2014)), llt(2015)); -} diff --git a/lanelet2_map_learning/test/test_route.cpp b/lanelet2_map_learning/test/test_route.cpp deleted file mode 100644 index 40314891..00000000 --- a/lanelet2_map_learning/test/test_route.cpp +++ /dev/null @@ -1,383 +0,0 @@ -#include - -#include "lanelet2_map_learning/Route.h" -#include "lanelet2_map_learning/MapGraph.h" -#include "test_routing_map.h" - -using namespace lanelet; -using namespace lanelet::routing; -using namespace lanelet::routing::tests; - -template -class TestRoute : public GermanVehicleGraph { - public: - explicit TestRoute(uint16_t routingCostId = 0, bool withLaneChanges = true) { - Ids viaIds{ViaIds...}; - start = lanelets.at(From); - end = lanelets.at(To); - via = utils::transform(viaIds, [&](auto id) -> ConstLanelet { return lanelets.at(id); }); - auto optRoute = via.empty() ? graph->getRoute(start, end, routingCostId, withLaneChanges) - : graph->getRouteVia(start, via, end, routingCostId, withLaneChanges); - EXPECT_TRUE(!!optRoute); - route = std::move(*optRoute); - } - ConstLanelet start; - ConstLanelet end; - ConstLanelets via; - Route route; - // these would better be defined as static constexpr, but c++14 doesnt support it well - const Id From{FromId}; // NOLINT - const Id To{ToId}; // NOLINT -}; - -class Route1 : public TestRoute<2001, 2014> {}; -class Route1NoLc : public TestRoute<2001, 2014> { - public: - Route1NoLc() : TestRoute(0, false) {} -}; -class Route2 : public TestRoute<2001, 2004> {}; -class Route3 : public TestRoute<2003, 2002> {}; -class Route4 : public TestRoute<2003, 2013> {}; -class Route5 : public TestRoute<2066, 2064> {}; -class RouteMaxHoseLeftRight : public TestRoute<2017, 2024> {}; -class RouteMaxHoseRightLeft : public TestRoute<2023, 2016> {}; -class RouteMaxHoseLeftRightDashedSolid : public TestRoute<2017, 2025> {}; -class RouteMaxHoseLeftRightDashedSolidFurther : public TestRoute<2017, 2027> {}; -class RouteSolidDashed : public TestRoute<2024, 2026> {}; -class RouteSolidDashedWithAdjacent : public TestRoute<2024, 2029> {}; -class RouteSplittedDiverging : public TestRoute<2029, 2038> {}; -class RouteSplittedDivergingAndMerging : public TestRoute<2041, 2049> {}; -class RouteViaSimple : public TestRoute<2003, 2015, 2009> {}; -class RouteMissingLanelet : public TestRoute<2003, 2015> {}; -class RouteInCircle : public TestRoute<2029, 2068> {}; -class RouteCircular : public TestRoute<2037, 2037, 2065> {}; -class RouteCircularNoLc : public TestRoute<2037, 2037, 2065> { - public: - RouteCircularNoLc() : TestRoute(0, false) {} -}; - -template -class AllRoutesTest : public T {}; - -using AllRoutes = - testing::Types; - -TYPED_TEST_SUITE(AllRoutesTest, AllRoutes); - -TYPED_TEST(AllRoutesTest, CheckValidity) { EXPECT_NO_THROW(this->route.checkValidity()); } // NOLINT - -TYPED_TEST(AllRoutesTest, ShortestMapInDebugMap) { - const auto map{this->route.getDebugLaneletMap()}; - for (const auto& el : this->route.shortestPath()) { - ASSERT_NE(map->pointLayer.find(el.id()), map->pointLayer.end()); - const Point3d point{*map->pointLayer.find(el.id())}; - EXPECT_TRUE(*point.attribute("shortest_path").asBool()); - } -} - -TYPED_TEST(AllRoutesTest, DebugMapCompleteness) { - const LaneletMapPtr debugMap{this->route.getDebugLaneletMap()}; - const LaneletSubmapConstPtr laneletMap{this->route.laneletSubmap()}; - - for (const auto& it : laneletMap->laneletLayer) { - ASSERT_NE(debugMap->pointLayer.find(it.id()), debugMap->pointLayer.end()); - Point3d point{*debugMap->pointLayer.find(it.id())}; - EXPECT_NO_THROW(point.attribute("id")); // NOLINT - EXPECT_NO_THROW(point.attribute("lane_id")); // NOLINT - } - - for (const auto& it : debugMap->lineStringLayer) { - EXPECT_NO_THROW(it.attribute("relation_1")); // NOLINT - } -} - -TEST_F(Route1, CreateRoute1) { // NOLINT - EXPECT_EQ(route.size(), 15ul); // NOLINT - EXPECT_GT(route.length2d(), 10.); - EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); // NOLINT - EXPECT_EQ(route.remainingShortestPath(start), route.shortestPath()); // NOLINT - EXPECT_TRUE(route.remainingShortestPath(lanelets.at(2003)).empty()); // NOLINT - EXPECT_NE(route.getDebugLaneletMap(), nullptr); - EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size()); // NOLINT - EXPECT_NE(route.laneletSubmap(), nullptr); - EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size()); // NOLINT -} - -bool hasRelation(const LaneletRelations& relations, const ConstLanelet& llt, RelationType relationType) { - LaneletRelation rel{llt, relationType}; - return utils::anyOf(relations, [&rel](auto& relation) { return rel == relation; }); -} - -template -bool hasLanelet(const ContainerT& llts, const ConstLanelet& llt) { - return utils::anyOf(llts, [&llt](auto& curr) { return curr == llt; }); -} - -TEST_F(Route1, Relations) { // NOLINT - LaneletRelations previous{route.previousRelations(lanelets.at(2007))}; - EXPECT_EQ(previous.size(), 2ul); // NOLINT - EXPECT_TRUE(hasRelation(previous, ConstLanelet(lanelets.at(2005)), RelationType::Successor)); - EXPECT_TRUE(hasRelation(previous, ConstLanelet(lanelets.at(2006)), RelationType::Successor)); - - LaneletRelations following{route.followingRelations(lanelets.at(2007))}; - EXPECT_EQ(following.size(), 2ul); // NOLINT - EXPECT_TRUE(hasRelation(following, ConstLanelet(lanelets.at(2008)), RelationType::Successor)); - EXPECT_TRUE(hasRelation(following, ConstLanelet(lanelets.at(2009)), RelationType::Successor)); -} - -TEST_F(Route1, Conflicting) { // NOLINT - auto conflictingInMap{route.conflictingInMap(lanelets.at(2009))}; - EXPECT_EQ(conflictingInMap.size(), 1ul); // NOLINT - EXPECT_TRUE(hasLanelet(conflictingInMap, lanelets.at(2008))); - - ConstLanelets conflictingInRoute{route.conflictingInRoute(lanelets.at(2009))}; - EXPECT_EQ(conflictingInRoute.size(), 1ul); // NOLINT - EXPECT_TRUE(hasLanelet(conflictingInRoute, lanelets.at(2008))); - - EXPECT_TRUE(route.conflictingInRoute(lanelets.at(2007)).empty()); - EXPECT_TRUE(route.conflictingInMap(lanelets.at(2007)).empty()); -} - -TEST_F(Route1, forEachSuccessor) { // NOLINT - double cLast = 0.; - route.forEachSuccessor(lanelets.at(2001), [&](const LaneletVisitInformation& i) { - EXPECT_TRUE(route.contains(i.lanelet)); - EXPECT_LE(cLast, i.cost); - cLast = i.cost; - return true; - }); -} - -TEST_F(Route1, Lanes) { // NOLINT - auto remainingLane{route.remainingLane(lanelets.at(2002))}; - EXPECT_EQ(remainingLane.size(), 2ul); // NOLINT - EXPECT_TRUE(hasLanelet(remainingLane, lanelets.at(2002))); - - auto fullLane{route.fullLane(lanelets.at(2001))}; - EXPECT_EQ(fullLane.size(), 3ul); // NOLINT - EXPECT_TRUE(hasLanelet(fullLane, lanelets.at(2002))); - EXPECT_EQ(route.numLanes(), 7ul); -} - -TEST_F(Route1NoLc, CreateRoute) { // NOLINT - EXPECT_EQ(route.size(), 7ul); // NOLINT - EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0, false)); // NOLINT - EXPECT_NE(route.getDebugLaneletMap(), nullptr); - EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size()); // NOLINT - EXPECT_NE(route.laneletSubmap(), nullptr); - EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size()); // NOLINT -} -TEST_F(Route1NoLc, Lanes) { // NOLINT - auto remainingLane{route.remainingLane(lanelets.at(2002))}; - EXPECT_EQ(remainingLane.size(), 6ul); // NOLINT - EXPECT_TRUE(hasLanelet(remainingLane, lanelets.at(2002))); - - auto fullLane{route.fullLane(lanelets.at(2002))}; - EXPECT_EQ(fullLane.size(), 7ul); // NOLINT - - EXPECT_TRUE(route.remainingLane(lanelets.at(2010)).empty()); - EXPECT_EQ(route.numLanes(), 1ul); -} - -TEST_F(Route1NoLc, forEachPredecessor) { // NOLINT - route.forEachPredecessor(lanelets.at(2001), [&](const LaneletVisitInformation& i) { - EXPECT_EQ(i.lanelet, lanelets.at(2001)); - return true; - }); -} - -TEST_F(Route2, CreateRoute2) { // NOLINT - EXPECT_EQ(route.size(), 3ul); // NOLINT - EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); // NOLINT - EXPECT_NE(route.getDebugLaneletMap(), nullptr); - EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size()); // NOLINT - EXPECT_NE(route.laneletSubmap(), nullptr); - EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size()); // NOLINT -} - -TEST_F(Route3, CreateRoute3) { // NOLINT - EXPECT_EQ(route.size(), 3ul); // NOLINT - EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); // NOLINT - EXPECT_NE(route.getDebugLaneletMap(), nullptr); - EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size()); // NOLINT - EXPECT_NE(route.laneletSubmap(), nullptr); - EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size()); // NOLINT -} - -TEST_F(Route4, CreateRoute4) { // NOLINT - EXPECT_EQ(route.size(), 15ul); // NOLINT - EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); // NOLINT - EXPECT_NE(route.getDebugLaneletMap(), nullptr); - EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size()); // NOLINT - EXPECT_NE(route.laneletSubmap(), nullptr); - EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size()); // NOLINT -} - -TEST_F(Route5, NoCircle) { - EXPECT_EQ(route.size(), 9); - EXPECT_FALSE(hasLanelet(route.laneletSubmap()->laneletLayer, lanelets.at(2065))); -} - -TEST_F(RouteMaxHoseLeftRight, CreateRouteMaxHose1) { // NOLINT - EXPECT_EQ(route.size(), 5ul); // NOLINT - EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); // NOLINT - EXPECT_NE(route.getDebugLaneletMap(), nullptr); - EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size()); // NOLINT - EXPECT_NE(route.laneletSubmap(), nullptr); - EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size()); // NOLINT -} - -TEST_F(RouteMaxHoseLeftRight, Lanes) { // NOLINT - auto remainingLane{route.remainingLane(lanelets.at(2020))}; - EXPECT_EQ(remainingLane.size(), 3ul); // NOLINT - EXPECT_TRUE(std::find(std::begin(remainingLane), std::end(remainingLane), ConstLanelet(lanelets.at(2022))) != - std::end(remainingLane)); - - auto fullLane{route.fullLane(lanelets.at(2020))}; - EXPECT_EQ(fullLane.size(), 5ul); // NOLINT - EXPECT_TRUE(std::find(std::begin(fullLane), std::end(fullLane), ConstLanelet(lanelets.at(2024))) != - std::end(fullLane)); - EXPECT_EQ(route.numLanes(), 1ul); -} - -TEST_F(RouteMaxHoseLeftRight, InvalidLane) { // NOLINT - auto remainingLane{route.remainingLane(lanelets.at(2020).invert())}; - EXPECT_TRUE(remainingLane.empty()); -} - -TEST_F(RouteMaxHoseRightLeft, CreateRouteMaxHose2) { // NOLINT - EXPECT_EQ(route.size(), 5ul); // NOLINT - EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); // NOLINT - EXPECT_NE(route.getDebugLaneletMap(), nullptr); - EXPECT_EQ(route.getDebugLaneletMap()->pointLayer.size(), route.size()); // NOLINT - EXPECT_NE(route.laneletSubmap(), nullptr); - EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), route.size()); // NOLINT -} - -TEST_F(RouteMaxHoseRightLeft, Lanes) { // NOLINT - auto remainingLane{route.remainingLane(lanelets.at(2020).invert())}; - EXPECT_EQ(remainingLane.size(), 3ul); // NOLINT - EXPECT_TRUE(std::find(std::begin(remainingLane), std::end(remainingLane), ConstLanelet(lanelets.at(2018))) != - std::end(remainingLane)); - - auto fullLane{route.fullLane(lanelets.at(2020).invert())}; - EXPECT_EQ(fullLane.size(), 5ul); // NOLINT - EXPECT_TRUE(std::find(std::begin(fullLane), std::end(fullLane), ConstLanelet(lanelets.at(2021))) != - std::end(fullLane)); - EXPECT_EQ(route.numLanes(), 1ul); // NOLINT -} - -TEST_F(RouteMaxHoseLeftRightDashedSolid, Lanes) { // NOLINT - auto remainingLane{route.remainingLane(lanelets.at(2020))}; - EXPECT_EQ(remainingLane.size(), 4ul); // NOLINT - EXPECT_FALSE(std::find(std::begin(remainingLane), std::end(remainingLane), ConstLanelet(lanelets.at(2018))) != - std::end(remainingLane)); - - auto fullLane{route.fullLane(lanelets.at(2020))}; - EXPECT_EQ(fullLane.size(), 6ul); // NOLINT - EXPECT_TRUE(std::find(std::begin(fullLane), std::end(fullLane), ConstLanelet(lanelets.at(2021))) == - std::end(fullLane)); - EXPECT_EQ(route.numLanes(), 1ul); // NOLINT -} - -TEST_F(RouteMaxHoseLeftRightDashedSolid, DashedSolidLineRegarded) { // NOLINT - EXPECT_FALSE(!!route.rightRelation(lanelets.at(2025))); -} - -TEST_F(RouteMaxHoseLeftRightDashedSolidFurther, Lanes) { // NOLINT - EXPECT_EQ(route.numLanes(), 1ul); // NOLINT - EXPECT_TRUE(route.remainingLane(lanelets.at(2026)).empty()); -} - -TEST_F(RouteMaxHoseLeftRightDashedSolidFurther, Elements) { // NOLINT - EXPECT_EQ(route.size(), 7ul); // NOLINT -} - -TEST_F(RouteMaxHoseLeftRightDashedSolidFurther, DashedSolidLineRegarded) { // NOLINT - EXPECT_FALSE(!!route.rightRelation(lanelets.at(2025))); -} - -TEST_F(RouteSolidDashed, Lanes) { // NOLINT - EXPECT_EQ(route.numLanes(), 2ul); // NOLINT -} - -TEST_F(RouteSolidDashedWithAdjacent, Lanes) { // NOLINT - EXPECT_EQ(route.numLanes(), 2ul); // NOLINT -} - -TEST_F(RouteSolidDashedWithAdjacent, AdjacentLaneletInRoute) { // NOLINT - ASSERT_TRUE(!!route.rightRelation(lanelets.at(2025))); - EXPECT_EQ(*route.rightRelation(lanelets.at(2025)), // NOLINT - (LaneletRelation{lanelets.at(2026), RelationType::Right})); - ASSERT_TRUE(!!route.leftRelation(lanelets.at(2026))); - EXPECT_EQ(*route.leftRelation(lanelets.at(2026)), // NOLINT - (LaneletRelation{lanelets.at(2025), RelationType::AdjacentLeft})); - ASSERT_TRUE(!!route.rightRelation(lanelets.at(2027))); - EXPECT_EQ(*route.rightRelation(lanelets.at(2027)), // NOLINT - (LaneletRelation{lanelets.at(2028), RelationType::AdjacentRight})); - ASSERT_TRUE(!!route.leftRelation(lanelets.at(2028))); - EXPECT_EQ(*route.leftRelation(lanelets.at(2028)), // NOLINT - (LaneletRelation{lanelets.at(2027), RelationType::AdjacentLeft})); - ASSERT_TRUE(!!route.rightRelation(lanelets.at(2029))); - EXPECT_EQ(*route.rightRelation(lanelets.at(2029)), // NOLINT - (LaneletRelation{lanelets.at(2030), RelationType::AdjacentRight})); - ASSERT_TRUE(!!route.leftRelation(lanelets.at(2030))); - EXPECT_EQ(*route.leftRelation(lanelets.at(2030)), // NOLINT - (LaneletRelation{lanelets.at(2029), RelationType::Left})); -} - -TEST_F(RouteViaSimple, create) { // NOLINT - EXPECT_EQ(route.size(), 15ul); - EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); -} - -TEST_F(RouteMissingLanelet, create) { // NOLINT - EXPECT_EQ(route.size(), 15ul); // NOLINT - EXPECT_EQ(route.shortestPath(), *graph->shortestPath(start, end, 0)); // NOLINT -} - -TEST_F(GermanVehicleGraph, CannotCreateRoute) { // NOLINT - Optional route{graph->getRoute(lanelets.at(2014), lanelets.at(2007), 0)}; - EXPECT_FALSE(!!route); - // NOLINTNEXTLINE - EXPECT_THROW(graph->getRoute(lanelets.at(2001), lanelets.at(2004), numCostModules), lanelet::InvalidInputError); -} - -TEST_F(RouteSplittedDiverging, Completeness) { // NOLINT - EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), 7ul); // NOLINT - EXPECT_EQ(route.numLanes(), 3ul); // NOLINT -} - -TEST_F(RouteSplittedDivergingAndMerging, Completeness) { // NOLINT - EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), 11ul); // NOLINT - EXPECT_EQ(route.numLanes(), 5ul); // NOLINT -} - -TEST_F(RouteInCircle, create) { // NOLINT - EXPECT_EQ(route.laneletSubmap()->laneletLayer.size(), 11ul); // NOLINT - EXPECT_EQ(route.numLanes(), 3ul); // NOLINT - EXPECT_EQ(route.remainingLane(lanelets.at(2067)).size(), 0ul); // NOLINT -} - -TEST_F(RouteCircular, Circularity) { // NOLINT - EXPECT_EQ(route.size(), 10ul); - // at any point of the circular route, the remaining lane should cross the circle - EXPECT_EQ(route.remainingLane(start).size(), 6ul); - EXPECT_EQ(*route.remainingLane(start).begin(), start); - EXPECT_EQ(route.remainingLane(lanelets.at(2036)).size(), 7ul); - EXPECT_EQ(route.remainingShortestPath(lanelets.at(2067)).size(), 7ul); - EXPECT_EQ(route.remainingLane(lanelets.at(2067)).size(), 1ul); - EXPECT_EQ(route.remainingShortestPath(lanelets.at(2067)).front(), lanelets.at(2067)); - EXPECT_EQ(route.remainingLane(lanelets.at(2033)).size(), 3ul); -} - -TEST_F(RouteCircularNoLc, Circularity) { // NOLINT - EXPECT_EQ(route.size(), 7ul); - EXPECT_EQ(route.numLanes(), 1ul); - EXPECT_EQ(route.remainingLane(start).size(), 7ul); - EXPECT_EQ(route.remainingShortestPath(lanelets.at(2036)).size(), 7ul); - EXPECT_EQ(route.remainingLane(lanelets.at(2067)).size(), 7ul); -} diff --git a/lanelet2_map_learning/test/test_routing.cpp b/lanelet2_map_learning/test/test_routing.cpp deleted file mode 100644 index 894536a7..00000000 --- a/lanelet2_map_learning/test/test_routing.cpp +++ /dev/null @@ -1,516 +0,0 @@ -#include -#include - -#include - -#include "lanelet2_map_learning/MapGraph.h" -#include "lanelet2_map_learning/internal/Graph.h" -#include "lanelet2_map_learning/internal/ShortestPath.h" -#include "test_routing_map.h" - -using namespace lanelet; -using namespace lanelet::routing; -using namespace lanelet::routing::internal; -using namespace lanelet::routing::tests; - -GraphType getSimpleGraph() { - /* 3 - * (1)---(3) - * /1 \1 / \3 - * (0) X (5) - * \2 /1 \ /1 - * (2)----(4) - * 3 - */ - GraphType g; - auto v0 = boost::add_vertex(g); - auto v1 = boost::add_vertex(g); - auto v2 = boost::add_vertex(g); - auto v3 = boost::add_vertex(g); - auto v4 = boost::add_vertex(g); - auto v5 = boost::add_vertex(g); - auto addEdge = [](auto v0, auto v1, auto& g, double c) { - auto e = boost::add_edge(v0, v1, g); - g[e.first].routingCost = c; - }; - addEdge(v0, v1, g, 1); - addEdge(v0, v2, g, 2); - addEdge(v1, v3, g, 3); - addEdge(v1, v4, g, 1); - addEdge(v2, v3, g, 1); - addEdge(v2, v4, g, 3); - addEdge(v3, v5, g, 3); - addEdge(v4, v5, g, 1); - return g; -} - -TEST(DijkstraSearch, onSimpleGraph) { - auto g = getSimpleGraph(); - DijkstraStyleSearch searcher(g); - std::vector expCost{0, 1, 2, 3, 2, 6}; - std::vector length{1, 2, 2, 3, 3, 4}; - std::vector predecessors{0, 0, 0, 2, 1, 3}; - searcher.query(0, [&](const VertexVisitInformation& v) -> bool { - EXPECT_DOUBLE_EQ(expCost[v.vertex], v.cost) << v.vertex; - EXPECT_EQ(length[v.vertex], v.length) << v.vertex; - EXPECT_EQ(predecessors[v.vertex], v.predecessor) << v.vertex; - EXPECT_EQ(v.length, v.numLaneChanges + 1); - return v.vertex != 4; - }); - EXPECT_EQ(searcher.getMap().size(), boost::num_vertices(g)); - for (const auto& v : searcher.getMap()) { - EXPECT_EQ(v.second.predicate, v.first != 4) << v.first; - EXPECT_EQ(v.second.isLeaf, v.first == 5 || v.first == 4) << v.first; - } -} - -TEST_F(GermanPedestrianGraph, NumberOfLanelets) { // NOLINT - EXPECT_EQ(graph->passableSubmap()->laneletLayer.size(), 5ul); - EXPECT_TRUE(graph->passableSubmap()->laneletLayer.exists(2031)); - EXPECT_TRUE(graph->passableSubmap()->laneletLayer.exists(2050)); - EXPECT_EQ(graph->passableSubmap()->areaLayer.size(), 3ul); - EXPECT_TRUE(graph->passableSubmap()->areaLayer.exists(3000)); - EXPECT_TRUE(graph->passableSubmap()->areaLayer.exists(3001)); - EXPECT_TRUE(graph->passableSubmap()->areaLayer.exists(3002)); -} - -TEST_F(GermanBicycleGraph, NumberOfLanelets) { // NOLINT - EXPECT_TRUE(graph->passableSubmap()->laneletLayer.exists(2013)); - EXPECT_FALSE(graph->passableSubmap()->laneletLayer.exists(2022)); -} - -TEST_F(GermanVehicleGraph, GetShortestPath) { // NOLINT - // Multiple1 - auto shortestPath = graph->shortestPath(lanelets.at(2001), lanelets.at(2004), 0); - ASSERT_TRUE(!!shortestPath); - EXPECT_EQ(shortestPath->size(), 3ul); - EXPECT_EQ((*shortestPath)[0], lanelets.at(2001)); - EXPECT_EQ((*shortestPath)[1], lanelets.at(2003)); - EXPECT_EQ((*shortestPath)[2], lanelets.at(2004)); - - shortestPath = graph->shortestPath(lanelets.at(2003), lanelets.at(2002), 0); - ASSERT_TRUE(!!shortestPath); - EXPECT_EQ(shortestPath->size(), 3ul); - EXPECT_EQ((*shortestPath)[0], lanelets.at(2003)); - EXPECT_EQ((*shortestPath)[1], lanelets.at(2001)); - EXPECT_EQ((*shortestPath)[2], lanelets.at(2002)); - - shortestPath = graph->shortestPath(lanelets.at(2003), lanelets.at(2001), 0); - ASSERT_TRUE(!!shortestPath); - EXPECT_EQ(shortestPath->size(), 2ul); - EXPECT_EQ((*shortestPath)[0], lanelets.at(2003)); - EXPECT_EQ((*shortestPath)[1], lanelets.at(2001)); - - shortestPath = graph->shortestPath(lanelets.at(2003), lanelets.at(2004), 0); - ASSERT_TRUE(!!shortestPath); - EXPECT_EQ(shortestPath->size(), 2ul); - EXPECT_EQ((*shortestPath)[0], lanelets.at(2003)); - EXPECT_EQ((*shortestPath)[1], lanelets.at(2004)); - - shortestPath = graph->shortestPath(lanelets.at(2001), lanelets.at(2002), 0); - ASSERT_TRUE(!!shortestPath); - EXPECT_EQ(shortestPath->size(), 2ul); - EXPECT_EQ((*shortestPath)[0], lanelets.at(2001)); - EXPECT_EQ((*shortestPath)[1], lanelets.at(2002)); - - shortestPath = graph->shortestPath(lanelets.at(2004), lanelets.at(2002), 0); - EXPECT_FALSE(!!shortestPath); - - shortestPath = graph->shortestPath(lanelets.at(2002), lanelets.at(2004), 0); - EXPECT_FALSE(!!shortestPath); -} - -TEST_F(GermanVehicleGraph, GetShortestPathMaxHose) { // NOLINT - auto shortestPath = graph->shortestPath(lanelets.at(2019), lanelets.at(2022), 0); - ASSERT_TRUE(!!shortestPath); - EXPECT_EQ(shortestPath->size(), 3ul); - EXPECT_EQ((*shortestPath)[0], lanelets.at(2019)); - EXPECT_EQ((*shortestPath)[1], lanelets.at(2020)); - EXPECT_EQ((*shortestPath)[2], lanelets.at(2022)); - - shortestPath = graph->shortestPath(lanelets.at(2021), lanelets.at(2018), 0); - ASSERT_TRUE(!!shortestPath); - EXPECT_EQ(shortestPath->size(), 3ul); - EXPECT_EQ((*shortestPath)[0], lanelets.at(2021)); - EXPECT_EQ((*shortestPath)[1], lanelets.at(2020).invert()); - EXPECT_EQ((*shortestPath)[2], lanelets.at(2018)); - - shortestPath = graph->shortestPath(lanelets.at(2019), lanelets.at(2018), 0); - EXPECT_FALSE(!!shortestPath); -} - -TEST_F(GermanVehicleGraph, GetShortestPathInvalid) { // NOLINT - ConstLanelet invalidLanelet; - auto shortestPath = graph->shortestPath(invalidLanelet, lanelets.at(2004), 0); - EXPECT_FALSE(!!shortestPath); - - EXPECT_THROW(graph->shortestPath(lanelets.at(2001), lanelets.at(2004), numCostModules), InvalidInputError); // NOLINT -} - -TEST_F(GermanVehicleGraph, GetShortestPathVia1) { // NOLINT - // Multiple1 - ConstLanelets interm; - interm.push_back(static_cast(lanelets.at(2003))); - auto shortestPath = graph->shortestPathVia(lanelets.at(2001), interm, lanelets.at(2004), 0); - ASSERT_TRUE(!!shortestPath); - EXPECT_EQ(shortestPath->size(), 3ul); - EXPECT_EQ((*shortestPath)[0], lanelets.at(2001)); - EXPECT_EQ((*shortestPath)[1], lanelets.at(2003)); - EXPECT_EQ((*shortestPath)[2], lanelets.at(2004)); - - shortestPath = graph->shortestPathVia(lanelets.at(2001), interm, lanelets.at(2002), 0); - ASSERT_TRUE(!!shortestPath); - EXPECT_EQ(shortestPath->size(), 4ul); - EXPECT_EQ((*shortestPath)[0], lanelets.at(2001)); - EXPECT_EQ((*shortestPath)[1], lanelets.at(2003)); - EXPECT_EQ((*shortestPath)[2], lanelets.at(2001)); - EXPECT_EQ((*shortestPath)[3], lanelets.at(2002)); - - shortestPath = graph->shortestPathVia(lanelets.at(2004), interm, lanelets.at(2002), 0); - ASSERT_FALSE(!!shortestPath); - - shortestPath = graph->shortestPathVia(lanelets.at(2002), interm, lanelets.at(2004), 0); - ASSERT_FALSE(!!shortestPath); -} - -TEST_F(GermanVehicleGraph, GetShortestPathVia2) { // NOLINT - // Multiple1 -> Multiple2 - ConstLanelets interm; - interm.push_back(static_cast(lanelets.at(2003))); - interm.push_back(static_cast(lanelets.at(2007))); - auto shortestPath = graph->shortestPathVia(lanelets.at(2001), interm, lanelets.at(2013), 0); - ASSERT_TRUE(!!shortestPath); - EXPECT_EQ(shortestPath->size(), 8ul); - auto pathIt = shortestPath->begin(); - EXPECT_EQ(*pathIt, lanelets.at(2001)); - EXPECT_EQ(*++pathIt, lanelets.at(2003)); - EXPECT_EQ(*++pathIt, lanelets.at(2004)); - EXPECT_EQ(*++pathIt, lanelets.at(2005)); - EXPECT_EQ(*++pathIt, lanelets.at(2007)); - EXPECT_EQ(*++pathIt, lanelets.at(2008)); - EXPECT_EQ(*++pathIt, lanelets.at(2010)); - EXPECT_EQ(*++pathIt, lanelets.at(2013)); -} - -TEST_F(GermanVehicleGraph, GetShortestPathViaInvalid) { // NOLINT - ConstLanelets interm; - interm.push_back(static_cast(lanelets.at(2003))); - interm.push_back(static_cast(lanelets.at(2007))); - // NOLINTNEXTLINE - EXPECT_THROW(graph->shortestPathVia(lanelets.at(2001), interm, lanelets.at(2002), numCostModules), InvalidInputError); -} - -template -bool containsLanelet(const LaneletsT& reachableSet, Id lltId) { - return !!utils::findIf(reachableSet, [lltId](auto& llt) { return llt.id() == lltId; }); -} - -TEST_F(GermanVehicleGraph, reachableSet) { // NOLINT - auto reachable = graph->reachableSet(lanelets.at(2001), 0., 0); - EXPECT_EQ(reachable.size(), 1ul); - EXPECT_TRUE(containsLanelet(reachable, 2001)); - - reachable = graph->reachableSet(lanelets.at(2001), 2.1, 0); - EXPECT_EQ(reachable.size(), 3ul); - EXPECT_TRUE(containsLanelet(reachable, 2002)); - EXPECT_TRUE(containsLanelet(reachable, 2003)); - - reachable = graph->reachableSet(lanelets.at(2001), 100, 0); - EXPECT_EQ(reachable.size(), 15ul); // Will fail if people extend the map - - reachable = graph->reachableSet(lanelets.at(2002), 100, 0); - EXPECT_EQ(reachable.size(), 11ul); // Will fail if people extend the map -} - -TEST_F(GermanVehicleGraph, reachableSetMaxHose) { // NOLINT - auto reachable = graph->reachableSet(lanelets.at(2017), 100, 0); - EXPECT_EQ(reachable.size(), 22ul); // Will fail if people extend the map - - reachable = graph->reachableSet(lanelets.at(2021), 100, 0); - EXPECT_EQ(reachable.size(), 4ul); -} - -TEST_F(GermanVehicleGraph, reachableSetInvalid) { // NOLINT - EXPECT_THROW(graph->reachableSet(lanelets.at(2021), 0.0, numCostModules), InvalidInputError); // NOLINT - ConstLanelet invalid; - auto reachable = graph->reachableSet(invalid, 0, 0); - EXPECT_TRUE(reachable.empty()); -} - -TEST_F(GermanPedestrianGraph, reachableSetCrossingWithArea) { // NOLINT - auto reachable = graph->reachableSetIncludingAreas(lanelets.at(2050), 100); - EXPECT_EQ(reachable.size(), 6ul); - EXPECT_TRUE(containsLanelet(reachable, 2050)); - EXPECT_TRUE(containsLanelet(reachable, 2053)); - EXPECT_TRUE(containsLanelet(reachable, 2052)); - EXPECT_TRUE(containsLanelet(reachable, 3000)); - EXPECT_TRUE(containsLanelet(reachable, 3001)); - EXPECT_TRUE(containsLanelet(reachable, 3002)); -} -TEST_F(GermanPedestrianGraph, reachableSetStartingFromArea) { // NOLINT - auto reachable = graph->reachableSetIncludingAreas(areas.at(3000), 100); - EXPECT_EQ(reachable.size(), 5ul); -} -TEST_F(GermanPedestrianGraph, reachableSetWithAreaFromTwoWayLanelet) { // NOLINT - auto reachable = graph->reachableSetIncludingAreas(lanelets.at(2053).invert(), 100); - EXPECT_TRUE(containsLanelet(reachable, 2053)); - EXPECT_EQ(reachable.size(), 6ul); -} -TEST_F(GermanPedestrianGraph, reachableSetWithAreaFromUnconnectedLanelet) { // NOLINT - auto reachable = graph->reachableSetIncludingAreas(lanelets.at(2051), 100); - EXPECT_EQ(reachable.size(), 1ul); -} - -TEST_F(GermanPedestrianGraph, possiblePathsWithAreaFromLanelet) { // NOLINT - auto reachable = graph->possiblePathsIncludingAreas(lanelets.at(2050), 10, 0, false); - ASSERT_EQ(reachable.size(), 3ul); - EXPECT_EQ(reachable[0].size(), 3ul); - EXPECT_EQ(reachable[1].size(), 3ul); - EXPECT_EQ(reachable[2].size(), 3ul); -} - -TEST_F(GermanPedestrianGraph, possiblePathsWithAreaFromUnconnectedLanelet) { // NOLINT - auto reachable = graph->possiblePathsIncludingAreas(lanelets.at(2050), 3, false); - ASSERT_EQ(reachable.size(), 3ul); - EXPECT_EQ(reachable[0].size(), 3ul); - EXPECT_EQ(reachable[1].size(), 3ul); - EXPECT_EQ(reachable[2].size(), 3ul); -} - -TEST_F(GermanVehicleGraph, possiblePathsMinRoutingCosts) { // NOLINT - // MIN ROUTING COST - With lane changes - // Multiple 1 - auto routes = graph->possiblePaths(lanelets.at(2001), 2.2, 0, true); - EXPECT_EQ(routes.size(), 2ul); - - routes = graph->possiblePaths(lanelets.at(2002), 4.0, 0, true); - ASSERT_EQ(routes.size(), 1ul); - auto& firstRoute = *routes.begin(); - EXPECT_EQ(firstRoute.size(), 3ul); - EXPECT_TRUE(containsLanelet(firstRoute, 2006)); -} - -TEST_F(GermanVehicleGraph, possiblePathsIncludeShorterLc) { // NOLINT - auto routes = graph->possiblePaths(lanelets.at(2041), PossiblePathsParams{1000, {}, 0, true, true}); - EXPECT_EQ(routes.size(), 3); - auto lastLLts = utils::transform(routes, [](auto& route) { return route.back(); }); - EXPECT_TRUE(has(lastLLts, lanelets.at(2062))); - EXPECT_TRUE(has(lastLLts, lanelets.at(2049))); - EXPECT_TRUE(has(lastLLts, lanelets.at(2048))); -} - -TEST_F(GermanVehicleGraph, possiblePathsIncludeShorterAllLimitsLc) { // NOLINT - auto routes = graph->possiblePaths(lanelets.at(2041), PossiblePathsParams{1000, 100, 0, true, true}); - EXPECT_EQ(routes.size(), 3); - auto lastLLts = utils::transform(routes, [](auto& route) { return route.back(); }); - EXPECT_TRUE(has(lastLLts, lanelets.at(2062))); - EXPECT_TRUE(has(lastLLts, lanelets.at(2049))); - EXPECT_TRUE(has(lastLLts, lanelets.at(2048))); -} - -TEST_F(GermanVehicleGraph, possiblePathsIncludeShorterNoLc) { // NOLINT - auto routes = graph->possiblePaths(lanelets.at(2041), PossiblePathsParams{1000, {}, 0, false, true}); - EXPECT_EQ(routes.size(), 3); - auto lastLLts = utils::transform(routes, [](auto& route) { return route.back(); }); - EXPECT_TRUE(has(lastLLts, lanelets.at(2063))); - EXPECT_TRUE(has(lastLLts, lanelets.at(2049))); - EXPECT_TRUE(has(lastLLts, lanelets.at(2047))); -} - -TEST_F(GermanVehicleGraph, possiblePathsIncludeShorterAllLimitsNoLc) { // NOLINT - auto routes = graph->possiblePaths(lanelets.at(2041), PossiblePathsParams{1000, 100, 0, false, true}); - EXPECT_EQ(routes.size(), 3); - auto lastLLts = utils::transform(routes, [](auto& route) { return route.back(); }); - EXPECT_TRUE(has(lastLLts, lanelets.at(2063))); - EXPECT_TRUE(has(lastLLts, lanelets.at(2049))); - EXPECT_TRUE(has(lastLLts, lanelets.at(2047))); -} - -TEST_F(GermanVehicleGraph, possiblePathsLimitLengthNoLc) { // NOLINT - auto routes = graph->possiblePaths(lanelets.at(2041), PossiblePathsParams{2.5, 3, 0, false, false}); - EXPECT_TRUE(std::all_of(routes.begin(), routes.end(), [](auto& r) { return r.size() <= 3; })); - EXPECT_EQ(routes.size(), 3); - auto lastLLts = utils::transform(routes, [](auto& route) { return route.back(); }); - EXPECT_TRUE(has(lastLLts, lanelets.at(2042))); -} - -TEST_F(GermanVehicleGraph, possiblePathsMaxHose) { // NOLINT - // Max Hose Problem - auto routes = graph->possiblePaths(lanelets.at(2017), 10.0, 0, true); - ASSERT_EQ(routes.size(), 1ul); - auto& firstRoute = *routes.begin(); - EXPECT_EQ(firstRoute.size(), 5ul); - EXPECT_TRUE(containsLanelet(firstRoute, 2024)); -} - -TEST_F(GermanVehicleGraph, possiblePathsMinLanelets) { // NOLINT - // MIN NUMBER LANELETS - With lane changes - auto routes = graph->possiblePaths(lanelets.at(2001), 2, false); - EXPECT_EQ(routes.size(), 1ul); - auto& firstRoute = *routes.begin(); - ASSERT_EQ(firstRoute.size(), 2ul); - EXPECT_TRUE(containsLanelet(firstRoute, 2002)); - EXPECT_EQ(firstRoute.getRemainingLane(firstRoute.begin()).size(), firstRoute.size()); - - routes = graph->possiblePaths(lanelets.at(2001), 30, false); - EXPECT_EQ(routes.size(), 0ul); - - routes = graph->possiblePaths(lanelets.at(2001), 10, true); - EXPECT_EQ(routes.size(), 0ul); - - routes = graph->possiblePaths(lanelets.at(2001), 7, true); - EXPECT_EQ(routes.size(), 3ul); -} - -TEST_F(GermanVehicleGraph, possiblePathsInvalid) { // NOLINT - // Invalid num costs length - EXPECT_THROW(graph->possiblePaths(lanelets.at(2002), 0., numCostModules, true), InvalidInputError); // NOLINT - auto routes = graph->possiblePaths(lanelets.at(2002), 0.); - ASSERT_EQ(routes.size(), 1ul); - EXPECT_EQ(routes[0].size(), 1ul); - ConstLanelet invalid; - routes = graph->possiblePaths(invalid, 10.0, 0, true); - EXPECT_EQ(routes.size(), 0ul); - - // Invalid min number lanelets - EXPECT_THROW(graph->possiblePaths(lanelets.at(2002), 1, numCostModules, true), InvalidInputError); // NOLINT - routes = graph->possiblePaths(invalid, 10, 0, true); - EXPECT_EQ(routes.size(), 0ul); -} - -TEST_F(GermanVehicleGraph, possiblePathsTowardsWithoutLc) { // NOLINT - auto routes = graph->possiblePathsTowards(lanelets.at(2024), 9, 0, false); - ASSERT_EQ(routes.size(), 1UL); - EXPECT_EQ(routes[0].front().id(), 2017); - EXPECT_EQ(routes[0].back().id(), 2024); -} - -TEST_F(GermanVehicleGraph, possiblePathsTowardsWithLc) { // NOLINT - auto routes = graph->possiblePathsTowards(lanelets.at(2015), 7, 0, true); - ASSERT_EQ(routes.size(), 2UL); - EXPECT_TRUE(containsLanelet(routes[0], 2009) || containsLanelet(routes[0], 2008)); - EXPECT_TRUE(containsLanelet(routes[1], 2009) || containsLanelet(routes[1], 2008)); -} - -TEST_F(GermanVehicleGraph, possiblePathsTowardsMinLanelets) { // NOLINT - auto routes = graph->possiblePathsTowards(lanelets.at(2015), 5, true); - ASSERT_EQ(routes.size(), 2UL); - EXPECT_TRUE(containsLanelet(routes[0], 2009) || containsLanelet(routes[0], 2008)); - EXPECT_TRUE(containsLanelet(routes[1], 2009) || containsLanelet(routes[1], 2008)); -} - -TEST_F(GermanVehicleGraph, forEachSuccessorIsMonotonic) { // NOLINT - double lastVal = 0.; - bool lanelet2010Seen = false; - bool lanelet2013Seen = false; - graph->forEachSuccessor(lanelets.at(2007), [&](const LaneletVisitInformation& i) { - if (i.cost == 0.) { - EXPECT_EQ(i.lanelet.id(), 2007) << "First lanelet must be 2007"; - } - EXPECT_LE(lastVal, i.cost); - lastVal = i.cost; - lanelet2010Seen |= i.lanelet.id() == 2010; - lanelet2013Seen |= i.lanelet.id() == 2013; - return i.lanelet.id() != 2010 && i.lanelet.id() != 2014; - }); - EXPECT_TRUE(lanelet2010Seen); - EXPECT_FALSE(lanelet2013Seen); -} - -TEST_F(GermanPedestrianGraph, forEachSuccessorIncludingAreasReachesLanelet) { // NOLINT - class TargetFound {}; - auto throwIfTarget = [&](const LaneletOrAreaVisitInformation& i) { - if (i.laneletOrArea.id() == 2053) { - throw TargetFound{}; - } - return true; - }; - EXPECT_THROW(graph->forEachSuccessorIncludingAreas(lanelets.at(2050), throwIfTarget), TargetFound); // NOLINT -} - -TEST_F(GermanPedestrianGraph, forEachPredecessorIncludingAreasReachesLanelet) { // NOLINT - class TargetFound {}; - auto throwIfTarget = [&](const LaneletOrAreaVisitInformation& i) { - if (i.laneletOrArea.id() == 2050) { - throw TargetFound{}; - } - return true; - }; - EXPECT_THROW(graph->forEachPredecessorIncludingAreas(lanelets.at(2053), throwIfTarget), TargetFound); // NOLINT -} - -TEST_F(GermanVehicleGraph, forEachPredecessorIncludingAreasReachesLanelet) { // NOLINT - class TargetFound {}; - auto throwIfTarget = [&](const LaneletOrAreaVisitInformation& i) { - if (i.laneletOrArea.id() == 2004) { - throw TargetFound{}; - } - return true; - }; - EXPECT_THROW(graph->forEachPredecessorIncludingAreas(lanelets.at(2007), throwIfTarget), TargetFound); // NOLINT -} - -TEST_F(GermanVehicleGraph, forEachPredecessorReachesLanelet) { // NOLINT - class TargetFound {}; - auto throwIfTarget = [&](const LaneletVisitInformation& i) { - if (i.lanelet.id() == 2004) { - throw TargetFound{}; - } - return true; - }; - EXPECT_THROW(graph->forEachPredecessor(lanelets.at(2007), throwIfTarget), TargetFound); // NOLINT -} - -TEST_F(GermanPedestrianGraph, shortestPathIncludingAreasFromArea) { // NOLINT - auto path = - graph->shortestPathIncludingAreas(ConstLaneletOrArea(areas.at(3001)), ConstLaneletOrArea(lanelets.at(2053))); - ASSERT_TRUE(!!path); - EXPECT_EQ(path->size(), 2ul); -} - -TEST_F(GermanPedestrianGraph, shortestPathIncludingAreasThroughAreas) { - auto path = - graph->shortestPathIncludingAreas(ConstLaneletOrArea(lanelets.at(2050)), ConstLaneletOrArea(lanelets.at(2053))); - ASSERT_TRUE(!!path); - EXPECT_EQ(path->size(), 4ul); -} - -TEST_F(GermanPedestrianGraph, shortestPathIncludingAreasViaThroughAreas) { - auto path = - graph->shortestPathIncludingAreasVia(ConstLaneletOrArea(lanelets.at(2050)), {ConstLaneletOrArea(areas.at(3002))}, - ConstLaneletOrArea(lanelets.at(2053))); - ASSERT_TRUE(!!path); - EXPECT_EQ(path->size(), 6ul); -} - -TEST(RoutingCostInitialization, NegativeLaneChangeCost) { // NOLINT - EXPECT_NO_THROW(RoutingCostDistance(1)); // NOLINT - EXPECT_THROW(RoutingCostDistance(-1), InvalidInputError); // NOLINT -} - -class ConfigurationInitialization : public ::testing::Test { - public: - LaneletMapPtr emptyMap = std::make_shared(); - - // Initialize traffic rules and routing cost calculation module - traffic_rules::TrafficRulesPtr trafficRules{traffic_rules::TrafficRulesFactory::create( - Locations::Germany, Participants::Vehicle, traffic_rules::TrafficRules::Configuration())}; - - RoutingCostPtrs costPtrs{std::make_shared(RoutingCostDistance{2.})}; - - // Optional: Initialize config for routing graph: - MapGraph::Configuration MapGraphConf; -}; - -TEST_F(ConfigurationInitialization, EmptyConfig) { // NOLINT - EXPECT_NO_THROW(MapGraph::build(*emptyMap, *trafficRules, costPtrs, MapGraphConf)); // NOLINT -} - -TEST_F(ConfigurationInitialization, Config) { // NOLINT - MapGraphConf.emplace(std::make_pair(MapGraph::ParticipantHeight, Attribute("2."))); // NOLINT - EXPECT_NO_THROW(MapGraph::build(*emptyMap, *trafficRules, costPtrs, MapGraphConf)); // NOLINT -} - -TEST_F(ConfigurationInitialization, NoConfig) { // NOLINT - EXPECT_NO_THROW(MapGraph::build(*emptyMap, *trafficRules, costPtrs)); // NOLINT -} diff --git a/lanelet2_routing/src/RoutingGraphBuilder.cpp b/lanelet2_routing/src/RoutingGraphBuilder.cpp index 8d2c6066..d03ebfc7 100644 --- a/lanelet2_routing/src/RoutingGraphBuilder.cpp +++ b/lanelet2_routing/src/RoutingGraphBuilder.cpp @@ -207,6 +207,7 @@ void RoutingGraphBuilder::addSidewayEdge(LaneChangeLaneletsCollector& laneChange // we process lane changes later, when we know all lanelets that can participate in lane change laneChangeLanelets.add(ll, it->second); } else { + std::cerr << "Lanelet with id " << ll.id() << "cannot change lane to lanelet " << it->second << "\n"; assignCosts(ll, it->second, relation); } } @@ -357,6 +358,8 @@ void RoutingGraphBuilder::assignLaneChangeCosts(ConstLanelets froms, ConstLanele void RoutingGraphBuilder::assignCosts(const ConstLaneletOrArea& from, const ConstLaneletOrArea& to, const RelationType& relation) { + std::cerr << "Adding Edge from ll with id " << from.id() << " to lanelet " << to.id() << " with type " + << relationToString(relation) << "\n"; for (RoutingCostId rci = 0; rci < RoutingCostId(routingCosts_.size()); rci++) { EdgeInfo edgeInfo{}; edgeInfo.costId = rci; From 60ea2c6a78daf37f51b77e2c565f61d360fd4876 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 20 Jul 2023 15:06:27 +0200 Subject: [PATCH 03/64] fix dot visualization --- .../lanelet2_map_learning/internal/MapGraphVisualization.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphVisualization.h b/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphVisualization.h index ba6ee963..7b272052 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphVisualization.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphVisualization.h @@ -43,7 +43,7 @@ class EdgeWriterGraphViz { template void operator()(std::ostream& out, const VertexOrEdge& v) const { const RelationType relation{(*graph_)[v].relation}; - out << "[label=\"" << relationToString(relation) << "\" color=\"" << relationToColor(relation); + out << "[label=\"" << relationToString(relation) << "\" color=\"" << relationToColor(relation) << "\"]"; } private: From 3f6c08b9e229ce4870c7d73a2aab7a37b96826e3 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 20 Jul 2023 19:44:37 +0200 Subject: [PATCH 04/64] wip map graph data provider --- .../include/lanelet2_map_learning/MapGraph.h | 2 +- .../MapGraphDataProvider.h | 54 +++++++++++++++++++ .../include/lanelet2_map_learning/Types.h | 21 -------- .../src/MapGraphDataProvider.cpp | 53 ++++++++++++++++++ 4 files changed, 108 insertions(+), 22 deletions(-) create mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h create mode 100644 lanelet2_map_learning/src/MapGraphDataProvider.cpp diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h index 531a8385..c706efd0 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h @@ -98,7 +98,7 @@ class MapGraph { Optional left(const ConstLanelet& lanelet) const; /** @brief Get adjacent left (non-routable) lanelet of a given lanelet if it exists. - * @param lanelet Start lanelet + * @param lanelet Start laneletfoo * @return Adjacent left lanelet if it exists. Nothing if it doesn't. * @see left, lefts, adjacentLefts */ Optional adjacentLeft(const ConstLanelet& lanelet) const; diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h new file mode 100644 index 00000000..4a7753dc --- /dev/null +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h @@ -0,0 +1,54 @@ +#pragma once + +#include +#include + +#include "lanelet2_map_learning/Forward.h" +#include "lanelet2_map_learning/MapGraph.h" +#include "lanelet2_map_learning/internal/Graph.h" + +namespace lanelet { +class LaneletLayer; + +namespace map_learning { + +struct TensorGraphData { + Eigen::MatrixXd x; // node features + Eigen::MatrixXd a; // adjacency matrix + Eigen::MatrixXd e; // edge features +}; + +class MapGraphDataProvider { + public: + enum class LaneletRepresentationType { Centerline, Boundaries }; + enum class ParametrizationType { Bezier, BezierEndpointFixed, Polyline8Pts, Polyline11Pts }; + + struct Configuration { + Configuration() noexcept {} + LaneletRepresentationType reprType{LaneletRepresentationType::Boundaries}; + ParametrizationType paramType{ParametrizationType::Polyline11Pts}; + double submapAreaX{100}; + double submapAreaY{100}; + }; + + MapGraphDataProvider(LaneletMapConstPtr laneletMap, Configuration config = Configuration(), + Optional currPos = boost::none); + + void setCurrPosAndExtractSubmap(const BasicPoint2d& pt); + TensorGraphData laneLaneTensors(); + TensorGraphData laneTETensors(); + + TensorGraphData laneLaneTensorsBatch(const BasicPoints2d& pts); + TensorGraphData laneTETensorsBatch(const BasicPoints2d& pts); + + private: + LaneletMapConstPtr laneletMap_; + LaneletSubmapConstPtr localSubmap_; + MapGraphConstPtr localSubmapGraph_; + Optional currPos_; // in the map frame + Configuration config_; + traffic_rules::TrafficRulesPtr trafficRules_; +}; + +} // namespace map_learning +} // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h index 21257394..a938f5bb 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h @@ -9,27 +9,6 @@ namespace lanelet { namespace map_learning { -//! This object carries the required information for the graph neighbourhood search -struct LaneletOrAreaVisitInformation { - ConstLaneletOrArea laneletOrArea; //!< The lanelet or area that is currently visited - ConstLaneletOrArea predecessor; //!< Its predecessor on the shortest path - double cost{}; //!< The accumulated cost from the start along shortest path - size_t length{}; //!< Number of elements from start to here along shortest path (including this) - size_t numLaneChanges{}; //!< Number of lane changes from start to here along shortest path -}; - -//! This object carries the required information for the graph neighbourhood search -struct LaneletVisitInformation { - ConstLanelet lanelet; //!< The lanelet or area that is currently visited - ConstLanelet predecessor; //!< Its predecessor on the shortest path - double cost{}; //!< The accumulated cost from the start along shortest path - size_t length{}; //!< The number of lanelets from start to here along shortest path (including this) - size_t numLaneChanges{}; //!< Number of lane changes from start to here along shortest path -}; - -using LaneletVisitFunction = std::function; -using LaneletOrAreaVisitFunction = std::function; - //! Represents the relation of a lanelet to another lanelet struct LaneletRelation { ConstLanelet lanelet; //!< the lanelet this relation refers to diff --git a/lanelet2_map_learning/src/MapGraphDataProvider.cpp b/lanelet2_map_learning/src/MapGraphDataProvider.cpp new file mode 100644 index 00000000..b8208f31 --- /dev/null +++ b/lanelet2_map_learning/src/MapGraphDataProvider.cpp @@ -0,0 +1,53 @@ +#include "lanelet2_map_learning/MapGraphDataProvider.h" + +#include +#include +#include + +namespace lanelet { +namespace map_learning { + +LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPoint2d& center, double extentX, + double extentY) { + BasicPoint2d regionRear = {center.x() - extentX, center.y() - extentY}; + BasicPoint2d regionFront = {center.x() + extentX, center.y() + extentY}; + BoundingBox2d searchRegion{regionRear, regionFront}; + ConstLanelets inRegion = laneletMap->laneletLayer.search(searchRegion); + return utils::createConstSubmap(inRegion, {}); +} + +void MapGraphDataProvider::setCurrPosAndExtractSubmap(const BasicPoint2d& pt) { + currPos_ = pt; + localSubmap_ = extractSubmap(laneletMap_, *currPos_, config_.submapAreaX, config_.submapAreaY); + localSubmapGraph_ = MapGraph::build(*laneletMap_, *trafficRules_); +} + +MapGraphDataProvider::MapGraphDataProvider(LaneletMapConstPtr laneletMap, Configuration config, + Optional currPos) + : laneletMap_{laneletMap}, + config_{config}, + currPos_{currPos}, + trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} + +void getLaneLaneData(MapGraphConstPtr localSubmapGraph) {} + +void getLaneTEData(MapGraphConstPtr localSubmapGraph) {} + +TensorGraphData MapGraphDataProvider::laneLaneTensors() { + if (!currPos_) { + throw InvalidObjectStateError( + "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); + } + return TensorGraphData(); +} + +TensorGraphData MapGraphDataProvider::laneTETensors() { + if (!currPos_) { + throw InvalidObjectStateError( + "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); + } + return TensorGraphData(); +} + +} // namespace map_learning +} // namespace lanelet \ No newline at end of file From 39faaa0d9a039c9e4d73d79f345aea8a7a172686 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Mon, 24 Jul 2023 20:51:57 +0200 Subject: [PATCH 05/64] wip graph conversion --- .../include/lanelet2_map_learning/Forward.h | 27 +++++++++++++++ .../include/lanelet2_map_learning/MapGraph.h | 6 ++++ .../MapGraphDataProvider.h | 6 ++-- .../lanelet2_map_learning/internal/Graph.h | 2 ++ lanelet2_map_learning/src/MapGraph.cpp | 12 +++++++ .../src/MapGraphDataProvider.cpp | 33 +++++++++++++++++-- 6 files changed, 81 insertions(+), 5 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h index 93e55f61..82950833 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h @@ -86,6 +86,28 @@ inline std::string relationToString(RelationType type) { return ""; // some compilers need that } +inline int relationToInt(RelationType type) { + switch (type) { + case RelationType::None: + return 0; + case RelationType::Successor: + return 1; + case RelationType::Left: + return 2; + case RelationType::Right: + return 3; + case RelationType::AdjacentLeft: + return 4; + case RelationType::AdjacentRight: + return 5; + case RelationType::Conflicting: + throw std::runtime_error("The relation type Conflicting should not exist in the graph!"); + case RelationType::Area: + throw std::runtime_error("The relation type Area should not exist in the graph!"); + } + return 0; // some compilers need that +} + inline std::string relationToColor(RelationType type) { switch (type) { case RelationType::None: @@ -106,5 +128,10 @@ inline std::string relationToColor(RelationType type) { } return ""; // some compilers need that } + +struct TensorGraphData; +TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph); +TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph); + } // namespace map_learning } // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h index c706efd0..d212ed02 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h @@ -58,6 +58,9 @@ class MapGraph { Optional routingRelation(const ConstLanelet& from, const ConstLanelet& to, bool includeConflicting = false) const; + //! Return type neccessary for vertex std::map comparison + ConstLaneletOrAreas getLaneletEdges(const ConstLanelet& lanelet, bool edgesOut = true) const; + /** @brief Returns the lanelets that can be reached from this lanelet. * @param lanelet Start lanelet * @param withLaneChanges Include left and right lanes or not @@ -214,6 +217,9 @@ class MapGraph { */ MapGraph(std::unique_ptr&& graph, lanelet::LaneletSubmapConstPtr&& passableMap); + friend TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph); + friend TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph); + private: //! Documentation to be found in the cpp file. std::unique_ptr graph_; ///< Wrapper of the graph diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h index 4a7753dc..ac2d6671 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h @@ -13,9 +13,9 @@ class LaneletLayer; namespace map_learning { struct TensorGraphData { - Eigen::MatrixXd x; // node features - Eigen::MatrixXd a; // adjacency matrix - Eigen::MatrixXd e; // edge features + Eigen::MatrixXd x; // node features + Eigen::MatrixX2i a; // adjacency matrix (sparse) + Eigen::MatrixXd e; // edge features }; class MapGraphDataProvider { diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h b/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h index a783cbaa..ea2f0866 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h @@ -90,6 +90,8 @@ class Graph { inline BaseGraphT& get() noexcept { return graph_; } inline const LaneletOrAreaToVertex& vertexLookup() const noexcept { return laneletOrAreaToVertex_; } + FilteredGraph withAllRelations() const { return FilteredGraph(graph_, Filter(graph_)); } + FilteredGraph withLaneChanges() const { return getFilteredGraph(RelationType::Successor | RelationType::Left | RelationType::Right); } diff --git a/lanelet2_map_learning/src/MapGraph.cpp b/lanelet2_map_learning/src/MapGraph.cpp index b0e0acbf..50fcf174 100644 --- a/lanelet2_map_learning/src/MapGraph.cpp +++ b/lanelet2_map_learning/src/MapGraph.cpp @@ -148,6 +148,18 @@ ConstLanelets getLaneletEdgesFromGraph(const MapGraphGraph& graph, const Filtere return result; } +ConstLaneletOrAreas MapGraph::getLaneletEdges(const ConstLanelet& lanelet, bool edgesOut) const { + ConstLaneletOrAreas result; + auto allEdges = getAllEdgesFromGraph(*graph_, graph_->withAllRelations(), lanelet, edgesOut); + result = reservedVector(allEdges.size()); + for (auto& edge : allEdges) { + if (edge.isLanelet()) { + result.push_back(edge); + } + } + return result; +} + MapGraph::MapGraph(MapGraph&& /*other*/) noexcept = default; MapGraph& MapGraph::operator=(MapGraph&& /*other*/) noexcept = default; MapGraph::~MapGraph() = default; diff --git a/lanelet2_map_learning/src/MapGraphDataProvider.cpp b/lanelet2_map_learning/src/MapGraphDataProvider.cpp index b8208f31..3258210c 100644 --- a/lanelet2_map_learning/src/MapGraphDataProvider.cpp +++ b/lanelet2_map_learning/src/MapGraphDataProvider.cpp @@ -29,15 +29,44 @@ MapGraphDataProvider::MapGraphDataProvider(LaneletMapConstPtr laneletMap, Config currPos_{currPos}, trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} -void getLaneLaneData(MapGraphConstPtr localSubmapGraph) {} +TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph) { + const auto& graph = localSubmapGraph->graph_; + const auto& llVertices = graph->vertexLookup(); -void getLaneTEData(MapGraphConstPtr localSubmapGraph) {} + int numNodes = llVertices.size(); + std::unordered_map key2index; + int i = 0; + for (const auto& entry : llVertices) { + key2index[entry.first] = i++; + } + + TensorGraphData result; + for (const auto& laWithVertex : llVertices) { + const auto& la = laWithVertex.first; + auto ll = laWithVertex.first.lanelet(); + const auto& vertex = laWithVertex.second; + auto id = la.id(); + + ConstLaneletOrAreas connectedLLs = localSubmapGraph->getLaneletEdges(*ll); + int32_t edgeCount = 1; + for (const auto& connectedLL : connectedLLs) { + result.a.resize(edgeCount, 2); + result.a(edgeCount - 1, 0) = key2index[la]; + result.a(edgeCount - 1, 0) = key2index[connectedLL]; + edgeCount++; + } + } + return result; +} + +TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph) { return TensorGraphData(); } TensorGraphData MapGraphDataProvider::laneLaneTensors() { if (!currPos_) { throw InvalidObjectStateError( "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); } + return TensorGraphData(); } From 8f757a58fc9813afbd0eeca71e16f6d5836ee3a7 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Tue, 25 Jul 2023 21:33:05 +0200 Subject: [PATCH 06/64] wip polyline repr --- .../include/lanelet2_map_learning/Forward.h | 9 ++-- .../include/lanelet2_map_learning/MapGraph.h | 6 ++- .../MapGraphDataProvider.h | 10 ++-- .../src/MapGraphDataProvider.cpp | 52 ++++++++++++++++++- 4 files changed, 66 insertions(+), 11 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h index 82950833..e2f6166d 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h @@ -129,9 +129,12 @@ inline std::string relationToColor(RelationType type) { return ""; // some compilers need that } +enum class LaneletRepresentationType; +enum class ParametrizationType; struct TensorGraphData; -TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph); -TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph); - +TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, + const ParametrizationType& paramType, int bezierNPoints); +TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, + const ParametrizationType& paramType, int bezierNPoints); } // namespace map_learning } // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h index d212ed02..9b092722 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h @@ -217,8 +217,10 @@ class MapGraph { */ MapGraph(std::unique_ptr&& graph, lanelet::LaneletSubmapConstPtr&& passableMap); - friend TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph); - friend TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph); + friend TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, + const ParametrizationType& paramType, int bezierNPoints); + friend TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, + const ParametrizationType& paramType, int bezierNPoints); private: //! Documentation to be found in the cpp file. diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h index ac2d6671..fdd902d4 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h @@ -12,7 +12,10 @@ class LaneletLayer; namespace map_learning { +enum class LaneletRepresentationType { Centerline, Boundaries }; +enum class ParametrizationType { Bezier, BezierEndpointFixed, Polyline11Pts, Polyline15Pts }; struct TensorGraphData { + TensorGraphData() noexcept {} Eigen::MatrixXd x; // node features Eigen::MatrixX2i a; // adjacency matrix (sparse) Eigen::MatrixXd e; // edge features @@ -20,15 +23,14 @@ struct TensorGraphData { class MapGraphDataProvider { public: - enum class LaneletRepresentationType { Centerline, Boundaries }; - enum class ParametrizationType { Bezier, BezierEndpointFixed, Polyline8Pts, Polyline11Pts }; - struct Configuration { Configuration() noexcept {} + int32_t getNodeFeatureLength(); LaneletRepresentationType reprType{LaneletRepresentationType::Boundaries}; - ParametrizationType paramType{ParametrizationType::Polyline11Pts}; + ParametrizationType paramType{ParametrizationType::Polyline15Pts}; double submapAreaX{100}; double submapAreaY{100}; + int bezierNPoints{11}; }; MapGraphDataProvider(LaneletMapConstPtr laneletMap, Configuration config = Configuration(), diff --git a/lanelet2_map_learning/src/MapGraphDataProvider.cpp b/lanelet2_map_learning/src/MapGraphDataProvider.cpp index 3258210c..9e09b447 100644 --- a/lanelet2_map_learning/src/MapGraphDataProvider.cpp +++ b/lanelet2_map_learning/src/MapGraphDataProvider.cpp @@ -2,8 +2,11 @@ #include #include +#include #include +#include + namespace lanelet { namespace map_learning { @@ -29,7 +32,47 @@ MapGraphDataProvider::MapGraphDataProvider(LaneletMapConstPtr laneletMap, Config currPos_{currPos}, trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} -TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph) { +int32_t getNodeFeatureLength(const LaneletRepresentationType& reprType, const ParametrizationType& paramType, + int bezierNPoints) { + int32_t nodeFeatureLength; + + int32_t bdParams; + if (paramType == ParametrizationType::Bezier) + bdParams = bezierNPoints; + else if (paramType == ParametrizationType::BezierEndpointFixed) + bdParams = bezierNPoints; + else if (paramType == ParametrizationType::Polyline11Pts) + bdParams = 11; + else if (paramType == ParametrizationType::Polyline15Pts) + bdParams = 15; + else + throw std::runtime_error("Unknown LaneletRepresentationType!"); + + if (reprType == LaneletRepresentationType::Boundaries) + nodeFeatureLength = 2 * bdParams + 4; // 2 boundary types with 2 possible values (one-hot encoding) + else if (reprType == LaneletRepresentationType::Centerline) + nodeFeatureLength = 1 * bdParams; + else + throw std::runtime_error("Unknown LaneletRepresentationType!"); + + return nodeFeatureLength; +} + +Eigen::Vector3d computePolylineRepr(const ConstLanelet& ll, int32_t noPts) { + Eigen::Vector3d repr; + std::vector boundaries{ll.leftBound3d().basicLineString(), ll.rightBound3d().basicLineString()}; + for (const auto& bd : boundaries) { + // Note boost doesnt work here for some reason + double length = boost::geometry::length(bd, boost::geometry::strategy::distance::pythagoras()); + double dist = length / static_cast(noPts); + boost::geometry::model::multi_point bdInterp; + boost::geometry::line_interpolate(bd, dist, bdInterp); + } + return repr; +} + +TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, + const ParametrizationType& paramType, int bezierNPoints) { const auto& graph = localSubmapGraph->graph_; const auto& llVertices = graph->vertexLookup(); @@ -47,6 +90,8 @@ TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph) { const auto& vertex = laWithVertex.second; auto id = la.id(); + int32_t nodeFeatLength = getNodeFeatureLength(reprType, paramType, bezierNPoints); + ConstLaneletOrAreas connectedLLs = localSubmapGraph->getLaneletEdges(*ll); int32_t edgeCount = 1; for (const auto& connectedLL : connectedLLs) { @@ -59,7 +104,10 @@ TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph) { return result; } -TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph) { return TensorGraphData(); } +TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, + const ParametrizationType& paramType, int bezierNPoints) { + return TensorGraphData(); +} TensorGraphData MapGraphDataProvider::laneLaneTensors() { if (!currPos_) { From 0c2168c380826aa0ad5517388e876c7046aad96d Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 27 Jul 2023 21:10:39 +0200 Subject: [PATCH 07/64] wip node feature calculation --- .../MapGraphDataProvider.h | 7 +- .../src/MapGraphDataProvider.cpp | 76 +++++++++++-------- 2 files changed, 47 insertions(+), 36 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h index fdd902d4..7b01694c 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h @@ -13,7 +13,8 @@ class LaneletLayer; namespace map_learning { enum class LaneletRepresentationType { Centerline, Boundaries }; -enum class ParametrizationType { Bezier, BezierEndpointFixed, Polyline11Pts, Polyline15Pts }; +enum class ParametrizationType { Bezier, BezierEndpointFixed, Polyline }; + struct TensorGraphData { TensorGraphData() noexcept {} Eigen::MatrixXd x; // node features @@ -27,10 +28,10 @@ class MapGraphDataProvider { Configuration() noexcept {} int32_t getNodeFeatureLength(); LaneletRepresentationType reprType{LaneletRepresentationType::Boundaries}; - ParametrizationType paramType{ParametrizationType::Polyline15Pts}; + ParametrizationType paramType{ParametrizationType::Polyline}; double submapAreaX{100}; double submapAreaY{100}; - int bezierNPoints{11}; + int nPoints{11}; }; MapGraphDataProvider(LaneletMapConstPtr laneletMap, Configuration config = Configuration(), diff --git a/lanelet2_map_learning/src/MapGraphDataProvider.cpp b/lanelet2_map_learning/src/MapGraphDataProvider.cpp index 9e09b447..8641285b 100644 --- a/lanelet2_map_learning/src/MapGraphDataProvider.cpp +++ b/lanelet2_map_learning/src/MapGraphDataProvider.cpp @@ -6,6 +6,7 @@ #include #include +#include namespace lanelet { namespace map_learning { @@ -33,71 +34,80 @@ MapGraphDataProvider::MapGraphDataProvider(LaneletMapConstPtr laneletMap, Config trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} int32_t getNodeFeatureLength(const LaneletRepresentationType& reprType, const ParametrizationType& paramType, - int bezierNPoints) { + int nPoints) { int32_t nodeFeatureLength; - - int32_t bdParams; - if (paramType == ParametrizationType::Bezier) - bdParams = bezierNPoints; - else if (paramType == ParametrizationType::BezierEndpointFixed) - bdParams = bezierNPoints; - else if (paramType == ParametrizationType::Polyline11Pts) - bdParams = 11; - else if (paramType == ParametrizationType::Polyline15Pts) - bdParams = 15; - else - throw std::runtime_error("Unknown LaneletRepresentationType!"); - if (reprType == LaneletRepresentationType::Boundaries) - nodeFeatureLength = 2 * bdParams + 4; // 2 boundary types with 2 possible values (one-hot encoding) + nodeFeatureLength = 2 * nPoints + 4; // 2 boundary types with 2 possible values (one-hot encoding) else if (reprType == LaneletRepresentationType::Centerline) - nodeFeatureLength = 1 * bdParams; + nodeFeatureLength = 1 * nPoints; else throw std::runtime_error("Unknown LaneletRepresentationType!"); return nodeFeatureLength; } -Eigen::Vector3d computePolylineRepr(const ConstLanelet& ll, int32_t noPts) { - Eigen::Vector3d repr; +Eigen::Vector3d computePolylineRepr(const BasicLineString3d& lstr, int32_t nPoints) { + Eigen::Vector3d repr(3 * nPoints); + double length = boost::geometry::length(lstr, boost::geometry::strategy::distance::pythagoras()); + double dist = length / static_cast(nPoints); + boost::geometry::model::multi_point bdInterp; + boost::geometry::line_interpolate(lstr, dist, bdInterp); + assert(bdInterp.size() == nPoints); + for (size_t i = 0; i < bdInterp.size(); i++) { + repr(Eigen::seq(i, i + 2)) = bdInterp[i](Eigen::seq(i, i + 2)); + } + return repr; +} + +Eigen::Vector3d getPolylineReprBoundaries(const ConstLanelet& ll, const ParametrizationType& paramType, int nPoints) { + Eigen::Vector3d repr(2 * 3 * nPoints); std::vector boundaries{ll.leftBound3d().basicLineString(), ll.rightBound3d().basicLineString()}; - for (const auto& bd : boundaries) { - // Note boost doesnt work here for some reason - double length = boost::geometry::length(bd, boost::geometry::strategy::distance::pythagoras()); - double dist = length / static_cast(noPts); - boost::geometry::model::multi_point bdInterp; - boost::geometry::line_interpolate(bd, dist, bdInterp); + for (size_t i = 0; i < boundaries.size(); i++) { + repr(Eigen::seq(i * 3 * nPoints, (i + 1) * 3 * nPoints)) = computePolylineRepr(boundaries[i], nPoints); } return repr; } +Eigen::Vector3d getPolylineReprCenterline(const ConstLanelet& ll, const ParametrizationType& paramType, int nPoints) { + Eigen::Vector3d repr(3 * nPoints); + BasicLineString3d centerline = ll.centerline3d().basicLineString(); + return computePolylineRepr(centerline, nPoints); +} + +Eigen::Vector3d getNodeFeatureVec(const ConstLanelet& ll, const LaneletRepresentationType& reprType, + const ParametrizationType& paramType, int nPoints, int nodeFeatLength) { + Eigen::Vector3d featureVec(nodeFeatLength); + return featureVec; +} + TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int bezierNPoints) { + const ParametrizationType& paramType, int nPoints) { const auto& graph = localSubmapGraph->graph_; const auto& llVertices = graph->vertexLookup(); + TensorGraphData result; int numNodes = llVertices.size(); + int32_t nodeFeatLength = getNodeFeatureLength(reprType, paramType, nPoints); + result.x.resize(numNodes, nodeFeatLength); + std::unordered_map key2index; int i = 0; for (const auto& entry : llVertices) { key2index[entry.first] = i++; } - TensorGraphData result; + int32_t edgeCount = 0; for (const auto& laWithVertex : llVertices) { const auto& la = laWithVertex.first; auto ll = laWithVertex.first.lanelet(); const auto& vertex = laWithVertex.second; auto id = la.id(); - int32_t nodeFeatLength = getNodeFeatureLength(reprType, paramType, bezierNPoints); - ConstLaneletOrAreas connectedLLs = localSubmapGraph->getLaneletEdges(*ll); - int32_t edgeCount = 1; for (const auto& connectedLL : connectedLLs) { - result.a.resize(edgeCount, 2); - result.a(edgeCount - 1, 0) = key2index[la]; - result.a(edgeCount - 1, 0) = key2index[connectedLL]; + result.a.resize(edgeCount + 1, 2); + result.a(edgeCount, 0) = key2index[la]; + result.a(edgeCount, 0) = key2index[connectedLL]; edgeCount++; } } @@ -105,7 +115,7 @@ TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph, const Lanelet } TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int bezierNPoints) { + const ParametrizationType& paramType, int nPoints) { return TensorGraphData(); } From 283c49f8fc40421a13f7a3453d1f454ce477b9c7 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Wed, 2 Aug 2023 21:16:26 +0200 Subject: [PATCH 08/64] draft lane lane graph --- .../include/lanelet2_map_learning/Forward.h | 4 +- .../include/lanelet2_map_learning/MapGraph.h | 6 +- .../MapGraphDataProvider.h | 1 + .../src/MapGraphDataProvider.cpp | 85 +++++++++++++++---- 4 files changed, 76 insertions(+), 20 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h index e2f6166d..de00b9a4 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h @@ -133,8 +133,8 @@ enum class LaneletRepresentationType; enum class ParametrizationType; struct TensorGraphData; TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int bezierNPoints); + const ParametrizationType& paramType, int nPoints, int noRelTypes, int noBdTypes); TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int bezierNPoints); + const ParametrizationType& paramType, int nPoints, int noRelTypes, int noBdTypes); } // namespace map_learning } // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h index 9b092722..127efeee 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h @@ -218,9 +218,11 @@ class MapGraph { MapGraph(std::unique_ptr&& graph, lanelet::LaneletSubmapConstPtr&& passableMap); friend TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int bezierNPoints); + const ParametrizationType& paramType, int nPoints, int noRelTypes, + int noBdTypes); friend TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int bezierNPoints); + const ParametrizationType& paramType, int nPoints, int noRelTypes, + int noBdTypes); private: //! Documentation to be found in the cpp file. diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h index 7b01694c..bb7b7638 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h @@ -32,6 +32,7 @@ class MapGraphDataProvider { double submapAreaX{100}; double submapAreaY{100}; int nPoints{11}; + int noRelTypes{7}; }; MapGraphDataProvider(LaneletMapConstPtr laneletMap, Configuration config = Configuration(), diff --git a/lanelet2_map_learning/src/MapGraphDataProvider.cpp b/lanelet2_map_learning/src/MapGraphDataProvider.cpp index 8641285b..a78c530d 100644 --- a/lanelet2_map_learning/src/MapGraphDataProvider.cpp +++ b/lanelet2_map_learning/src/MapGraphDataProvider.cpp @@ -34,10 +34,11 @@ MapGraphDataProvider::MapGraphDataProvider(LaneletMapConstPtr laneletMap, Config trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} int32_t getNodeFeatureLength(const LaneletRepresentationType& reprType, const ParametrizationType& paramType, - int nPoints) { + int nPoints, int noBdTypes) { int32_t nodeFeatureLength; if (reprType == LaneletRepresentationType::Boundaries) - nodeFeatureLength = 2 * nPoints + 4; // 2 boundary types with 2 possible values (one-hot encoding) + nodeFeatureLength = + 2 * nPoints + 2 * noBdTypes; // 2 boundary types with 2 possible values and unknown types (one-hot encoding) else if (reprType == LaneletRepresentationType::Centerline) nodeFeatureLength = 1 * nPoints; else @@ -59,35 +60,77 @@ Eigen::Vector3d computePolylineRepr(const BasicLineString3d& lstr, int32_t nPoin return repr; } -Eigen::Vector3d getPolylineReprBoundaries(const ConstLanelet& ll, const ParametrizationType& paramType, int nPoints) { - Eigen::Vector3d repr(2 * 3 * nPoints); - std::vector boundaries{ll.leftBound3d().basicLineString(), ll.rightBound3d().basicLineString()}; - for (size_t i = 0; i < boundaries.size(); i++) { - repr(Eigen::seq(i * 3 * nPoints, (i + 1) * 3 * nPoints)) = computePolylineRepr(boundaries[i], nPoints); +Eigen::Vector3d getPolylineRepr(const ConstLanelet& ll, const LaneletRepresentationType& reprType, + const ParametrizationType& paramType, int nPoints) { + if (paramType != ParametrizationType::Polyline) { + throw std::runtime_error("Only polyline parametrization is implemented so far!"); + } + if (reprType == LaneletRepresentationType::Centerline) { + Eigen::Vector3d repr(3 * nPoints); + BasicLineString3d centerline = ll.centerline3d().basicLineString(); + return computePolylineRepr(centerline, nPoints); + } else if (reprType == LaneletRepresentationType::Boundaries) { + Eigen::Vector3d repr(2 * 3 * nPoints); + std::vector boundaries{ll.leftBound3d().basicLineString(), ll.rightBound3d().basicLineString()}; + for (size_t i = 0; i < boundaries.size(); i++) { + repr(Eigen::seq(i * 3 * nPoints, (i + 1) * 3 * nPoints - 1)) = computePolylineRepr(boundaries[i], nPoints); + } + return repr; + } else { + throw std::runtime_error("Unknown LaneletRepresentationType!"); + return Eigen::Vector3d(3 * nPoints); } - return repr; } -Eigen::Vector3d getPolylineReprCenterline(const ConstLanelet& ll, const ParametrizationType& paramType, int nPoints) { - Eigen::Vector3d repr(3 * nPoints); - BasicLineString3d centerline = ll.centerline3d().basicLineString(); - return computePolylineRepr(centerline, nPoints); +inline int linestringSubtypeToInt(ConstLineString3d lString) { + std::string subtype = lString.attribute(AttributeName::Subtype).value(); + if (subtype == AttributeValueString::Dashed) + return 0; + else if (subtype == AttributeValueString::Solid) + return 1; + else if (subtype == AttributeValueString::SolidSolid) + return 1; + else if (subtype == AttributeValueString::SolidDashed) + return 1; + else if (subtype == AttributeValueString::DashedSolid) + return 1; + else { + throw std::runtime_error("Unexpected Line String Subtype!"); + return 1; + } } Eigen::Vector3d getNodeFeatureVec(const ConstLanelet& ll, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints, int nodeFeatLength) { + const ParametrizationType& paramType, int nPoints, int nodeFeatLength, + int noBdTypes) { Eigen::Vector3d featureVec(nodeFeatLength); + Eigen::Vector3d polylineRepr = getPolylineRepr(ll, reprType, paramType, nPoints); + featureVec(Eigen::seq(0, polylineRepr.size() - 1)) = polylineRepr; + + Eigen::Vector3d typeFeatureVecLeft(noBdTypes); + typeFeatureVecLeft.setZero(); + typeFeatureVecLeft[linestringSubtypeToInt(ll.leftBound3d())] = 1; + + Eigen::Vector3d typeFeatureVecRight(noBdTypes); + typeFeatureVecRight.setZero(); + typeFeatureVecRight[linestringSubtypeToInt(ll.rightBound3d())] = 1; + + featureVec(Eigen::seq(polylineRepr.size(), noBdTypes - 1)) = typeFeatureVecLeft; + featureVec(Eigen::seq(polylineRepr.size() + noBdTypes, polylineRepr.size() + 2 * noBdTypes - 1)) = + typeFeatureVecRight; + return featureVec; } TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints) { + const ParametrizationType& paramType, int nPoints, int noRelTypes = 7, + int noBdTypes = 3) { const auto& graph = localSubmapGraph->graph_; const auto& llVertices = graph->vertexLookup(); TensorGraphData result; int numNodes = llVertices.size(); - int32_t nodeFeatLength = getNodeFeatureLength(reprType, paramType, nPoints); + int32_t nodeFeatLength = getNodeFeatureLength(reprType, paramType, nPoints, noBdTypes); result.x.resize(numNodes, nodeFeatLength); std::unordered_map key2index; @@ -102,12 +145,21 @@ TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph, const Lanelet auto ll = laWithVertex.first.lanelet(); const auto& vertex = laWithVertex.second; auto id = la.id(); + result.x.row(key2index[la]) = getNodeFeatureVec(*ll, reprType, paramType, nPoints, nodeFeatLength, noBdTypes); ConstLaneletOrAreas connectedLLs = localSubmapGraph->getLaneletEdges(*ll); for (const auto& connectedLL : connectedLLs) { result.a.resize(edgeCount + 1, 2); result.a(edgeCount, 0) = key2index[la]; result.a(edgeCount, 0) = key2index[connectedLL]; + + result.e.resize(edgeCount + 1, noRelTypes); + Eigen::Vector3d edgeFeatureVec(noRelTypes); + edgeFeatureVec.setZero(); + ConstLanelet connectedLLasLL = connectedLL.lanelet().get(); + RelationType edgeType = graph->getEdgeInfo(*ll, connectedLLasLL).get().relation; + edgeFeatureVec[relationToInt(edgeType)] = 1; + result.e.row(edgeCount) = edgeFeatureVec; edgeCount++; } } @@ -115,7 +167,8 @@ TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph, const Lanelet } TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints) { + const ParametrizationType& paramType, int nPoints, int noRelTypes = 7, + int noBdTypes = 3) { return TensorGraphData(); } From 5155b53b70a323fa441dd68b18a872763552db91 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 3 Aug 2023 21:02:45 +0200 Subject: [PATCH 09/64] wip lane to te graph --- .../include/lanelet2_map_learning/Forward.h | 12 +-- .../include/lanelet2_map_learning/MapGraph.h | 14 ++-- ...DataProvider.h => MapGraphDataInterface.h} | 31 +++++--- .../lanelet2_map_learning/internal/Graph.h | 15 +++- ...Provider.cpp => MapGraphDataInterface.cpp} | 76 ++++++++++++++----- 5 files changed, 106 insertions(+), 42 deletions(-) rename lanelet2_map_learning/include/lanelet2_map_learning/{MapGraphDataProvider.h => MapGraphDataInterface.h} (59%) rename lanelet2_map_learning/src/{MapGraphDataProvider.cpp => MapGraphDataInterface.cpp} (71%) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h index de00b9a4..6b3eeb64 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h @@ -131,10 +131,12 @@ inline std::string relationToColor(RelationType type) { enum class LaneletRepresentationType; enum class ParametrizationType; -struct TensorGraphData; -TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints, int noRelTypes, int noBdTypes); -TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints, int noRelTypes, int noBdTypes); +struct TensorGraphDataLaneLane; +struct TensorGraphDataLaneTE; +TensorGraphDataLaneLane getLaneLaneData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, + const ParametrizationType& paramType, int nPoints, int noRelTypes, + int noBdTypes); +TensorGraphDataLaneTE getLaneTEData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, + const ParametrizationType& paramType, int nPoints, int noRelTypes, int noBdTypes); } // namespace map_learning } // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h index 127efeee..33dd0881 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h @@ -217,12 +217,14 @@ class MapGraph { */ MapGraph(std::unique_ptr&& graph, lanelet::LaneletSubmapConstPtr&& passableMap); - friend TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints, int noRelTypes, - int noBdTypes); - friend TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints, int noRelTypes, - int noBdTypes); + friend TensorGraphDataLaneLane getLaneLaneData(MapGraphConstPtr localSubmapGraph, + const LaneletRepresentationType& reprType, + const ParametrizationType& paramType, int nPoints, int noRelTypes, + int noBdTypes); + friend TensorGraphDataLaneTE getLaneTEData(MapGraphConstPtr localSubmapGraph, + const LaneletRepresentationType& reprType, + const ParametrizationType& paramType, int nPoints, int noRelTypes, + int noBdTypes); private: //! Documentation to be found in the cpp file. diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h similarity index 59% rename from lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h rename to lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h index bb7b7638..7770d6bb 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataProvider.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h @@ -15,14 +15,26 @@ namespace map_learning { enum class LaneletRepresentationType { Centerline, Boundaries }; enum class ParametrizationType { Bezier, BezierEndpointFixed, Polyline }; -struct TensorGraphData { - TensorGraphData() noexcept {} +struct TensorGraphDataLaneLane { + TensorGraphDataLaneLane() noexcept {} Eigen::MatrixXd x; // node features Eigen::MatrixX2i a; // adjacency matrix (sparse) Eigen::MatrixXd e; // edge features }; -class MapGraphDataProvider { +struct TensorGraphDataLaneTE { + TensorGraphDataLaneTE() noexcept {} + Eigen::MatrixXd xLane; // node features lanelets + Eigen::MatrixXd xTE; // node features traffic elements + + /// @brief adjacency matrix (sparse). Node indices are assigned as + /// xLane and xTE stacked, with xLane being first + Eigen::MatrixX2i a; + + Eigen::MatrixXd e; // edge features +}; + +class MapGraphDataInterface { public: struct Configuration { Configuration() noexcept {} @@ -33,17 +45,18 @@ class MapGraphDataProvider { double submapAreaY{100}; int nPoints{11}; int noRelTypes{7}; + int noBdTypes{3}; }; - MapGraphDataProvider(LaneletMapConstPtr laneletMap, Configuration config = Configuration(), - Optional currPos = boost::none); + MapGraphDataInterface(LaneletMapConstPtr laneletMap, Configuration config = Configuration(), + Optional currPos = boost::none); void setCurrPosAndExtractSubmap(const BasicPoint2d& pt); - TensorGraphData laneLaneTensors(); - TensorGraphData laneTETensors(); + TensorGraphDataLaneLane laneLaneTensors(); + TensorGraphDataLaneTE laneTETensors(); - TensorGraphData laneLaneTensorsBatch(const BasicPoints2d& pts); - TensorGraphData laneTETensorsBatch(const BasicPoints2d& pts); + TensorGraphDataLaneLane laneLaneTensorsBatch(const BasicPoints2d& pts); + TensorGraphDataLaneTE laneTETensorsBatch(const BasicPoints2d& pts); private: LaneletMapConstPtr laneletMap_; diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h b/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h index ea2f0866..6037e337 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h @@ -179,11 +179,24 @@ class Graph { } } + std::unordered_map getKey2Index(bool recompute = false) { + if (recompute || !key2index_) { + std::unordered_map key2index; + int i = 0; + for (const auto& entry : laneletOrAreaToVertex_) { + key2index[entry.first] = i++; + } + key2index_ = key2index; + } + return *key2index_; + } + private: FilteredGraph getFilteredGraph(RelationType relations) const { return FilteredGraph(graph_, Filter(graph_, relations)); } - BaseGraphT graph_; //!< The actual graph object + BaseGraphT graph_; //!< The actual graph object + Optional> key2index_; LaneletOrAreaToVertex laneletOrAreaToVertex_; //!< Mapping of lanelets/areas to vertices of the graph }; diff --git a/lanelet2_map_learning/src/MapGraphDataProvider.cpp b/lanelet2_map_learning/src/MapGraphDataInterface.cpp similarity index 71% rename from lanelet2_map_learning/src/MapGraphDataProvider.cpp rename to lanelet2_map_learning/src/MapGraphDataInterface.cpp index a78c530d..e5156301 100644 --- a/lanelet2_map_learning/src/MapGraphDataProvider.cpp +++ b/lanelet2_map_learning/src/MapGraphDataInterface.cpp @@ -1,4 +1,4 @@ -#include "lanelet2_map_learning/MapGraphDataProvider.h" +#include "lanelet2_map_learning/MapGraphDataInterface.h" #include #include @@ -20,14 +20,14 @@ LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPo return utils::createConstSubmap(inRegion, {}); } -void MapGraphDataProvider::setCurrPosAndExtractSubmap(const BasicPoint2d& pt) { +void MapGraphDataInterface::setCurrPosAndExtractSubmap(const BasicPoint2d& pt) { currPos_ = pt; localSubmap_ = extractSubmap(laneletMap_, *currPos_, config_.submapAreaX, config_.submapAreaY); localSubmapGraph_ = MapGraph::build(*laneletMap_, *trafficRules_); } -MapGraphDataProvider::MapGraphDataProvider(LaneletMapConstPtr laneletMap, Configuration config, - Optional currPos) +MapGraphDataInterface::MapGraphDataInterface(LaneletMapConstPtr laneletMap, Configuration config, + Optional currPos) : laneletMap_{laneletMap}, config_{config}, currPos_{currPos}, @@ -122,18 +122,57 @@ Eigen::Vector3d getNodeFeatureVec(const ConstLanelet& ll, const LaneletRepresent return featureVec; } -TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints, int noRelTypes = 7, - int noBdTypes = 3) { +TensorGraphDataLaneLane getLaneLaneData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, + const ParametrizationType& paramType, int nPoints, int noRelTypes = 7, + int noBdTypes = 3) { const auto& graph = localSubmapGraph->graph_; const auto& llVertices = graph->vertexLookup(); - TensorGraphData result; + TensorGraphDataLaneLane result; int numNodes = llVertices.size(); int32_t nodeFeatLength = getNodeFeatureLength(reprType, paramType, nPoints, noBdTypes); result.x.resize(numNodes, nodeFeatLength); - std::unordered_map key2index; + std::unordered_map key2index = graph->getKey2Index(); + + int32_t edgeCount = 0; + for (const auto& laWithVertex : llVertices) { + const auto& la = laWithVertex.first; + auto ll = laWithVertex.first.lanelet(); + const auto& vertex = laWithVertex.second; + result.x.row(key2index[la]) = getNodeFeatureVec(*ll, reprType, paramType, nPoints, nodeFeatLength, noBdTypes); + + ConstLaneletOrAreas connectedLLs = localSubmapGraph->getLaneletEdges(*ll); + for (const auto& connectedLL : connectedLLs) { + result.a.resize(edgeCount + 1, 2); + result.a(edgeCount, 0) = key2index[la]; + result.a(edgeCount, 0) = key2index[connectedLL]; + + result.e.resize(edgeCount + 1, noRelTypes); + Eigen::Vector3d edgeFeatureVec(noRelTypes); + edgeFeatureVec.setZero(); + ConstLanelet connectedLLasLL = connectedLL.lanelet().get(); + RelationType edgeType = graph->getEdgeInfo(*ll, connectedLLasLL).get().relation; + edgeFeatureVec[relationToInt(edgeType)] = 1; + result.e.row(edgeCount) = edgeFeatureVec; + edgeCount++; + } + } + return result; +} + +TensorGraphDataLaneTE getLaneTEData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, + const ParametrizationType& paramType, int nPoints, int noRelTypes = 7, + int noBdTypes = 3) { + const auto& graph = localSubmapGraph->graph_; + const auto& llVertices = graph->vertexLookup(); + + TensorGraphDataLaneTE result; + int numNodesLane = llVertices.size(); + int32_t nodeFeatLengthLane = getNodeFeatureLength(reprType, paramType, nPoints, noBdTypes); + result.xLane.resize(numNodesLane, nodeFeatLengthLane); + + std::unordered_map key2index = graph->getKey2Index(); int i = 0; for (const auto& entry : llVertices) { key2index[entry.first] = i++; @@ -144,8 +183,8 @@ TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph, const Lanelet const auto& la = laWithVertex.first; auto ll = laWithVertex.first.lanelet(); const auto& vertex = laWithVertex.second; - auto id = la.id(); - result.x.row(key2index[la]) = getNodeFeatureVec(*ll, reprType, paramType, nPoints, nodeFeatLength, noBdTypes); + result.xLane.row(key2index[la]) = + getNodeFeatureVec(*ll, reprType, paramType, nPoints, nodeFeatLengthLane, noBdTypes); ConstLaneletOrAreas connectedLLs = localSubmapGraph->getLaneletEdges(*ll); for (const auto& connectedLL : connectedLLs) { @@ -166,27 +205,22 @@ TensorGraphData getLaneLaneData(MapGraphConstPtr localSubmapGraph, const Lanelet return result; } -TensorGraphData getLaneTEData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints, int noRelTypes = 7, - int noBdTypes = 3) { - return TensorGraphData(); -} - -TensorGraphData MapGraphDataProvider::laneLaneTensors() { +TensorGraphDataLaneLane MapGraphDataInterface::laneLaneTensors() { if (!currPos_) { throw InvalidObjectStateError( "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); } - return TensorGraphData(); + return getLaneLaneData(localSubmapGraph_, config_.reprType, config_.paramType, config_.nPoints, config_.noRelTypes, + config_.noBdTypes); } -TensorGraphData MapGraphDataProvider::laneTETensors() { +TensorGraphDataLaneTE MapGraphDataInterface::laneTETensors() { if (!currPos_) { throw InvalidObjectStateError( "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); } - return TensorGraphData(); + return TensorGraphDataLaneTE(); } } // namespace map_learning From fb65688595c605b248e06ecd96b84a883257e4f3 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Tue, 8 Aug 2023 20:27:47 +0200 Subject: [PATCH 10/64] draft lane to te graph --- .../include/lanelet2_map_learning/Forward.h | 7 +- .../include/lanelet2_map_learning/MapGraph.h | 9 +- .../MapGraphDataInterface.h | 13 +- .../lanelet2_map_learning/internal/Graph.h | 14 +- .../src/MapGraphDataInterface.cpp | 178 +++++++++++------- 5 files changed, 135 insertions(+), 86 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h index 6b3eeb64..57df3928 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h @@ -133,10 +133,7 @@ enum class LaneletRepresentationType; enum class ParametrizationType; struct TensorGraphDataLaneLane; struct TensorGraphDataLaneTE; -TensorGraphDataLaneLane getLaneLaneData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints, int noRelTypes, - int noBdTypes); -TensorGraphDataLaneTE getLaneTEData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints, int noRelTypes, int noBdTypes); +class MapGraphDataInterface; + } // namespace map_learning } // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h index 33dd0881..6a96bcb5 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h @@ -217,14 +217,7 @@ class MapGraph { */ MapGraph(std::unique_ptr&& graph, lanelet::LaneletSubmapConstPtr&& passableMap); - friend TensorGraphDataLaneLane getLaneLaneData(MapGraphConstPtr localSubmapGraph, - const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints, int noRelTypes, - int noBdTypes); - friend TensorGraphDataLaneTE getLaneTEData(MapGraphConstPtr localSubmapGraph, - const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints, int noRelTypes, - int noBdTypes); + friend class MapGraphDataInterface; private: //! Documentation to be found in the cpp file. diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h index 7770d6bb..0915e906 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h @@ -36,16 +36,15 @@ struct TensorGraphDataLaneTE { class MapGraphDataInterface { public: + using FeatureBuffer = std::unordered_map, std::equal_to, + Eigen::aligned_allocator>>; struct Configuration { Configuration() noexcept {} - int32_t getNodeFeatureLength(); LaneletRepresentationType reprType{LaneletRepresentationType::Boundaries}; ParametrizationType paramType{ParametrizationType::Polyline}; double submapAreaX{100}; double submapAreaY{100}; int nPoints{11}; - int noRelTypes{7}; - int noBdTypes{3}; }; MapGraphDataInterface(LaneletMapConstPtr laneletMap, Configuration config = Configuration(), @@ -59,9 +58,17 @@ class MapGraphDataInterface { TensorGraphDataLaneTE laneTETensorsBatch(const BasicPoints2d& pts); private: + TensorGraphDataLaneLane getLaneLaneData(MapGraphConstPtr localSubmapGraph); + + TensorGraphDataLaneTE getLaneTEData(MapGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, + std::unordered_map& teId2Index); + LaneletMapConstPtr laneletMap_; LaneletSubmapConstPtr localSubmap_; + std::unordered_map teId2Index_; MapGraphConstPtr localSubmapGraph_; + FeatureBuffer nodeFeatureBuffer_; + FeatureBuffer teFeatureBuffer_; Optional currPos_; // in the map frame Configuration config_; traffic_rules::TrafficRulesPtr trafficRules_; diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h b/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h index 6037e337..8ccf9a70 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h @@ -179,16 +179,16 @@ class Graph { } } - std::unordered_map getKey2Index(bool recompute = false) { - if (recompute || !key2index_) { - std::unordered_map key2index; + std::unordered_map getllId2Index(bool recompute = false) { + if (recompute || !llId2index_) { + std::unordered_map llId2index; int i = 0; for (const auto& entry : laneletOrAreaToVertex_) { - key2index[entry.first] = i++; + llId2index[entry.first.id()] = i++; } - key2index_ = key2index; + llId2index_ = llId2index; } - return *key2index_; + return *llId2index_; } private: @@ -196,7 +196,7 @@ class Graph { return FilteredGraph(graph_, Filter(graph_, relations)); } BaseGraphT graph_; //!< The actual graph object - Optional> key2index_; + Optional> llId2index_; LaneletOrAreaToVertex laneletOrAreaToVertex_; //!< Mapping of lanelets/areas to vertices of the graph }; diff --git a/lanelet2_map_learning/src/MapGraphDataInterface.cpp b/lanelet2_map_learning/src/MapGraphDataInterface.cpp index e5156301..ff8585e5 100644 --- a/lanelet2_map_learning/src/MapGraphDataInterface.cpp +++ b/lanelet2_map_learning/src/MapGraphDataInterface.cpp @@ -3,6 +3,7 @@ #include #include #include +#include ; #include #include @@ -34,13 +35,12 @@ MapGraphDataInterface::MapGraphDataInterface(LaneletMapConstPtr laneletMap, Conf trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} int32_t getNodeFeatureLength(const LaneletRepresentationType& reprType, const ParametrizationType& paramType, - int nPoints, int noBdTypes) { + int nPoints) { int32_t nodeFeatureLength; if (reprType == LaneletRepresentationType::Boundaries) - nodeFeatureLength = - 2 * nPoints + 2 * noBdTypes; // 2 boundary types with 2 possible values and unknown types (one-hot encoding) + nodeFeatureLength = 2 * 3 * nPoints + 2; // 2 boundary types with 2 possible types else if (reprType == LaneletRepresentationType::Centerline) - nodeFeatureLength = 1 * nPoints; + nodeFeatureLength = 3 * nPoints + 2; else throw std::runtime_error("Unknown LaneletRepresentationType!"); @@ -82,100 +82,137 @@ Eigen::Vector3d getPolylineRepr(const ConstLanelet& ll, const LaneletRepresentat } } -inline int linestringSubtypeToInt(ConstLineString3d lString) { +inline int bdSubtypeToInt(ConstLineString3d lString) { std::string subtype = lString.attribute(AttributeName::Subtype).value(); if (subtype == AttributeValueString::Dashed) - return 0; - else if (subtype == AttributeValueString::Solid) return 1; + else if (subtype == AttributeValueString::Solid) + return 2; else if (subtype == AttributeValueString::SolidSolid) - return 1; + return 2; else if (subtype == AttributeValueString::SolidDashed) - return 1; + return 2; else if (subtype == AttributeValueString::DashedSolid) - return 1; + return 2; else { throw std::runtime_error("Unexpected Line String Subtype!"); - return 1; + return 0; } } Eigen::Vector3d getNodeFeatureVec(const ConstLanelet& ll, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints, int nodeFeatLength, - int noBdTypes) { + const ParametrizationType& paramType, int nPoints, int nodeFeatLength) { Eigen::Vector3d featureVec(nodeFeatLength); Eigen::Vector3d polylineRepr = getPolylineRepr(ll, reprType, paramType, nPoints); featureVec(Eigen::seq(0, polylineRepr.size() - 1)) = polylineRepr; - - Eigen::Vector3d typeFeatureVecLeft(noBdTypes); - typeFeatureVecLeft.setZero(); - typeFeatureVecLeft[linestringSubtypeToInt(ll.leftBound3d())] = 1; - - Eigen::Vector3d typeFeatureVecRight(noBdTypes); - typeFeatureVecRight.setZero(); - typeFeatureVecRight[linestringSubtypeToInt(ll.rightBound3d())] = 1; - - featureVec(Eigen::seq(polylineRepr.size(), noBdTypes - 1)) = typeFeatureVecLeft; - featureVec(Eigen::seq(polylineRepr.size() + noBdTypes, polylineRepr.size() + 2 * noBdTypes - 1)) = - typeFeatureVecRight; - + featureVec[polylineRepr.size()] = bdSubtypeToInt(ll.leftBound3d()); + featureVec[polylineRepr.size() + 1] = bdSubtypeToInt(ll.rightBound3d()); return featureVec; } -TensorGraphDataLaneLane getLaneLaneData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints, int noRelTypes = 7, - int noBdTypes = 3) { +TensorGraphDataLaneLane MapGraphDataInterface::getLaneLaneData(MapGraphConstPtr localSubmapGraph) { const auto& graph = localSubmapGraph->graph_; const auto& llVertices = graph->vertexLookup(); TensorGraphDataLaneLane result; int numNodes = llVertices.size(); - int32_t nodeFeatLength = getNodeFeatureLength(reprType, paramType, nPoints, noBdTypes); + int32_t nodeFeatLength = getNodeFeatureLength(config_.reprType, config_.paramType, config_.nPoints); result.x.resize(numNodes, nodeFeatLength); - std::unordered_map key2index = graph->getKey2Index(); + std::unordered_map llId2Index = graph->getllId2Index(); int32_t edgeCount = 0; for (const auto& laWithVertex : llVertices) { const auto& la = laWithVertex.first; auto ll = laWithVertex.first.lanelet(); const auto& vertex = laWithVertex.second; - result.x.row(key2index[la]) = getNodeFeatureVec(*ll, reprType, paramType, nPoints, nodeFeatLength, noBdTypes); + + if (nodeFeatureBuffer_.find(la.id()) != nodeFeatureBuffer_.end()) { + result.x.row(llId2Index[la.id()]) = nodeFeatureBuffer_[la.id()]; + } else { + Eigen::Vector3d nodeFeatureVec = + getNodeFeatureVec(*ll, config_.reprType, config_.paramType, config_.nPoints, nodeFeatLength); + nodeFeatureBuffer_[la.id()] = nodeFeatureVec; + result.x.row(llId2Index[la.id()]) = nodeFeatureVec; + } ConstLaneletOrAreas connectedLLs = localSubmapGraph->getLaneletEdges(*ll); for (const auto& connectedLL : connectedLLs) { result.a.resize(edgeCount + 1, 2); - result.a(edgeCount, 0) = key2index[la]; - result.a(edgeCount, 0) = key2index[connectedLL]; + result.a(edgeCount, 0) = llId2Index[la.id()]; + result.a(edgeCount, 1) = llId2Index[connectedLL.id()]; - result.e.resize(edgeCount + 1, noRelTypes); - Eigen::Vector3d edgeFeatureVec(noRelTypes); - edgeFeatureVec.setZero(); + result.e.resize(edgeCount + 1, 1); ConstLanelet connectedLLasLL = connectedLL.lanelet().get(); RelationType edgeType = graph->getEdgeInfo(*ll, connectedLLasLL).get().relation; - edgeFeatureVec[relationToInt(edgeType)] = 1; - result.e.row(edgeCount) = edgeFeatureVec; + result.e.row(edgeCount).array() = relationToInt(edgeType); edgeCount++; } } return result; } -TensorGraphDataLaneTE getLaneTEData(MapGraphConstPtr localSubmapGraph, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints, int noRelTypes = 7, - int noBdTypes = 3) { +inline int teTypeToInt(const ConstLineString3d& te) { + std::string type = te.attribute(AttributeName::Type).value(); + std::string subtype = te.attribute(AttributeName::Subtype).value(); + if (type == AttributeValueString::TrafficLight) { + return 1; + } else if (type == AttributeValueString::TrafficSign) { + return 2; + } else { + throw std::runtime_error("Unexpected Traffic Element Type!"); + return 0; + } +} + +Eigen::Vector3d getTEPolylineRepr(const BasicLineString3d& te) { + Eigen::Vector3d repr(12); + for (size_t i = 0; i < te.size(); i++) { + repr(Eigen::seq(i, i + 2)) = te[i](Eigen::seq(i, i + 2)); + } + return repr; +} + +Eigen::Vector3d getTEFeatureVec(const ConstLineString3d& te) { + if (te.size() != 4) { + throw std::runtime_error("Number of points in traffic element is not 4!"); + } + Eigen::Vector3d featureVec(13); // 4 points with 3 dims + type + Eigen::Vector3d polylineRepr = getTEPolylineRepr(te.basicLineString()); + featureVec(Eigen::seq(0, polylineRepr.size() - 1)) = polylineRepr; + featureVec[polylineRepr.size()] = teTypeToInt(te); + return featureVec; +} + +bool isTe(ConstLineString3d ls) { + std::string type = ls.attribute(AttributeName::Type).value(); + return type == AttributeValueString::TrafficLight || type == AttributeValueString::TrafficSign; +} + +TensorGraphDataLaneTE MapGraphDataInterface::getLaneTEData(MapGraphConstPtr localSubmapGraph, + LaneletSubmapConstPtr localSubmap, + std::unordered_map& teId2Index) { const auto& graph = localSubmapGraph->graph_; const auto& llVertices = graph->vertexLookup(); TensorGraphDataLaneTE result; int numNodesLane = llVertices.size(); - int32_t nodeFeatLengthLane = getNodeFeatureLength(reprType, paramType, nPoints, noBdTypes); + int32_t nodeFeatLengthLane = getNodeFeatureLength(config_.reprType, config_.paramType, config_.nPoints); result.xLane.resize(numNodesLane, nodeFeatLengthLane); - std::unordered_map key2index = graph->getKey2Index(); + std::unordered_map llId2Index = graph->getllId2Index(); + + std::unordered_map trafficElems; + for (const auto& ls : localSubmap->lineStringLayer) { + if (isTe(ls)) { + trafficElems[ls.id()] = ls; + } + } + + teId2Index.clear(); int i = 0; - for (const auto& entry : llVertices) { - key2index[entry.first] = i++; + for (const auto& entry : trafficElems) { + teId2Index[entry.second.id()] = i++; } int32_t edgeCount = 0; @@ -183,23 +220,39 @@ TensorGraphDataLaneTE getLaneTEData(MapGraphConstPtr localSubmapGraph, const Lan const auto& la = laWithVertex.first; auto ll = laWithVertex.first.lanelet(); const auto& vertex = laWithVertex.second; - result.xLane.row(key2index[la]) = - getNodeFeatureVec(*ll, reprType, paramType, nPoints, nodeFeatLengthLane, noBdTypes); - ConstLaneletOrAreas connectedLLs = localSubmapGraph->getLaneletEdges(*ll); - for (const auto& connectedLL : connectedLLs) { - result.a.resize(edgeCount + 1, 2); - result.a(edgeCount, 0) = key2index[la]; - result.a(edgeCount, 0) = key2index[connectedLL]; + if (nodeFeatureBuffer_.find(la.id()) != nodeFeatureBuffer_.end()) { + result.xLane.row(llId2Index[la.id()]) = nodeFeatureBuffer_[la.id()]; + } else { + Eigen::Vector3d nodeFeatureVec = + getNodeFeatureVec(*ll, config_.reprType, config_.paramType, config_.nPoints, nodeFeatLengthLane); + nodeFeatureBuffer_[la.id()] = nodeFeatureVec; + result.xLane.row(llId2Index[la.id()]) = nodeFeatureVec; + } - result.e.resize(edgeCount + 1, noRelTypes); - Eigen::Vector3d edgeFeatureVec(noRelTypes); - edgeFeatureVec.setZero(); - ConstLanelet connectedLLasLL = connectedLL.lanelet().get(); - RelationType edgeType = graph->getEdgeInfo(*ll, connectedLLasLL).get().relation; - edgeFeatureVec[relationToInt(edgeType)] = 1; - result.e.row(edgeCount) = edgeFeatureVec; - edgeCount++; + RegulatoryElementConstPtrs regElems = ll->regulatoryElements(); + for (const auto& regElem : regElems) { + ConstLineStrings3d refs = regElem->getParameters(RoleName::Refers); + for (const auto& ref : refs) { + if (isTe(ref)) { + result.a.resize(edgeCount + 1, 2); + result.a(edgeCount, 0) = llId2Index[la.id()]; + result.a(edgeCount, 1) = result.xLane.rows() + teId2Index[ref.id()]; + + result.xTE.resize(edgeCount + 1, 13); + if (teFeatureBuffer_.find(ref.id()) != teFeatureBuffer_.end()) { + result.xTE.row(llId2Index[ref.id()]) = teFeatureBuffer_[ref.id()]; + } else { + Eigen::Vector3d teFeatureVec = getTEFeatureVec(ref); + teFeatureBuffer_[ref.id()] = teFeatureVec; + result.xTE.row(llId2Index[ref.id()]) = teFeatureVec; + } + + result.e.resize(edgeCount + 1, 1); + result.e.row(edgeCount).array() = 1; + edgeCount++; + } + } } } return result; @@ -211,8 +264,7 @@ TensorGraphDataLaneLane MapGraphDataInterface::laneLaneTensors() { "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); } - return getLaneLaneData(localSubmapGraph_, config_.reprType, config_.paramType, config_.nPoints, config_.noRelTypes, - config_.noBdTypes); + return getLaneLaneData(localSubmapGraph_); } TensorGraphDataLaneTE MapGraphDataInterface::laneTETensors() { From d3e3f3407661271525ccb95ff417c5902a030d38 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Mon, 30 Oct 2023 20:32:27 +0100 Subject: [PATCH 11/64] wip partial rewrite new concept --- .../include/lanelet2_map_learning/Forward.h | 4 +- .../MapGraphDataInterface.h | 46 ++---- .../include/lanelet2_map_learning/Types.h | 40 +++++ .../include/lanelet2_map_learning/Utils.h | 31 ++++ .../src/MapGraphDataInterface.cpp | 153 +++++------------- lanelet2_map_learning/src/Types.cpp | 19 +++ lanelet2_map_learning/src/Utils.cpp | 86 ++++++++++ 7 files changed, 234 insertions(+), 145 deletions(-) create mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/Utils.h create mode 100644 lanelet2_map_learning/src/Types.cpp create mode 100644 lanelet2_map_learning/src/Utils.cpp diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h index 57df3928..f1f410b7 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h @@ -131,8 +131,8 @@ inline std::string relationToColor(RelationType type) { enum class LaneletRepresentationType; enum class ParametrizationType; -struct TensorGraphDataLaneLane; -struct TensorGraphDataLaneTE; +struct TensorLaneData; +struct TensorTEData; class MapGraphDataInterface; } // namespace map_learning diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h index 0915e906..6488e474 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h @@ -12,38 +12,17 @@ class LaneletLayer; namespace map_learning { -enum class LaneletRepresentationType { Centerline, Boundaries }; -enum class ParametrizationType { Bezier, BezierEndpointFixed, Polyline }; - -struct TensorGraphDataLaneLane { - TensorGraphDataLaneLane() noexcept {} - Eigen::MatrixXd x; // node features - Eigen::MatrixX2i a; // adjacency matrix (sparse) - Eigen::MatrixXd e; // edge features -}; - -struct TensorGraphDataLaneTE { - TensorGraphDataLaneTE() noexcept {} - Eigen::MatrixXd xLane; // node features lanelets - Eigen::MatrixXd xTE; // node features traffic elements - - /// @brief adjacency matrix (sparse). Node indices are assigned as - /// xLane and xTE stacked, with xLane being first - Eigen::MatrixX2i a; - - Eigen::MatrixXd e; // edge features -}; - class MapGraphDataInterface { public: - using FeatureBuffer = std::unordered_map, std::equal_to, - Eigen::aligned_allocator>>; + using FeatureBuffer = std::unordered_map, std::equal_to, + Eigen::aligned_allocator>>; + struct Configuration { Configuration() noexcept {} LaneletRepresentationType reprType{LaneletRepresentationType::Boundaries}; ParametrizationType paramType{ParametrizationType::Polyline}; - double submapAreaX{100}; - double submapAreaY{100}; + double submapAreaLongitudinal{30}; // in driving direction + double submapAreaLateral{15}; // in lateral direction int nPoints{11}; }; @@ -51,17 +30,17 @@ class MapGraphDataInterface { Optional currPos = boost::none); void setCurrPosAndExtractSubmap(const BasicPoint2d& pt); - TensorGraphDataLaneLane laneLaneTensors(); - TensorGraphDataLaneTE laneTETensors(); + TensorLaneData laneLaneTensors(); + TensorTEData laneTETensors(); - TensorGraphDataLaneLane laneLaneTensorsBatch(const BasicPoints2d& pts); - TensorGraphDataLaneTE laneTETensorsBatch(const BasicPoints2d& pts); + TensorLaneData laneLaneTensorsBatch(const BasicPoints2d& pts, const std::vector& yaws); + TensorTEData laneTETensorsBatch(const BasicPoints2d& pts, const std::vector& yaws); private: - TensorGraphDataLaneLane getLaneLaneData(MapGraphConstPtr localSubmapGraph); + TensorLaneData getLaneLaneData(MapGraphConstPtr localSubmapGraph); - TensorGraphDataLaneTE getLaneTEData(MapGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, - std::unordered_map& teId2Index); + TensorTEData getLaneTEData(MapGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, + std::unordered_map& teId2Index); LaneletMapConstPtr laneletMap_; LaneletSubmapConstPtr localSubmap_; @@ -70,6 +49,7 @@ class MapGraphDataInterface { FeatureBuffer nodeFeatureBuffer_; FeatureBuffer teFeatureBuffer_; Optional currPos_; // in the map frame + Optional currYaw_; // in the map frame Configuration config_; traffic_rules::TrafficRulesPtr trafficRules_; }; diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h index a938f5bb..e704bb9a 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h @@ -9,6 +9,46 @@ namespace lanelet { namespace map_learning { +enum class LaneletRepresentationType { Centerline, Boundaries }; +enum class ParametrizationType { Bezier, BezierEndpointFixed, Polyline }; + +struct PolylineFeature { + BasicLineString3d originalFeature_; + BasicLineString3d processedFeature_; + Id mapID_; + bool wasCut_{false}; + int subtype_; + + PolylineFeature(BasicLineString3d feature, Id mapID) : originalFeature_{feature} {} + + void computePolyline(int32_t nPoints); + Eigen::Vector3d computeFeatureVector +}; + +using PolylineFeatures = std::vector; + +struct TensorLaneData { + TensorLaneData() noexcept {} + MapFeatures roadBorders; // auxilliary features + MapFeatures laneDividers; // auxilliary features + + MapFeatures laneletFeatures; // node features + Eigen::MatrixX2i edgeList; // adjacency matrix (sparse) + Eigen::MatrixXd edgeFeatures; // edge features +}; + +struct TensorTEData { + TensorTEData() noexcept {} + MapFeatures laneletFeatures; // node features lanelets + MapFeatures teFeatures; // node features traffic elements + + /// @brief adjacency matrix (sparse). Node indices are assigned as + /// xLane and xTE stacked, with xLane being first + Eigen::MatrixX2i edgeList; + + Eigen::MatrixXd edgeFeatures; // edge features +}; + //! Represents the relation of a lanelet to another lanelet struct LaneletRelation { ConstLanelet lanelet; //!< the lanelet this relation refers to diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h new file mode 100644 index 00000000..313ab6a5 --- /dev/null +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h @@ -0,0 +1,31 @@ +#include +#include +#include +#include ; +#include + +#include +#include + +#include "lanelet2_map_learning/Forward.h" + +namespace lanelet { +namespace map_learning { + +boost::geometry::model::polygon getRotatedRect(const BasicPoint2d& center, double extentLongitudinal, + double extentLateral, double yaw); + +LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPoint2d& center, double yaw, + double extentLongitudinal, double extentLateral); + +int32_t getNodeFeatureLength(const LaneletRepresentationType& reprType, const ParametrizationType& paramType, + int nPoints); + +inline int bdSubtypeToInt(ConstLineString3d lString); + +inline int teTypeToInt(const ConstLineString3d& te); + +Eigen::Vector3d getTEPolylineRepr(const BasicLineString3d& te); + +} // namespace map_learning +} // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapGraphDataInterface.cpp b/lanelet2_map_learning/src/MapGraphDataInterface.cpp index ff8585e5..cabcc869 100644 --- a/lanelet2_map_learning/src/MapGraphDataInterface.cpp +++ b/lanelet2_map_learning/src/MapGraphDataInterface.cpp @@ -9,21 +9,15 @@ #include #include +#include "lanelet2_map_learning/Utils.h" + namespace lanelet { namespace map_learning { -LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPoint2d& center, double extentX, - double extentY) { - BasicPoint2d regionRear = {center.x() - extentX, center.y() - extentY}; - BasicPoint2d regionFront = {center.x() + extentX, center.y() + extentY}; - BoundingBox2d searchRegion{regionRear, regionFront}; - ConstLanelets inRegion = laneletMap->laneletLayer.search(searchRegion); - return utils::createConstSubmap(inRegion, {}); -} - void MapGraphDataInterface::setCurrPosAndExtractSubmap(const BasicPoint2d& pt) { currPos_ = pt; - localSubmap_ = extractSubmap(laneletMap_, *currPos_, config_.submapAreaX, config_.submapAreaY); + localSubmap_ = + extractSubmap(laneletMap_, *currPos_, *currYaw_, config_.submapAreaLongitudinal, config_.submapAreaLateral); localSubmapGraph_ = MapGraph::build(*laneletMap_, *trafficRules_); } @@ -34,41 +28,16 @@ MapGraphDataInterface::MapGraphDataInterface(LaneletMapConstPtr laneletMap, Conf currPos_{currPos}, trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} -int32_t getNodeFeatureLength(const LaneletRepresentationType& reprType, const ParametrizationType& paramType, - int nPoints) { - int32_t nodeFeatureLength; - if (reprType == LaneletRepresentationType::Boundaries) - nodeFeatureLength = 2 * 3 * nPoints + 2; // 2 boundary types with 2 possible types - else if (reprType == LaneletRepresentationType::Centerline) - nodeFeatureLength = 3 * nPoints + 2; - else - throw std::runtime_error("Unknown LaneletRepresentationType!"); - - return nodeFeatureLength; -} - -Eigen::Vector3d computePolylineRepr(const BasicLineString3d& lstr, int32_t nPoints) { - Eigen::Vector3d repr(3 * nPoints); - double length = boost::geometry::length(lstr, boost::geometry::strategy::distance::pythagoras()); - double dist = length / static_cast(nPoints); - boost::geometry::model::multi_point bdInterp; - boost::geometry::line_interpolate(lstr, dist, bdInterp); - assert(bdInterp.size() == nPoints); - for (size_t i = 0; i < bdInterp.size(); i++) { - repr(Eigen::seq(i, i + 2)) = bdInterp[i](Eigen::seq(i, i + 2)); - } - return repr; -} - -Eigen::Vector3d getPolylineRepr(const ConstLanelet& ll, const LaneletRepresentationType& reprType, +PolylineFeature getPolylineRepr(const ConstLanelet& ll, const LaneletRepresentationType& reprType, const ParametrizationType& paramType, int nPoints) { if (paramType != ParametrizationType::Polyline) { throw std::runtime_error("Only polyline parametrization is implemented so far!"); } if (reprType == LaneletRepresentationType::Centerline) { Eigen::Vector3d repr(3 * nPoints); - BasicLineString3d centerline = ll.centerline3d().basicLineString(); - return computePolylineRepr(centerline, nPoints); + PolylineFeature centerlineFeat(ll.centerline3d().basicLineString(), ll.id()); + centerlineFeat.computePolyline(nPoints); + return centerlineFeat; } else if (reprType == LaneletRepresentationType::Boundaries) { Eigen::Vector3d repr(2 * 3 * nPoints); std::vector boundaries{ll.leftBound3d().basicLineString(), ll.rightBound3d().basicLineString()}; @@ -82,42 +51,28 @@ Eigen::Vector3d getPolylineRepr(const ConstLanelet& ll, const LaneletRepresentat } } -inline int bdSubtypeToInt(ConstLineString3d lString) { - std::string subtype = lString.attribute(AttributeName::Subtype).value(); - if (subtype == AttributeValueString::Dashed) - return 1; - else if (subtype == AttributeValueString::Solid) - return 2; - else if (subtype == AttributeValueString::SolidSolid) - return 2; - else if (subtype == AttributeValueString::SolidDashed) - return 2; - else if (subtype == AttributeValueString::DashedSolid) - return 2; - else { - throw std::runtime_error("Unexpected Line String Subtype!"); - return 0; - } -} - -Eigen::Vector3d getNodeFeatureVec(const ConstLanelet& ll, const LaneletRepresentationType& reprType, +PolylineFeature getLaneletFeature(const ConstLanelet& ll, const LaneletRepresentationType& reprType, const ParametrizationType& paramType, int nPoints, int nodeFeatLength) { Eigen::Vector3d featureVec(nodeFeatLength); - Eigen::Vector3d polylineRepr = getPolylineRepr(ll, reprType, paramType, nPoints); + Eigen::Vector3d polylineRepr(3 * nPoints); + PolylineFeature mapFeat = getPolylineRepr(ll, reprType, paramType, nPoints); + for (size_t i = 0; i < mapFeat.processedFeature_.size(); i++) { + polylineRepr(Eigen::seq(i, i + 2)) = mapFeat.processedFeature_[i](Eigen::seq(i, i + 2)); + } featureVec(Eigen::seq(0, polylineRepr.size() - 1)) = polylineRepr; featureVec[polylineRepr.size()] = bdSubtypeToInt(ll.leftBound3d()); featureVec[polylineRepr.size() + 1] = bdSubtypeToInt(ll.rightBound3d()); return featureVec; } -TensorGraphDataLaneLane MapGraphDataInterface::getLaneLaneData(MapGraphConstPtr localSubmapGraph) { +TensorLaneData MapGraphDataInterface::getLaneLaneData(MapGraphConstPtr localSubmapGraph) { const auto& graph = localSubmapGraph->graph_; const auto& llVertices = graph->vertexLookup(); - TensorGraphDataLaneLane result; + TensorLaneData result; int numNodes = llVertices.size(); int32_t nodeFeatLength = getNodeFeatureLength(config_.reprType, config_.paramType, config_.nPoints); - result.x.resize(numNodes, nodeFeatLength); + result.laneletFeatures.resize(numNodes); std::unordered_map llId2Index = graph->getllId2Index(); @@ -128,51 +83,30 @@ TensorGraphDataLaneLane MapGraphDataInterface::getLaneLaneData(MapGraphConstPtr const auto& vertex = laWithVertex.second; if (nodeFeatureBuffer_.find(la.id()) != nodeFeatureBuffer_.end()) { - result.x.row(llId2Index[la.id()]) = nodeFeatureBuffer_[la.id()]; + result.laneletFeatures[llId2Index[la.id()]] = nodeFeatureBuffer_[la.id()]; } else { - Eigen::Vector3d nodeFeatureVec = - getNodeFeatureVec(*ll, config_.reprType, config_.paramType, config_.nPoints, nodeFeatLength); + MapFeature nodeFeatureVec = + getMapFeature(*ll, config_.reprType, config_.paramType, config_.nPoints, nodeFeatLength); nodeFeatureBuffer_[la.id()] = nodeFeatureVec; - result.x.row(llId2Index[la.id()]) = nodeFeatureVec; + result.laneletFeatures[llId2Index[la.id()]] = nodeFeatureVec; } ConstLaneletOrAreas connectedLLs = localSubmapGraph->getLaneletEdges(*ll); for (const auto& connectedLL : connectedLLs) { - result.a.resize(edgeCount + 1, 2); - result.a(edgeCount, 0) = llId2Index[la.id()]; - result.a(edgeCount, 1) = llId2Index[connectedLL.id()]; + result.edgeList.resize(edgeCount + 1, 2); + result.edgeList(edgeCount, 0) = llId2Index[la.id()]; + result.edgeList(edgeCount, 1) = llId2Index[connectedLL.id()]; - result.e.resize(edgeCount + 1, 1); + result.edgeList.resize(edgeCount + 1, 1); ConstLanelet connectedLLasLL = connectedLL.lanelet().get(); RelationType edgeType = graph->getEdgeInfo(*ll, connectedLLasLL).get().relation; - result.e.row(edgeCount).array() = relationToInt(edgeType); + result.edgeList.row(edgeCount).array() = relationToInt(edgeType); edgeCount++; } } return result; } -inline int teTypeToInt(const ConstLineString3d& te) { - std::string type = te.attribute(AttributeName::Type).value(); - std::string subtype = te.attribute(AttributeName::Subtype).value(); - if (type == AttributeValueString::TrafficLight) { - return 1; - } else if (type == AttributeValueString::TrafficSign) { - return 2; - } else { - throw std::runtime_error("Unexpected Traffic Element Type!"); - return 0; - } -} - -Eigen::Vector3d getTEPolylineRepr(const BasicLineString3d& te) { - Eigen::Vector3d repr(12); - for (size_t i = 0; i < te.size(); i++) { - repr(Eigen::seq(i, i + 2)) = te[i](Eigen::seq(i, i + 2)); - } - return repr; -} - Eigen::Vector3d getTEFeatureVec(const ConstLineString3d& te) { if (te.size() != 4) { throw std::runtime_error("Number of points in traffic element is not 4!"); @@ -189,16 +123,15 @@ bool isTe(ConstLineString3d ls) { return type == AttributeValueString::TrafficLight || type == AttributeValueString::TrafficSign; } -TensorGraphDataLaneTE MapGraphDataInterface::getLaneTEData(MapGraphConstPtr localSubmapGraph, - LaneletSubmapConstPtr localSubmap, - std::unordered_map& teId2Index) { +TensorTEData MapGraphDataInterface::getLaneTEData(MapGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, + std::unordered_map& teId2Index) { const auto& graph = localSubmapGraph->graph_; const auto& llVertices = graph->vertexLookup(); - TensorGraphDataLaneTE result; + TensorTEData result; int numNodesLane = llVertices.size(); int32_t nodeFeatLengthLane = getNodeFeatureLength(config_.reprType, config_.paramType, config_.nPoints); - result.xLane.resize(numNodesLane, nodeFeatLengthLane); + result.laneletFeatures.resize(numNodesLane, nodeFeatLengthLane); std::unordered_map llId2Index = graph->getllId2Index(); @@ -222,12 +155,12 @@ TensorGraphDataLaneTE MapGraphDataInterface::getLaneTEData(MapGraphConstPtr loca const auto& vertex = laWithVertex.second; if (nodeFeatureBuffer_.find(la.id()) != nodeFeatureBuffer_.end()) { - result.xLane.row(llId2Index[la.id()]) = nodeFeatureBuffer_[la.id()]; + result.laneletFeatures.row(llId2Index[la.id()]) = nodeFeatureBuffer_[la.id()]; } else { Eigen::Vector3d nodeFeatureVec = getNodeFeatureVec(*ll, config_.reprType, config_.paramType, config_.nPoints, nodeFeatLengthLane); nodeFeatureBuffer_[la.id()] = nodeFeatureVec; - result.xLane.row(llId2Index[la.id()]) = nodeFeatureVec; + result.laneletFeatures.row(llId2Index[la.id()]) = nodeFeatureVec; } RegulatoryElementConstPtrs regElems = ll->regulatoryElements(); @@ -235,21 +168,21 @@ TensorGraphDataLaneTE MapGraphDataInterface::getLaneTEData(MapGraphConstPtr loca ConstLineStrings3d refs = regElem->getParameters(RoleName::Refers); for (const auto& ref : refs) { if (isTe(ref)) { - result.a.resize(edgeCount + 1, 2); - result.a(edgeCount, 0) = llId2Index[la.id()]; - result.a(edgeCount, 1) = result.xLane.rows() + teId2Index[ref.id()]; + result.edgeList.resize(edgeCount + 1, 2); + result.edgeList(edgeCount, 0) = llId2Index[la.id()]; + result.edgeList(edgeCount, 1) = result.laneletFeatures.rows() + teId2Index[ref.id()]; - result.xTE.resize(edgeCount + 1, 13); + result.teFeatures.resize(edgeCount + 1, 13); if (teFeatureBuffer_.find(ref.id()) != teFeatureBuffer_.end()) { - result.xTE.row(llId2Index[ref.id()]) = teFeatureBuffer_[ref.id()]; + result.teFeatures.row(llId2Index[ref.id()]) = teFeatureBuffer_[ref.id()]; } else { Eigen::Vector3d teFeatureVec = getTEFeatureVec(ref); teFeatureBuffer_[ref.id()] = teFeatureVec; - result.xTE.row(llId2Index[ref.id()]) = teFeatureVec; + result.teFeatures.row(llId2Index[ref.id()]) = teFeatureVec; } - result.e.resize(edgeCount + 1, 1); - result.e.row(edgeCount).array() = 1; + result.edgeFeatures.resize(edgeCount + 1, 1); + result.edgeFeatures.row(edgeCount).array() = 1; edgeCount++; } } @@ -258,7 +191,7 @@ TensorGraphDataLaneTE MapGraphDataInterface::getLaneTEData(MapGraphConstPtr loca return result; } -TensorGraphDataLaneLane MapGraphDataInterface::laneLaneTensors() { +TensorLaneData MapGraphDataInterface::laneLaneTensors() { if (!currPos_) { throw InvalidObjectStateError( "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); @@ -267,12 +200,12 @@ TensorGraphDataLaneLane MapGraphDataInterface::laneLaneTensors() { return getLaneLaneData(localSubmapGraph_); } -TensorGraphDataLaneTE MapGraphDataInterface::laneTETensors() { +TensorTEData MapGraphDataInterface::laneTETensors() { if (!currPos_) { throw InvalidObjectStateError( "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); } - return TensorGraphDataLaneTE(); + return TensorTEData(); } } // namespace map_learning diff --git a/lanelet2_map_learning/src/Types.cpp b/lanelet2_map_learning/src/Types.cpp new file mode 100644 index 00000000..f96125c9 --- /dev/null +++ b/lanelet2_map_learning/src/Types.cpp @@ -0,0 +1,19 @@ +#include "lanelet2_map_learning/Types.h" + +#include +#include + +namespace lanelet { +namespace map_learning { + +void MapFeature::computePolyline(int32_t nPoints) { + double length = boost::geometry::length(originalFeature_, boost::geometry::strategy::distance::pythagoras()); + double dist = length / static_cast(nPoints); + boost::geometry::model::multi_point bdInterp; + boost::geometry::line_interpolate(originalFeature_, dist, bdInterp); + assert(bdInterp.size() == nPoints); + processedFeature_ = bdInterp; +} + +} // namespace map_learning +} // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp new file mode 100644 index 00000000..833071f8 --- /dev/null +++ b/lanelet2_map_learning/src/Utils.cpp @@ -0,0 +1,86 @@ +#include "lanelet2_map_learning/Utils.h" + +namespace lanelet { +namespace map_learning { + +boost::geometry::model::polygon getRotatedRect(const BasicPoint2d& center, double extentLongitudinal, + double extentLateral, double yaw) { + BasicPoints2d pts{BasicPoint2d{center.x() - extentLongitudinal, center.y() - extentLateral}, + BasicPoint2d{center.x() - extentLongitudinal, center.y() + extentLateral}, + BasicPoint2d{center.x() + extentLongitudinal, center.y() + extentLateral}, + BasicPoint2d{center.x() + extentLongitudinal, center.y() - extentLateral}}; + boost::geometry::model::polygon axisAlignedRect; + boost::geometry::assign_points(axisAlignedRect, pts); + + boost::geometry::strategy::transform::matrix_transformer trans( + cos(yaw), sin(yaw), center.x(), -sin(yaw), cos(yaw), center.y(), 0, 0, 1); + boost::geometry::model::polygon rotatedRect; + boost::geometry::transform(axisAlignedRect, rotatedRect, trans); + return rotatedRect; +} + +LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPoint2d& center, double yaw, + double extentLongitudinal, double extentLateral) { + double maxExtent = std::max(extentLongitudinal, extentLateral); + BasicPoint2d initRegionRear = {center.x() - 1.1 * maxExtent, center.y() - 1.1 * maxExtent}; + BasicPoint2d initRegionFront = {center.x() + 1.1 * maxExtent, center.y() + 1.1 * maxExtent}; + BoundingBox2d initSearchRegion{initRegionRear, initRegionFront}; + ConstLanelets initRegion = laneletMap->laneletLayer.search(initSearchRegion); + + return utils::createConstSubmap(initRegion, {}); +} + +int32_t getNodeFeatureLength(const LaneletRepresentationType& reprType, const ParametrizationType& paramType, + int nPoints) { + int32_t nodeFeatureLength; + if (reprType == LaneletRepresentationType::Boundaries) + nodeFeatureLength = 2 * 3 * nPoints + 2; // 2 boundary types with 2 possible types + else if (reprType == LaneletRepresentationType::Centerline) + nodeFeatureLength = 3 * nPoints + 2; + else + throw std::runtime_error("Unknown LaneletRepresentationType!"); + + return nodeFeatureLength; +} + +inline int bdSubtypeToInt(ConstLineString3d lString) { + std::string subtype = lString.attribute(AttributeName::Subtype).value(); + if (subtype == AttributeValueString::Dashed) + return 1; + else if (subtype == AttributeValueString::Solid) + return 2; + else if (subtype == AttributeValueString::SolidSolid) + return 2; + else if (subtype == AttributeValueString::SolidDashed) + return 2; + else if (subtype == AttributeValueString::DashedSolid) + return 2; + else { + throw std::runtime_error("Unexpected Line String Subtype!"); + return 0; + } +} + +inline int teTypeToInt(const ConstLineString3d& te) { + std::string type = te.attribute(AttributeName::Type).value(); + std::string subtype = te.attribute(AttributeName::Subtype).value(); + if (type == AttributeValueString::TrafficLight) { + return 1; + } else if (type == AttributeValueString::TrafficSign) { + return 2; + } else { + throw std::runtime_error("Unexpected Traffic Element Type!"); + return 0; + } +} + +Eigen::Vector3d getTEPolylineRepr(const BasicLineString3d& te) { + Eigen::Vector3d repr(12); + for (size_t i = 0; i < te.size(); i++) { + repr(Eigen::seq(i, i + 2)) = te[i](Eigen::seq(i, i + 2)); + } + return repr; +} + +} // namespace map_learning +} // namespace lanelet \ No newline at end of file From 8fe4100d1b7b926c5b2623b09991e83234acb55c Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Tue, 31 Oct 2023 21:02:30 +0100 Subject: [PATCH 12/64] wip partial rewrite, continue map feature classes --- .../include/lanelet2_map_learning/Forward.h | 4 +- .../lanelet2_map_learning/MapFeatures.h | 75 +++++++++++++++++++ .../MapGraphDataInterface.h | 24 +++--- .../include/lanelet2_map_learning/Types.h | 39 +--------- .../include/lanelet2_map_learning/Utils.h | 12 ++- lanelet2_map_learning/src/MapFeatures.cpp | 50 +++++++++++++ .../src/MapGraphDataInterface.cpp | 67 +++-------------- lanelet2_map_learning/src/Types.cpp | 19 ----- lanelet2_map_learning/src/Utils.cpp | 45 +++++++---- 9 files changed, 192 insertions(+), 143 deletions(-) create mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h create mode 100644 lanelet2_map_learning/src/MapFeatures.cpp delete mode 100644 lanelet2_map_learning/src/Types.cpp diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h index f1f410b7..bebc584e 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h @@ -131,8 +131,8 @@ inline std::string relationToColor(RelationType type) { enum class LaneletRepresentationType; enum class ParametrizationType; -struct TensorLaneData; -struct TensorTEData; +struct LaneData; +struct TEData; class MapGraphDataInterface; } // namespace map_learning diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h new file mode 100644 index 00000000..440eed8e --- /dev/null +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -0,0 +1,75 @@ +#pragma once +#include +#include +#include + +#include +#include + +#include "lanelet2_map_learning/Forward.h" +#include "lanelet2_map_learning/Types.h" + +namespace lanelet { +namespace map_learning { + +struct PolylineFeature { + BasicLineString3d originalFeature_; + BasicLineString3d processedFeature_; + Id mapID_; + bool wasCut_{false}; + int subtype_; + + ParametrizationType paramType; + + PolylineFeature() {} + PolylineFeature(const BasicLineString3d& feature, Id mapID) : originalFeature_{feature}, mapID_{mapID} {} + + void processPolyline(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints); + Eigen::Vector3d computeFeatureVector(); +}; + +struct LaneletFeature { + PolylineFeature leftBoundary_; + PolylineFeature rightBoundary_; + PolylineFeature centerline_; + Id mapID_; + bool wasCut_{false}; + int bdTypeLeft_; + int bdTypeRight_; + + LaneletFeature() {} + LaneletFeature(const PolylineFeature& leftBoundary, const PolylineFeature& rightBoundary, + const PolylineFeature& centerline, Id mapID); + LaneletFeature(const ConstLanelet& ll); + + void processLanelet(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints); + Eigen::Vector3d computeFeatureVector(const LaneletRepresentationType& reprType); +}; + +using PolylineFeatures = std::vector; +using LaneletFeatures = std::vector; + +struct LaneData { + LaneData() noexcept {} + PolylineFeatures roadBorders; // auxilliary features + PolylineFeatures laneDividers; // auxilliary features + + LaneletFeatures laneletFeatures; // node features + Eigen::MatrixX2i edgeList; // adjacency matrix (sparse) + Eigen::MatrixXd edgeFeatures; // edge features +}; + +struct TEData { + TEData() noexcept {} + LaneletFeatures laneletFeatures; // node features lanelets + PolylineFeatures teFeatures; // node features traffic elements + + /// @brief adjacency matrix (sparse). Node indices are assigned as + /// xLane and xTE stacked, with xLane being first + Eigen::MatrixX2i edgeList; + + Eigen::MatrixXd edgeFeatures; // edge features +}; + +} // namespace map_learning +} // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h index 6488e474..f4743b08 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h @@ -14,8 +14,9 @@ namespace map_learning { class MapGraphDataInterface { public: - using FeatureBuffer = std::unordered_map, std::equal_to, - Eigen::aligned_allocator>>; + template + using FeatureBuffer = + std::unordered_map, std::equal_to, Eigen::aligned_allocator>>; struct Configuration { Configuration() noexcept {} @@ -30,24 +31,25 @@ class MapGraphDataInterface { Optional currPos = boost::none); void setCurrPosAndExtractSubmap(const BasicPoint2d& pt); - TensorLaneData laneLaneTensors(); - TensorTEData laneTETensors(); + LaneData laneData(); + TEData teData(); - TensorLaneData laneLaneTensorsBatch(const BasicPoints2d& pts, const std::vector& yaws); - TensorTEData laneTETensorsBatch(const BasicPoints2d& pts, const std::vector& yaws); + LaneData laneDataBatch(const BasicPoints2d& pts, const std::vector& yaws); + TEData laneTEDataBatch(const BasicPoints2d& pts, const std::vector& yaws); private: - TensorLaneData getLaneLaneData(MapGraphConstPtr localSubmapGraph); + LaneData getLaneData(MapGraphConstPtr localSubmapGraph); - TensorTEData getLaneTEData(MapGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, - std::unordered_map& teId2Index); + TEData getLaneTEData(MapGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, + std::unordered_map& teId2Index); LaneletMapConstPtr laneletMap_; LaneletSubmapConstPtr localSubmap_; std::unordered_map teId2Index_; MapGraphConstPtr localSubmapGraph_; - FeatureBuffer nodeFeatureBuffer_; - FeatureBuffer teFeatureBuffer_; + FeatureBuffer laneletFeatureBuffer_; + FeatureBuffer polylineFeatureBuffer_; + FeatureBuffer teFeatureBuffer_; Optional currPos_; // in the map frame Optional currYaw_; // in the map frame Configuration config_; diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h index e704bb9a..7c96cfef 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h @@ -2,7 +2,9 @@ #include #include +#include #include +#include #include "lanelet2_map_learning/Forward.h" @@ -12,42 +14,7 @@ namespace map_learning { enum class LaneletRepresentationType { Centerline, Boundaries }; enum class ParametrizationType { Bezier, BezierEndpointFixed, Polyline }; -struct PolylineFeature { - BasicLineString3d originalFeature_; - BasicLineString3d processedFeature_; - Id mapID_; - bool wasCut_{false}; - int subtype_; - - PolylineFeature(BasicLineString3d feature, Id mapID) : originalFeature_{feature} {} - - void computePolyline(int32_t nPoints); - Eigen::Vector3d computeFeatureVector -}; - -using PolylineFeatures = std::vector; - -struct TensorLaneData { - TensorLaneData() noexcept {} - MapFeatures roadBorders; // auxilliary features - MapFeatures laneDividers; // auxilliary features - - MapFeatures laneletFeatures; // node features - Eigen::MatrixX2i edgeList; // adjacency matrix (sparse) - Eigen::MatrixXd edgeFeatures; // edge features -}; - -struct TensorTEData { - TensorTEData() noexcept {} - MapFeatures laneletFeatures; // node features lanelets - MapFeatures teFeatures; // node features traffic elements - - /// @brief adjacency matrix (sparse). Node indices are assigned as - /// xLane and xTE stacked, with xLane being first - Eigen::MatrixX2i edgeList; - - Eigen::MatrixXd edgeFeatures; // edge features -}; +using OrientedRect = boost::geometry::model::polygon; //! Represents the relation of a lanelet to another lanelet struct LaneletRelation { diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h index 313ab6a5..deb71892 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h @@ -1,3 +1,4 @@ +#pragma once #include #include #include @@ -8,6 +9,7 @@ #include #include "lanelet2_map_learning/Forward.h" +#include "lanelet2_map_learning/Types.h" namespace lanelet { namespace map_learning { @@ -18,14 +20,18 @@ boost::geometry::model::polygon getRotatedRect(const BasicPoint2d& LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPoint2d& center, double yaw, double extentLongitudinal, double extentLateral); -int32_t getNodeFeatureLength(const LaneletRepresentationType& reprType, const ParametrizationType& paramType, - int nPoints); +Eigen::Vector3d getLaneletRepr(const LaneletRepresentationType& reprType, const ParametrizationType& paramType, + int nPoints); inline int bdSubtypeToInt(ConstLineString3d lString); inline int teTypeToInt(const ConstLineString3d& te); -Eigen::Vector3d getTEPolylineRepr(const BasicLineString3d& te); +Eigen::Vector3d getTERepr(); + +BasicLineString3d resamplePolyline(const BasicLineString3d& polyline, int32_t nPoints); + +BasicLineString3d cutPolyline(const OrientedRect& bbox, const BasicLineString3d& polyline); } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp new file mode 100644 index 00000000..da344392 --- /dev/null +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -0,0 +1,50 @@ +#include "lanelet2_map_learning/MapFeatures.h" + +#include +#include + +#include "lanelet2_map_learning/Utils.h" + +namespace lanelet { +namespace map_learning { + +void PolylineFeature::processPolyline(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { + if (paramType != ParametrizationType::Polyline) { + throw std::runtime_error("Only polyline parametrization is implemented so far!"); + } + processedFeature_ = cutPolyline(bbox, originalFeature_); + processedFeature_ = resamplePolyline(originalFeature_, nPoints); + + double lengthOriginal = + boost::geometry::length(originalFeature_, boost::geometry::strategy::distance::pythagoras()); + double lengthProcessed = + boost::geometry::length(originalFeature_, boost::geometry::strategy::distance::pythagoras()); + + if (lengthOriginal - lengthProcessed > 1e-2) { + wasCut_ = true; + } +} + +LaneletFeature::LaneletFeature(const PolylineFeature& leftBoundary, const PolylineFeature& rightBoundary, + const PolylineFeature& centerline, Id mapID) + : leftBoundary_{leftBoundary}, rightBoundary_{rightBoundary}, centerline_{centerline}, mapID_{mapID} {} + +LaneletFeature::LaneletFeature(const ConstLanelet& ll) + : leftBoundary_{PolylineFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id())}, + rightBoundary_{PolylineFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id())}, + centerline_{PolylineFeature(ll.centerline3d().basicLineString(), ll.centerline3d().id())}, + bdTypeLeft_{bdSubtypeToInt(ll.leftBound3d())}, + bdTypeRight_{bdSubtypeToInt(ll.rightBound3d())} {} + +void LaneletFeature::processLanelet(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { + leftBoundary_.processPolyline(bbox, paramType, nPoints); + rightBoundary_.processPolyline(bbox, paramType, nPoints); + centerline_.processPolyline(bbox, paramType, nPoints); + + if (leftBoundary_.wasCut_ || rightBoundary_.wasCut_ || centerline_.wasCut_) { + wasCut_ = true; + } +} + +} // namespace map_learning +} // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapGraphDataInterface.cpp b/lanelet2_map_learning/src/MapGraphDataInterface.cpp index cabcc869..eef009cc 100644 --- a/lanelet2_map_learning/src/MapGraphDataInterface.cpp +++ b/lanelet2_map_learning/src/MapGraphDataInterface.cpp @@ -9,6 +9,7 @@ #include #include +#include "lanelet2_map_learning/MapFeatures.h" #include "lanelet2_map_learning/Utils.h" namespace lanelet { @@ -28,48 +29,11 @@ MapGraphDataInterface::MapGraphDataInterface(LaneletMapConstPtr laneletMap, Conf currPos_{currPos}, trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} -PolylineFeature getPolylineRepr(const ConstLanelet& ll, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints) { - if (paramType != ParametrizationType::Polyline) { - throw std::runtime_error("Only polyline parametrization is implemented so far!"); - } - if (reprType == LaneletRepresentationType::Centerline) { - Eigen::Vector3d repr(3 * nPoints); - PolylineFeature centerlineFeat(ll.centerline3d().basicLineString(), ll.id()); - centerlineFeat.computePolyline(nPoints); - return centerlineFeat; - } else if (reprType == LaneletRepresentationType::Boundaries) { - Eigen::Vector3d repr(2 * 3 * nPoints); - std::vector boundaries{ll.leftBound3d().basicLineString(), ll.rightBound3d().basicLineString()}; - for (size_t i = 0; i < boundaries.size(); i++) { - repr(Eigen::seq(i * 3 * nPoints, (i + 1) * 3 * nPoints - 1)) = computePolylineRepr(boundaries[i], nPoints); - } - return repr; - } else { - throw std::runtime_error("Unknown LaneletRepresentationType!"); - return Eigen::Vector3d(3 * nPoints); - } -} - -PolylineFeature getLaneletFeature(const ConstLanelet& ll, const LaneletRepresentationType& reprType, - const ParametrizationType& paramType, int nPoints, int nodeFeatLength) { - Eigen::Vector3d featureVec(nodeFeatLength); - Eigen::Vector3d polylineRepr(3 * nPoints); - PolylineFeature mapFeat = getPolylineRepr(ll, reprType, paramType, nPoints); - for (size_t i = 0; i < mapFeat.processedFeature_.size(); i++) { - polylineRepr(Eigen::seq(i, i + 2)) = mapFeat.processedFeature_[i](Eigen::seq(i, i + 2)); - } - featureVec(Eigen::seq(0, polylineRepr.size() - 1)) = polylineRepr; - featureVec[polylineRepr.size()] = bdSubtypeToInt(ll.leftBound3d()); - featureVec[polylineRepr.size() + 1] = bdSubtypeToInt(ll.rightBound3d()); - return featureVec; -} - -TensorLaneData MapGraphDataInterface::getLaneLaneData(MapGraphConstPtr localSubmapGraph) { +LaneData MapGraphDataInterface::getLaneData(MapGraphConstPtr localSubmapGraph) { const auto& graph = localSubmapGraph->graph_; const auto& llVertices = graph->vertexLookup(); - TensorLaneData result; + LaneData result; int numNodes = llVertices.size(); int32_t nodeFeatLength = getNodeFeatureLength(config_.reprType, config_.paramType, config_.nPoints); result.laneletFeatures.resize(numNodes); @@ -107,28 +71,17 @@ TensorLaneData MapGraphDataInterface::getLaneLaneData(MapGraphConstPtr localSubm return result; } -Eigen::Vector3d getTEFeatureVec(const ConstLineString3d& te) { - if (te.size() != 4) { - throw std::runtime_error("Number of points in traffic element is not 4!"); - } - Eigen::Vector3d featureVec(13); // 4 points with 3 dims + type - Eigen::Vector3d polylineRepr = getTEPolylineRepr(te.basicLineString()); - featureVec(Eigen::seq(0, polylineRepr.size() - 1)) = polylineRepr; - featureVec[polylineRepr.size()] = teTypeToInt(te); - return featureVec; -} - bool isTe(ConstLineString3d ls) { std::string type = ls.attribute(AttributeName::Type).value(); return type == AttributeValueString::TrafficLight || type == AttributeValueString::TrafficSign; } -TensorTEData MapGraphDataInterface::getLaneTEData(MapGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, - std::unordered_map& teId2Index) { +TEData MapGraphDataInterface::getLaneTEData(MapGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, + std::unordered_map& teId2Index) { const auto& graph = localSubmapGraph->graph_; const auto& llVertices = graph->vertexLookup(); - TensorTEData result; + TEData result; int numNodesLane = llVertices.size(); int32_t nodeFeatLengthLane = getNodeFeatureLength(config_.reprType, config_.paramType, config_.nPoints); result.laneletFeatures.resize(numNodesLane, nodeFeatLengthLane); @@ -191,21 +144,21 @@ TensorTEData MapGraphDataInterface::getLaneTEData(MapGraphConstPtr localSubmapGr return result; } -TensorLaneData MapGraphDataInterface::laneLaneTensors() { +LaneData MapGraphDataInterface::laneData() { if (!currPos_) { throw InvalidObjectStateError( "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); } - return getLaneLaneData(localSubmapGraph_); + return getLaneData(localSubmapGraph_); } -TensorTEData MapGraphDataInterface::laneTETensors() { +TEData MapGraphDataInterface::teData() { if (!currPos_) { throw InvalidObjectStateError( "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); } - return TensorTEData(); + return TEData(); } } // namespace map_learning diff --git a/lanelet2_map_learning/src/Types.cpp b/lanelet2_map_learning/src/Types.cpp deleted file mode 100644 index f96125c9..00000000 --- a/lanelet2_map_learning/src/Types.cpp +++ /dev/null @@ -1,19 +0,0 @@ -#include "lanelet2_map_learning/Types.h" - -#include -#include - -namespace lanelet { -namespace map_learning { - -void MapFeature::computePolyline(int32_t nPoints) { - double length = boost::geometry::length(originalFeature_, boost::geometry::strategy::distance::pythagoras()); - double dist = length / static_cast(nPoints); - boost::geometry::model::multi_point bdInterp; - boost::geometry::line_interpolate(originalFeature_, dist, bdInterp); - assert(bdInterp.size() == nPoints); - processedFeature_ = bdInterp; -} - -} // namespace map_learning -} // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index 833071f8..f7ea9fac 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -1,20 +1,22 @@ #include "lanelet2_map_learning/Utils.h" +#include +#include + namespace lanelet { namespace map_learning { -boost::geometry::model::polygon getRotatedRect(const BasicPoint2d& center, double extentLongitudinal, - double extentLateral, double yaw) { +OrientedRect getRotatedRect(const BasicPoint2d& center, double extentLongitudinal, double extentLateral, double yaw) { BasicPoints2d pts{BasicPoint2d{center.x() - extentLongitudinal, center.y() - extentLateral}, BasicPoint2d{center.x() - extentLongitudinal, center.y() + extentLateral}, BasicPoint2d{center.x() + extentLongitudinal, center.y() + extentLateral}, BasicPoint2d{center.x() + extentLongitudinal, center.y() - extentLateral}}; - boost::geometry::model::polygon axisAlignedRect; + OrientedRect axisAlignedRect; boost::geometry::assign_points(axisAlignedRect, pts); boost::geometry::strategy::transform::matrix_transformer trans( cos(yaw), sin(yaw), center.x(), -sin(yaw), cos(yaw), center.y(), 0, 0, 1); - boost::geometry::model::polygon rotatedRect; + OrientedRect rotatedRect; boost::geometry::transform(axisAlignedRect, rotatedRect, trans); return rotatedRect; } @@ -26,21 +28,20 @@ LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPo BasicPoint2d initRegionFront = {center.x() + 1.1 * maxExtent, center.y() + 1.1 * maxExtent}; BoundingBox2d initSearchRegion{initRegionRear, initRegionFront}; ConstLanelets initRegion = laneletMap->laneletLayer.search(initSearchRegion); - return utils::createConstSubmap(initRegion, {}); } -int32_t getNodeFeatureLength(const LaneletRepresentationType& reprType, const ParametrizationType& paramType, - int nPoints) { +Eigen::Vector3d getLaneletRepr(const LaneletRepresentationType& reprType, const ParametrizationType& paramType, + int nPoints) { int32_t nodeFeatureLength; if (reprType == LaneletRepresentationType::Boundaries) - nodeFeatureLength = 2 * 3 * nPoints + 2; // 2 boundary types with 2 possible types + nodeFeatureLength = 2 * 3 * nPoints + 2; // 2 boundary types else if (reprType == LaneletRepresentationType::Centerline) nodeFeatureLength = 3 * nPoints + 2; else throw std::runtime_error("Unknown LaneletRepresentationType!"); - return nodeFeatureLength; + return Eigen::Vector3d(nodeFeatureLength); } inline int bdSubtypeToInt(ConstLineString3d lString) { @@ -55,6 +56,8 @@ inline int bdSubtypeToInt(ConstLineString3d lString) { return 2; else if (subtype == AttributeValueString::DashedSolid) return 2; + else if (subtype == AttributeValueString::Virtual) + return 3; else { throw std::runtime_error("Unexpected Line String Subtype!"); return 0; @@ -74,12 +77,24 @@ inline int teTypeToInt(const ConstLineString3d& te) { } } -Eigen::Vector3d getTEPolylineRepr(const BasicLineString3d& te) { - Eigen::Vector3d repr(12); - for (size_t i = 0; i < te.size(); i++) { - repr(Eigen::seq(i, i + 2)) = te[i](Eigen::seq(i, i + 2)); - } - return repr; +Eigen::Vector3d getTERepr() { + return Eigen::Vector3d(13); // 4 points with 3 dims + type; +} + +BasicLineString3d resamplePolyline(const BasicLineString3d& polyline, int32_t nPoints) { + double length = boost::geometry::length(polyline, boost::geometry::strategy::distance::pythagoras()); + double dist = length / static_cast(nPoints); + boost::geometry::model::multi_point bdInterp; + boost::geometry::line_interpolate(polyline, dist, bdInterp); + assert(bdInterp.size() == nPoints); + return bdInterp; +} + +BasicLineString3d cutPolyline(const OrientedRect& bbox, const BasicLineString3d& polyline, int32_t nPoints) { + std::deque output; + boost::geometry::intersection(bbox, polyline, output); + assert(output.size() == 1); + return output[0]; } } // namespace map_learning From 071a6b257c4cda933fe9dee64ee6b22c1290974c Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Tue, 7 Nov 2023 19:03:06 +0100 Subject: [PATCH 13/64] continue rewrite --- .../lanelet2_map_learning/MapFeatures.h | 59 +++++++----- .../MapGraphDataInterface.h | 7 +- .../include/lanelet2_map_learning/Types.h | 4 +- .../include/lanelet2_map_learning/Utils.h | 13 +-- lanelet2_map_learning/src/MapFeatures.cpp | 89 +++++++++++++++---- lanelet2_map_learning/src/Utils.cpp | 45 +++------- 6 files changed, 137 insertions(+), 80 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index 440eed8e..35dd4309 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -12,47 +12,66 @@ namespace lanelet { namespace map_learning { -struct PolylineFeature { - BasicLineString3d originalFeature_; - BasicLineString3d processedFeature_; +struct LineStringFeature { + BasicLineString3d rawFeature_; Id mapID_; + + LineStringFeature() {} + LineStringFeature(const BasicLineString3d& feature, Id mapID) : rawFeature_{feature}, mapID_{mapID} {} + + virtual Eigen::VectorXd computeFeatureVector(); +}; + +struct LaneLineStringFeature : LineStringFeature { + BasicLineString3d processedFeature_; bool wasCut_{false}; - int subtype_; + LineStringType type_; ParametrizationType paramType; - PolylineFeature() {} - PolylineFeature(const BasicLineString3d& feature, Id mapID) : originalFeature_{feature}, mapID_{mapID} {} + LaneLineStringFeature() {} + LaneLineStringFeature(const BasicLineString3d& feature, Id mapID, LineStringType type) + : LineStringFeature(feature, mapID), type_{type} {} + + void processLineString(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints); + Eigen::VectorXd computeFeatureVector(bool onlyPoints = false); // uses processedFeature_ when available +}; + +struct TEFeature : LineStringFeature { + TEType teType_; + + TEFeature() {} + TEFeature(const BasicLineString3d& feature, Id mapID, TEType type) + : LineStringFeature(feature, mapID), teType_{type} {} - void processPolyline(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints); - Eigen::Vector3d computeFeatureVector(); + Eigen::VectorXd computeFeatureVector(); }; struct LaneletFeature { - PolylineFeature leftBoundary_; - PolylineFeature rightBoundary_; - PolylineFeature centerline_; + LaneLineStringFeature leftBoundary_; + LaneLineStringFeature rightBoundary_; + LaneLineStringFeature centerline_; Id mapID_; bool wasCut_{false}; - int bdTypeLeft_; - int bdTypeRight_; LaneletFeature() {} - LaneletFeature(const PolylineFeature& leftBoundary, const PolylineFeature& rightBoundary, - const PolylineFeature& centerline, Id mapID); + LaneletFeature(const LaneLineStringFeature& leftBoundary, const LaneLineStringFeature& rightBoundary, + const LaneLineStringFeature& centerline, Id mapID); LaneletFeature(const ConstLanelet& ll); void processLanelet(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints); - Eigen::Vector3d computeFeatureVector(const LaneletRepresentationType& reprType); + Eigen::VectorXd computeFeatureVector(const LaneletRepresentationType& reprType, bool onlyPoints = false); }; -using PolylineFeatures = std::vector; +using LineStringFeatures = std::vector; +using LaneLineStringFeatures = std::vector; +using TEFeatures = std::vector; using LaneletFeatures = std::vector; struct LaneData { LaneData() noexcept {} - PolylineFeatures roadBorders; // auxilliary features - PolylineFeatures laneDividers; // auxilliary features + LaneLineStringFeatures roadBorders; // auxilliary features + LaneLineStringFeatures laneDividers; // auxilliary features LaneletFeatures laneletFeatures; // node features Eigen::MatrixX2i edgeList; // adjacency matrix (sparse) @@ -62,7 +81,7 @@ struct LaneData { struct TEData { TEData() noexcept {} LaneletFeatures laneletFeatures; // node features lanelets - PolylineFeatures teFeatures; // node features traffic elements + TEFeatures teFeatures; // node features traffic elements /// @brief adjacency matrix (sparse). Node indices are assigned as /// xLane and xTE stacked, with xLane being first diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h index f4743b08..e75c0cb8 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h @@ -21,7 +21,8 @@ class MapGraphDataInterface { struct Configuration { Configuration() noexcept {} LaneletRepresentationType reprType{LaneletRepresentationType::Boundaries}; - ParametrizationType paramType{ParametrizationType::Polyline}; + ParametrizationType paramType{ParametrizationType::LineString}; + bool includeLaneletBdTypes{false}; double submapAreaLongitudinal{30}; // in driving direction double submapAreaLateral{15}; // in lateral direction int nPoints{11}; @@ -48,8 +49,8 @@ class MapGraphDataInterface { std::unordered_map teId2Index_; MapGraphConstPtr localSubmapGraph_; FeatureBuffer laneletFeatureBuffer_; - FeatureBuffer polylineFeatureBuffer_; - FeatureBuffer teFeatureBuffer_; + FeatureBuffer polylineFeatureBuffer_; + FeatureBuffer teFeatureBuffer_; Optional currPos_; // in the map frame Optional currYaw_; // in the map frame Configuration config_; diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h index 7c96cfef..e111cb62 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h @@ -12,7 +12,9 @@ namespace lanelet { namespace map_learning { enum class LaneletRepresentationType { Centerline, Boundaries }; -enum class ParametrizationType { Bezier, BezierEndpointFixed, Polyline }; +enum class ParametrizationType { Bezier, BezierEndpointFixed, LineString }; +enum class LineStringType { RoadBorder, Dashed, Solid, Virtual, Centerline, Unknown }; +enum class TEType { TrafficLight, TrafficSign, Unknown }; using OrientedRect = boost::geometry::model::polygon; diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h index deb71892..5b32e54b 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h @@ -20,18 +20,13 @@ boost::geometry::model::polygon getRotatedRect(const BasicPoint2d& LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPoint2d& center, double yaw, double extentLongitudinal, double extentLateral); -Eigen::Vector3d getLaneletRepr(const LaneletRepresentationType& reprType, const ParametrizationType& paramType, - int nPoints); +inline LineStringType bdSubtypeToEnum(ConstLineString3d lString); -inline int bdSubtypeToInt(ConstLineString3d lString); +inline TEType teTypeToEnum(const ConstLineString3d& te); -inline int teTypeToInt(const ConstLineString3d& te); +BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t nPoints); -Eigen::Vector3d getTERepr(); - -BasicLineString3d resamplePolyline(const BasicLineString3d& polyline, int32_t nPoints); - -BasicLineString3d cutPolyline(const OrientedRect& bbox, const BasicLineString3d& polyline); +BasicLineString3d cutLineString(const OrientedRect& bbox, const BasicLineString3d& polyline); } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index da344392..9f34b32a 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -8,43 +8,100 @@ namespace lanelet { namespace map_learning { -void PolylineFeature::processPolyline(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { - if (paramType != ParametrizationType::Polyline) { +void LaneLineStringFeature::processLineString(const OrientedRect& bbox, const ParametrizationType& paramType, + int32_t nPoints) { + if (paramType != ParametrizationType::LineString) { throw std::runtime_error("Only polyline parametrization is implemented so far!"); } - processedFeature_ = cutPolyline(bbox, originalFeature_); - processedFeature_ = resamplePolyline(originalFeature_, nPoints); + processedFeature_ = cutLineString(bbox, rawFeature_); + processedFeature_ = resampleLineString(processedFeature_, nPoints); double lengthOriginal = - boost::geometry::length(originalFeature_, boost::geometry::strategy::distance::pythagoras()); + boost::geometry::length(rawFeature_, boost::geometry::strategy::distance::pythagoras()); double lengthProcessed = - boost::geometry::length(originalFeature_, boost::geometry::strategy::distance::pythagoras()); + boost::geometry::length(processedFeature_, boost::geometry::strategy::distance::pythagoras()); + assert(lengthOriginal - lengthProcessed > -1e-2); if (lengthOriginal - lengthProcessed > 1e-2) { wasCut_ = true; } } -LaneletFeature::LaneletFeature(const PolylineFeature& leftBoundary, const PolylineFeature& rightBoundary, - const PolylineFeature& centerline, Id mapID) +Eigen::VectorXd LaneLineStringFeature::computeFeatureVector(bool onlyPoints) { + const BasicLineString3d& selectedFeature = (processedFeature_.size() > 0) ? processedFeature_ : rawFeature_; + Eigen::VectorXd vec(3 * selectedFeature.size() + 1); // n points with 3 dims + type + for (size_t i = 0; i < selectedFeature.size(); i++) { + vec(Eigen::seq(i, i + 2)) = selectedFeature[i](Eigen::seq(i, i + 2)); + } + vec[vec.size() - 1] = static_cast(type_); + if (onlyPoints) { + return vec(Eigen::seq(0, vec.size() - 1)); + } else { + return vec; + } +} + +Eigen::VectorXd TEFeature::computeFeatureVector() { + Eigen::VectorXd vec(3 * rawFeature_.size() + 1); // n points with 3 dims + type + for (size_t i = 0; i < rawFeature_.size(); i++) { + vec(Eigen::seq(i, i + 2)) = rawFeature_[i](Eigen::seq(i, i + 2)); + } + vec[vec.size() - 1] = static_cast(teType_); + return vec; +} + +LaneletFeature::LaneletFeature(const LaneLineStringFeature& leftBoundary, const LaneLineStringFeature& rightBoundary, + const LaneLineStringFeature& centerline, Id mapID) : leftBoundary_{leftBoundary}, rightBoundary_{rightBoundary}, centerline_{centerline}, mapID_{mapID} {} LaneletFeature::LaneletFeature(const ConstLanelet& ll) - : leftBoundary_{PolylineFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id())}, - rightBoundary_{PolylineFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id())}, - centerline_{PolylineFeature(ll.centerline3d().basicLineString(), ll.centerline3d().id())}, - bdTypeLeft_{bdSubtypeToInt(ll.leftBound3d())}, - bdTypeRight_{bdSubtypeToInt(ll.rightBound3d())} {} + : leftBoundary_{LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), + bdSubtypeToEnum(ll.leftBound3d()))}, + rightBoundary_{LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), + bdSubtypeToEnum(ll.rightBound3d()))}, + centerline_{LaneLineStringFeature(ll.centerline3d().basicLineString(), ll.centerline3d().id(), + LineStringType::Centerline)} {} void LaneletFeature::processLanelet(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { - leftBoundary_.processPolyline(bbox, paramType, nPoints); - rightBoundary_.processPolyline(bbox, paramType, nPoints); - centerline_.processPolyline(bbox, paramType, nPoints); + leftBoundary_.processLineString(bbox, paramType, nPoints); + rightBoundary_.processLineString(bbox, paramType, nPoints); + centerline_.processLineString(bbox, paramType, nPoints); if (leftBoundary_.wasCut_ || rightBoundary_.wasCut_ || centerline_.wasCut_) { wasCut_ = true; } } +Eigen::VectorXd LaneletFeature::computeFeatureVector(const LaneletRepresentationType& reprType, bool onlyPoints) { + if (reprType == LaneletRepresentationType::Centerline) { + Eigen::VectorXd vecCenterlinePts = centerline_.computeFeatureVector(true); + Eigen::VectorXd vec(vecCenterlinePts.size() + 2); // pts vec + left and right type + vec(Eigen::seq(0, vecCenterlinePts.size() - 1)) = vecCenterlinePts; + vec[vec.size() - 2] = static_cast(leftBoundary_.type_); + vec[vec.size() - 1] = static_cast(rightBoundary_.type_); + if (onlyPoints) { + return vec(Eigen::seq(0, vec.size() - 2)); + } else { + return vec; + } + } else if (reprType == LaneletRepresentationType::Boundaries) { + Eigen::VectorXd vecLeftBdPts = leftBoundary_.computeFeatureVector(true); + Eigen::VectorXd vecRightBdPts = rightBoundary_.computeFeatureVector(true); + Eigen::VectorXd vec(vecLeftBdPts.size() + vecRightBdPts.size() + 2); // pts vec + left and right type + vec(Eigen::seq(0, vecLeftBdPts.size() - 1)) = vecLeftBdPts; + vec(Eigen::seq(vecLeftBdPts.size(), vecLeftBdPts.size() + vecRightBdPts.size() - 1)) = vecRightBdPts; + vec[vec.size() - 2] = static_cast(leftBoundary_.type_); + vec[vec.size() - 1] = static_cast(rightBoundary_.type_); + if (onlyPoints) { + return vec(Eigen::seq(0, vec.size() - 2)); + } else { + return vec; + } + } else { + throw std::runtime_error("Unknown LaneletRepresentationType!"); + return Eigen::VectorXd(); + } +} + } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index f7ea9fac..a628e87d 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -31,57 +31,40 @@ LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPo return utils::createConstSubmap(initRegion, {}); } -Eigen::Vector3d getLaneletRepr(const LaneletRepresentationType& reprType, const ParametrizationType& paramType, - int nPoints) { - int32_t nodeFeatureLength; - if (reprType == LaneletRepresentationType::Boundaries) - nodeFeatureLength = 2 * 3 * nPoints + 2; // 2 boundary types - else if (reprType == LaneletRepresentationType::Centerline) - nodeFeatureLength = 3 * nPoints + 2; - else - throw std::runtime_error("Unknown LaneletRepresentationType!"); - - return Eigen::Vector3d(nodeFeatureLength); -} - -inline int bdSubtypeToInt(ConstLineString3d lString) { +inline LineStringType bdSubtypeToEnum(ConstLineString3d lString) { std::string subtype = lString.attribute(AttributeName::Subtype).value(); if (subtype == AttributeValueString::Dashed) - return 1; + return LineStringType::Dashed; else if (subtype == AttributeValueString::Solid) - return 2; + return LineStringType::Solid; else if (subtype == AttributeValueString::SolidSolid) - return 2; + return LineStringType::Solid; else if (subtype == AttributeValueString::SolidDashed) - return 2; + return LineStringType::Solid; else if (subtype == AttributeValueString::DashedSolid) - return 2; + return LineStringType::Solid; else if (subtype == AttributeValueString::Virtual) - return 3; + return LineStringType::Virtual; else { throw std::runtime_error("Unexpected Line String Subtype!"); - return 0; + return LineStringType::Unknown; } } -inline int teTypeToInt(const ConstLineString3d& te) { +inline TEType teTypeToEnum(const ConstLineString3d& te) { std::string type = te.attribute(AttributeName::Type).value(); std::string subtype = te.attribute(AttributeName::Subtype).value(); if (type == AttributeValueString::TrafficLight) { - return 1; + return TEType::TrafficLight; } else if (type == AttributeValueString::TrafficSign) { - return 2; + return TEType::TrafficSign; } else { throw std::runtime_error("Unexpected Traffic Element Type!"); - return 0; + return TEType::Unknown; } } -Eigen::Vector3d getTERepr() { - return Eigen::Vector3d(13); // 4 points with 3 dims + type; -} - -BasicLineString3d resamplePolyline(const BasicLineString3d& polyline, int32_t nPoints) { +BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t nPoints) { double length = boost::geometry::length(polyline, boost::geometry::strategy::distance::pythagoras()); double dist = length / static_cast(nPoints); boost::geometry::model::multi_point bdInterp; @@ -90,7 +73,7 @@ BasicLineString3d resamplePolyline(const BasicLineString3d& polyline, int32_t nP return bdInterp; } -BasicLineString3d cutPolyline(const OrientedRect& bbox, const BasicLineString3d& polyline, int32_t nPoints) { +BasicLineString3d cutLineString(const OrientedRect& bbox, const BasicLineString3d& polyline, int32_t nPoints) { std::deque output; boost::geometry::intersection(bbox, polyline, output); assert(output.size() == 1); From d07a486bee5db0f442954b6635ca09193d20b6b6 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Wed, 8 Nov 2023 21:23:23 +0100 Subject: [PATCH 14/64] wip compound features --- .../include/lanelet2_map_learning/MapFeatures.h | 10 ++++++++++ lanelet2_map_learning/src/MapFeatures.cpp | 14 ++++++++++++++ 2 files changed, 24 insertions(+) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index 35dd4309..904c009a 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -63,6 +63,16 @@ struct LaneletFeature { Eigen::VectorXd computeFeatureVector(const LaneletRepresentationType& reprType, bool onlyPoints = false); }; +struct CompoundLaneLineStringFeature { + LaneLineStringFeatures features_; + std::vector pathLengths_; + LineStringType compoundType_; + CompoundLaneLineStringFeature() {} + // features for this constructor are required to be given in sorted order + CompoundLaneLineStringFeature(const LaneLineStringFeatures& features, LineStringType compoundType); + virtual Eigen::VectorXd computeFeatureVector(); +}; + using LineStringFeatures = std::vector; using LaneLineStringFeatures = std::vector; using TEFeatures = std::vector; diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index 9f34b32a..d8c4c721 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -103,5 +103,19 @@ Eigen::VectorXd LaneletFeature::computeFeatureVector(const LaneletRepresentation } } +CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStringFeatures& features, + LineStringType compoundType) + : features_{features}, pathLengths_{std::vector(features.size())}, compoundType_{compoundType} { + for (size_t i = 0; i < features.size(); i++) { + double currLength = + boost::geometry::length(features[i].rawFeature_, boost::geometry::strategy::distance::pythagoras()); + if (i > 0) { + pathLengths_[i] = pathLengths_[i - 1] + currLength; + } else { + pathLengths_[i] = currLength; + } + } +} + } // namespace map_learning } // namespace lanelet \ No newline at end of file From 6aadd3f3db39bd98c8de4428d24f9a3f3abbdaad Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 9 Nov 2023 21:40:14 +0100 Subject: [PATCH 15/64] further work map features --- .../include/lanelet2_map_learning/MapData.h | 50 ++++++ .../lanelet2_map_learning/MapFeatures.h | 146 +++++++++++------- .../MapGraphDataInterface.h | 5 +- .../lanelet2_map_learning/internal/Graph.h | 15 +- lanelet2_map_learning/src/MapData.cpp | 5 + lanelet2_map_learning/src/MapFeatures.cpp | 101 ++++++++---- .../src/MapGraphDataInterface.cpp | 22 +-- lanelet2_map_learning/src/Utils.cpp | 32 +++- 8 files changed, 255 insertions(+), 121 deletions(-) create mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/MapData.h create mode 100644 lanelet2_map_learning/src/MapData.cpp diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h new file mode 100644 index 00000000..33fcc10a --- /dev/null +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -0,0 +1,50 @@ +#pragma once +#include +#include +#include + +#include +#include + +#include "lanelet2_map_learning/Forward.h" +#include "lanelet2_map_learning/MapFeatures.h" +#include "lanelet2_map_learning/Types.h" + +namespace lanelet { +namespace map_learning { + +class LaneData { + public: + LaneData() noexcept {} + LaneData(const ConstLanelets& lls); + + private: + LaneLineStringFeatures roadBorders_; // auxilliary features + LaneLineStringFeatures laneDividers_; // auxilliary features + + CompoundLaneLineStringFeatures compoundRoadBorders_; + CompoundLaneLineStringFeatures compoundLaneDividers_; + CompoundLaneLineStringFeatures compoundCenterlines_; + + LaneletFeatures laneletFeatures_; // node features + Eigen::MatrixX2i edgeList_; // adjacency matrix (sparse) + Eigen::MatrixXd edgeFeatures_; // edge features +}; + +class TEData { + public: + TEData() noexcept {} + + private: + LaneletFeatures laneletFeatures; // node features lanelets + TEFeatures teFeatures; // node features traffic elements + + /// @brief adjacency matrix (sparse). Node indices are assigned as + /// xLane and xTE stacked, with xLane being first + Eigen::MatrixX2i edgeList; + + Eigen::MatrixXd edgeFeatures; // edge features +}; + +} // namespace map_learning +} // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index 904c009a..e6502c49 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -2,6 +2,7 @@ #include #include #include +#include #include #include @@ -12,93 +13,130 @@ namespace lanelet { namespace map_learning { -struct LineStringFeature { - BasicLineString3d rawFeature_; - Id mapID_; +class MapFeature { + public: + const Optional& mapID() const { return mapID_; } + const bool& initialized() const { return initialized_; } + const bool& valid() const { return valid_; } - LineStringFeature() {} - LineStringFeature(const BasicLineString3d& feature, Id mapID) : rawFeature_{feature}, mapID_{mapID} {} + virtual Eigen::VectorXd computeFeatureVector(bool /*unused*/) const = 0; + virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) = 0; + + protected: + bool initialized_{false}; + bool wasCut_{false}; + bool valid_{true}; + Optional mapID_; - virtual Eigen::VectorXd computeFeatureVector(); + MapFeature() {} + MapFeature(Id mapID) : mapID_{mapID}, initialized_{true} {} + virtual ~MapFeature() noexcept = default; }; -struct LaneLineStringFeature : LineStringFeature { - BasicLineString3d processedFeature_; - bool wasCut_{false}; - LineStringType type_; +class LineStringFeature : public MapFeature { + public: + const BasicLineString3d& rawFeature() const { return rawFeature_; } - ParametrizationType paramType; + virtual Eigen::VectorXd computeFeatureVector(bool /*unused*/) const = 0; + virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) = 0; + + protected: + BasicLineString3d rawFeature_; + + LineStringFeature() {} + LineStringFeature(const BasicLineString3d& feature, Id mapID) : MapFeature(mapID), rawFeature_{feature} {} + virtual ~LineStringFeature() noexcept = default; +}; +class LaneLineStringFeature : public LineStringFeature { + public: LaneLineStringFeature() {} LaneLineStringFeature(const BasicLineString3d& feature, Id mapID, LineStringType type) : LineStringFeature(feature, mapID), type_{type} {} + virtual ~LaneLineStringFeature() noexcept = default; - void processLineString(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints); - Eigen::VectorXd computeFeatureVector(bool onlyPoints = false); // uses processedFeature_ when available -}; + virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; + virtual Eigen::VectorXd computeFeatureVector( + bool onlyPoints = false) const override; // uses processedFeature_ when available -struct TEFeature : LineStringFeature { - TEType teType_; + const bool& wasCut() const { return wasCut_; } + const BasicLineString3d& cutFeature() const { return cutFeature_; } + const BasicLineString3d& cutAndResampledFeature() const { return cutAndResampledFeature_; } + const LineStringType& type() const { return type_; } + + protected: + BasicLineString3d cutFeature_; + BasicLineString3d cutAndResampledFeature_; + bool wasCut_{false}; + LineStringType type_; +}; +class TEFeature : public LineStringFeature { + public: TEFeature() {} TEFeature(const BasicLineString3d& feature, Id mapID, TEType type) : LineStringFeature(feature, mapID), teType_{type} {} + virtual ~TEFeature() noexcept = default; - Eigen::VectorXd computeFeatureVector(); -}; + bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) override; + Eigen::VectorXd computeFeatureVector(bool onlyPoints = false) const override; -struct LaneletFeature { - LaneLineStringFeature leftBoundary_; - LaneLineStringFeature rightBoundary_; - LaneLineStringFeature centerline_; - Id mapID_; - bool wasCut_{false}; + const TEType& teType() { return teType_; } + + private: + TEType teType_; +}; +class LaneletFeature : public MapFeature { + public: LaneletFeature() {} LaneletFeature(const LaneLineStringFeature& leftBoundary, const LaneLineStringFeature& rightBoundary, const LaneLineStringFeature& centerline, Id mapID); LaneletFeature(const ConstLanelet& ll); + virtual ~LaneletFeature() noexcept = default; + + bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; + Eigen::VectorXd computeFeatureVector(bool onlyPoints = false) const override; + + void setReprType(LaneletRepresentationType reprType) { reprType_ = reprType; } - void processLanelet(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints); - Eigen::VectorXd computeFeatureVector(const LaneletRepresentationType& reprType, bool onlyPoints = false); + const LaneLineStringFeature& leftBoundary() { return leftBoundary_; } + const LaneLineStringFeature& rightBoundary() { return rightBoundary_; } + const LaneLineStringFeature& centerline() { return centerline_; } + + private: + LaneLineStringFeature leftBoundary_; + LaneLineStringFeature rightBoundary_; + LaneLineStringFeature centerline_; + LaneletRepresentationType reprType_; }; -struct CompoundLaneLineStringFeature { - LaneLineStringFeatures features_; - std::vector pathLengths_; - LineStringType compoundType_; +class CompoundLaneLineStringFeature : public LaneLineStringFeature { + public: CompoundLaneLineStringFeature() {} - // features for this constructor are required to be given in sorted order + // features for this constructor are required to be given in already sorted order CompoundLaneLineStringFeature(const LaneLineStringFeatures& features, LineStringType compoundType); - virtual Eigen::VectorXd computeFeatureVector(); + + virtual ~CompoundLaneLineStringFeature() noexcept = default; + + const LaneLineStringFeatures& features() const { return individualFeatures_; } + const std::vector& pathLengthsRaw() const { return pathLengthsRaw_; } + const std::vector& pathLengthsProcessed() const { return pathLengthsProcessed_; } + + bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; + Eigen::VectorXd computeFeatureVector(bool onlyPoints = false) const override; + + private: + LaneLineStringFeatures individualFeatures_; + std::vector pathLengthsRaw_; + std::vector pathLengthsProcessed_; }; using LineStringFeatures = std::vector; using LaneLineStringFeatures = std::vector; +using CompoundLaneLineStringFeatures = std::vector; using TEFeatures = std::vector; using LaneletFeatures = std::vector; -struct LaneData { - LaneData() noexcept {} - LaneLineStringFeatures roadBorders; // auxilliary features - LaneLineStringFeatures laneDividers; // auxilliary features - - LaneletFeatures laneletFeatures; // node features - Eigen::MatrixX2i edgeList; // adjacency matrix (sparse) - Eigen::MatrixXd edgeFeatures; // edge features -}; - -struct TEData { - TEData() noexcept {} - LaneletFeatures laneletFeatures; // node features lanelets - TEFeatures teFeatures; // node features traffic elements - - /// @brief adjacency matrix (sparse). Node indices are assigned as - /// xLane and xTE stacked, with xLane being first - Eigen::MatrixX2i edgeList; - - Eigen::MatrixXd edgeFeatures; // edge features -}; - } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h index e75c0cb8..59e59e65 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h @@ -48,10 +48,7 @@ class MapGraphDataInterface { LaneletSubmapConstPtr localSubmap_; std::unordered_map teId2Index_; MapGraphConstPtr localSubmapGraph_; - FeatureBuffer laneletFeatureBuffer_; - FeatureBuffer polylineFeatureBuffer_; - FeatureBuffer teFeatureBuffer_; - Optional currPos_; // in the map frame + Optional currPos_; // in the map frame Optional currYaw_; // in the map frame Configuration config_; traffic_rules::TrafficRulesPtr trafficRules_; diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h b/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h index 8ccf9a70..ea2f0866 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h @@ -179,24 +179,11 @@ class Graph { } } - std::unordered_map getllId2Index(bool recompute = false) { - if (recompute || !llId2index_) { - std::unordered_map llId2index; - int i = 0; - for (const auto& entry : laneletOrAreaToVertex_) { - llId2index[entry.first.id()] = i++; - } - llId2index_ = llId2index; - } - return *llId2index_; - } - private: FilteredGraph getFilteredGraph(RelationType relations) const { return FilteredGraph(graph_, Filter(graph_, relations)); } - BaseGraphT graph_; //!< The actual graph object - Optional> llId2index_; + BaseGraphT graph_; //!< The actual graph object LaneletOrAreaToVertex laneletOrAreaToVertex_; //!< Mapping of lanelets/areas to vertices of the graph }; diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp new file mode 100644 index 00000000..47b5c4d9 --- /dev/null +++ b/lanelet2_map_learning/src/MapData.cpp @@ -0,0 +1,5 @@ +#include "lanelet2_map_learning/MapData.h" + +namespace lanelet { +namespace map_learning {} // namespace map_learning +} // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index d8c4c721..57bdc965 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -8,27 +8,53 @@ namespace lanelet { namespace map_learning { -void LaneLineStringFeature::processLineString(const OrientedRect& bbox, const ParametrizationType& paramType, - int32_t nPoints) { +struct LStringProcessResult { + BasicLineString3d cutFeature_; + BasicLineString3d cutAndResampledFeature_; + bool wasCut_{false}; + bool valid_{true}; +}; + +LStringProcessResult processLineStringImpl(const BasicLineString3d& lstring, const OrientedRect& bbox, + const ParametrizationType& paramType, int32_t nPoints) { + LStringProcessResult result; if (paramType != ParametrizationType::LineString) { throw std::runtime_error("Only polyline parametrization is implemented so far!"); } - processedFeature_ = cutLineString(bbox, rawFeature_); - processedFeature_ = resampleLineString(processedFeature_, nPoints); + result.cutFeature_ = cutLineString(bbox, lstring); + if (result.cutFeature_.empty()) { + result.wasCut_ = true; + result.valid_ = false; + return result; + } + result.cutAndResampledFeature_ = resampleLineString(result.cutFeature_, nPoints); - double lengthOriginal = - boost::geometry::length(rawFeature_, boost::geometry::strategy::distance::pythagoras()); - double lengthProcessed = - boost::geometry::length(processedFeature_, boost::geometry::strategy::distance::pythagoras()); + double lengthOriginal = boost::geometry::length(lstring, boost::geometry::strategy::distance::pythagoras()); + double lengthProcessed = boost::geometry::length(result.cutAndResampledFeature_, + boost::geometry::strategy::distance::pythagoras()); assert(lengthOriginal - lengthProcessed > -1e-2); if (lengthOriginal - lengthProcessed > 1e-2) { - wasCut_ = true; + result.wasCut_ = true; + } + return result; +} + +bool LaneLineStringFeature::process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { + LStringProcessResult result = processLineStringImpl(rawFeature_, bbox, paramType, nPoints); + if (result.valid_) { + cutFeature_ = result.cutFeature_; + cutAndResampledFeature_ = result.cutAndResampledFeature_; + } else { + valid_ = result.valid_; } + wasCut_ = result.wasCut_; + return result.valid_; } -Eigen::VectorXd LaneLineStringFeature::computeFeatureVector(bool onlyPoints) { - const BasicLineString3d& selectedFeature = (processedFeature_.size() > 0) ? processedFeature_ : rawFeature_; +Eigen::VectorXd LaneLineStringFeature::computeFeatureVector(bool onlyPoints) const { + const BasicLineString3d& selectedFeature = + (cutAndResampledFeature_.size() > 0) ? cutAndResampledFeature_ : rawFeature_; Eigen::VectorXd vec(3 * selectedFeature.size() + 1); // n points with 3 dims + type for (size_t i = 0; i < selectedFeature.size(); i++) { vec(Eigen::seq(i, i + 2)) = selectedFeature[i](Eigen::seq(i, i + 2)); @@ -41,7 +67,7 @@ Eigen::VectorXd LaneLineStringFeature::computeFeatureVector(bool onlyPoints) { } } -Eigen::VectorXd TEFeature::computeFeatureVector() { +Eigen::VectorXd TEFeature::computeFeatureVector(bool onlyPoints = false) const { Eigen::VectorXd vec(3 * rawFeature_.size() + 1); // n points with 3 dims + type for (size_t i = 0; i < rawFeature_.size(); i++) { vec(Eigen::seq(i, i + 2)) = rawFeature_[i](Eigen::seq(i, i + 2)); @@ -52,7 +78,7 @@ Eigen::VectorXd TEFeature::computeFeatureVector() { LaneletFeature::LaneletFeature(const LaneLineStringFeature& leftBoundary, const LaneLineStringFeature& rightBoundary, const LaneLineStringFeature& centerline, Id mapID) - : leftBoundary_{leftBoundary}, rightBoundary_{rightBoundary}, centerline_{centerline}, mapID_{mapID} {} + : MapFeature(mapID), leftBoundary_{leftBoundary}, rightBoundary_{rightBoundary}, centerline_{centerline} {} LaneletFeature::LaneletFeature(const ConstLanelet& ll) : leftBoundary_{LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), @@ -62,36 +88,39 @@ LaneletFeature::LaneletFeature(const ConstLanelet& ll) centerline_{LaneLineStringFeature(ll.centerline3d().basicLineString(), ll.centerline3d().id(), LineStringType::Centerline)} {} -void LaneletFeature::processLanelet(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { - leftBoundary_.processLineString(bbox, paramType, nPoints); - rightBoundary_.processLineString(bbox, paramType, nPoints); - centerline_.processLineString(bbox, paramType, nPoints); +bool LaneletFeature::process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { + leftBoundary_.process(bbox, paramType, nPoints); + rightBoundary_.process(bbox, paramType, nPoints); + centerline_.process(bbox, paramType, nPoints); - if (leftBoundary_.wasCut_ || rightBoundary_.wasCut_ || centerline_.wasCut_) { + if (leftBoundary_.wasCut() || rightBoundary_.wasCut() || centerline_.wasCut()) { wasCut_ = true; } + if (!leftBoundary_.valid() || !rightBoundary_.valid() || !centerline_.valid()) { + valid_ = false; + } } -Eigen::VectorXd LaneletFeature::computeFeatureVector(const LaneletRepresentationType& reprType, bool onlyPoints) { - if (reprType == LaneletRepresentationType::Centerline) { +Eigen::VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints) const { + if (reprType_ == LaneletRepresentationType::Centerline) { Eigen::VectorXd vecCenterlinePts = centerline_.computeFeatureVector(true); Eigen::VectorXd vec(vecCenterlinePts.size() + 2); // pts vec + left and right type vec(Eigen::seq(0, vecCenterlinePts.size() - 1)) = vecCenterlinePts; - vec[vec.size() - 2] = static_cast(leftBoundary_.type_); - vec[vec.size() - 1] = static_cast(rightBoundary_.type_); + vec[vec.size() - 2] = static_cast(leftBoundary_.type()); + vec[vec.size() - 1] = static_cast(rightBoundary_.type()); if (onlyPoints) { return vec(Eigen::seq(0, vec.size() - 2)); } else { return vec; } - } else if (reprType == LaneletRepresentationType::Boundaries) { + } else if (reprType_ == LaneletRepresentationType::Boundaries) { Eigen::VectorXd vecLeftBdPts = leftBoundary_.computeFeatureVector(true); Eigen::VectorXd vecRightBdPts = rightBoundary_.computeFeatureVector(true); Eigen::VectorXd vec(vecLeftBdPts.size() + vecRightBdPts.size() + 2); // pts vec + left and right type vec(Eigen::seq(0, vecLeftBdPts.size() - 1)) = vecLeftBdPts; vec(Eigen::seq(vecLeftBdPts.size(), vecLeftBdPts.size() + vecRightBdPts.size() - 1)) = vecRightBdPts; - vec[vec.size() - 2] = static_cast(leftBoundary_.type_); - vec[vec.size() - 1] = static_cast(rightBoundary_.type_); + vec[vec.size() - 2] = static_cast(leftBoundary_.type()); + vec[vec.size() - 1] = static_cast(rightBoundary_.type()); if (onlyPoints) { return vec(Eigen::seq(0, vec.size() - 2)); } else { @@ -105,15 +134,21 @@ Eigen::VectorXd LaneletFeature::computeFeatureVector(const LaneletRepresentation CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStringFeatures& features, LineStringType compoundType) - : features_{features}, pathLengths_{std::vector(features.size())}, compoundType_{compoundType} { + : individualFeatures_{features}, pathLengthsRaw_{std::vector(features.size())} { + type_ = compoundType; + for (size_t i = 0; i < features.size(); i++) { - double currLength = - boost::geometry::length(features[i].rawFeature_, boost::geometry::strategy::distance::pythagoras()); - if (i > 0) { - pathLengths_[i] = pathLengths_[i - 1] + currLength; - } else { - pathLengths_[i] = currLength; - } + assert(features[i].rawFeature().size() > 1); + rawFeature_.insert(rawFeature_.end(), features[i].rawFeature().begin(), features[i].rawFeature().end() - 1); + + double rawLength = + boost::geometry::length(features[i].rawFeature(), boost::geometry::strategy::distance::pythagoras()); + if (features[i].cutFeature().size() > 1) + if (i > 0) { + pathLengthsRaw_[i] = pathLengthsRaw_[i - 1] + rawLength; + } else { + pathLengthsRaw_[i] = rawLength; + } } } diff --git a/lanelet2_map_learning/src/MapGraphDataInterface.cpp b/lanelet2_map_learning/src/MapGraphDataInterface.cpp index eef009cc..5c0e8a1c 100644 --- a/lanelet2_map_learning/src/MapGraphDataInterface.cpp +++ b/lanelet2_map_learning/src/MapGraphDataInterface.cpp @@ -9,13 +9,13 @@ #include #include -#include "lanelet2_map_learning/MapFeatures.h" +#include "lanelet2_map_learning/MapData.h" #include "lanelet2_map_learning/Utils.h" namespace lanelet { namespace map_learning { -void MapGraphDataInterface::setCurrPosAndExtractSubmap(const BasicPoint2d& pt) { +void MapGraphDataInterface::setCurrPosAndExtractSubmap(const BasicPoint3d& pt) { currPos_ = pt; localSubmap_ = extractSubmap(laneletMap_, *currPos_, *currYaw_, config_.submapAreaLongitudinal, config_.submapAreaLateral); @@ -33,21 +33,19 @@ LaneData MapGraphDataInterface::getLaneData(MapGraphConstPtr localSubmapGraph) { const auto& graph = localSubmapGraph->graph_; const auto& llVertices = graph->vertexLookup(); - LaneData result; - int numNodes = llVertices.size(); - int32_t nodeFeatLength = getNodeFeatureLength(config_.reprType, config_.paramType, config_.nPoints); - result.laneletFeatures.resize(numNodes); - - std::unordered_map llId2Index = graph->getllId2Index(); - + ConstLanelets lls; + Eigen::MatrixX2i edgeList; + Eigen::MatrixXd edgeFeatures; int32_t edgeCount = 0; for (const auto& laWithVertex : llVertices) { const auto& la = laWithVertex.first; + if (!la.isLanelet()) continue; + auto ll = laWithVertex.first.lanelet(); const auto& vertex = laWithVertex.second; - if (nodeFeatureBuffer_.find(la.id()) != nodeFeatureBuffer_.end()) { - result.laneletFeatures[llId2Index[la.id()]] = nodeFeatureBuffer_[la.id()]; + if (laneletFeatureBuffer_.find(la.id()) != laneletFeatureBuffer_.end()) { + lls.push_back(nodeFeatureBuffer_[la.id()]); } else { MapFeature nodeFeatureVec = getMapFeature(*ll, config_.reprType, config_.paramType, config_.nPoints, nodeFeatLength); @@ -57,6 +55,8 @@ LaneData MapGraphDataInterface::getLaneData(MapGraphConstPtr localSubmapGraph) { ConstLaneletOrAreas connectedLLs = localSubmapGraph->getLaneletEdges(*ll); for (const auto& connectedLL : connectedLLs) { + if (!connectedLL.isLanelet()) continue; + result.edgeList.resize(edgeCount + 1, 2); result.edgeList(edgeCount, 0) = llId2Index[la.id()]; result.edgeList(edgeCount, 1) = llId2Index[connectedLL.id()]; diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index a628e87d..f639452b 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -73,11 +73,33 @@ BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t return bdInterp; } -BasicLineString3d cutLineString(const OrientedRect& bbox, const BasicLineString3d& polyline, int32_t nPoints) { - std::deque output; - boost::geometry::intersection(bbox, polyline, output); - assert(output.size() == 1); - return output[0]; +BasicLineString3d cutLineString(const OrientedRect& bbox, const BasicLineString3d& polyline) { + BasicLineString2d polyline2d; + for (const auto& pt : polyline) { + polyline2d.push_back(BasicPoint2d(pt.x(), pt.y())); + } + std::deque cut2d; + boost::geometry::intersection(bbox, polyline2d, cut2d); + assert(cut2d.size() == 1); + + // restore z value from closest point on the original linestring + BasicLineString3d cut3d; + for (const auto& pt2d : cut2d[0]) { + double lastDist = std::numeric_limits::max(); + for (const auto& pt : polyline) { + double currDist = (pt2d - BasicPoint2d(pt.x(), pt.y())).norm(); + if (currDist < lastDist) { + lastDist = currDist; + if (pt == polyline.back()) { + cut3d.push_back(BasicPoint3d(pt2d.x(), pt2d.y(), pt.z())); + } + } else { + cut3d.push_back(BasicPoint3d(pt2d.x(), pt2d.y(), pt.z())); + } + } + } + + return cut3d; } } // namespace map_learning From 44d61948819072a6e233d40722db8f2a91d7e248 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Wed, 22 Nov 2023 21:29:38 +0100 Subject: [PATCH 16/64] first draft map features --- .../include/lanelet2_map_learning/MapData.h | 6 +- ...raphDataInterface.h => MapDataInterface.h} | 6 +- .../lanelet2_map_learning/MapFeatures.h | 25 ++++--- ...DataInterface.cpp => MapDataInterface.cpp} | 17 +++-- lanelet2_map_learning/src/MapFeatures.cpp | 70 +++++++++++++++---- 5 files changed, 86 insertions(+), 38 deletions(-) rename lanelet2_map_learning/include/lanelet2_map_learning/{MapGraphDataInterface.h => MapDataInterface.h} (90%) rename lanelet2_map_learning/src/{MapGraphDataInterface.cpp => MapDataInterface.cpp} (88%) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index 33fcc10a..ceffbe0b 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -22,12 +22,12 @@ class LaneData { LaneLineStringFeatures roadBorders_; // auxilliary features LaneLineStringFeatures laneDividers_; // auxilliary features - CompoundLaneLineStringFeatures compoundRoadBorders_; - CompoundLaneLineStringFeatures compoundLaneDividers_; + CompoundLaneLineStringFeatures compoundRoadBorders_; // auxilliary features + CompoundLaneLineStringFeatures compoundLaneDividers_; // auxilliary features CompoundLaneLineStringFeatures compoundCenterlines_; LaneletFeatures laneletFeatures_; // node features - Eigen::MatrixX2i edgeList_; // adjacency matrix (sparse) + Eigen::MatrixX2i edgeList_; // adjacency matrix (sparse) for centerlines Eigen::MatrixXd edgeFeatures_; // edge features }; diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h similarity index 90% rename from lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h rename to lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h index 59e59e65..fc3da698 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphDataInterface.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h @@ -12,7 +12,7 @@ class LaneletLayer; namespace map_learning { -class MapGraphDataInterface { +class MapDataInterface { public: template using FeatureBuffer = @@ -28,8 +28,8 @@ class MapGraphDataInterface { int nPoints{11}; }; - MapGraphDataInterface(LaneletMapConstPtr laneletMap, Configuration config = Configuration(), - Optional currPos = boost::none); + MapDataInterface(LaneletMapConstPtr laneletMap, Configuration config = Configuration(), + Optional currPos = boost::none); void setCurrPosAndExtractSubmap(const BasicPoint2d& pt); LaneData laneData(); diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index e6502c49..ee1fa4c2 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -19,7 +19,7 @@ class MapFeature { const bool& initialized() const { return initialized_; } const bool& valid() const { return valid_; } - virtual Eigen::VectorXd computeFeatureVector(bool /*unused*/) const = 0; + virtual Eigen::VectorXd computeFeatureVector(bool /*unused*/, bool /*unused*/) const = 0; virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) = 0; protected: @@ -37,7 +37,7 @@ class LineStringFeature : public MapFeature { public: const BasicLineString3d& rawFeature() const { return rawFeature_; } - virtual Eigen::VectorXd computeFeatureVector(bool /*unused*/) const = 0; + virtual Eigen::VectorXd computeFeatureVector(bool /*unused*/, bool /*unused*/) const = 0; virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) = 0; protected: @@ -57,7 +57,8 @@ class LaneLineStringFeature : public LineStringFeature { virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; virtual Eigen::VectorXd computeFeatureVector( - bool onlyPoints = false) const override; // uses processedFeature_ when available + bool onlyPoints, + bool pointsIn2d) const override; // uses processedFeature_ when available const bool& wasCut() const { return wasCut_; } const BasicLineString3d& cutFeature() const { return cutFeature_; } @@ -71,6 +72,10 @@ class LaneLineStringFeature : public LineStringFeature { LineStringType type_; }; +using MapFeatures = std::vector; +using LineStringFeatures = std::vector; +using LaneLineStringFeatures = std::vector; + class TEFeature : public LineStringFeature { public: TEFeature() {} @@ -78,8 +83,10 @@ class TEFeature : public LineStringFeature { : LineStringFeature(feature, mapID), teType_{type} {} virtual ~TEFeature() noexcept = default; - bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) override; - Eigen::VectorXd computeFeatureVector(bool onlyPoints = false) const override; + bool process(const OrientedRect& bbox, const ParametrizationType& paramType, + int32_t /*unused*/) override; // not implemented yet + Eigen::VectorXd computeFeatureVector(bool onlyPoints, + bool pointsIn2d) const override; // currently uses raw feature only const TEType& teType() { return teType_; } @@ -96,7 +103,7 @@ class LaneletFeature : public MapFeature { virtual ~LaneletFeature() noexcept = default; bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; - Eigen::VectorXd computeFeatureVector(bool onlyPoints = false) const override; + Eigen::VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const override; void setReprType(LaneletRepresentationType reprType) { reprType_ = reprType; } @@ -124,7 +131,7 @@ class CompoundLaneLineStringFeature : public LaneLineStringFeature { const std::vector& pathLengthsProcessed() const { return pathLengthsProcessed_; } bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; - Eigen::VectorXd computeFeatureVector(bool onlyPoints = false) const override; + Eigen::VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const override; private: LaneLineStringFeatures individualFeatures_; @@ -132,11 +139,11 @@ class CompoundLaneLineStringFeature : public LaneLineStringFeature { std::vector pathLengthsProcessed_; }; -using LineStringFeatures = std::vector; -using LaneLineStringFeatures = std::vector; using CompoundLaneLineStringFeatures = std::vector; using TEFeatures = std::vector; using LaneletFeatures = std::vector; +Eigen::MatrixXd getFeatureVectorMatrix(const MapFeatures& mapFeatures, bool onlyPoints, bool pointsIn2d); + } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapGraphDataInterface.cpp b/lanelet2_map_learning/src/MapDataInterface.cpp similarity index 88% rename from lanelet2_map_learning/src/MapGraphDataInterface.cpp rename to lanelet2_map_learning/src/MapDataInterface.cpp index 5c0e8a1c..39841814 100644 --- a/lanelet2_map_learning/src/MapGraphDataInterface.cpp +++ b/lanelet2_map_learning/src/MapDataInterface.cpp @@ -1,4 +1,4 @@ -#include "lanelet2_map_learning/MapGraphDataInterface.h" +#include "lanelet2_map_learning/MapDataInterface.h" #include #include @@ -15,21 +15,20 @@ namespace lanelet { namespace map_learning { -void MapGraphDataInterface::setCurrPosAndExtractSubmap(const BasicPoint3d& pt) { +void MapDataInterface::setCurrPosAndExtractSubmap(const BasicPoint3d& pt) { currPos_ = pt; localSubmap_ = extractSubmap(laneletMap_, *currPos_, *currYaw_, config_.submapAreaLongitudinal, config_.submapAreaLateral); localSubmapGraph_ = MapGraph::build(*laneletMap_, *trafficRules_); } -MapGraphDataInterface::MapGraphDataInterface(LaneletMapConstPtr laneletMap, Configuration config, - Optional currPos) +MapDataInterface::MapDataInterface(LaneletMapConstPtr laneletMap, Configuration config, Optional currPos) : laneletMap_{laneletMap}, config_{config}, currPos_{currPos}, trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} -LaneData MapGraphDataInterface::getLaneData(MapGraphConstPtr localSubmapGraph) { +LaneData MapDataInterface::getLaneData(MapGraphConstPtr localSubmapGraph) { const auto& graph = localSubmapGraph->graph_; const auto& llVertices = graph->vertexLookup(); @@ -76,8 +75,8 @@ bool isTe(ConstLineString3d ls) { return type == AttributeValueString::TrafficLight || type == AttributeValueString::TrafficSign; } -TEData MapGraphDataInterface::getLaneTEData(MapGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, - std::unordered_map& teId2Index) { +TEData MapDataInterface::getLaneTEData(MapGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, + std::unordered_map& teId2Index) { const auto& graph = localSubmapGraph->graph_; const auto& llVertices = graph->vertexLookup(); @@ -144,7 +143,7 @@ TEData MapGraphDataInterface::getLaneTEData(MapGraphConstPtr localSubmapGraph, L return result; } -LaneData MapGraphDataInterface::laneData() { +LaneData MapDataInterface::laneData() { if (!currPos_) { throw InvalidObjectStateError( "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); @@ -153,7 +152,7 @@ LaneData MapGraphDataInterface::laneData() { return getLaneData(localSubmapGraph_); } -TEData MapGraphDataInterface::teData() { +TEData MapDataInterface::teData() { if (!currPos_) { throw InvalidObjectStateError( "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index 57bdc965..7fb57b2a 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -52,13 +52,21 @@ bool LaneLineStringFeature::process(const OrientedRect& bbox, const Parametrizat return result.valid_; } -Eigen::VectorXd LaneLineStringFeature::computeFeatureVector(bool onlyPoints) const { +Eigen::VectorXd LaneLineStringFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { const BasicLineString3d& selectedFeature = (cutAndResampledFeature_.size() > 0) ? cutAndResampledFeature_ : rawFeature_; - Eigen::VectorXd vec(3 * selectedFeature.size() + 1); // n points with 3 dims + type - for (size_t i = 0; i < selectedFeature.size(); i++) { - vec(Eigen::seq(i, i + 2)) = selectedFeature[i](Eigen::seq(i, i + 2)); + Eigen::VectorXd vec = pointsIn2d ? Eigen::VectorXd(2 * selectedFeature.size() + 1) + : Eigen::VectorXd(3 * selectedFeature.size() + 1); // n points with 2/3 dims + type + if (pointsIn2d == true) { + for (size_t i = 0; i < selectedFeature.size(); i++) { + vec(Eigen::seq(2 * i, 2 * i + 1)) = selectedFeature[i](Eigen::seq(0, 1)); + } + } else { + for (size_t i = 0; i < selectedFeature.size(); i++) { + vec(Eigen::seq(3 * i, 3 * i + 2)) = selectedFeature[i](Eigen::seq(0, 2)); + } } + vec[vec.size() - 1] = static_cast(type_); if (onlyPoints) { return vec(Eigen::seq(0, vec.size() - 1)); @@ -67,13 +75,24 @@ Eigen::VectorXd LaneLineStringFeature::computeFeatureVector(bool onlyPoints) con } } -Eigen::VectorXd TEFeature::computeFeatureVector(bool onlyPoints = false) const { - Eigen::VectorXd vec(3 * rawFeature_.size() + 1); // n points with 3 dims + type - for (size_t i = 0; i < rawFeature_.size(); i++) { - vec(Eigen::seq(i, i + 2)) = rawFeature_[i](Eigen::seq(i, i + 2)); +Eigen::VectorXd TEFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { + Eigen::VectorXd vec = pointsIn2d ? Eigen::VectorXd(2 * rawFeature_.size() + 1) + : Eigen::VectorXd(3 * rawFeature_.size() + 1); // n points with 2/3 dims + type + if (pointsIn2d == true) { + for (size_t i = 0; i < rawFeature_.size(); i++) { + vec(Eigen::seq(2 * i, 2 * i + 1)) = rawFeature_[i](Eigen::seq(0, 1)); + } + } else { + for (size_t i = 0; i < rawFeature_.size(); i++) { + vec(Eigen::seq(3 * i, 3 * i + 2)) = rawFeature_[i](Eigen::seq(0, 2)); + } } vec[vec.size() - 1] = static_cast(teType_); - return vec; + if (onlyPoints) { + return vec(Eigen::seq(0, vec.size() - 1)); + } else { + return vec; + } } LaneletFeature::LaneletFeature(const LaneLineStringFeature& leftBoundary, const LaneLineStringFeature& rightBoundary, @@ -101,9 +120,9 @@ bool LaneletFeature::process(const OrientedRect& bbox, const ParametrizationType } } -Eigen::VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints) const { +Eigen::VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { if (reprType_ == LaneletRepresentationType::Centerline) { - Eigen::VectorXd vecCenterlinePts = centerline_.computeFeatureVector(true); + Eigen::VectorXd vecCenterlinePts = centerline_.computeFeatureVector(true, pointsIn2d); Eigen::VectorXd vec(vecCenterlinePts.size() + 2); // pts vec + left and right type vec(Eigen::seq(0, vecCenterlinePts.size() - 1)) = vecCenterlinePts; vec[vec.size() - 2] = static_cast(leftBoundary_.type()); @@ -114,8 +133,8 @@ Eigen::VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints) const { return vec; } } else if (reprType_ == LaneletRepresentationType::Boundaries) { - Eigen::VectorXd vecLeftBdPts = leftBoundary_.computeFeatureVector(true); - Eigen::VectorXd vecRightBdPts = rightBoundary_.computeFeatureVector(true); + Eigen::VectorXd vecLeftBdPts = leftBoundary_.computeFeatureVector(true, pointsIn2d); + Eigen::VectorXd vecRightBdPts = rightBoundary_.computeFeatureVector(true, pointsIn2d); Eigen::VectorXd vec(vecLeftBdPts.size() + vecRightBdPts.size() + 2); // pts vec + left and right type vec(Eigen::seq(0, vecLeftBdPts.size() - 1)) = vecLeftBdPts; vec(Eigen::seq(vecLeftBdPts.size(), vecLeftBdPts.size() + vecRightBdPts.size() - 1)) = vecRightBdPts; @@ -139,7 +158,11 @@ CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStrin for (size_t i = 0; i < features.size(); i++) { assert(features[i].rawFeature().size() > 1); - rawFeature_.insert(rawFeature_.end(), features[i].rawFeature().begin(), features[i].rawFeature().end() - 1); + if (i == features.size() - 1) { + rawFeature_.insert(rawFeature_.end(), features[i].rawFeature().begin(), features[i].rawFeature().end()); + } else { + rawFeature_.insert(rawFeature_.end(), features[i].rawFeature().begin(), features[i].rawFeature().end() - 1); + } double rawLength = boost::geometry::length(features[i].rawFeature(), boost::geometry::strategy::distance::pythagoras()); @@ -152,5 +175,24 @@ CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStrin } } +Eigen::MatrixXd getFeatureVectorMatrix(const MapFeatures& mapFeatures, bool onlyPoints, bool pointsIn2d) { + assert(!mapFeatures.empty()); + std::vector featureVectors; + for (const auto& feat : mapFeatures) { + featureVectors.push_back(feat.computeFeatureVector(onlyPoints, pointsIn2d)); + } + if (std::adjacent_find(featureVectors.begin(), featureVectors.end(), + [](const Eigen::VectorXd& v1, const Eigen::VectorXd& v2) { return v1.size() != v2.size(); }) == + featureVectors.end()) { + throw std::runtime_error( + "Unequal length of feature vectors! To create a matrix all feature vectors must have the same length!"); + } + Eigen::MatrixXd featureMat(featureVectors.size(), featureVectors[0].size()); + for (size_t i = 0; i < featureVectors.size(); i++) { + featureMat.row(i) = featureVectors[i]; + } + return featureMat; +} + } // namespace map_learning } // namespace lanelet \ No newline at end of file From 132021646ee76976cd4f3432585d1d7459afcd75 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 23 Nov 2023 18:10:35 +0100 Subject: [PATCH 17/64] wip MapData --- .vscode/settings.json | 9 +++++++ .../include/lanelet2_map_learning/MapData.h | 13 ++++++++- .../include/lanelet2_map_learning/MapGraph.h | 2 +- lanelet2_map_learning/src/MapData.cpp | 27 ++++++++++++++++++- lanelet2_map_learning/src/MapFeatures.cpp | 4 ++- 5 files changed, 51 insertions(+), 4 deletions(-) create mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..0ee09f1b --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,9 @@ +{ + "python.autoComplete.extraPaths": [ + "/opt/mrtros/lib/python3/dist-packages" + ], + "python.analysis.extraPaths": [ + "/opt/mrtros/lib/python3/dist-packages" + ], + "cmake.sourceDirectory": "/home/immel/workspaces/av2_link_pred_ws/src" +} \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index ceffbe0b..f7549cba 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -8,15 +8,26 @@ #include "lanelet2_map_learning/Forward.h" #include "lanelet2_map_learning/MapFeatures.h" +#include "lanelet2_map_learning/MapGraph.h" #include "lanelet2_map_learning/Types.h" +#include "lanelet2_map_learning/internal/Graph.h" namespace lanelet { namespace map_learning { +// results are given in whatever order they are in the graph object, no guarantees! +LaneLineStringFeatures extractRoadBorders(MapGraphConstPtr localSubmapGraph); +LaneLineStringFeatures extractLaneDividers(MapGraphConstPtr localSubmapGraph); + +// results are given in whatever order they are in the graph object, no guarantees! +CompoundLaneLineStringFeatures extractCompoundRoadBorders(MapGraphConstPtr localSubmapGraph); +CompoundLaneLineStringFeatures extractCompoundLaneDividers(MapGraphConstPtr localSubmapGraph); +CompoundLaneLineStringFeatures extractCompoundCenterlines(MapGraphConstPtr localSubmapGraph); + class LaneData { public: LaneData() noexcept {} - LaneData(const ConstLanelets& lls); + LaneData(MapGraphConstPtr localSubmapGraph); private: LaneLineStringFeatures roadBorders_; // auxilliary features diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h index 6a96bcb5..7e936966 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h @@ -217,7 +217,7 @@ class MapGraph { */ MapGraph(std::unique_ptr&& graph, lanelet::LaneletSubmapConstPtr&& passableMap); - friend class MapGraphDataInterface; + const internal::MapGraphGraph& internalGraph() const { return *graph_; } private: //! Documentation to be found in the cpp file. diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 47b5c4d9..5247f6ea 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -1,5 +1,30 @@ #include "lanelet2_map_learning/MapData.h" namespace lanelet { -namespace map_learning {} // namespace map_learning +namespace map_learning { + +LaneLineStringFeatures extractRoadBorders(MapGraphConstPtr localSubmapGraph) { + LaneLineStringFeatures roadBorders; + const auto& internalGraph = localSubmapGraph->internalGraph(); + const auto& llVertices = internalGraph.vertexLookup(); + + for (const auto& laWithVertex : llVertices) { + const auto& la = laWithVertex.first; + if (!la.isLanelet()) continue; + auto ll = laWithVertex.first.lanelet(); + const auto& vertex = laWithVertex.second; + } +} + +LaneLineStringFeatures extractLaneDividers(MapGraphConstPtr localSubmapGraph) {} + +CompoundLaneLineStringFeatures extractCompoundRoadBorders(MapGraphConstPtr localSubmapGraph) {} + +CompoundLaneLineStringFeatures extractCompoundLaneDividers(MapGraphConstPtr localSubmapGraph) {} + +CompoundLaneLineStringFeatures extractCompoundCenterlines(MapGraphConstPtr localSubmapGraph) {} + +LaneData::LaneData(MapGraphConstPtr localSubmapGraph) {} + +} // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index 7fb57b2a..e659b44f 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -179,7 +179,9 @@ Eigen::MatrixXd getFeatureVectorMatrix(const MapFeatures& mapFeatures, bool only assert(!mapFeatures.empty()); std::vector featureVectors; for (const auto& feat : mapFeatures) { - featureVectors.push_back(feat.computeFeatureVector(onlyPoints, pointsIn2d)); + if (!feat.valid()) { + throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); + } } if (std::adjacent_find(featureVectors.begin(), featureVectors.end(), [](const Eigen::VectorXd& v1, const Eigen::VectorXd& v2) { return v1.size() != v2.size(); }) == From aadf3947940caf953abca822ec2039092cf213b0 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 23 Nov 2023 20:51:10 +0100 Subject: [PATCH 18/64] wip MapData and remove copy of lanelet2_routing classes --- .../lanelet2_map_learning/Exceptions.h | 23 - .../include/lanelet2_map_learning/Forward.h | 130 +----- .../include/lanelet2_map_learning/MapData.h | 32 +- .../lanelet2_map_learning/MapDataInterface.h | 12 +- .../lanelet2_map_learning/MapFeatures.h | 12 +- .../include/lanelet2_map_learning/MapGraph.h | 229 ---------- .../lanelet2_map_learning/MapGraphContainer.h | 78 ---- .../include/lanelet2_map_learning/Types.h | 12 - .../lanelet2_map_learning/internal/.gitignore | 0 .../lanelet2_map_learning/internal/Graph.h | 196 --------- .../internal/GraphUtils.h | 399 ------------------ .../internal/MapGraphBuilder.h | 64 --- .../internal/MapGraphVisualization.h | 122 ------ lanelet2_map_learning/package.xml | 1 + lanelet2_map_learning/src/MapData.cpp | 75 +++- .../src/MapDataInterface.cpp | 4 +- 16 files changed, 104 insertions(+), 1285 deletions(-) delete mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/Exceptions.h delete mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h delete mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/MapGraphContainer.h delete mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/internal/.gitignore delete mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h delete mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/internal/GraphUtils.h delete mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphBuilder.h delete mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphVisualization.h diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Exceptions.h b/lanelet2_map_learning/include/lanelet2_map_learning/Exceptions.h deleted file mode 100644 index 8a86912c..00000000 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Exceptions.h +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once -#include -#include - -#include - -namespace lanelet { -/** - * @brief Thrown when an export to the provided file(name) cannot be done. - */ -class ExportError : public LaneletError { - using LaneletError::LaneletError; -}; - -/** - * @brief Thrown when an there's an error in the graph. - * It will feature further information. - */ -class MapGraphError : public LaneletError { - using LaneletError::LaneletError; -}; - -} // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h index bebc584e..75fcab26 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h @@ -1,139 +1,37 @@ #pragma once #include - -#include -#include -#include -#include +#include namespace lanelet { namespace map_learning { -namespace internal { - -using IdPair = std::pair; - -template -class Graph; - -class MapGraphGraph; -class RouteGraph; -} // namespace internal - -using LaneId = uint16_t; -class MapGraph; -using MapGraphPtr = std::shared_ptr; -using MapGraphUPtr = std::unique_ptr; -using MapGraphConstPtr = std::shared_ptr; - -class MapGraphContainer; -using MapGraphContainerUPtr = std::unique_ptr; -struct LaneletRelation; -using LaneletRelations = std::vector; - -//! This enum expresses the types of relations lanelet2 distiguishes internally. Between two lanelets a and b (in this -//! order), exactly one of these relation exists. -//! -//! @note The relation between b and a is different than between a and b. There is also no obvious -//! symmetry. When a is left of b, b can be either right or adjacent right to b. -enum class RelationType : uint8_t { - None = 0, //!< No relation - Successor = 0b1, //!< A (the only) direct, reachable successor. Not merging and not diverging. - Left = 0b10, //!< (the only) directly adjacent, reachable left neighbour - Right = 0b100, //!< (the only) directly adjacent, reachable right neighbour - AdjacentLeft = 0b1000, //!< directly adjacent, unreachable left neighbor - AdjacentRight = 0b10000, //!< directly adjacent, unreachable right neighbor - Conflicting = 0b100000, //!< Unreachable but with overlapping shape - Area = 0b1000000 //!< Adjacent to a reachable area -}; - -using RelationUnderlyingType = std::underlying_type_t; - -constexpr RelationType allRelations() { return static_cast(0b1111111); } -static_assert(allRelations() > RelationType::Area, "allRelations is wrong!"); - -constexpr RelationType operator~(RelationType r) { return RelationType(~RelationUnderlyingType(r)); } -constexpr RelationType operator&(RelationType r1, RelationType r2) { - return RelationType(RelationUnderlyingType(r1) & RelationUnderlyingType(r2)); -} -constexpr RelationType operator&=(RelationType& r1, RelationType r2) { return r1 = r1 & r2; } -constexpr RelationType operator|(RelationType r1, RelationType r2) { - return RelationType(std::underlying_type_t(r1) | RelationUnderlyingType(r2)); -} -constexpr RelationType operator|=(RelationType& r1, RelationType r2) { return r1 = r1 | r2; } - -// Used for graph export -inline std::string relationToString(RelationType type) { - switch (type) { - case RelationType::None: - return "None"; - case RelationType::Successor: - return "Successor"; - case RelationType::Left: - return "Left"; - case RelationType::Right: - return "Right"; - case RelationType::AdjacentLeft: - return "AdjacentLeft"; - case RelationType::AdjacentRight: - return "AdjacentRight"; - case RelationType::Conflicting: - return "Conflicting"; - case RelationType::Area: - return "Area"; - } - return ""; // some compilers need that -} - -inline int relationToInt(RelationType type) { +/// @brief the distinction between left and right disappears here, +/// generalized to passable and not passable (adjacent) +inline int relationToInt(lanelet::routing::RelationType type) { switch (type) { - case RelationType::None: + case routing::RelationType::None: return 0; - case RelationType::Successor: + case routing::RelationType::Successor: return 1; - case RelationType::Left: + case routing::RelationType::Left: + return 2; + case routing::RelationType::Right: return 2; - case RelationType::Right: + case routing::RelationType::AdjacentLeft: return 3; - case RelationType::AdjacentLeft: - return 4; - case RelationType::AdjacentRight: - return 5; - case RelationType::Conflicting: + case routing::RelationType::AdjacentRight: + return 3; + case routing::RelationType::Conflicting: throw std::runtime_error("The relation type Conflicting should not exist in the graph!"); - case RelationType::Area: + case routing::RelationType::Area: throw std::runtime_error("The relation type Area should not exist in the graph!"); } return 0; // some compilers need that } -inline std::string relationToColor(RelationType type) { - switch (type) { - case RelationType::None: - return ""; - case RelationType::Successor: - return "green"; - case RelationType::Left: - return "blue"; - case RelationType::Right: - return "magenta"; - case RelationType::Conflicting: - return "red"; - case RelationType::AdjacentLeft: - case RelationType::AdjacentRight: - return "black"; - case RelationType::Area: - return "yellow"; - } - return ""; // some compilers need that -} - enum class LaneletRepresentationType; enum class ParametrizationType; -struct LaneData; -struct TEData; -class MapGraphDataInterface; } // namespace map_learning } // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index f7549cba..6bc869f7 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -2,32 +2,37 @@ #include #include #include +#include #include #include #include "lanelet2_map_learning/Forward.h" #include "lanelet2_map_learning/MapFeatures.h" -#include "lanelet2_map_learning/MapGraph.h" #include "lanelet2_map_learning/Types.h" -#include "lanelet2_map_learning/internal/Graph.h" +#include "lanelet2_routing/RoutingGraph.h" +#include "lanelet2_routing/internal/Graph.h" namespace lanelet { namespace map_learning { -// results are given in whatever order they are in the graph object, no guarantees! -LaneLineStringFeatures extractRoadBorders(MapGraphConstPtr localSubmapGraph); -LaneLineStringFeatures extractLaneDividers(MapGraphConstPtr localSubmapGraph); +struct Edge { + Edge(Id el1, Id el2) : el1_{el1}, el2_{el2} {} + Id el1_; + Id el2_; +}; +struct LaneEdge : Edge { + LaneEdge(Id el1, Id el2, lanelet::routing::RelationType type) : Edge(el1, el2), type_{type} {} + lanelet::routing::RelationType type_; +}; -// results are given in whatever order they are in the graph object, no guarantees! -CompoundLaneLineStringFeatures extractCompoundRoadBorders(MapGraphConstPtr localSubmapGraph); -CompoundLaneLineStringFeatures extractCompoundLaneDividers(MapGraphConstPtr localSubmapGraph); -CompoundLaneLineStringFeatures extractCompoundCenterlines(MapGraphConstPtr localSubmapGraph); +using Edges = std::vector; +using LaneEdges = std::vector; class LaneData { public: LaneData() noexcept {} - LaneData(MapGraphConstPtr localSubmapGraph); + LaneData(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); private: LaneLineStringFeatures roadBorders_; // auxilliary features @@ -38,8 +43,7 @@ class LaneData { CompoundLaneLineStringFeatures compoundCenterlines_; LaneletFeatures laneletFeatures_; // node features - Eigen::MatrixX2i edgeList_; // adjacency matrix (sparse) for centerlines - Eigen::MatrixXd edgeFeatures_; // edge features + LaneEdges edgeList_; // edge list for centerlines }; class TEData { @@ -50,11 +54,9 @@ class TEData { LaneletFeatures laneletFeatures; // node features lanelets TEFeatures teFeatures; // node features traffic elements + Edges edgeList_; // edge list /// @brief adjacency matrix (sparse). Node indices are assigned as /// xLane and xTE stacked, with xLane being first - Eigen::MatrixX2i edgeList; - - Eigen::MatrixXd edgeFeatures; // edge features }; } // namespace map_learning diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h index fc3da698..819889ec 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h @@ -4,8 +4,10 @@ #include #include "lanelet2_map_learning/Forward.h" -#include "lanelet2_map_learning/MapGraph.h" -#include "lanelet2_map_learning/internal/Graph.h" +#include "lanelet2_map_learning/MapData.h" +#include "lanelet2_map_learning/Types.h" +#include "lanelet2_routing/RoutingGraph.h" +#include "lanelet2_routing/internal/Graph.h" namespace lanelet { class LaneletLayer; @@ -39,15 +41,15 @@ class MapDataInterface { TEData laneTEDataBatch(const BasicPoints2d& pts, const std::vector& yaws); private: - LaneData getLaneData(MapGraphConstPtr localSubmapGraph); + LaneData getLaneData(lanelet::routing::RoutingGraphConstPtr localSubmapGraph); - TEData getLaneTEData(MapGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, + TEData getLaneTEData(lanelet::routing::RoutingGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, std::unordered_map& teId2Index); LaneletMapConstPtr laneletMap_; LaneletSubmapConstPtr localSubmap_; std::unordered_map teId2Index_; - MapGraphConstPtr localSubmapGraph_; + lanelet::routing::RoutingGraphConstPtr localSubmapGraph_; Optional currPos_; // in the map frame Optional currYaw_; // in the map frame Configuration config_; diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index ee1fa4c2..d7bc8d5d 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -72,9 +72,9 @@ class LaneLineStringFeature : public LineStringFeature { LineStringType type_; }; -using MapFeatures = std::vector; -using LineStringFeatures = std::vector; -using LaneLineStringFeatures = std::vector; +using MapFeatures = std::map; +using LineStringFeatures = std::map; +using LaneLineStringFeatures = std::map; class TEFeature : public LineStringFeature { public: @@ -139,9 +139,9 @@ class CompoundLaneLineStringFeature : public LaneLineStringFeature { std::vector pathLengthsProcessed_; }; -using CompoundLaneLineStringFeatures = std::vector; -using TEFeatures = std::vector; -using LaneletFeatures = std::vector; +using CompoundLaneLineStringFeatures = std::map; +using TEFeatures = std::map; +using LaneletFeatures = std::map; Eigen::MatrixXd getFeatureVectorMatrix(const MapFeatures& mapFeatures, bool onlyPoints, bool pointsIn2d); diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h deleted file mode 100644 index 7e936966..00000000 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraph.h +++ /dev/null @@ -1,229 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include -#include - -#include - -#include "lanelet2_map_learning/Forward.h" -#include "lanelet2_map_learning/Types.h" - -namespace lanelet { -namespace map_learning { - -/** @brief Main class of the map learning module that holds the graph information and can be queried. - * The MapGraph class is the central object of this module and is initialized with a LaneletMap and TrafficRules. - * A graph with all interesting relations will be created for the traffic participant featured in the provided - * TrafficRules module. The graph can answer queries like "left", "following", "conflicting" lanelets. - * - * @note The direction of lanelets matters! 'lanelet' and 'lanelet.invert()' are differentiated since this matters when - * lanelets are passable in both directions. - * @note 'adjacent_left' and 'adjacent_right' means that there is a passable lanelet left/right of another passable - * lanelet, but a lane change is not allowed. */ -class MapGraph { - public: - using Errors = std::vector; ///< For the checkValidity function - using Configuration = std::map; ///< Used to provide a configuration - //! Defined configuration attributes - static constexpr const char ParticipantHeight[] = "participant_height"; - - /** @brief Main constructor with optional configuration. - * @param laneletMap Map that should be used to build the graph - * @param trafficRules Traffic rules that apply to find passable lanelets - * @param config Optional configuration */ - static MapGraphUPtr build(const LaneletMap& laneletMap, const traffic_rules::TrafficRules& trafficRules, - const Configuration& config = Configuration()); - - //! Similar to the above but for a LaneletSubmap - static MapGraphUPtr build(const LaneletSubmap& laneletSubmap, const traffic_rules::TrafficRules& trafficRules, - const Configuration& config = Configuration()); - - //! The graph can not be copied, only moved - MapGraph() = delete; - MapGraph(const MapGraph&) = delete; - MapGraph& operator=(const MapGraph&) = delete; - MapGraph(MapGraph&& /*other*/) noexcept; - MapGraph& operator=(MapGraph&& /*other*/) noexcept; - ~MapGraph(); - - /** @brief Determines the relation between two lanelets - * @param from Start lanelet - * @param to Goal lanelet - * @param includeConflicting if false, conflicting lanelets are not considered as relations - * @return Relation between the lanelets or nothing if no relation exists. Nothing if at least one of the lanelets - * is not passable. */ - Optional routingRelation(const ConstLanelet& from, const ConstLanelet& to, - bool includeConflicting = false) const; - - //! Return type neccessary for vertex std::map comparison - ConstLaneletOrAreas getLaneletEdges(const ConstLanelet& lanelet, bool edgesOut = true) const; - - /** @brief Returns the lanelets that can be reached from this lanelet. - * @param lanelet Start lanelet - * @param withLaneChanges Include left and right lanes or not - * @return Lanelets that can be directly reached - * @see followingRelations */ - ConstLanelets following(const ConstLanelet& lanelet, bool withLaneChanges = false) const; - - /** @brief Returns the lanelets that can be reached from this lanelet and the relation. - * @param lanelet Start lanelet - * @param withLaneChanges Include left and right lanes or not - * @return Lanelets can be directly reached - * @see following */ - LaneletRelations followingRelations(const ConstLanelet& lanelet, bool withLaneChanges = false) const; - - /** @brief Returns the possible previous lanelets of this lanelet. - * @param lanelet Start lanelet - * @param withLaneChanges Include left and right lanes or not - * @return All previous lanelets - * @see previousRelations */ - ConstLanelets previous(const ConstLanelet& lanelet, bool withLaneChanges = false) const; - - /** @brief Returns the possible previous lanelets of this lanelet and the relation. - * @param lanelet Start lanelet - * @param withLaneChanges Include left and right lanes or not - * @return Lanelets that could be used to reach this lanelet - * @see previous */ - LaneletRelations previousRelations(const ConstLanelet& lanelet, bool withLaneChanges = false) const; - - /** @brief Retrieve all reachable left and right lanelets - * @param lanelet Start lanelet - * @return All left and right lanelets that can be reached, including lanelet, ordered left to right. */ - ConstLanelets besides(const ConstLanelet& lanelet) const; - - /** @brief Get left (routable) lanelet of a given lanelet if it exists. - * @param lanelet Start lanelet - * @return Left lanelet if it exists. Nothing if it doesn't. - * @see adjacentLeft, lefts, adjacentLefts */ - Optional left(const ConstLanelet& lanelet) const; - - /** @brief Get adjacent left (non-routable) lanelet of a given lanelet if it exists. - * @param lanelet Start laneletfoo - * @return Adjacent left lanelet if it exists. Nothing if it doesn't. - * @see left, lefts, adjacentLefts */ - Optional adjacentLeft(const ConstLanelet& lanelet) const; - - /** @brief Get right (routable) lanelet of a given lanelet if it exists. - * @param lanelet Start lanelet - * @return Right lanelet if it exists. Nothing if it doesn't. - * @see adjacentRight, rights, adjacentRights */ - Optional right(const ConstLanelet& lanelet) const; - - /** @brief Get adjacent right (non-routable) lanelet of a given lanelet if it exists. - * @param lanelet Start lanelet - * @return Adjacent right lanelet if it exists. Nothing if it doesn't. - * @see right, rights, adjacentRights */ - Optional adjacentRight(const ConstLanelet& lanelet) const; - - /** @brief Get all left (routable) lanelets of a given lanelet if they exist. - * @param lanelet Start lanelet - * @return Left lanelets if they exists. Empty if they don't. - * @see adjacentLeft, left, adjacentLefts */ - ConstLanelets lefts(const ConstLanelet& lanelet) const; - - /** @brief Get all adjacent left (non-routable) lanelets of a given lanelet if they exist. - * @param lanelet Start lanelet - * @return Adjacent left lanelets if they exists. Empty if they don't. - * @see adjacentLeft, left, lefts */ - ConstLanelets adjacentLefts(const ConstLanelet& lanelet) const; - - /** @brief Retrieve all lanelets and relations left of a given lanelet. - * @param lanelet Start lanelet - * @return All lanelets and relations left of a given lanelet. - * @see lefts, adjacentLefts */ - LaneletRelations leftRelations(const ConstLanelet& lanelet) const; - - /** @brief Get all right (routable) lanelets of a given lanelet if they exist. - * @param lanelet Start lanelet - * @return Right lanelets if they exists. Empty if they don't. - * @see adjacentRight, right, adjacentRights */ - ConstLanelets rights(const ConstLanelet& lanelet) const; - - /** @brief Get all adjacent right (non-routable) lanelets of a given lanelet if they exist. - * @param lanelet Start lanelet - * @return Adjacent right lanelets if they exists. Empty if they don't. - * @see adjacentRight, right, rights */ - ConstLanelets adjacentRights(const ConstLanelet& lanelet) const; - - /** @brief Retrieve all lanelets and relations right of a given lanelet. - * @param lanelet Start lanelet - * @return All lanelets and relations right of a given lanelet. - * @see rights, adjacentRights */ - LaneletRelations rightRelations(const ConstLanelet& lanelet) const; - - /** @brief Retrieve all lanelets that are conflicting with the given lanelet. - * - * Conflicting means that their bounding boxes overlap and the height clearance is smaller than the specified - * "participant_height". - * @param laneletOrArea Lanelet/Area to get conflicting lanelets for. - * @return All conflicting lanelets. */ - ConstLaneletOrAreas conflicting(const ConstLaneletOrArea& laneletOrArea) const; - - /** @brief Export the internal graph to graphML (xml-based) file format. - * @param filename Fully qualified file name - ideally with extension (.graphml) - * @param edgeTypesToExclude Exclude the specified relations. E.g. conflicting. Combine them with "|". - @see exportGraphViz */ - void exportGraphML(const std::string& filename, const RelationType& edgeTypesToExclude = RelationType::None) const; - - /** @brief Export the internal graph to graphViz (DOT) file format. - * This format includes coloring of the edges in the graph and bears little more information than graphML export. - * @param filename Fully qualified file name - ideally with extension (.gv) - * @param edgeTypesToExclude Exclude the specified relations. E.g. conflicting. Combine them with "|". */ - void exportGraphViz(const std::string& filename, const RelationType& edgeTypesToExclude = RelationType::None) const; - - /** @brief An abstract lanelet map holding the information of the graph. - * A good way to view the graph since it can be exported using the lanelet2_io module and there can be - * viewed in tools like JOSM. Each lanelet is represented by a point at the center of gravity of the lanelet. - * Relations are linestrings between points representing lanelets. - * @param includeAdjacent Also include adjacent (non-routable) relations - * @param includeConflicting Also include conflicting relations - * @return LaneletMap with the requested information */ - LaneletMapPtr getDebugLaneletMap(bool includeAdjacent = false, bool includeConflicting = false) const; - - /** - * @brief Returns a submap that contains all lanelets and areas within this graph, and nothing else. - * You can obtain a full map of the graph by calling passabelSubmap()->laneletMap(), which ist a potentially - * costly operation. - */ - inline LaneletSubmapConstPtr passableSubmap() const noexcept { return passableLaneletSubmap_; } - - /** @brief LaneletSubmap that includes all passable lanelets and areas. - * This map contains all passable lanelets and areas with all primitives (linestrings, points), including regulatory - * elements and lanelets referenced by them. It can be used to perform spacial queries e.g. for localization. When - * selecting a lanelet from this map please be aware that the graph may also contain the inverted lanelet. - * @return LaneletMap with all passable lanelets and areas - * - * This function is deprecated because it was misleading that the map also contained lanelets referenced by regulatory - * elements and not only the lanelets from the graph. - */ - [[deprecated("Use passableSubmap to obtain the lanelets and areas within the graph!")]] inline LaneletMapConstPtr - passableMap() const noexcept { - return passableSubmap()->laneletMap(); - } - - /** @brief Performs some basic sanity checks. - * It is recommended to call this function after the graph has been generated since it can point out some - * mapping errors. - * @throws MapGraphError if an error is found an 'throwOnError' is true - * @param throwOnError Decide wheter to throw an exception or just return the errors - * @return Possible errors if 'throwOnError' is false. */ - Errors checkValidity(bool throwOnError = true) const; - - /** - * Constructs the graph. Don't call this directly, use MapGraph::build instead. - */ - MapGraph(std::unique_ptr&& graph, lanelet::LaneletSubmapConstPtr&& passableMap); - - const internal::MapGraphGraph& internalGraph() const { return *graph_; } - - private: - //! Documentation to be found in the cpp file. - std::unique_ptr graph_; ///< Wrapper of the graph - LaneletSubmapConstPtr passableLaneletSubmap_; ///< Lanelet map of all passable lanelets -}; - -} // namespace map_learning -} // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphContainer.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphContainer.h deleted file mode 100644 index 2053c774..00000000 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapGraphContainer.h +++ /dev/null @@ -1,78 +0,0 @@ -#pragma once - -#include -#include -#include - -#include -#include - -#include "lanelet2_map_learning/MapGraph.h" - -namespace lanelet { -namespace map_learning { - -/** @brief Container to associate multiple graphs to allow queries on multiple graphs - * @note We cannot use the 'conflicting' relations that have been determined when creating the individual graphs - * because they used their respective height in 3D (e.g. 2m for a pedestrian), but the participant we want to query for - * could be taller (e.g. 4m truck). Therefore we can't rely on that. */ -class MapGraphContainer { - public: - using ConflictingInGraph = std::pair; //!< id of conflicing graph, lanelets in conflict there - using ConflictingInGraphs = std::vector; - - /** @brief Constructor of graph container - * @param MapGraphs The graphs that should be used in the container */ - explicit MapGraphContainer(std::vector MapGraphs) : graphs_{std::move(MapGraphs)} {} - - /** @brief Constructor of graph container - * @param MapGraphs The graphs that should be used in the container */ - explicit MapGraphContainer(const std::vector& MapGraphs) - : graphs_{utils::transform(MapGraphs, [](auto& g) { return MapGraphConstPtr(g); })} {} - - /** @brief Find the conflicting lanelets of a given lanelet within a specified graph - * @param lanelet Find conflicting ones for this lanelet - * @param MapGraphId ID/position in vector of the graph - * @param participantHeight Optional height of the participant - * @return Conflicting lanelets within that graph - * @throws InvalidInputError if the MapGraphId is too high */ - ConstLanelets conflictingInGraph(const ConstLanelet& lanelet, size_t MapGraphId, - double participantHeight = .0) const { - if (MapGraphId >= graphs_.size()) { - throw InvalidInputError("Graph ID is higher than the number of graphs."); - } - auto overlaps = [lanelet, participantHeight](const ConstLanelet& ll) { - return participantHeight != .0 ? !geometry::overlaps3d(lanelet, ll, participantHeight) - : !geometry::overlaps2d(lanelet, ll); - }; - const auto map{graphs_[MapGraphId]->passableSubmap()}; - ConstLanelets conflicting{map->laneletLayer.search(geometry::boundingBox2d(lanelet))}; - auto begin = conflicting.begin(); - auto end = conflicting.end(); - end = std::remove(begin, end, lanelet); - end = std::remove_if(begin, end, overlaps); - conflicting.erase(end, conflicting.end()); - return conflicting; - } - - /** @brief Find the conflicting lanelets of a given lanelet within all graphs - * @param lanelet Find conflicting ones for this lanelet - * @param participantHeight Optional height of the participant - * @return Conflicting lanelets within the graphs */ - ConflictingInGraphs conflictingInGraphs(const ConstLanelet& lanelet, double participantHeight = .0) const { - ConflictingInGraphs result; - for (size_t it = 0; it < graphs_.size(); it++) { - result.emplace_back(std::make_pair(it, conflictingInGraph(lanelet, it, participantHeight))); - } - return result; - } - - //! Returns the graphs stored in the container - const std::vector& MapGraphs() const { return graphs_; } - - private: - std::vector graphs_; ///< Map graphs of the container. -}; - -} // namespace map_learning -} // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h index e111cb62..1e2f40ab 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h @@ -18,17 +18,5 @@ enum class TEType { TrafficLight, TrafficSign, Unknown }; using OrientedRect = boost::geometry::model::polygon; -//! Represents the relation of a lanelet to another lanelet -struct LaneletRelation { - ConstLanelet lanelet; //!< the lanelet this relation refers to - RelationType relationType; //!< the type of relation to that -}; -inline bool operator==(const LaneletRelation& lhs, const LaneletRelation& rhs) { - return lhs.lanelet == rhs.lanelet && lhs.relationType == rhs.relationType; -} -inline bool operator!=(const LaneletRelation& rhs, const LaneletRelation& lhs) { return !(rhs == lhs); } - -using LaneletRelations = std::vector; - } // namespace map_learning } // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/.gitignore b/lanelet2_map_learning/include/lanelet2_map_learning/internal/.gitignore deleted file mode 100644 index e69de29b..00000000 diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h b/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h deleted file mode 100644 index ea2f0866..00000000 --- a/lanelet2_map_learning/include/lanelet2_map_learning/internal/Graph.h +++ /dev/null @@ -1,196 +0,0 @@ -#pragma once - -#include - -#include -#include -#include -#include - -#include "lanelet2_map_learning/Exceptions.h" -#include "lanelet2_map_learning/Forward.h" - -namespace lanelet { -namespace map_learning { -namespace internal { -/** @brief Internal information of a vertex in the graph - * If A* search is adapted, this could hold information about longitude and latitude. */ -struct VertexInfo { - // careful. You must be sure that this is indeed a lanelet - const ConstLanelet& lanelet() const { return static_cast(laneletOrArea); } - const ConstArea& area() const { return static_cast(laneletOrArea); } - const ConstLaneletOrArea& get() const { return laneletOrArea; } - ConstLaneletOrArea laneletOrArea; -}; - -/** @brief Internal information of an edge in the graph */ -struct EdgeInfo { - RelationType relation; ///< Relation between the two lanelets. E.g. SUCCESSOR or CONFLICTING. -}; - -/// General graph type definitions -using GraphType = boost::adjacency_list; -using GraphTraits = boost::graph_traits; - -template -struct EdgeFilter; - -/// Filtered graphs provide a filtered view on a graph. Here we can filter out types of relations (e.g. CONFLICTING) -template -using FilteredGraphT = boost::filtered_graph>; - -template -using FilteredGraphTraits = boost::graph_traits>; - -using FilteredMapGraph = FilteredGraphT; - -/** @brief An internal edge filter to get a filtered view on the graph. */ -template -struct EdgeFilter { - /// Needed to be able to iterate edges - EdgeFilter() = default; - - /// @brief Constructor for a filter that includes all relation types. - /// @param graph Base graph - EdgeFilter(const GraphT& graph) : pmRelation_{boost::get(&EdgeInfo::relation, graph)} {} - - /// @brief Constructor for a filter that includes all allowed relations in 'relation' - /// @param graph Base graph - /// @param relation Relation that will pass the filter - EdgeFilter(const GraphT& graph, const RelationType& relation) - : relations_{relation}, pmRelation_{boost::get(&EdgeInfo::relation, graph)} {} - - /// @brief Operator that determines wheter an edge should pass the filter or not. - template - inline bool operator()(const Edge& e) const { - return relations_ == allRelations() || bool(relations_ & boost::get(pmRelation_, e)); - } - - private: - RelationType relations_{allRelations()}; ///< Relations that pass the filter - typename boost::property_map::const_type - pmRelation_; ///< Property map to the relations of edges in the graph -}; - -using LaneletOrAreaToVertex = std::unordered_map; -using FilteredGraphDesc = std::pair; - -/// @brief Manages the actual graph and provides different views on the edges (lazily computed) -template -class Graph { - public: - using FilteredGraph = FilteredGraphT; - using Filter = EdgeFilter; - using Vertex = typename boost::graph_traits::vertex_descriptor; - using Edge = typename boost::graph_traits::edge_descriptor; - - explicit Graph() {} - - inline const BaseGraphT& get() const noexcept { return graph_; } - inline BaseGraphT& get() noexcept { return graph_; } - inline const LaneletOrAreaToVertex& vertexLookup() const noexcept { return laneletOrAreaToVertex_; } - - FilteredGraph withAllRelations() const { return FilteredGraph(graph_, Filter(graph_)); } - - FilteredGraph withLaneChanges() const { - return getFilteredGraph(RelationType::Successor | RelationType::Left | RelationType::Right); - } - - FilteredGraph withoutLaneChanges() const { return getFilteredGraph(RelationType::Successor); } - - FilteredGraph withAreasAndLaneChanges() const { - return getFilteredGraph(RelationType::Successor | RelationType::Left | RelationType::Right | RelationType::Area); - } - - FilteredGraph withAreasWithoutLaneChanges() const { - return getFilteredGraph(RelationType::Successor | RelationType::Area); - } - - FilteredGraph left() const { return getFilteredGraph(RelationType::Left); } - FilteredGraph somehowLeft() const { return getFilteredGraph(RelationType::Left | RelationType::AdjacentLeft); } - - FilteredGraph right() const { return getFilteredGraph(RelationType::Right); } - FilteredGraph somehowRight() const { return getFilteredGraph(RelationType::Right | RelationType::AdjacentRight); } - - FilteredGraph adjacentLeft() const { return getFilteredGraph(RelationType::AdjacentLeft); } - - FilteredGraph adjacentRight() const { return getFilteredGraph(RelationType::AdjacentRight); } - - FilteredGraph conflicting() const { return getFilteredGraph(RelationType::Conflicting); } - - FilteredGraph withoutConflicting() const { return getFilteredGraph(allRelations() | ~RelationType::Conflicting); } - - inline bool empty() const noexcept { return laneletOrAreaToVertex_.empty(); } - - //! add new lanelet to graph - inline Vertex addVertex(const typename BaseGraphT::vertex_property_type& property) { - GraphType::vertex_descriptor vd = 0; - vd = boost::add_vertex(graph_); - graph_[vd] = property; - laneletOrAreaToVertex_.emplace(property.get(), vd); - return vd; - } - - void addEdge(const ConstLaneletOrArea& from, const ConstLaneletOrArea& to, const EdgeInfo& edgeInfo) { - auto fromVertex = getVertex(from); - auto toVertex = getVertex(to); - if (!fromVertex || !toVertex) { - assert(false && "Lanelet/Area is not part of the graph."); // NOLINT - return; - } - addEdge(*fromVertex, *toVertex, edgeInfo); - } - - void addEdge(Vertex from, Vertex to, const EdgeInfo& edgeInfo) { - auto edge = boost::add_edge(from, to, graph_); - assert(edge.second && "Edge could not be added to the graph."); - graph_[edge.first] = edgeInfo; - } - - /** @brief Received the edgeInfo between two given vertices - * @return EdgeInfo or nothing if there's no edge */ - Optional getEdgeInfo(const ConstLanelet& from, const ConstLanelet& to) const noexcept { - return getEdgeInfoFor(from, to, graph_); - } - - /** @brief Received the edgeInfo between two given vertices and a given (filtered)graph - * @return EdgeInfo or nothing if there's no edge */ - template - Optional getEdgeInfoFor(const ConstLanelet& from, const ConstLanelet& to, const Graph& g) const noexcept { - auto fromVertex = getVertex(from); - auto toVertex = getVertex(to); - if (!fromVertex || !toVertex) { - return {}; - } - auto edgeToNext{boost::edge(*fromVertex, *toVertex, g)}; - if (edgeToNext.second) { - return g[edgeToNext.first]; - } - return {}; - } - - //! Helper function to determine the graph vertex of a given lanelet - Optional::vertex_descriptor> getVertex( - const ConstLaneletOrArea& lanelet) const noexcept { - try { - return laneletOrAreaToVertex_.at(lanelet); - } catch (std::out_of_range&) { - return {}; - } - } - - private: - FilteredGraph getFilteredGraph(RelationType relations) const { - return FilteredGraph(graph_, Filter(graph_, relations)); - } - BaseGraphT graph_; //!< The actual graph object - LaneletOrAreaToVertex laneletOrAreaToVertex_; //!< Mapping of lanelets/areas to vertices of the graph -}; - -class MapGraphGraph : public Graph { - using Graph::Graph; -}; - -} // namespace internal -} // namespace map_learning -} // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/GraphUtils.h b/lanelet2_map_learning/include/lanelet2_map_learning/internal/GraphUtils.h deleted file mode 100644 index 9506415b..00000000 --- a/lanelet2_map_learning/include/lanelet2_map_learning/internal/GraphUtils.h +++ /dev/null @@ -1,399 +0,0 @@ -#pragma once -#include - -#include -#include - -#include "lanelet2_map_learning/internal/Graph.h" - -namespace lanelet { -namespace map_learning { -namespace internal { -using LaneletVertexId = GraphTraits::vertex_descriptor; -using LaneletVertexIds = std::vector; -using RouteLanelets = std::set; - -template -inline bool has(const ContainerT& c, const T& t) { - return std::find(c.begin(), c.end(), t) != c.end(); -} -template -inline bool has(const std::set& c, const T& t) { - return c.find(t) != c.end(); -} -template -inline bool hasRelation(const GraphT& g, EdgeT e) { - return (g[e].relation & R) != RelationType::None; -} -template -Optional getNext(LaneletVertexId ofVertex, const Graph& g) { - auto edges = boost::out_edges(ofVertex, g); - auto it = std::find_if(edges.first, edges.second, [&](auto e) { return hasRelation(g, e); }); - if (it != edges.second) { - return boost::target(*it, g); - } - return {}; -} - -// Class that selects between in_edges and out_edges (i wish I had c++17...) -template -struct GetEdges {}; -template <> -struct GetEdges { - template - inline auto operator()(Id id, Graph& g) { - return in_edges(id, g); - } -}; -template <> -struct GetEdges { - template - inline auto operator()(Id id, Graph& g) { - return out_edges(id, g); - } -}; - -// Class that selects between in_edges and out_edges (i wish I had c++17...) -template -struct GetTarget {}; -template <> -struct GetTarget { - using T = FilteredMapGraph::vertex_descriptor; - template - inline T operator()(Id id, Graph& g) { - return boost::source(id, g); - } -}; -template <> -struct GetTarget { - using T = FilteredMapGraph::vertex_descriptor; - template - inline T operator()(Id id, Graph& g) { - return boost::target(id, g); - } -}; - -template -bool anySidewayNeighbourIs(Vertex v, const Graph& g, Func&& f) { - Optional currVertex = v; - while (!!currVertex && !f(*currVertex)) { - currVertex = getNext(*currVertex, g); - } - if (!!currVertex) { - return true; - } - currVertex = getNext(v, g); - while (!!currVertex && !f(*currVertex)) { - currVertex = getNext(*currVertex, g); - } - return !!currVertex; -} - -template -std::set getAllNeighbourLanelets(LaneletVertexId ofVertex, const Graph& ofRoute) { - std::set result{ofVertex}; - Optional currVertex = ofVertex; - while (!!currVertex) { - result.insert(*currVertex); - currVertex = getNext(*currVertex, ofRoute); - } - currVertex = ofVertex; - while (!!currVertex) { - result.insert(*currVertex); - currVertex = getNext(*currVertex, ofRoute); - } - return result; -} - -//! Filter that reduces the original graph by edges that belong to different cost types or lane changes -class OriginalGraphFilter { - public: - OriginalGraphFilter() = default; - OriginalGraphFilter(const GraphType& g, bool withLaneChange) - : g_{&g}, filterMask_{RelationType::Successor | RelationType::Conflicting} { - if (withLaneChange) { - filterMask_ |= - RelationType::Left | RelationType::Right | RelationType::AdjacentLeft | RelationType::AdjacentRight; - } - } - bool operator()(const GraphTraits::edge_descriptor& v) const { - const auto& edge = (*g_)[v]; - return (edge.relation & filterMask_) != RelationType::None; - } - - private: - const GraphType* g_; - RelationType filterMask_; -}; -using OriginalGraph = boost::filtered_graph; - -//! Reduces the graph to a set of desired vertices -class OnRouteFilter { - public: - OnRouteFilter() = default; - explicit OnRouteFilter(const RouteLanelets& onRoute) : onRoute_{&onRoute} {} - - bool operator()(LaneletVertexId vertexId) const { return onRoute_->find(vertexId) != onRoute_->end(); } - - private: - const RouteLanelets* onRoute_{}; -}; - -template -class EdgeRelationFilter { - public: - EdgeRelationFilter() = default; - explicit EdgeRelationFilter(const GraphType& graph) : graph_{&graph} {} - bool operator()(FilteredMapGraph::edge_descriptor e) const { - auto type = (*graph_)[e].relation; - return (type & Relation) != RelationType::None; - } - - private: - const GraphType* graph_{}; -}; - -//! Removes edges from the graph that are not drivable (e.g. adjacent or conflicing) -using OnlyDrivableEdgesFilter = - EdgeRelationFilter; -using OnlyConflictingFilter = EdgeRelationFilter; - -//! Removes conflicting edges from the graph -class NoConflictingFilter { - public: - NoConflictingFilter() = default; - explicit NoConflictingFilter(const OriginalGraph& originalGraph) : originalGraph_{&originalGraph} {} - bool operator()(FilteredMapGraph::edge_descriptor e) const { - auto type = (*originalGraph_)[e].relation; - return type != RelationType::Conflicting; - } - - private: - const OriginalGraph* originalGraph_{}; -}; - -//! Removes vertices from the graph that are not adjacent to a set of vertices. Adjacent can also mean conflicting! -class NextToRouteFilter { - public: - NextToRouteFilter() = default; - NextToRouteFilter(const RouteLanelets& onRoute, const OriginalGraph& originalGraph) - : onRoute_{&onRoute}, originalGraph_{&originalGraph} {} - - bool operator()(LaneletVertexId vertexId) const { - // at least one out edge must be connected to lanelets on the route. This includes conflicting and adjacent! - if (onRoute_->find(vertexId) != onRoute_->end()) { - return true; - } - auto outEdges = boost::out_edges(vertexId, *originalGraph_); - auto connectedToRoute = std::any_of(outEdges.first, outEdges.second, [&](OriginalGraph::edge_descriptor e) { - auto dest = boost::target(e, *originalGraph_); - return onRoute_->find(dest) != onRoute_->end() || - onRoute_->find(boost::source(e, *originalGraph_)) != onRoute_->end(); - }); - return connectedToRoute; - } - - private: - const RouteLanelets* onRoute_{}; - const OriginalGraph* originalGraph_{}; -}; - -//! Removes edges from the graph that are not drivable (e.g. adjacent or conflicing) OR leave the route at its end -class OnlyDrivableEdgesWithinFilter { - using SuccessorFilter = EdgeRelationFilter; - - public: - OnlyDrivableEdgesWithinFilter() = default; - explicit OnlyDrivableEdgesWithinFilter(RouteLanelets withinLanelets, const OriginalGraph& originalGraph) - : drivableEdge_{originalGraph}, successorEdge_{originalGraph}, withinLanelets_{std::move(withinLanelets)} {} - bool operator()(FilteredMapGraph::edge_descriptor e) const { - return drivableEdge_(e) && (!successorEdge_(e) || withinLanelets_.find(e.m_source) == withinLanelets_.end()); - } - - private: - OnlyDrivableEdgesFilter drivableEdge_; - SuccessorFilter successorEdge_; - RouteLanelets withinLanelets_; -}; - -//! Finds vertices that are in conflict or adjacent to some vertices (not reachable though bidirectional lane changes) -class ConflictingSectionFilter { - public: - ConflictingSectionFilter() = default; - explicit ConflictingSectionFilter(const OriginalGraph& g, const RouteLanelets& onRoute) - : g_{&g}, onRoute_{&onRoute} {} - - bool operator()(LaneletVertexId vertexId) const { - // conflicting if it is not yet part of the route but in conflict with a route lanelet - if (has(*onRoute_, vertexId)) { - return false; - } - auto outEdges = boost::out_edges(vertexId, *g_); - bool isNeighbour = false; - bool isConflicting = false; - std::for_each(outEdges.first, outEdges.second, [&](auto edge) { - if (onRoute_->find(boost::target(edge, *g_)) == onRoute_->end()) { - return; - } - auto type = (*g_)[edge].relation; - auto neighbourTypes = RelationType::Left | RelationType::Right; - auto conflictTypes = RelationType::Conflicting | RelationType::AdjacentLeft | RelationType::AdjacentRight; - if ((type & (neighbourTypes)) != RelationType::None) { - auto outEdge = boost::edge(boost::target(edge, *g_), boost::source(edge, *g_), *g_); - auto reverseIsNeigh = outEdge.second && ((*g_)[outEdge.first].relation & neighbourTypes) != RelationType::None; - isNeighbour |= reverseIsNeigh; - isConflicting |= !isNeighbour; - } - isConflicting |= (type & (conflictTypes)) != RelationType::None; - }); - return isConflicting && !isNeighbour; - } - - private: - const OriginalGraph* g_{}; - const RouteLanelets* onRoute_{}; -}; - -//! Finds vertices within a set of vertices that are in conflict with another set of vertices -class OnRouteAndConflictFilter { - public: - OnRouteAndConflictFilter() = default; - explicit OnRouteAndConflictFilter(const RouteLanelets& onRoute, const std::vector& conflictWith, - const OriginalGraph& g) - : onRoute_{&onRoute}, conflictWith_{&conflictWith}, g_{&g} {} - - bool operator()(LaneletVertexId vertexId) const { - auto isOk = anySidewayNeighbourIs(vertexId, *g_, [&](auto v) { - if (!has(*onRoute_, vertexId)) { - return false; - } - auto outEdges = boost::out_edges(v, *g_); - auto isConflicting = [&](OriginalGraph::edge_descriptor e) { - auto conflictTypes = RelationType::Conflicting | RelationType::AdjacentLeft | RelationType::AdjacentRight; - auto type = (*g_)[e].relation; - return (type & conflictTypes) != RelationType::None && has(*conflictWith_, boost::target(e, *g_)); - }; - return v == start_ || v == end_ || std::any_of(outEdges.first, outEdges.second, isConflicting); - }); - return isOk; - } - - void setConflicting(const LaneletVertexIds& conflictWith) { conflictWith_ = &conflictWith; } - void setStart(LaneletVertexId start) { start_ = start; } - void setEnd(LaneletVertexId end) { end_ = end; } - - private: - const RouteLanelets* onRoute_{}; - const LaneletVertexIds* conflictWith_{}; - LaneletVertexId start_{}; - LaneletVertexId end_{}; - const OriginalGraph* g_{}; -}; - -//! For graph queries, we implement our own color map because boost's color does not perform well on sparse graphs -struct SparseColorMap { - using key_type = LaneletVertexId; // NOLINT - using value_type = boost::two_bit_color_type; // NOLINT - using reference = void; // NOLINT - using category = boost::read_write_property_map_tag; // NOLINT - using MapT = std::map; - std::shared_ptr data{std::make_shared()}; -}; - -inline SparseColorMap::value_type get(const SparseColorMap& map, LaneletVertexId key) { - auto val = map.data->find(key); - if (val != map.data->end()) { - return static_cast(val->second); - } - return SparseColorMap::value_type::two_bit_white; -} - -inline void put(SparseColorMap& map, LaneletVertexId key, SparseColorMap::value_type value) { - (*map.data)[key] = static_cast(value); -} - -//! An iterator that finds paths from a start vertex to all reachable destinations. -template -class ConnectedPathIterator { - public: - using Vertices = std::vector; - - private: - template - class PathVisitor : public boost::default_dfs_visitor { - public: - PathVisitor(Func& f, Vertices& path) : path_{&path}, f_{&f} {} - void discover_vertex(typename GraphT::vertex_descriptor v, const GraphT& /*g*/) { // NOLINT - path_->push_back(v); - movingForward_ = true; - } - void finish_vertex(typename GraphT::vertex_descriptor v, const GraphT& /*g*/) { // NOLINT - if (movingForward_) { - (*f_)(*path_); - } - movingForward_ = false; - assert(!path_->empty()); - assert(path_->back() == v); - path_->pop_back(); - } - - private: - bool movingForward_{true}; - Vertices* path_; - const std::decay_t* f_; - }; - - public: - ConnectedPathIterator() = default; - explicit ConnectedPathIterator(const GraphT& g) : g_{g} {} - - //! Calls a function for all full paths starting from start. A full path is a path from start to either a leaf or the - //! last unvisited vertex in loops. - template - void forEachPath(LaneletVertexId start, Func&& f) { - assert(g_.m_vertex_pred(start)); - path_.clear(); - PathVisitor vis(f, path_); - SparseColorMap cm; - boost::depth_first_visit(g_, start, vis, cm); - } - - //! Returns whether a path exists in the graph that connects from and to - bool hasPathFromTo(LaneletVertexId from, LaneletVertexId to) { - class PathFound {}; - auto destinationReached = [&](const auto& path) { - return std::any_of(std::make_reverse_iterator(path.end()), std::make_reverse_iterator(path.begin()), - [&](auto p) { return p == to; }); - }; - try { - forEachPath(from, [&](const auto& path) { - if (destinationReached(path)) { - throw PathFound{}; - } - }); - } catch (PathFound) { // NOLINT(misc-throw-by-value-catch-by-reference) - return true; - } - return false; - } - - GraphT& graph() { return g_; } - - private: - GraphT g_; - Vertices path_; -}; - -// Aliases for some graphs needed by us -using OnRouteGraph = boost::filtered_graph; -using DrivableGraph = boost::filtered_graph; -using NoConflictingGraph = boost::filtered_graph; -using OnlyConflictingGraph = boost::filtered_graph; -using NextToRouteGraph = boost::filtered_graph; -using ConflictOrAdjacentToRouteGraph = - boost::filtered_graph; -using ConflictsWithPathGraph = boost::filtered_graph; - -} // namespace internal -} // namespace map_learning -} // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphBuilder.h b/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphBuilder.h deleted file mode 100644 index f9ee94a3..00000000 --- a/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphBuilder.h +++ /dev/null @@ -1,64 +0,0 @@ -#pragma once -#include - -#include "lanelet2_map_learning/MapGraph.h" -#include "lanelet2_map_learning/internal/Graph.h" - -namespace lanelet { -class LaneletLayer; - -namespace map_learning { -namespace internal { - -class LaneChangeLaneletsCollector; - -class MapGraphBuilder { - public: - MapGraphBuilder(const traffic_rules::TrafficRules& trafficRules, const MapGraph::Configuration& config); - - MapGraphUPtr build(const LaneletMapLayers& laneletMapLayers); - - private: - using PointsLaneletMap = std::multimap; - using PointsLaneletMapIt = PointsLaneletMap::iterator; - using PointsLaneletMapResult = std::pair; - - static ConstLanelets getPassableLanelets(const LaneletLayer& lanelets, - const traffic_rules::TrafficRules& trafficRules); - static ConstAreas getPassableAreas(const AreaLayer& areas, const traffic_rules::TrafficRules& trafficRules); - void appendBidirectionalLanelets(ConstLanelets& llts); - void addLaneletsToGraph(ConstLanelets& llts); - void addAreasToGraph(ConstAreas& areas); - void addEdges(const ConstLanelets& lanelets, const LaneletLayer& passableLanelets); - void addEdges(const ConstAreas& areas, const LaneletLayer& passableLanelets, const AreaLayer& passableAreas); - void addFollowingEdges(const ConstLanelet& ll); - void addSidewayEdge(LaneChangeLaneletsCollector& laneChangeLanelets, const ConstLanelet& ll, - const ConstLineString3d& bound, const RelationType& relation); - void addConflictingEdge(const ConstLanelet& ll, const LaneletLayer& passableLanelets); - void addLaneChangeEdges(LaneChangeLaneletsCollector& laneChanges, const RelationType& relation); - void addAreaEdge(const ConstArea& area, const LaneletLayer& passableLanelets); - void addAreaEdge(const ConstArea& area, const AreaLayer& passableAreas); - - //! Helper function to read the participant height from the configuration - Optional participantHeight() const; - - //! Adds the first and last points of a lanelet to the search index - void addPointsToSearchIndex(const ConstLanelet& ll); - bool hasEdge(const ConstLanelet& from, const ConstLanelet& to); - - void assignLaneChange(ConstLanelets froms, ConstLanelets tos, const RelationType& relation); - /** @brief Assigns edge to a relation between two lanelets - * @param from Start lanelet - * @param to Goal lanelet - * @param relation Relation between the two lanelets */ - void assignEdge(const ConstLaneletOrArea& from, const ConstLaneletOrArea& to, const RelationType& relation); - - std::unique_ptr graph_; - PointsLaneletMap pointsToLanelets_; ///< A map of tuples (first or last left and right boundary points) to lanelets - std::set bothWaysLaneletIds_; - const traffic_rules::TrafficRules& trafficRules_; - const MapGraph::Configuration& config_; -}; -} // namespace internal -} // namespace map_learning -} // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphVisualization.h b/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphVisualization.h deleted file mode 100644 index 7b272052..00000000 --- a/lanelet2_map_learning/include/lanelet2_map_learning/internal/MapGraphVisualization.h +++ /dev/null @@ -1,122 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "lanelet2_map_learning/Exceptions.h" -#include "lanelet2_map_learning/Forward.h" -#include "lanelet2_map_learning/internal/Graph.h" - -namespace lanelet { - -// This one needs to be in this namepace. Otherwise it's not found. -inline std::istream& operator>>(std::istream& is, ConstLaneletOrArea& /*r*/) { return is; } - -namespace map_learning { -inline std::ostream& operator<<(std::ostream& os, const RelationType& r) { return os << relationToString(r); } -inline std::istream& operator>>(std::istream& is, const RelationType& /*r*/) { return is; } - -namespace internal { - -/** @brief Internal vertex writer for graphViz file export. */ -template -class VertexWriterGraphViz { - public: - explicit VertexWriterGraphViz(const Graph* g) : graph_(g) {} - template - void operator()(std::ostream& out, const VertexOrEdge& v) const { - const Id id{(*graph_)[v].laneletOrArea.id()}; - out << "[label=\"" << id << "\" lanelet=\"" << id << "\"]"; - } - - private: - const Graph* graph_; -}; - -/** @brief Internal edge writer for graphViz file export. - Includes label and color. */ -template -class EdgeWriterGraphViz { - public: - explicit EdgeWriterGraphViz(const Graph* g) : graph_(g) {} - template - void operator()(std::ostream& out, const VertexOrEdge& v) const { - const RelationType relation{(*graph_)[v].relation}; - out << "[label=\"" << relationToString(relation) << "\" color=\"" << relationToColor(relation) << "\"]"; - } - - private: - const Graph* graph_; -}; - -/** @brief Implementation of graphViz export function - * @param filename Fully qualified file name - ideally with extension (.gv) - * @param g Graph to export - * @param edgeFilter Edge filter that will be used to create a filtered graph - * @param vertexFilter Vertex filter. Not used yet */ -template -inline void exportGraphVizImpl(const std::string& filename, const G& g, E edgeFilter = boost::keep_all(), - V vertexFilter = boost::keep_all()) { - std::ofstream file; - file.open(filename); - if (!file.is_open()) { - throw lanelet::ExportError("Could not open file at " + filename + "."); - } - - VertexWriterGraphViz vertexWriter(&g); - EdgeWriterGraphViz edgeWriter(&g); - boost::filtered_graph fg(g, edgeFilter, vertexFilter); - boost::write_graphviz(file, fg, vertexWriter, edgeWriter); - - file.close(); -} - -/** @brief GraphViz export function - * @param filename Fully qualified file name - ideally with extension (.gv) - * @param g Graph to export - * @param relationTypes Relations that will be included in the export */ -template -inline void exportGraphVizImpl(const std::string& filename, const G& g, const RelationType& relationTypes) { - auto edgeFilter = EdgeFilter(g, relationTypes); - exportGraphVizImpl(filename, g, edgeFilter); -} - -/** @brief Implementation of graphML export function - * @param filename Fully qualified file name - ideally with extension (.graphml) - * @param g Graph to export - * @param eFilter Edge filter that will be used to create a filtered graph - * @param vFilter Vertex filter. Not used yet */ -template -inline void exportGraphMLImpl(const std::string& filename, const G& g, E eFilter = boost::keep_all(), - V vFilter = boost::keep_all()) { - std::ofstream file; - file.open(filename); - if (!file.is_open()) { - throw lanelet::ExportError("Could not open file at " + filename + "."); - } - - boost::filtered_graph> filteredGraph(g, eFilter, vFilter); - - auto pmId = boost::get(&VertexInfo::laneletOrArea, filteredGraph); // NOLINT - auto pmRelation = boost::get(&EdgeInfo::relation, filteredGraph); - - boost::dynamic_properties dp; - dp.property("info", pmId); - dp.property("relation", pmRelation); - - boost::write_graphml(file, filteredGraph, dp, false); -} - -/** @brief GraphML export function - * @param filename Fully qualified file name - ideally with extension (.graphml) - * @param g Graph to export - * @param relationTypes Relations that will be included in the export */ -template -inline void exportGraphMLImpl(const std::string& filename, const G& g, const RelationType& relationTypes) { - auto edgeFilter = EdgeFilter(g, relationTypes); - exportGraphMLImpl(filename, g, edgeFilter); -} -} // namespace internal -} // namespace map_learning -} // namespace lanelet diff --git a/lanelet2_map_learning/package.xml b/lanelet2_map_learning/package.xml index 2644da55..372104a9 100644 --- a/lanelet2_map_learning/package.xml +++ b/lanelet2_map_learning/package.xml @@ -17,6 +17,7 @@ gtest boost lanelet2_core + lanelet2_routing lanelet2_traffic_rules diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 5247f6ea..96b1feea 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -3,28 +3,67 @@ namespace lanelet { namespace map_learning { -LaneLineStringFeatures extractRoadBorders(MapGraphConstPtr localSubmapGraph) { - LaneLineStringFeatures roadBorders; - const auto& internalGraph = localSubmapGraph->internalGraph(); - const auto& llVertices = internalGraph.vertexLookup(); - - for (const auto& laWithVertex : llVertices) { - const auto& la = laWithVertex.first; - if (!la.isLanelet()) continue; - auto ll = laWithVertex.first.lanelet(); - const auto& vertex = laWithVertex.second; - } +bool isRoadBorder(const ConstLineString3d& lstring) { + Attribute type = lstring.attribute(AttributeName::Type); + return type == AttributeValueString::RoadBorder || type == AttributeValueString::Curbstone || + type == AttributeValueString::Fence; } -LaneLineStringFeatures extractLaneDividers(MapGraphConstPtr localSubmapGraph) {} - -CompoundLaneLineStringFeatures extractCompoundRoadBorders(MapGraphConstPtr localSubmapGraph) {} - -CompoundLaneLineStringFeatures extractCompoundLaneDividers(MapGraphConstPtr localSubmapGraph) {} +LaneData::LaneData(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { + for (const auto& lstring : localSubmap->lineStringLayer) { + if (isRoadBorder(lstring)) { + roadBorders_.insert( + {lstring.id(), LaneLineStringFeature(lstring.basicLineString(), lstring.id(), LineStringType::RoadBorder)}); + continue; + } + } -CompoundLaneLineStringFeatures extractCompoundCenterlines(MapGraphConstPtr localSubmapGraph) {} + std::map alreadyCompounded; -LaneData::LaneData(MapGraphConstPtr localSubmapGraph) {} + // process left boundaries + for (const auto& ll : localSubmap->laneletLayer) { + Optional leftLL = localSubmapGraph->left(ll); + Optional adjLeftLL = localSubmapGraph->adjacentLeft(ll); + if (leftLL) { + laneDividers_.insert( + {ll.leftBound3d().id(), + LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), LineStringType::Dashed)}); + edgeList_.push_back(LaneEdge(ll.id(), leftLL->id(), routing::RelationType::Left)); + } else if (adjLeftLL) { + laneDividers_.insert( + {ll.leftBound3d().id(), + LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), LineStringType::Solid)}); + edgeList_.push_back(LaneEdge(ll.id(), leftLL->id(), routing::RelationType::AdjacentLeft)); + } else if (ll.leftBound3d().attribute(AttributeName::Type) != AttributeValueString::Virtual) { + laneDividers_.insert( + {ll.leftBound3d().id(), + LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), LineStringType::Solid)}); + } + } + for (const auto& ll : localSubmap->laneletLayer) { + Optional rightLL = localSubmapGraph->right(ll); + Optional adjRightLL = localSubmapGraph->adjacentRight(ll); + if (rightLL) { + laneDividers_.insert( + {ll.rightBound3d().id(), + LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), LineStringType::Dashed)}); + edgeList_.push_back(LaneEdge(ll.id(), rightLL->id(), routing::RelationType::Right)); + } else if (adjRightLL) { + laneDividers_.insert( + {ll.rightBound3d().id(), + LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), LineStringType::Solid)}); + edgeList_.push_back(LaneEdge(ll.id(), adjRightLL->id(), routing::RelationType::AdjacentRight)); + } else if (ll.rightBound3d().attribute(AttributeName::Type) != AttributeValueString::Virtual) { + laneDividers_.insert( + {ll.rightBound3d().id(), + LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), LineStringType::Solid)}); + } + } + for (const auto& ll : localSubmap->laneletLayer) { + ConstLanelets previousLLs = localSubmapGraph->previous(ll); + ConstLanelets followingLLs = localSubmapGraph->following(ll); + } +} } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapDataInterface.cpp b/lanelet2_map_learning/src/MapDataInterface.cpp index 39841814..96cb9c86 100644 --- a/lanelet2_map_learning/src/MapDataInterface.cpp +++ b/lanelet2_map_learning/src/MapDataInterface.cpp @@ -19,7 +19,7 @@ void MapDataInterface::setCurrPosAndExtractSubmap(const BasicPoint3d& pt) { currPos_ = pt; localSubmap_ = extractSubmap(laneletMap_, *currPos_, *currYaw_, config_.submapAreaLongitudinal, config_.submapAreaLateral); - localSubmapGraph_ = MapGraph::build(*laneletMap_, *trafficRules_); + localSubmapGraph_ = lanelet::routing::RoutingGraph::build(*laneletMap_, *trafficRules_); } MapDataInterface::MapDataInterface(LaneletMapConstPtr laneletMap, Configuration config, Optional currPos) @@ -28,7 +28,7 @@ MapDataInterface::MapDataInterface(LaneletMapConstPtr laneletMap, Configuration currPos_{currPos}, trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} -LaneData MapDataInterface::getLaneData(MapGraphConstPtr localSubmapGraph) { +LaneData MapDataInterface::getLaneData(lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { const auto& graph = localSubmapGraph->graph_; const auto& llVertices = graph->vertexLookup(); From 15cabc2bd5ffd63a7b2e0cc23e6ce3f95cf50471 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Fri, 24 Nov 2023 19:25:49 +0100 Subject: [PATCH 19/64] wip map data --- .../include/lanelet2_map_learning/MapData.h | 2 +- lanelet2_map_learning/src/MapData.cpp | 29 +++++++++++-------- 2 files changed, 18 insertions(+), 13 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index 6bc869f7..0528b2c0 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -32,7 +32,7 @@ using LaneEdges = std::vector; class LaneData { public: LaneData() noexcept {} - LaneData(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); + static LaneData build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); private: LaneLineStringFeatures roadBorders_; // auxilliary features diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 96b1feea..427e078d 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -9,10 +9,11 @@ bool isRoadBorder(const ConstLineString3d& lstring) { type == AttributeValueString::Fence; } -LaneData::LaneData(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { +LaneData LaneData::build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { + LaneData data; for (const auto& lstring : localSubmap->lineStringLayer) { if (isRoadBorder(lstring)) { - roadBorders_.insert( + data.roadBorders_.insert( {lstring.id(), LaneLineStringFeature(lstring.basicLineString(), lstring.id(), LineStringType::RoadBorder)}); continue; } @@ -25,17 +26,17 @@ LaneData::LaneData(LaneletSubmapConstPtr& localSubmap, lanelet::routing::Routing Optional leftLL = localSubmapGraph->left(ll); Optional adjLeftLL = localSubmapGraph->adjacentLeft(ll); if (leftLL) { - laneDividers_.insert( + data.laneDividers_.insert( {ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), LineStringType::Dashed)}); - edgeList_.push_back(LaneEdge(ll.id(), leftLL->id(), routing::RelationType::Left)); + data.edgeList_.push_back(LaneEdge(ll.id(), leftLL->id(), routing::RelationType::Left)); } else if (adjLeftLL) { - laneDividers_.insert( + data.laneDividers_.insert( {ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), LineStringType::Solid)}); - edgeList_.push_back(LaneEdge(ll.id(), leftLL->id(), routing::RelationType::AdjacentLeft)); + data.edgeList_.push_back(LaneEdge(ll.id(), leftLL->id(), routing::RelationType::AdjacentLeft)); } else if (ll.leftBound3d().attribute(AttributeName::Type) != AttributeValueString::Virtual) { - laneDividers_.insert( + data.laneDividers_.insert( {ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), LineStringType::Solid)}); } @@ -44,17 +45,17 @@ LaneData::LaneData(LaneletSubmapConstPtr& localSubmap, lanelet::routing::Routing Optional rightLL = localSubmapGraph->right(ll); Optional adjRightLL = localSubmapGraph->adjacentRight(ll); if (rightLL) { - laneDividers_.insert( + data.laneDividers_.insert( {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), LineStringType::Dashed)}); - edgeList_.push_back(LaneEdge(ll.id(), rightLL->id(), routing::RelationType::Right)); + data.edgeList_.push_back(LaneEdge(ll.id(), rightLL->id(), routing::RelationType::Right)); } else if (adjRightLL) { - laneDividers_.insert( + data.laneDividers_.insert( {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), LineStringType::Solid)}); - edgeList_.push_back(LaneEdge(ll.id(), adjRightLL->id(), routing::RelationType::AdjacentRight)); + data.edgeList_.push_back(LaneEdge(ll.id(), adjRightLL->id(), routing::RelationType::AdjacentRight)); } else if (ll.rightBound3d().attribute(AttributeName::Type) != AttributeValueString::Virtual) { - laneDividers_.insert( + data.laneDividers_.insert( {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), LineStringType::Solid)}); } @@ -62,7 +63,11 @@ LaneData::LaneData(LaneletSubmapConstPtr& localSubmap, lanelet::routing::Routing for (const auto& ll : localSubmap->laneletLayer) { ConstLanelets previousLLs = localSubmapGraph->previous(ll); ConstLanelets followingLLs = localSubmapGraph->following(ll); + + // Idea: algorithm for paths that starts with LLs with no previous, splits on junctions and terminates on LLs with + // no successors } + return data; } } // namespace map_learning From 1ac2c69249f25c0c3d98ccd66d73aeb81243aab8 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Wed, 29 Nov 2023 19:07:37 +0100 Subject: [PATCH 20/64] first draft compound feature calculation --- .../include/lanelet2_map_learning/MapData.h | 19 +- .../lanelet2_map_learning/MapFeatures.h | 21 +- lanelet2_map_learning/src/MapData.cpp | 255 ++++++++- lanelet2_map_learning/src/MapFeatures.cpp | 58 +- lanelet2_map_learning/src/MapGraph.cpp | 520 ------------------ lanelet2_map_learning/src/MapGraphBuilder.cpp | 359 ------------ 6 files changed, 309 insertions(+), 923 deletions(-) delete mode 100644 lanelet2_map_learning/src/MapGraph.cpp delete mode 100644 lanelet2_map_learning/src/MapGraphBuilder.cpp diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index 0528b2c0..3fb04248 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -35,12 +35,25 @@ class LaneData { static LaneData build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); private: + void processLeftBoundaries(LaneletSubmapConstPtr& localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph); + void processRightBoundaries(LaneletSubmapConstPtr& localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph); + void processCompoundFeatures(LaneletSubmapConstPtr& localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph); + + LineStringType getLineStringTypeFromId(Id id); + LaneLineStringFeature getLineStringFeatFromId(Id id); + std::vector> computeCompoundLeftBorders(const ConstLanelets& path); + std::vector> LaneData::computeCompoundRightBorders(const ConstLanelets& path); + CompoundLaneLineStringFeature LaneData::computeCompoundCenterline(const ConstLanelets& path); + LaneLineStringFeatures roadBorders_; // auxilliary features LaneLineStringFeatures laneDividers_; // auxilliary features - CompoundLaneLineStringFeatures compoundRoadBorders_; // auxilliary features - CompoundLaneLineStringFeatures compoundLaneDividers_; // auxilliary features - CompoundLaneLineStringFeatures compoundCenterlines_; + CompoundLaneLineStringFeatureList compoundRoadBorders_; // auxilliary features + CompoundLaneLineStringFeatureList compoundLaneDividers_; // auxilliary features + CompoundLaneLineStringFeatureList compoundCenterlines_; LaneletFeatures laneletFeatures_; // node features LaneEdges edgeList_; // edge list for centerlines diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index d7bc8d5d..ab83d1ce 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -38,6 +38,7 @@ class LineStringFeature : public MapFeature { const BasicLineString3d& rawFeature() const { return rawFeature_; } virtual Eigen::VectorXd computeFeatureVector(bool /*unused*/, bool /*unused*/) const = 0; + virtual Eigen::MatrixXd pointMatrix(bool pointsIn2d) const = 0; virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) = 0; protected: @@ -58,12 +59,14 @@ class LaneLineStringFeature : public LineStringFeature { virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; virtual Eigen::VectorXd computeFeatureVector( bool onlyPoints, - bool pointsIn2d) const override; // uses processedFeature_ when available + bool pointsIn2d) const override; // uses processedFeature_ when available + virtual Eigen::MatrixXd pointMatrix(bool pointsIn2d) const; // uses processedFeature_ when available const bool& wasCut() const { return wasCut_; } const BasicLineString3d& cutFeature() const { return cutFeature_; } const BasicLineString3d& cutAndResampledFeature() const { return cutAndResampledFeature_; } const LineStringType& type() const { return type_; } + int typeInt() const { return static_cast(type_); } protected: BasicLineString3d cutFeature_; @@ -76,6 +79,10 @@ using MapFeatures = std::map; using LineStringFeatures = std::map; using LaneLineStringFeatures = std::map; +using MapFeatureList = std::vector; +using LineStringFeatureList = std::vector; +using LaneLineStringFeatureList = std::vector; + class TEFeature : public LineStringFeature { public: TEFeature() {} @@ -122,11 +129,11 @@ class CompoundLaneLineStringFeature : public LaneLineStringFeature { public: CompoundLaneLineStringFeature() {} // features for this constructor are required to be given in already sorted order - CompoundLaneLineStringFeature(const LaneLineStringFeatures& features, LineStringType compoundType); + CompoundLaneLineStringFeature(const LaneLineStringFeatureList& features, LineStringType compoundType); virtual ~CompoundLaneLineStringFeature() noexcept = default; - const LaneLineStringFeatures& features() const { return individualFeatures_; } + const LaneLineStringFeatureList& features() const { return individualFeatures_; } const std::vector& pathLengthsRaw() const { return pathLengthsRaw_; } const std::vector& pathLengthsProcessed() const { return pathLengthsProcessed_; } @@ -134,16 +141,20 @@ class CompoundLaneLineStringFeature : public LaneLineStringFeature { Eigen::VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const override; private: - LaneLineStringFeatures individualFeatures_; + LaneLineStringFeatureList individualFeatures_; std::vector pathLengthsRaw_; std::vector pathLengthsProcessed_; }; -using CompoundLaneLineStringFeatures = std::map; +using CompoundLaneLineStringFeatureList = std::vector; using TEFeatures = std::map; using LaneletFeatures = std::map; Eigen::MatrixXd getFeatureVectorMatrix(const MapFeatures& mapFeatures, bool onlyPoints, bool pointsIn2d); +Eigen::MatrixXd getFeatureVectorMatrix(const MapFeatureList& mapFeatures, bool onlyPoints, bool pointsIn2d); + +std::vector getPointsMatrixList(const LaneLineStringFeatures& mapFeatures, bool pointsIn2d); +std::vector getPointsMatrixList(const LaneLineStringFeatureList& mapFeatures, bool pointsIn2d); } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 427e078d..9527a4a7 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -11,63 +11,258 @@ bool isRoadBorder(const ConstLineString3d& lstring) { LaneData LaneData::build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { LaneData data; - for (const auto& lstring : localSubmap->lineStringLayer) { - if (isRoadBorder(lstring)) { - data.roadBorders_.insert( - {lstring.id(), LaneLineStringFeature(lstring.basicLineString(), lstring.id(), LineStringType::RoadBorder)}); - continue; - } - } - std::map alreadyCompounded; + data.processLeftBoundaries(localSubmap, localSubmapGraph); + data.processRightBoundaries(localSubmap, localSubmapGraph); + data.processCompoundFeatures(localSubmap, localSubmapGraph); + + return data; +} - // process left boundaries +void LaneData::processLeftBoundaries(LaneletSubmapConstPtr& localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { for (const auto& ll : localSubmap->laneletLayer) { + if (isRoadBorder(ll.leftBound3d())) { + roadBorders_.insert( + {ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), + LineStringType::RoadBorder)}); + } + Optional leftLL = localSubmapGraph->left(ll); Optional adjLeftLL = localSubmapGraph->adjacentLeft(ll); if (leftLL) { - data.laneDividers_.insert( + laneDividers_.insert( {ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), LineStringType::Dashed)}); - data.edgeList_.push_back(LaneEdge(ll.id(), leftLL->id(), routing::RelationType::Left)); + edgeList_.push_back(LaneEdge(ll.id(), leftLL->id(), routing::RelationType::Left)); } else if (adjLeftLL) { - data.laneDividers_.insert( + laneDividers_.insert( {ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), LineStringType::Solid)}); - data.edgeList_.push_back(LaneEdge(ll.id(), leftLL->id(), routing::RelationType::AdjacentLeft)); - } else if (ll.leftBound3d().attribute(AttributeName::Type) != AttributeValueString::Virtual) { - data.laneDividers_.insert( + edgeList_.push_back(LaneEdge(ll.id(), leftLL->id(), routing::RelationType::AdjacentLeft)); + } else if (ll.leftBound3d().attribute(AttributeName::Type) == AttributeValueString::Virtual) { + laneDividers_.insert( {ll.leftBound3d().id(), - LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), LineStringType::Solid)}); + LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), LineStringType::Virtual)}); } } +} + +void LaneData::processRightBoundaries(LaneletSubmapConstPtr& localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { for (const auto& ll : localSubmap->laneletLayer) { + if (isRoadBorder(ll.rightBound3d())) { + roadBorders_.insert( + {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), + LineStringType::RoadBorder)}); + } + Optional rightLL = localSubmapGraph->right(ll); Optional adjRightLL = localSubmapGraph->adjacentRight(ll); if (rightLL) { - data.laneDividers_.insert( + laneDividers_.insert( {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), LineStringType::Dashed)}); - data.edgeList_.push_back(LaneEdge(ll.id(), rightLL->id(), routing::RelationType::Right)); + edgeList_.push_back(LaneEdge(ll.id(), rightLL->id(), routing::RelationType::Right)); } else if (adjRightLL) { - data.laneDividers_.insert( - {ll.rightBound3d().id(), - LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), LineStringType::Solid)}); - data.edgeList_.push_back(LaneEdge(ll.id(), adjRightLL->id(), routing::RelationType::AdjacentRight)); - } else if (ll.rightBound3d().attribute(AttributeName::Type) != AttributeValueString::Virtual) { - data.laneDividers_.insert( + laneDividers_.insert( {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), LineStringType::Solid)}); + edgeList_.push_back(LaneEdge(ll.id(), adjRightLL->id(), routing::RelationType::AdjacentRight)); + } else if (ll.rightBound3d().attribute(AttributeName::Type) == AttributeValueString::Virtual) { + laneDividers_.insert( + {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), + LineStringType::Virtual)}); } } - for (const auto& ll : localSubmap->laneletLayer) { - ConstLanelets previousLLs = localSubmapGraph->previous(ll); - ConstLanelets followingLLs = localSubmapGraph->following(ll); +} + +// template +// Optional> getElementIndex2d(const std::vector>& vec, const T& el) { +// Optional> result; +// for (size_t i = 0; i < vec.size(); i++) { +// for (size_t j = 0; j < vec[i].size(); j++) { +// if (vec[i][j] == el) { +// std::pair resIdx; +// resIdx->first = i; +// resIdx->second = j; +// result = resIdx; +// break; +// } +// } +// } +// return result +// } - // Idea: algorithm for paths that starts with LLs with no previous, splits on junctions and terminates on LLs with - // no successors +// Idea: algorithm for paths that starts with LLs with no previous, splits on junctions and terminates on LLs with +// no successors +void getPaths(lanelet::routing::RoutingGraphConstPtr localSubmapGraph, std::vector& paths, + ConstLanelet start, ConstLanelets initPath = ConstLanelets()) { + initPath.push_back(start); + ConstLanelets successorLLs = localSubmapGraph->following(start, true); + while (!successorLLs.empty()) { + initPath.push_back(successorLLs.front()); + for (size_t i = 1; i != successorLLs.size(); i++) { + getPaths(localSubmapGraph, paths, successorLLs[i], initPath); + } + successorLLs = localSubmapGraph->following(successorLLs.front(), true); + } + paths.push_back(initPath); +} + +LineStringType LaneData::getLineStringTypeFromId(Id id) { + LineStringType bdType; + LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(id); + LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(id); + if (itLaneBd != laneDividers_.end()) { + bdType = itLaneBd->second.type(); + } else if (itRoadBd != roadBorders_.end()) { + bdType = itRoadBd->second.type(); + } else { + throw std::runtime_error("Lanelet boundary is neither a road border nor a lane divider!"); + } + return bdType; +} + +LaneLineStringFeature LaneData::getLineStringFeatFromId(Id id) { + LaneLineStringFeature lstringFeat; + LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(id); + LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(id); + if (itLaneBd != laneDividers_.end()) { + lstringFeat = itLaneBd->second; + } else if (itRoadBd != roadBorders_.end()) { + lstringFeat = itRoadBd->second; + } else { + throw std::runtime_error("Lanelet boundary is neither a road border nor a lane divider!"); + } + return lstringFeat; +} + +std::vector> LaneData::computeCompoundLeftBorders(const ConstLanelets& path) { + std::vector> compoundBorders; + ConstLanelet start = path.front(); + LineStringType currType = getLineStringTypeFromId(start.leftBound3d().id()); + + compoundBorders.push_back(std::vector{start.leftBound3d().id()}); + + for (size_t i = 1; i != path.size(); i++) { + LineStringType newType = getLineStringTypeFromId(path[i].leftBound3d().id()); + if (currType == newType) { + compoundBorders.back().push_back(path[i].leftBound3d().id()); + } else { + compoundBorders.push_back(std::vector{start.leftBound3d().id()}); + } + } +} + +std::vector> LaneData::computeCompoundRightBorders(const ConstLanelets& path) { + std::vector> compoundBorders; + ConstLanelet start = path.front(); + LineStringType currType = getLineStringTypeFromId(start.rightBound3d().id()); + + compoundBorders.push_back(std::vector{start.rightBound3d().id()}); + + for (size_t i = 1; i != path.size(); i++) { + LineStringType newType = getLineStringTypeFromId(path[i].rightBound3d().id()); + if (currType == newType) { + compoundBorders.back().push_back(path[i].rightBound3d().id()); + } else { + compoundBorders.push_back(std::vector{start.rightBound3d().id()}); + } + } +} + +CompoundLaneLineStringFeature LaneData::computeCompoundCenterline(const ConstLanelets& path) { + LaneLineStringFeatureList compoundCenterlines; + for (const auto& ll : path) { + compoundCenterlines.push_back( + LaneLineStringFeature(ll.centerline3d().basicLineString(), ll.id(), LineStringType::Centerline)); + } + return CompoundLaneLineStringFeature(compoundCenterlines, LineStringType::Centerline); +} + +std::map::const_iterator findFirstOccElement(const std::vector& els, + const std::map& searchMap) { + for (const auto& el : els) { + std::map::const_iterator it = searchMap.find(el); + if (it != searchMap.end()) { + return it; + } + } + return searchMap.end(); +} + +void insertAndCheckNewCompoundFeatures(std::vector>& compFeats, + const std::vector>& newCompFeats, + std::map& elInsertIdx) { + for (const auto& compEl : newCompFeats) { + std::map::const_iterator firstOccIt = findFirstOccElement(compEl, elInsertIdx); + if (firstOccIt == elInsertIdx.end()) { + compFeats.push_back(compEl); + for (const Id& el : compEl) { + elInsertIdx[el] = compFeats.size() - 1; + } + } else if (compFeats[firstOccIt->second].size() < compEl.size()) { + compFeats[firstOccIt->second] = compEl; + for (const Id& el : compEl) { + elInsertIdx[el] = firstOccIt->second; + } + } + } +} + +void LaneData::processCompoundFeatures(LaneletSubmapConstPtr& localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { + std::vector> compoundedBordersAndDividers; + std::map elInsertIdx; + for (const auto& bd : roadBorders_) { + std::map::iterator it = elInsertIdx.find(bd.first); + if (it == elInsertIdx.end()) { + continue; + } + Optional prevEl; + Optional succEl; + for (const auto& bdComp : roadBorders_) { + if (bd.first == bdComp.first) { + continue; + } + if (bd.second.rawFeature().back() == bdComp.second.rawFeature().front()) { + succEl = bdComp.first; + } + if (bd.second.rawFeature().front() == bdComp.second.rawFeature().back()) { + prevEl = bdComp.first; + } + } + } + + std::vector paths; + for (const auto& ll : localSubmap->laneletLayer) { + ConstLanelets previousLLs = localSubmapGraph->previous(ll, true); + ConstLanelets successorLLs = localSubmapGraph->following(ll, true); + if (previousLLs.empty() && !successorLLs.empty()) { + getPaths(localSubmapGraph, paths, ll); + } + } + for (const auto& path : paths) { + std::vector> compoundedLeft = computeCompoundLeftBorders(path); + insertAndCheckNewCompoundFeatures(compoundedBordersAndDividers, compoundedLeft, elInsertIdx); + std::vector> compoundedRight = computeCompoundRightBorders(path); + insertAndCheckNewCompoundFeatures(compoundedBordersAndDividers, compoundedRight, elInsertIdx); + compoundCenterlines_.push_back(computeCompoundCenterline(path)); + } + for (const auto& compFeat : compoundedBordersAndDividers) { + LaneLineStringFeatureList toBeCompounded; + for (const auto& el : compFeat) { + LaneLineStringFeature cmpdFeat = getLineStringFeatFromId(el); + toBeCompounded.push_back(cmpdFeat); + } + LineStringType cmpdType = toBeCompounded.front().type(); + if (cmpdType == LineStringType::RoadBorder) { + compoundRoadBorders_.push_back(CompoundLaneLineStringFeature(toBeCompounded, cmpdType)); + } else { + compoundLaneDividers_.push_back(CompoundLaneLineStringFeature(toBeCompounded, cmpdType)); + } } - return data; } } // namespace map_learning diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index e659b44f..56c9f3ea 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -67,7 +67,7 @@ Eigen::VectorXd LaneLineStringFeature::computeFeatureVector(bool onlyPoints, boo } } - vec[vec.size() - 1] = static_cast(type_); + vec[vec.size() - 1] = typeInt(); if (onlyPoints) { return vec(Eigen::seq(0, vec.size() - 1)); } else { @@ -95,6 +95,23 @@ Eigen::VectorXd TEFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d } } +Eigen::MatrixXd LaneLineStringFeature::pointMatrix(bool pointsIn2d) const { + const BasicLineString3d& selectedFeature = + (cutAndResampledFeature_.size() > 0) ? cutAndResampledFeature_ : rawFeature_; + Eigen::MatrixXd mat = pointsIn2d ? Eigen::MatrixXd(selectedFeature.size(), 2) + : Eigen::MatrixXd(3 * selectedFeature.size(), 3); // n points with 2/3 dims + type + if (pointsIn2d == true) { + for (size_t i = 0; i < selectedFeature.size(); i++) { + mat.row(i) = selectedFeature[i](Eigen::seq(0, 1)); + } + } else { + for (size_t i = 0; i < selectedFeature.size(); i++) { + mat.row(i) = selectedFeature[i](Eigen::seq(0, 2)); + } + } + return mat; +} + LaneletFeature::LaneletFeature(const LaneLineStringFeature& leftBoundary, const LaneLineStringFeature& rightBoundary, const LaneLineStringFeature& centerline, Id mapID) : MapFeature(mapID), leftBoundary_{leftBoundary}, rightBoundary_{rightBoundary}, centerline_{centerline} {} @@ -125,8 +142,8 @@ Eigen::VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool point Eigen::VectorXd vecCenterlinePts = centerline_.computeFeatureVector(true, pointsIn2d); Eigen::VectorXd vec(vecCenterlinePts.size() + 2); // pts vec + left and right type vec(Eigen::seq(0, vecCenterlinePts.size() - 1)) = vecCenterlinePts; - vec[vec.size() - 2] = static_cast(leftBoundary_.type()); - vec[vec.size() - 1] = static_cast(rightBoundary_.type()); + vec[vec.size() - 2] = leftBoundary_.typeInt(); + vec[vec.size() - 1] = rightBoundary_.typeInt(); if (onlyPoints) { return vec(Eigen::seq(0, vec.size() - 2)); } else { @@ -138,8 +155,8 @@ Eigen::VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool point Eigen::VectorXd vec(vecLeftBdPts.size() + vecRightBdPts.size() + 2); // pts vec + left and right type vec(Eigen::seq(0, vecLeftBdPts.size() - 1)) = vecLeftBdPts; vec(Eigen::seq(vecLeftBdPts.size(), vecLeftBdPts.size() + vecRightBdPts.size() - 1)) = vecRightBdPts; - vec[vec.size() - 2] = static_cast(leftBoundary_.type()); - vec[vec.size() - 1] = static_cast(rightBoundary_.type()); + vec[vec.size() - 2] = leftBoundary_.typeInt(); + vec[vec.size() - 1] = rightBoundary_.typeInt(); if (onlyPoints) { return vec(Eigen::seq(0, vec.size() - 2)); } else { @@ -151,7 +168,7 @@ Eigen::VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool point } } -CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStringFeatures& features, +CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStringFeatureList& features, LineStringType compoundType) : individualFeatures_{features}, pathLengthsRaw_{std::vector(features.size())} { type_ = compoundType; @@ -176,12 +193,21 @@ CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStrin } Eigen::MatrixXd getFeatureVectorMatrix(const MapFeatures& mapFeatures, bool onlyPoints, bool pointsIn2d) { + MapFeatureList featList; + for (const auto& pair : mapFeatures) { + featList.push_back(pair.second); + } + return getFeatureVectorMatrix(featList, onlyPoints, pointsIn2d); +} + +Eigen::MatrixXd getFeatureVectorMatrix(const MapFeatureList& mapFeatures, bool onlyPoints, bool pointsIn2d) { assert(!mapFeatures.empty()); std::vector featureVectors; for (const auto& feat : mapFeatures) { if (!feat.valid()) { throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); } + featureVectors.push_back(feat.computeFeatureVector(onlyPoints, pointsIn2d)); } if (std::adjacent_find(featureVectors.begin(), featureVectors.end(), [](const Eigen::VectorXd& v1, const Eigen::VectorXd& v2) { return v1.size() != v2.size(); }) == @@ -196,5 +222,25 @@ Eigen::MatrixXd getFeatureVectorMatrix(const MapFeatures& mapFeatures, bool only return featureMat; } +std::vector getPointsMatrixList(const LaneLineStringFeatures& mapFeatures, bool pointsIn2d) { + LaneLineStringFeatureList featList; + for (const auto& pair : mapFeatures) { + featList.push_back(pair.second); + } + return getPointsMatrixList(featList, pointsIn2d); +} + +std::vector getPointsMatrixList(const LaneLineStringFeatureList& mapFeatures, bool pointsIn2d) { + assert(!mapFeatures.empty()); + std::vector pointMatrices; + for (const auto& feat : mapFeatures) { + if (!feat.valid()) { + throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); + } + pointMatrices.push_back(feat.pointMatrix(pointsIn2d)); + } + return pointMatrices; +} + } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapGraph.cpp b/lanelet2_map_learning/src/MapGraph.cpp deleted file mode 100644 index 50fcf174..00000000 --- a/lanelet2_map_learning/src/MapGraph.cpp +++ /dev/null @@ -1,520 +0,0 @@ -#include "lanelet2_map_learning/MapGraph.h" - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include // Asserts -#include -#include - -#include "lanelet2_map_learning/Exceptions.h" -#include "lanelet2_map_learning/Forward.h" -#include "lanelet2_map_learning/internal/Graph.h" -#include "lanelet2_map_learning/internal/GraphUtils.h" -#include "lanelet2_map_learning/internal/MapGraphBuilder.h" -#include "lanelet2_map_learning/internal/MapGraphVisualization.h" - -namespace lanelet { -namespace map_learning { - -#if __cplusplus < 201703L -constexpr const char MapGraph::ParticipantHeight[]; -#endif - -using internal::FilteredMapGraph; -using internal::GraphType; -using internal::LaneletVertexId; -using internal::MapGraphGraph; - -template -T reservedVector(size_t size) { - T t; - t.reserve(size); - return t; -} - -//! Helper function to create a new point that represents a lanelet. -template -PointT createPoint(const ConstLaneletOrArea& ll) { - PointT p; - p.setId(ll.id()); - p.setAttribute("id", Attribute(ll.id())); - if (ll.isLanelet()) { - boost::geometry::centroid(utils::toHybrid(ll.lanelet()->polygon2d()), p); - } - if (ll.isArea()) { - boost::geometry::centroid(utils::toHybrid(utils::to2D(ll.area()->outerBoundPolygon())), p); - } - return p; -} - -/** @brief Implementation function to retrieve a neighboring vertex - * @throws MapGraphError if 'throwOnError' is true and there is more than one neighboring lanelet - * @param vertex Start vertex - * @param graph Filtered graph with the allowed type of edge - * @param throwOnError Decides wheter to throw or ignore an error - * @return Neighboring lanelet */ -Optional neighboringImpl(const GraphType::vertex_descriptor vertex, const FilteredMapGraph& graph, - bool throwOnError = false) { - auto outEdges = boost::out_edges(vertex, graph); - if (throwOnError && std::distance(outEdges.first, outEdges.second) > 1) { - std::string ids; - std::for_each(outEdges.first, outEdges.second, [&graph, &ids](const auto& edge) { - ids += " " + std::to_string(graph[boost::target(edge, graph)].laneletOrArea.id()); - }); - throw MapGraphError("More than one neighboring lanelet to " + std::to_string(graph[vertex].laneletOrArea.id()) + - " with this relation:" + ids); - } - if (outEdges.first != outEdges.second) { - return graph[boost::target(*(outEdges.first), graph)].laneletOrArea; - } - return {}; -} - -Optional neighboringLaneletImpl(const GraphType::vertex_descriptor vertex, const FilteredMapGraph& graph, - bool throwOnError = false) { - auto value = neighboringImpl(vertex, graph, throwOnError); - if (!!value && value->isLanelet()) { - return value->lanelet(); - } - return {}; -} - -template -Optional ifInGraph(const MapGraphGraph& g, const ConstLaneletOrArea& llt, Func f) { - auto vertex = g.getVertex(llt); - if (!vertex) { - return {}; - } - return f(*vertex); -} - -template -Optional ifLaneletInGraph(const MapGraphGraph& g, const ConstLanelet& llt, Func f) { - auto laneletVertex = g.getVertex(llt); - if (!laneletVertex) { - return {}; - } - return f(*laneletVertex); -} - -template -ConstLanelets getUntilEnd(const ConstLanelet& start, Func next) { - auto result = reservedVector(3); - Optional current = start; - while (!!(current = next(*current))) { - result.emplace_back(*current); - } - return result; -} - -ConstLaneletOrAreas getAllEdgesFromGraph(const MapGraphGraph& graph, const FilteredMapGraph& subgraph, - const ConstLaneletOrArea& laneletOrArea, bool edgesOut) { - ConstLaneletOrAreas result; - auto laneletVertex = graph.getVertex(laneletOrArea); - if (!laneletVertex) { - return result; - } - auto processEdges = [&](auto edgeRange) { - result.reserve(size_t(std::distance(edgeRange.first, edgeRange.second))); - for (; edgeRange.first != edgeRange.second; edgeRange.first++) { - auto node = - edgesOut ? boost::target(*edgeRange.first, graph.get()) : boost::source(*edgeRange.first, graph.get()); - result.emplace_back(graph.get()[node].laneletOrArea); - } - return result; - }; - return edgesOut ? processEdges(boost::out_edges(*laneletVertex, subgraph)) - : processEdges(boost::in_edges(*laneletVertex, subgraph)); -} - -ConstLanelets getLaneletEdgesFromGraph(const MapGraphGraph& graph, const FilteredMapGraph& subgraph, - const ConstLanelet& lanelet, bool edgesOut) { - ConstLanelets result; - auto allEdges = getAllEdgesFromGraph(graph, subgraph, lanelet, edgesOut); - result = reservedVector(allEdges.size()); - for (auto& edge : allEdges) { - if (edge.isLanelet()) { - result.push_back(*edge.lanelet()); - } - } - return result; -} - -ConstLaneletOrAreas MapGraph::getLaneletEdges(const ConstLanelet& lanelet, bool edgesOut) const { - ConstLaneletOrAreas result; - auto allEdges = getAllEdgesFromGraph(*graph_, graph_->withAllRelations(), lanelet, edgesOut); - result = reservedVector(allEdges.size()); - for (auto& edge : allEdges) { - if (edge.isLanelet()) { - result.push_back(edge); - } - } - return result; -} - -MapGraph::MapGraph(MapGraph&& /*other*/) noexcept = default; -MapGraph& MapGraph::operator=(MapGraph&& /*other*/) noexcept = default; -MapGraph::~MapGraph() = default; - -MapGraphUPtr MapGraph::build(const LaneletMap& laneletMap, const traffic_rules::TrafficRules& trafficRules, - const MapGraph::Configuration& config) { - return internal::MapGraphBuilder(trafficRules, config).build(laneletMap); -} - -MapGraphUPtr MapGraph::build(const LaneletSubmap& laneletSubmap, const traffic_rules::TrafficRules& trafficRules, - const MapGraph::Configuration& config) { - return internal::MapGraphBuilder(trafficRules, config).build(laneletSubmap); -} - -Optional MapGraph::routingRelation(const ConstLanelet& from, const ConstLanelet& to, - bool includeConflicting) const { - auto edgeInfo = includeConflicting ? graph_->getEdgeInfo(from, to) - : graph_->getEdgeInfoFor(from, to, graph_->withoutConflicting()); - if (!!edgeInfo) { - return edgeInfo->relation; - } - return {}; -} - -ConstLanelets MapGraph::following(const ConstLanelet& lanelet, bool withLaneChanges) const { - auto subgraph = withLaneChanges ? graph_->withLaneChanges() : graph_->withoutLaneChanges(); - return getLaneletEdgesFromGraph(*graph_, subgraph, lanelet, true); -} - -LaneletRelations MapGraph::followingRelations(const ConstLanelet& lanelet, bool withLaneChanges) const { - ConstLanelets foll{following(lanelet, withLaneChanges)}; - LaneletRelations result; - for (auto const& it : foll) { - result.emplace_back(LaneletRelation{it, *routingRelation(lanelet, it)}); - } - return result; -} // namespace map_learning - -ConstLanelets MapGraph::previous(const ConstLanelet& lanelet, bool withLaneChanges) const { - auto subgraph = withLaneChanges ? graph_->withLaneChanges() : graph_->withoutLaneChanges(); - return getLaneletEdgesFromGraph(*graph_, subgraph, lanelet, false); -} - -LaneletRelations MapGraph::previousRelations(const ConstLanelet& lanelet, bool withLaneChanges) const { - ConstLanelets prev{previous(lanelet, withLaneChanges)}; - LaneletRelations result; - result.reserve(prev.size()); - for (auto const& it : prev) { - Optional relation{routingRelation(it, lanelet)}; - if (!!relation) { - result.emplace_back(LaneletRelation{it, *relation}); - } else { - assert(false && "Two Lanelets in a route are not connected. This shouldn't happen."); // NOLINT - } - } - return result; -} - -ConstLanelets MapGraph::besides(const ConstLanelet& lanelet) const { - auto move = [](auto it) { return std::make_move_iterator(it); }; - ConstLanelets left{lefts(lanelet)}; - ConstLanelets right{rights(lanelet)}; - ConstLanelets result; - result.reserve(left.size() + right.size() + 1); - result.insert(std::end(result), move(left.rbegin()), move(left.rend())); - result.push_back(lanelet); - result.insert(std::end(result), move(std::begin(right)), move(std::end(right))); - return result; -} - -Optional MapGraph::left(const ConstLanelet& lanelet) const { - return ifLaneletInGraph(*graph_, lanelet, - [&](auto& vertex) { return neighboringLaneletImpl(vertex, graph_->left()); }); -} - -Optional MapGraph::adjacentLeft(const ConstLanelet& lanelet) const { - return ifLaneletInGraph(*graph_, lanelet, - [&](auto& vertex) { return neighboringLaneletImpl(vertex, graph_->adjacentLeft()); }); -} - -Optional MapGraph::right(const ConstLanelet& lanelet) const { - return ifLaneletInGraph(*graph_, lanelet, - [&](auto& vertex) { return neighboringLaneletImpl(vertex, graph_->right()); }); -} - -Optional MapGraph::adjacentRight(const ConstLanelet& lanelet) const { - return ifLaneletInGraph(*graph_, lanelet, - [&](auto& vertex) { return neighboringLaneletImpl(vertex, graph_->adjacentRight()); }); -} - -ConstLanelets MapGraph::lefts(const ConstLanelet& lanelet) const { - return getUntilEnd(lanelet, [&](const ConstLanelet& llt) { return left(llt); }); -} - -ConstLanelets MapGraph::adjacentLefts(const ConstLanelet& lanelet) const { - return getUntilEnd(lanelet, [&](const ConstLanelet& llt) { return adjacentLeft(llt); }); -} - -LaneletRelations MapGraph::leftRelations(const ConstLanelet& lanelet) const { - bool leftReached{false}; - ConstLanelet current = lanelet; - LaneletRelations result; - while (!leftReached) { - const ConstLanelets leftOf{lefts(current)}; - for (auto const& it : leftOf) { - result.emplace_back(LaneletRelation{it, RelationType::Left}); - current = it; - } - const ConstLanelets adjacentLeftOf{adjacentLefts(current)}; - for (auto const& it : adjacentLeftOf) { - result.emplace_back(LaneletRelation{it, RelationType::AdjacentLeft}); - current = it; - } - leftReached = (leftOf.empty() && adjacentLeftOf.empty()); - } - return result; -} - -ConstLanelets MapGraph::rights(const ConstLanelet& lanelet) const { - return getUntilEnd(lanelet, [&](const ConstLanelet& llt) { return right(llt); }); -} - -ConstLanelets MapGraph::adjacentRights(const ConstLanelet& lanelet) const { - return getUntilEnd(lanelet, [&](const ConstLanelet& llt) { return adjacentRight(llt); }); -} - -LaneletRelations MapGraph::rightRelations(const ConstLanelet& lanelet) const { - bool rightReached{false}; - ConstLanelet current = lanelet; - auto result = reservedVector(3); - while (!rightReached) { - const ConstLanelets rightOf{rights(current)}; - for (auto const& it : rightOf) { - result.emplace_back(LaneletRelation{it, RelationType::Right}); - current = it; - } - const ConstLanelets adjacentRightOf{adjacentRights(current)}; - for (auto const& it : adjacentRightOf) { - result.emplace_back(LaneletRelation{it, RelationType::AdjacentRight}); - current = it; - } - rightReached = (rightOf.empty() && adjacentRightOf.empty()); - } - return result; -} - -ConstLaneletOrAreas MapGraph::conflicting(const ConstLaneletOrArea& laneletOrArea) const { - return getAllEdgesFromGraph(*graph_, graph_->conflicting(), laneletOrArea, true); -} - -void MapGraph::exportGraphML(const std::string& filename, const RelationType& edgeTypesToExclude) const { - if (filename.empty()) { - throw InvalidInputError("No filename passed"); - } - RelationType relations = allRelations() & ~edgeTypesToExclude; - internal::exportGraphMLImpl(filename, graph_->get(), relations); -} - -void MapGraph::exportGraphViz(const std::string& filename, const RelationType& edgeTypesToExclude) const { - if (filename.empty()) { - throw InvalidInputError("No filename passed"); - } - RelationType relations = allRelations() & ~edgeTypesToExclude; - internal::exportGraphVizImpl(filename, graph_->get(), relations); -} - -//! Helper function to slim down getDebugLaneletMap -RelationType allowedRelationsfromConfiguration(bool includeAdjacent, bool includeConflicting) { - RelationType allowedRelations{RelationType::Successor | RelationType::Left | RelationType::Right | - RelationType::Area}; - if (includeAdjacent) { - allowedRelations |= RelationType::AdjacentLeft; - allowedRelations |= RelationType::AdjacentRight; - } - if (includeConflicting) { - allowedRelations |= RelationType::Conflicting; - } - return allowedRelations; -} - -LineString3d createLineString(const Point2d& from, const Point2d& to, RelationType relation) { - LineString2d lineString(utils::getId()); - lineString.push_back(from); - lineString.push_back(to); - LineString3d lineString3d(lineString); - lineString3d.setAttribute("relation", relationToString(relation)); - return lineString3d; -} - -class DebugMapBuilder { - public: - using LaneletOrAreaPair = std::pair; - explicit DebugMapBuilder(const FilteredMapGraph& graph) : graph_{graph} {} - LaneletMapPtr run(const internal::LaneletOrAreaToVertex& loa) { - LaneletMapPtr output = std::make_shared(); - for (const auto& vertex : loa) { - visitVertex(vertex); - } - auto lineStrings = utils::transform(lineStringMap_, [](auto& mapLs) { return mapLs.second; }); - auto map = utils::createMap(lineStrings); - for (auto& p : pointMap_) { - map->add(utils::to3D(p.second)); - } - return map; - } - - private: - void visitVertex(const internal::LaneletOrAreaToVertex::value_type& vertex) { - addPoint(vertex.first); - auto edges = boost::out_edges(vertex.second, graph_); - for (auto edge = edges.first; edge != edges.second; ++edge) { - const auto& target = graph_[boost::target(*edge, graph_)].laneletOrArea; - addPoint(target); - const auto& edgeInfo = graph_[*edge]; - addEdge(vertex.first, target, edgeInfo); - } - } - - static LaneletOrAreaPair getPair(const ConstLaneletOrArea& first, const ConstLaneletOrArea& second) { - return first.id() < second.id() ? LaneletOrAreaPair(first, second) : LaneletOrAreaPair(second, first); - } - - void addPoint(const ConstLaneletOrArea& point) { - auto inMap = pointMap_.find(point); - if (inMap == pointMap_.end()) { - pointMap_.emplace(point, createPoint(point)); - } - } - - void addEdge(const ConstLaneletOrArea& from, const ConstLaneletOrArea& to, internal::EdgeInfo edge) { - auto pair = getPair(from, to); - auto inMap = lineStringMap_.find(pair); - if (inMap != lineStringMap_.end()) { - inMap->second.setAttribute("relation_reverse", relationToString(edge.relation)); - - } else { - auto pFrom = pointMap_.at(from); - auto pTo = pointMap_.at(to); - LineString3d lineString3d{createLineString(pFrom, pTo, edge.relation)}; - lineStringMap_.emplace(pair, lineString3d); - } - } - - FilteredMapGraph graph_; - std::unordered_map lineStringMap_; // Stores all relations - std::unordered_map pointMap_; // Stores all 'edges' -}; - -LaneletMapPtr MapGraph::getDebugLaneletMap(bool includeAdjacent, bool includeConflicting) const { - internal::EdgeFilter edgeFilter(graph_->get(), - allowedRelationsfromConfiguration(includeAdjacent, includeConflicting)); - FilteredMapGraph filteredGraph(graph_->get(), edgeFilter); - return DebugMapBuilder(filteredGraph).run(graph_->vertexLookup()); -} - -MapGraph::Errors MapGraph::checkValidity(bool throwOnError) const { - Errors errors; - for (const auto& laWithVertex : graph_->vertexLookup()) { - const auto& la = laWithVertex.first; - auto ll = laWithVertex.first.lanelet(); - const auto& vertex = laWithVertex.second; - auto id = la.id(); - // Check left relation - Optional left; - try { - left = neighboringLaneletImpl(vertex, graph_->left(), true); - - } catch (MapGraphError& e) { - errors.emplace_back(std::string("Left: ") + e.what()); - } - Optional adjacentLeft; - try { - adjacentLeft = neighboringLaneletImpl(vertex, graph_->adjacentLeft(), true); - } catch (MapGraphError& e) { - errors.emplace_back(std::string("Adjacent left: ") + e.what()); - } - if (left && adjacentLeft) { - errors.emplace_back("Lanelet " + std::to_string(id) + " has both 'left' (id: " + std::to_string(left->id()) + - ") and 'adjancent_left' (id: " + std::to_string(adjacentLeft->id()) + ") lanelet"); - } - if (left) { - LaneletRelations rel{rightRelations(*left)}; - if (rel.empty()) { - errors.emplace_back("There is a 'left' relation from " + std::to_string(id) + " to " + - std::to_string(left->id()) + " but no relation back"); - } else if (rel.front().lanelet != ll) { - errors.emplace_back("There is a 'left' relation from " + std::to_string(id) + " to " + - std::to_string(left->id()) + ", but " + std::to_string(id) + - " isn't the closest lanelet the other way round"); - } - } - if (adjacentLeft) { - LaneletRelations rel{rightRelations(*adjacentLeft)}; - if (rel.empty()) { - errors.emplace_back("There is a 'adjacentLeft' relation from " + std::to_string(id) + " to " + - std::to_string(adjacentLeft->id()) + " but no relation back"); - } else if (rel.front().lanelet != ll) { - errors.emplace_back("There is a 'adjacentLeft' relation from " + std::to_string(id) + " to " + - std::to_string(adjacentLeft->id()) + ", but " + std::to_string(id) + - " isn't the closest lanelet the other way round"); - } - } - // Check right - Optional right; - try { - right = neighboringLaneletImpl(vertex, graph_->right(), true); - } catch (MapGraphError& e) { - errors.emplace_back(std::string("Right: ") + e.what()); - } - Optional adjacentRight; - try { - adjacentRight = neighboringLaneletImpl(vertex, graph_->adjacentRight(), true); - } catch (MapGraphError& e) { - errors.emplace_back(std::string("Adjacent right: ") + e.what()); - } - if (right && adjacentRight) { - errors.emplace_back("Lanelet " + std::to_string(id) + " has both 'right' (id: " + std::to_string(right->id()) + - ") and 'adjancent_right' (id: " + std::to_string(adjacentRight->id()) + ") lanelet"); - } - if (right) { - LaneletRelations rel{leftRelations(*right)}; - if (rel.empty()) { - errors.emplace_back("There is a 'right' relation from " + std::to_string(id) + " to " + - std::to_string(right->id()) + " but no relation back"); - } else if (rel.front().lanelet != ll) { - errors.emplace_back("There is a 'right' relation from " + std::to_string(id) + " to " + - std::to_string(right->id()) + ", but " + std::to_string(id) + - " isn't the closest lanelet the other way round"); - } - } - if (adjacentRight) { - LaneletRelations rel{leftRelations(*adjacentRight)}; - if (rel.empty()) { - errors.emplace_back("There is a 'adjacentRight' relation from " + std::to_string(id) + " to " + - std::to_string(adjacentRight->id()) + " but no relation back"); - } else if (rel.front().lanelet != ll) { - errors.emplace_back("There is a 'adjacentRight' relation from " + std::to_string(id) + " to " + - std::to_string(adjacentRight->id()) + ", but " + std::to_string(id) + - " isn't the closest lanelet the other way round"); - } - } - } - if (throwOnError && !errors.empty()) { - std::stringstream ss; - ss << "Errors found in graph:"; - for (const auto& err : errors) { - ss << "\n\t- " << err; - } - throw MapGraphError(ss.str()); - } - return errors; -} - -MapGraph::MapGraph(std::unique_ptr&& graph, LaneletSubmapConstPtr&& passableMap) - : graph_{std::move(graph)}, passableLaneletSubmap_{std::move(passableMap)} {} - -} // namespace map_learning -} // namespace lanelet diff --git a/lanelet2_map_learning/src/MapGraphBuilder.cpp b/lanelet2_map_learning/src/MapGraphBuilder.cpp deleted file mode 100644 index e7f99259..00000000 --- a/lanelet2_map_learning/src/MapGraphBuilder.cpp +++ /dev/null @@ -1,359 +0,0 @@ -#include "lanelet2_map_learning/internal/MapGraphBuilder.h" - -#include -#include -#include - -#include - -#include "lanelet2_map_learning/Exceptions.h" -#include "lanelet2_map_learning/MapGraph.h" -#include "lanelet2_map_learning/internal/Graph.h" - -namespace lanelet { -namespace map_learning { -namespace internal { -namespace { -inline IdPair orderedIdPair(const Id id1, const Id id2) { return (id1 < id2) ? IdPair(id1, id2) : IdPair(id2, id1); } -} // namespace - -//! This class collects lane changable lanelets and combines them to a sequence of adjacent lanechangable lanelets -class LaneChangeLaneletsCollector { - struct LaneChangeInfo { - ConstLanelet target; - bool visited; - }; - using LaneChangeMap = std::unordered_map; - - public: - using LaneChangeLanelets = std::pair; - - LaneChangeLaneletsCollector() = default; - void add(ConstLanelet from, ConstLanelet to) { - laneChanges_.emplace(std::move(from), LaneChangeInfo{std::move(to), false}); - currPos_ = laneChanges_.begin(); - } - - template - Optional getNextChangeLanelets(Func1&& prev, Func2&& next) { - for (; currPos_ != laneChanges_.end() && currPos_->second.visited; ++currPos_) { - } - if (currPos_ == laneChanges_.end()) { - return {}; - } - return getLaneChangeLanelets(currPos_, std::forward(prev), std::forward(next)); - } - - private: - template - LaneChangeLanelets getLaneChangeLanelets(LaneChangeMap::iterator iter, Func1&& prev, Func2&& next) { - iter->second.visited = true; - auto followers = getAdjacentLaneChangeLanelets(iter, std::forward(next)); - auto predecessors = getAdjacentLaneChangeLanelets(iter, std::forward(prev)); - std::reverse(predecessors.first.begin(), predecessors.first.end()); - std::reverse(predecessors.second.begin(), predecessors.second.end()); - std::pair result; - result.first = utils::concatenate({predecessors.first, ConstLanelets{iter->first}, followers.first}); - result.second = utils::concatenate({predecessors.second, ConstLanelets{iter->second.target}, followers.second}); - return result; - } - - template - LaneChangeLanelets getAdjacentLaneChangeLanelets(LaneChangeMap::iterator iter, Func1&& adjacent) { - std::pair successors; - while (true) { - auto nextSourceLlts = adjacent(iter->first); - auto nextTargetLlts = adjacent(iter->second.target); - if (nextSourceLlts.size() != 1 || nextTargetLlts.size() != 1) { - break; - } - ConstLanelet& nextSourceLlt = nextSourceLlts.front(); - ConstLanelet& nextTargetLlt = nextTargetLlts.front(); - iter = laneChanges_.find(nextSourceLlt); - if (iter == laneChanges_.end() || iter->second.visited || iter->second.target != nextTargetLlt) { - break; - } - iter->second.visited = true; - successors.first.push_back(nextSourceLlt); - successors.second.push_back(nextTargetLlt); - } - return successors; - } - - LaneChangeMap laneChanges_; - LaneChangeMap::iterator currPos_{laneChanges_.end()}; -}; - -MapGraphBuilder::MapGraphBuilder(const traffic_rules::TrafficRules& trafficRules, const MapGraph::Configuration& config) - : graph_{std::make_unique()}, trafficRules_{trafficRules}, config_{config} {} - -MapGraphUPtr MapGraphBuilder::build(const LaneletMapLayers& laneletMapLayers) { - auto passableLanelets = getPassableLanelets(laneletMapLayers.laneletLayer, trafficRules_); - auto passableAreas = getPassableAreas(laneletMapLayers.areaLayer, trafficRules_); - auto passableMap = utils::createConstSubmap(passableLanelets, passableAreas); - appendBidirectionalLanelets(passableLanelets); - addLaneletsToGraph(passableLanelets); - addAreasToGraph(passableAreas); - addEdges(passableLanelets, passableMap->laneletLayer); - addEdges(passableAreas, passableMap->laneletLayer, passableMap->areaLayer); - return std::make_unique(std::move(graph_), std::move(passableMap)); -} - -ConstLanelets MapGraphBuilder::getPassableLanelets(const LaneletLayer& lanelets, - const traffic_rules::TrafficRules& trafficRules) { - ConstLanelets llts; - llts.reserve(lanelets.size()); - std::copy_if(lanelets.begin(), lanelets.end(), std::back_inserter(llts), - [&trafficRules](const ConstLanelet& llt) { return trafficRules.canPass(llt); }); - return llts; -} - -ConstAreas MapGraphBuilder::getPassableAreas(const AreaLayer& areas, const traffic_rules::TrafficRules& trafficRules) { - ConstAreas ars; - ars.reserve(areas.size()); - std::copy_if(areas.begin(), areas.end(), std::back_inserter(ars), - [&trafficRules](const ConstArea& area) { return trafficRules.canPass(area); }); - return ars; -} - -void MapGraphBuilder::appendBidirectionalLanelets(ConstLanelets& llts) { - std::deque invLanelets; - for (auto& ll : llts) { - if (trafficRules_.canPass(ll.invert())) { - invLanelets.push_back(ll.invert()); - bothWaysLaneletIds_.emplace(ll.id()); - } - } - llts.insert(llts.end(), invLanelets.begin(), invLanelets.end()); -} - -void MapGraphBuilder::addLaneletsToGraph(ConstLanelets& llts) { - for (auto& ll : llts) { - graph_->addVertex(VertexInfo{ll}); - addPointsToSearchIndex(ll); - } -} - -void MapGraphBuilder::addAreasToGraph(ConstAreas& areas) { - for (auto& ar : areas) { - graph_->addVertex(VertexInfo{ar}); - } -} - -void MapGraphBuilder::addEdges(const ConstLanelets& lanelets, const LaneletLayer& passableLanelets) { - LaneChangeLaneletsCollector leftToRight; - LaneChangeLaneletsCollector rightToLeft; - // Check relations between lanelets - for (auto const& ll : lanelets) { - addFollowingEdges(ll); - addSidewayEdge(rightToLeft, ll, ll.leftBound(), RelationType::AdjacentLeft); - addSidewayEdge(leftToRight, ll, ll.rightBound(), RelationType::AdjacentRight); - addConflictingEdge(ll, passableLanelets); - } - - // now process the lane changes - addLaneChangeEdges(rightToLeft, RelationType::Left); - addLaneChangeEdges(leftToRight, RelationType::Right); -} - -void MapGraphBuilder::addEdges(const ConstAreas& areas, const LaneletLayer& passableLanelets, - const AreaLayer& passableAreas) { - for (const auto& area : areas) { - addAreaEdge(area, passableLanelets); - addAreaEdge(area, passableAreas); - } -} - -void MapGraphBuilder::addFollowingEdges(const ConstLanelet& ll) { - auto endPointsLanelets = - pointsToLanelets_.equal_range(orderedIdPair(ll.leftBound().back().id(), ll.rightBound().back().id())); - // Following - ConstLanelets following; - std::for_each(endPointsLanelets.first, endPointsLanelets.second, [&ll, this, &following](auto it) { - if (geometry::follows(ll, it.second) && this->trafficRules_.canPass(ll, it.second)) { - following.push_back(it.second); - } - }); - if (following.empty()) { - return; - } - // find out if there are several previous merging lanelets - ConstLanelets merging; - std::for_each(endPointsLanelets.first, endPointsLanelets.second, [&following, this, &merging](auto it) { - if (geometry::follows(it.second, following.front()) && this->trafficRules_.canPass(it.second, following.front())) { - merging.push_back(it.second); - } - }); - RelationType relation = RelationType::Successor; - for (auto& followingIt : following) { - assignEdge(ll, followingIt, relation); - } -} - -void MapGraphBuilder::addSidewayEdge(LaneChangeLaneletsCollector& laneChangeLanelets, const ConstLanelet& ll, - const ConstLineString3d& bound, const RelationType& relation) { - auto directlySideway = [&relation, &ll](const ConstLanelet& sideLl) { - return relation == RelationType::AdjacentLeft ? geometry::leftOf(sideLl, ll) : geometry::rightOf(sideLl, ll); - }; - auto sideOf = pointsToLanelets_.equal_range(orderedIdPair(bound.front().id(), bound.back().id())); - for (auto it = sideOf.first; it != sideOf.second; ++it) { - if (ll != it->second && !hasEdge(ll, it->second) && directlySideway(it->second)) { - if (trafficRules_.canChangeLane(ll, it->second)) { - // we process lane changes later, when we know all lanelets that can participate in lane change - laneChangeLanelets.add(ll, it->second); - } else { - assignEdge(ll, it->second, relation); - } - } - } -} - -void MapGraphBuilder::addConflictingEdge(const ConstLanelet& ll, const LaneletLayer& passableLanelets) { - // Conflicting - ConstLanelets results = passableLanelets.search(geometry::boundingBox2d(ll)); - ConstLanelet other; - for (auto& result : results) { - if (bothWaysLaneletIds_.find(ll.id()) != bothWaysLaneletIds_.end() && result == ll) { - other = result.invert(); - assignEdge(ll, other, RelationType::Conflicting); - assignEdge(other, ll, RelationType::Conflicting); - continue; - } - other = result; - if (hasEdge(ll, result)) { - continue; - } - auto vertex = graph_->getVertex(other); - if (!vertex || result == ll) { - continue; - } - auto maxHeight = participantHeight(); - if ((maxHeight && geometry::overlaps3d(ll, other, *maxHeight)) || (!maxHeight && geometry::overlaps2d(ll, other))) { - assignEdge(ll, other, RelationType::Conflicting); - assignEdge(other, ll, RelationType::Conflicting); - } - } -} - -void MapGraphBuilder::addLaneChangeEdges(LaneChangeLaneletsCollector& laneChanges, const RelationType& relation) { - auto getSuccessors = [this](auto beginEdgeIt, auto endEdgeIt, auto getVertex) { - ConstLanelets nexts; - for (; beginEdgeIt != endEdgeIt; ++beginEdgeIt) { - auto& edgeInfo = graph_->get()[*beginEdgeIt]; - if (edgeInfo.relation == RelationType::Successor) { - nexts.push_back(graph_->get()[getVertex(*beginEdgeIt, graph_->get())].lanelet()); - } - } - return nexts; - }; - auto next = [this, &getSuccessors](const ConstLanelet& llt) { - auto edges = boost::out_edges(*graph_->getVertex(llt), graph_->get()); - return getSuccessors(edges.first, edges.second, [](auto edge, const auto& g) { return boost::target(edge, g); }); - }; - auto prev = [this, &getSuccessors](const ConstLanelet& llt) { - auto edges = boost::in_edges(*graph_->getVertex(llt), graph_->get()); - return getSuccessors(edges.first, edges.second, [](auto edge, const auto& g) { return boost::source(edge, g); }); - }; - Optional laneChangeLanelets; - while (!!(laneChangeLanelets = laneChanges.getNextChangeLanelets(prev, next))) { - assignLaneChange(std::move(laneChangeLanelets->first), std::move(laneChangeLanelets->second), relation); - } -} - -void MapGraphBuilder::addAreaEdge(const ConstArea& area, const LaneletLayer& passableLanelets) { - auto candidates = passableLanelets.search(geometry::boundingBox2d(area)); - for (auto& candidate : candidates) { - bool canPass = false; - if (trafficRules_.canPass(area, candidate)) { - canPass = true; - assignEdge(area, candidate, RelationType::Area); - } - if (trafficRules_.canPass(area, candidate.invert())) { - canPass = true; - assignEdge(area, candidate.invert(), RelationType::Area); - } - if (trafficRules_.canPass(candidate, area)) { - canPass = true; - assignEdge(candidate, area, RelationType::Area); - } - if (trafficRules_.canPass(candidate.invert(), area)) { - canPass = true; - assignEdge(candidate.invert(), area, RelationType::Area); - } - if (canPass) { - continue; - } - auto maxHeight = participantHeight(); - if ((maxHeight && geometry::overlaps3d(area, candidate, *maxHeight)) || - (!maxHeight && geometry::overlaps2d(area, candidate))) { - assignEdge(candidate, area, RelationType::Conflicting); - } - } -} - -void MapGraphBuilder::addAreaEdge(const ConstArea& area, const AreaLayer& passableAreas) { - auto candidates = passableAreas.search(geometry::boundingBox2d(area)); - for (auto& candidate : candidates) { - if (candidate == area) { - continue; - } - if (trafficRules_.canPass(area, candidate)) { - assignEdge(area, candidate, RelationType::Area); - continue; - } - auto maxHeight = participantHeight(); - if ((maxHeight && geometry::overlaps3d(ConstArea(area), candidate, *maxHeight)) || - (!maxHeight && geometry::overlaps2d(ConstArea(area), candidate))) { - assignEdge(candidate, area, RelationType::Conflicting); - } - } -} - -Optional MapGraphBuilder::participantHeight() const { - auto height = config_.find(MapGraph::ParticipantHeight); - if (height != config_.end()) { - return height->second.asDouble(); - } - return {}; -} - -void MapGraphBuilder::addPointsToSearchIndex(const ConstLanelet& ll) { - using PointLaneletPair = std::pair; - pointsToLanelets_.insert( - PointLaneletPair(orderedIdPair(ll.leftBound().front().id(), ll.rightBound().front().id()), ll)); - pointsToLanelets_.insert( - PointLaneletPair(orderedIdPair(ll.leftBound().back().id(), ll.rightBound().back().id()), ll)); - pointsToLanelets_.insert( - PointLaneletPair(orderedIdPair(ll.leftBound().front().id(), ll.leftBound().back().id()), ll)); - pointsToLanelets_.insert( - PointLaneletPair(orderedIdPair(ll.rightBound().front().id(), ll.rightBound().back().id()), ll)); -} - -bool MapGraphBuilder::hasEdge(const ConstLanelet& from, const ConstLanelet& to) { - return !!graph_->getEdgeInfo(from, to); -} - -void MapGraphBuilder::assignLaneChange(ConstLanelets froms, ConstLanelets tos, const RelationType& relation) { - assert(relation == RelationType::Left || relation == RelationType::Right); - assert(froms.size() == tos.size()); - for (; !froms.empty(); froms.erase(froms.begin()), tos.erase(tos.begin())) { - graph_->addEdge(froms.front(), tos.front(), EdgeInfo{relation}); - } -} - -void MapGraphBuilder::assignEdge(const ConstLaneletOrArea& from, const ConstLaneletOrArea& to, - const RelationType& relation) { - EdgeInfo edgeInfo{relation}; - if (relation == RelationType::Successor || relation == RelationType::Area || relation == RelationType::Left || - relation == RelationType::Right || relation == RelationType::AdjacentLeft || - relation == RelationType::AdjacentRight || relation == RelationType::Conflicting) { - graph_->addEdge(from, to, edgeInfo); - } else { - assert(false && "Trying to add edge with wrong relation type to graph."); // NOLINT - } - return; -} - -} // namespace internal -} // namespace map_learning -} // namespace lanelet From d8cf01563a2e5a4b200ccfb096fc374277a1f3fa Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Fri, 1 Dec 2023 15:36:10 +0100 Subject: [PATCH 21/64] draft, whole module builds again --- .../include/lanelet2_map_learning/MapData.h | 4 +- .../lanelet2_map_learning/MapDataInterface.h | 9 +- .../lanelet2_map_learning/MapFeatures.h | 18 ++- .../include/lanelet2_map_learning/Utils.h | 2 +- lanelet2_map_learning/src/MapData.cpp | 2 + .../src/MapDataInterface.cpp | 117 ++---------------- lanelet2_map_learning/src/MapFeatures.cpp | 26 +++- 7 files changed, 48 insertions(+), 130 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index 3fb04248..ce2413c6 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -45,8 +45,8 @@ class LaneData { LineStringType getLineStringTypeFromId(Id id); LaneLineStringFeature getLineStringFeatFromId(Id id); std::vector> computeCompoundLeftBorders(const ConstLanelets& path); - std::vector> LaneData::computeCompoundRightBorders(const ConstLanelets& path); - CompoundLaneLineStringFeature LaneData::computeCompoundCenterline(const ConstLanelets& path); + std::vector> computeCompoundRightBorders(const ConstLanelets& path); + CompoundLaneLineStringFeature computeCompoundCenterline(const ConstLanelets& path); LaneLineStringFeatures roadBorders_; // auxilliary features LaneLineStringFeatures laneDividers_; // auxilliary features diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h index 819889ec..a62f3884 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h @@ -41,16 +41,15 @@ class MapDataInterface { TEData laneTEDataBatch(const BasicPoints2d& pts, const std::vector& yaws); private: - LaneData getLaneData(lanelet::routing::RoutingGraphConstPtr localSubmapGraph); + LaneData getLaneData(LaneletSubmapConstPtr localSubmap, routing::RoutingGraphConstPtr localSubmapGraph); - TEData getLaneTEData(lanelet::routing::RoutingGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, - std::unordered_map& teId2Index); + TEData getLaneTEData(routing::RoutingGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap); LaneletMapConstPtr laneletMap_; LaneletSubmapConstPtr localSubmap_; std::unordered_map teId2Index_; - lanelet::routing::RoutingGraphConstPtr localSubmapGraph_; - Optional currPos_; // in the map frame + routing::RoutingGraphConstPtr localSubmapGraph_; + Optional currPos_; // in the map frame Optional currYaw_; // in the map frame Configuration config_; traffic_rules::TrafficRulesPtr trafficRules_; diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index ab83d1ce..362eca40 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -28,7 +28,7 @@ class MapFeature { bool valid_{true}; Optional mapID_; - MapFeature() {} + MapFeature(); MapFeature(Id mapID) : mapID_{mapID}, initialized_{true} {} virtual ~MapFeature() noexcept = default; }; @@ -59,8 +59,8 @@ class LaneLineStringFeature : public LineStringFeature { virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; virtual Eigen::VectorXd computeFeatureVector( bool onlyPoints, - bool pointsIn2d) const override; // uses processedFeature_ when available - virtual Eigen::MatrixXd pointMatrix(bool pointsIn2d) const; // uses processedFeature_ when available + bool pointsIn2d) const override; // uses processedFeature_ when available + virtual Eigen::MatrixXd pointMatrix(bool pointsIn2d) const override; // uses processedFeature_ when available const bool& wasCut() const { return wasCut_; } const BasicLineString3d& cutFeature() const { return cutFeature_; } @@ -75,12 +75,7 @@ class LaneLineStringFeature : public LineStringFeature { LineStringType type_; }; -using MapFeatures = std::map; -using LineStringFeatures = std::map; using LaneLineStringFeatures = std::map; - -using MapFeatureList = std::vector; -using LineStringFeatureList = std::vector; using LaneLineStringFeatureList = std::vector; class TEFeature : public LineStringFeature { @@ -94,6 +89,7 @@ class TEFeature : public LineStringFeature { int32_t /*unused*/) override; // not implemented yet Eigen::VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const override; // currently uses raw feature only + virtual Eigen::MatrixXd pointMatrix(bool pointsIn2d) const override; const TEType& teType() { return teType_; } @@ -150,8 +146,10 @@ using CompoundLaneLineStringFeatureList = std::vector; using LaneletFeatures = std::map; -Eigen::MatrixXd getFeatureVectorMatrix(const MapFeatures& mapFeatures, bool onlyPoints, bool pointsIn2d); -Eigen::MatrixXd getFeatureVectorMatrix(const MapFeatureList& mapFeatures, bool onlyPoints, bool pointsIn2d); +template +Eigen::MatrixXd getFeatureVectorMatrix(const std::map& mapFeatures, bool onlyPoints, bool pointsIn2d); +template +Eigen::MatrixXd getFeatureVectorMatrix(const std::vector& mapFeatures, bool onlyPoints, bool pointsIn2d); std::vector getPointsMatrixList(const LaneLineStringFeatures& mapFeatures, bool pointsIn2d); std::vector getPointsMatrixList(const LaneLineStringFeatureList& mapFeatures, bool pointsIn2d); diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h index 5b32e54b..13209e37 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h @@ -2,7 +2,7 @@ #include #include #include -#include ; +#include #include #include diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 9527a4a7..05b18d65 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -153,6 +153,7 @@ std::vector> LaneData::computeCompoundLeftBorders(const ConstLan compoundBorders.push_back(std::vector{start.leftBound3d().id()}); } } + return compoundBorders; } std::vector> LaneData::computeCompoundRightBorders(const ConstLanelets& path) { @@ -170,6 +171,7 @@ std::vector> LaneData::computeCompoundRightBorders(const ConstLa compoundBorders.push_back(std::vector{start.rightBound3d().id()}); } } + return compoundBorders; } CompoundLaneLineStringFeature LaneData::computeCompoundCenterline(const ConstLanelets& path) { diff --git a/lanelet2_map_learning/src/MapDataInterface.cpp b/lanelet2_map_learning/src/MapDataInterface.cpp index 96cb9c86..ede406a4 100644 --- a/lanelet2_map_learning/src/MapDataInterface.cpp +++ b/lanelet2_map_learning/src/MapDataInterface.cpp @@ -15,7 +15,7 @@ namespace lanelet { namespace map_learning { -void MapDataInterface::setCurrPosAndExtractSubmap(const BasicPoint3d& pt) { +void MapDataInterface::setCurrPosAndExtractSubmap(const BasicPoint2d& pt) { currPos_ = pt; localSubmap_ = extractSubmap(laneletMap_, *currPos_, *currYaw_, config_.submapAreaLongitudinal, config_.submapAreaLateral); @@ -28,46 +28,9 @@ MapDataInterface::MapDataInterface(LaneletMapConstPtr laneletMap, Configuration currPos_{currPos}, trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} -LaneData MapDataInterface::getLaneData(lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { - const auto& graph = localSubmapGraph->graph_; - const auto& llVertices = graph->vertexLookup(); - - ConstLanelets lls; - Eigen::MatrixX2i edgeList; - Eigen::MatrixXd edgeFeatures; - int32_t edgeCount = 0; - for (const auto& laWithVertex : llVertices) { - const auto& la = laWithVertex.first; - if (!la.isLanelet()) continue; - - auto ll = laWithVertex.first.lanelet(); - const auto& vertex = laWithVertex.second; - - if (laneletFeatureBuffer_.find(la.id()) != laneletFeatureBuffer_.end()) { - lls.push_back(nodeFeatureBuffer_[la.id()]); - } else { - MapFeature nodeFeatureVec = - getMapFeature(*ll, config_.reprType, config_.paramType, config_.nPoints, nodeFeatLength); - nodeFeatureBuffer_[la.id()] = nodeFeatureVec; - result.laneletFeatures[llId2Index[la.id()]] = nodeFeatureVec; - } - - ConstLaneletOrAreas connectedLLs = localSubmapGraph->getLaneletEdges(*ll); - for (const auto& connectedLL : connectedLLs) { - if (!connectedLL.isLanelet()) continue; - - result.edgeList.resize(edgeCount + 1, 2); - result.edgeList(edgeCount, 0) = llId2Index[la.id()]; - result.edgeList(edgeCount, 1) = llId2Index[connectedLL.id()]; - - result.edgeList.resize(edgeCount + 1, 1); - ConstLanelet connectedLLasLL = connectedLL.lanelet().get(); - RelationType edgeType = graph->getEdgeInfo(*ll, connectedLLasLL).get().relation; - result.edgeList.row(edgeCount).array() = relationToInt(edgeType); - edgeCount++; - } - } - return result; +LaneData MapDataInterface::getLaneData(LaneletSubmapConstPtr localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { + return LaneData::build(localSubmap, localSubmapGraph); } bool isTe(ConstLineString3d ls) { @@ -75,72 +38,9 @@ bool isTe(ConstLineString3d ls) { return type == AttributeValueString::TrafficLight || type == AttributeValueString::TrafficSign; } -TEData MapDataInterface::getLaneTEData(MapGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, - std::unordered_map& teId2Index) { - const auto& graph = localSubmapGraph->graph_; - const auto& llVertices = graph->vertexLookup(); - - TEData result; - int numNodesLane = llVertices.size(); - int32_t nodeFeatLengthLane = getNodeFeatureLength(config_.reprType, config_.paramType, config_.nPoints); - result.laneletFeatures.resize(numNodesLane, nodeFeatLengthLane); - - std::unordered_map llId2Index = graph->getllId2Index(); - - std::unordered_map trafficElems; - for (const auto& ls : localSubmap->lineStringLayer) { - if (isTe(ls)) { - trafficElems[ls.id()] = ls; - } - } - - teId2Index.clear(); - int i = 0; - for (const auto& entry : trafficElems) { - teId2Index[entry.second.id()] = i++; - } - - int32_t edgeCount = 0; - for (const auto& laWithVertex : llVertices) { - const auto& la = laWithVertex.first; - auto ll = laWithVertex.first.lanelet(); - const auto& vertex = laWithVertex.second; - - if (nodeFeatureBuffer_.find(la.id()) != nodeFeatureBuffer_.end()) { - result.laneletFeatures.row(llId2Index[la.id()]) = nodeFeatureBuffer_[la.id()]; - } else { - Eigen::Vector3d nodeFeatureVec = - getNodeFeatureVec(*ll, config_.reprType, config_.paramType, config_.nPoints, nodeFeatLengthLane); - nodeFeatureBuffer_[la.id()] = nodeFeatureVec; - result.laneletFeatures.row(llId2Index[la.id()]) = nodeFeatureVec; - } - - RegulatoryElementConstPtrs regElems = ll->regulatoryElements(); - for (const auto& regElem : regElems) { - ConstLineStrings3d refs = regElem->getParameters(RoleName::Refers); - for (const auto& ref : refs) { - if (isTe(ref)) { - result.edgeList.resize(edgeCount + 1, 2); - result.edgeList(edgeCount, 0) = llId2Index[la.id()]; - result.edgeList(edgeCount, 1) = result.laneletFeatures.rows() + teId2Index[ref.id()]; - - result.teFeatures.resize(edgeCount + 1, 13); - if (teFeatureBuffer_.find(ref.id()) != teFeatureBuffer_.end()) { - result.teFeatures.row(llId2Index[ref.id()]) = teFeatureBuffer_[ref.id()]; - } else { - Eigen::Vector3d teFeatureVec = getTEFeatureVec(ref); - teFeatureBuffer_[ref.id()] = teFeatureVec; - result.teFeatures.row(llId2Index[ref.id()]) = teFeatureVec; - } - - result.edgeFeatures.resize(edgeCount + 1, 1); - result.edgeFeatures.row(edgeCount).array() = 1; - edgeCount++; - } - } - } - } - return result; +TEData MapDataInterface::getLaneTEData(routing::RoutingGraphConstPtr localSubmapGraph, + LaneletSubmapConstPtr localSubmap) { + throw std::runtime_error("Not implemented yet!"); } LaneData MapDataInterface::laneData() { @@ -149,7 +49,7 @@ LaneData MapDataInterface::laneData() { "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); } - return getLaneData(localSubmapGraph_); + return getLaneData(localSubmap_, localSubmapGraph_); } TEData MapDataInterface::teData() { @@ -157,6 +57,7 @@ TEData MapDataInterface::teData() { throw InvalidObjectStateError( "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); } + throw std::runtime_error("Not implemented yet!"); return TEData(); } diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index 56c9f3ea..bf517e62 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -99,7 +99,7 @@ Eigen::MatrixXd LaneLineStringFeature::pointMatrix(bool pointsIn2d) const { const BasicLineString3d& selectedFeature = (cutAndResampledFeature_.size() > 0) ? cutAndResampledFeature_ : rawFeature_; Eigen::MatrixXd mat = pointsIn2d ? Eigen::MatrixXd(selectedFeature.size(), 2) - : Eigen::MatrixXd(3 * selectedFeature.size(), 3); // n points with 2/3 dims + type + : Eigen::MatrixXd(selectedFeature.size(), 3); // n points with 2/3 dims + type if (pointsIn2d == true) { for (size_t i = 0; i < selectedFeature.size(); i++) { mat.row(i) = selectedFeature[i](Eigen::seq(0, 1)); @@ -112,6 +112,21 @@ Eigen::MatrixXd LaneLineStringFeature::pointMatrix(bool pointsIn2d) const { return mat; } +Eigen::MatrixXd TEFeature::pointMatrix(bool pointsIn2d) const { + Eigen::MatrixXd mat = pointsIn2d ? Eigen::MatrixXd(rawFeature_.size(), 2) + : Eigen::MatrixXd(rawFeature_.size(), 3); // n points with 2/3 dims + type + if (pointsIn2d == true) { + for (size_t i = 0; i < rawFeature_.size(); i++) { + mat.row(i) = rawFeature_[i](Eigen::seq(0, 1)); + } + } else { + for (size_t i = 0; i < rawFeature_.size(); i++) { + mat.row(i) = rawFeature_[i](Eigen::seq(0, 2)); + } + } + return mat; +} + LaneletFeature::LaneletFeature(const LaneLineStringFeature& leftBoundary, const LaneLineStringFeature& rightBoundary, const LaneLineStringFeature& centerline, Id mapID) : MapFeature(mapID), leftBoundary_{leftBoundary}, rightBoundary_{rightBoundary}, centerline_{centerline} {} @@ -135,6 +150,7 @@ bool LaneletFeature::process(const OrientedRect& bbox, const ParametrizationType if (!leftBoundary_.valid() || !rightBoundary_.valid() || !centerline_.valid()) { valid_ = false; } + return valid_; } Eigen::VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { @@ -192,15 +208,17 @@ CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStrin } } -Eigen::MatrixXd getFeatureVectorMatrix(const MapFeatures& mapFeatures, bool onlyPoints, bool pointsIn2d) { - MapFeatureList featList; +template +Eigen::MatrixXd getFeatureVectorMatrix(const std::map& mapFeatures, bool onlyPoints, bool pointsIn2d) { + std::vector featList; for (const auto& pair : mapFeatures) { featList.push_back(pair.second); } return getFeatureVectorMatrix(featList, onlyPoints, pointsIn2d); } -Eigen::MatrixXd getFeatureVectorMatrix(const MapFeatureList& mapFeatures, bool onlyPoints, bool pointsIn2d) { +template +Eigen::MatrixXd getFeatureVectorMatrix(const std::vector& mapFeatures, bool onlyPoints, bool pointsIn2d) { assert(!mapFeatures.empty()); std::vector featureVectors; for (const auto& feat : mapFeatures) { From 95ccdd4e28cb12f00346f5fa97b18e49fc0618aa Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Wed, 6 Dec 2023 20:56:06 +0100 Subject: [PATCH 22/64] add unit test map --- lanelet2_map_learning/src/MapData.cpp | 17 - lanelet2_map_learning/test/test_map.h | 564 +++++--------------------- 2 files changed, 109 insertions(+), 472 deletions(-) diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 05b18d65..dc7b4a8d 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -77,23 +77,6 @@ void LaneData::processRightBoundaries(LaneletSubmapConstPtr& localSubmap, } } -// template -// Optional> getElementIndex2d(const std::vector>& vec, const T& el) { -// Optional> result; -// for (size_t i = 0; i < vec.size(); i++) { -// for (size_t j = 0; j < vec[i].size(); j++) { -// if (vec[i][j] == el) { -// std::pair resIdx; -// resIdx->first = i; -// resIdx->second = j; -// result = resIdx; -// break; -// } -// } -// } -// return result -// } - // Idea: algorithm for paths that starts with LLs with no previous, splits on junctions and terminates on LLs with // no successors void getPaths(lanelet::routing::RoutingGraphConstPtr localSubmapGraph, std::vector& paths, diff --git a/lanelet2_map_learning/test/test_map.h b/lanelet2_map_learning/test/test_map.h index faded50c..9154ee1a 100644 --- a/lanelet2_map_learning/test/test_map.h +++ b/lanelet2_map_learning/test/test_map.h @@ -8,7 +8,6 @@ #include #include "lanelet2_map_learning/Forward.h" -#include "lanelet2_map_learning/MapGraph.h" /// The coordinates and relations for this test can be found in "LaneletTestMap.xml" which can be viewed in /// https://www.draw.io @@ -16,26 +15,6 @@ namespace lanelet { namespace map_learning { namespace tests { -inline MapGraphPtr setUpGermanVehicleGraph(LaneletMap& map, double participantHeight = 2.) { - traffic_rules::TrafficRulesPtr trafficRules{traffic_rules::TrafficRulesFactory::create( - Locations::Germany, Participants::Vehicle, traffic_rules::TrafficRules::Configuration())}; - MapGraph::Configuration configuration; - configuration.insert(std::make_pair(MapGraph::ParticipantHeight, participantHeight)); - return MapGraph::build(map, *trafficRules, configuration); -} - -inline MapGraphPtr setUpGermanPedestrianGraph(LaneletMap& map) { - traffic_rules::TrafficRulesPtr trafficRules{ - traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Pedestrian)}; - return MapGraph::build(map, *trafficRules); -} - -inline MapGraphPtr setUpGermanBicycleGraph(LaneletMap& map) { - traffic_rules::TrafficRulesPtr trafficRules{ - traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Bicycle)}; - return MapGraph::build(map, *trafficRules); -} - class MapGraphTestData { public: MapGraphTestData() { @@ -45,480 +24,155 @@ class MapGraphTestData { initAreas(); laneletMap = std::make_shared(lanelets, areas, std::unordered_map(), std::unordered_map(), lines, points); - vehicleGraph = setUpGermanVehicleGraph(*laneletMap); - pedestrianGraph = setUpGermanPedestrianGraph(*laneletMap); - bicycleGraph = setUpGermanBicycleGraph(*laneletMap); } void addPoint(double x, double y, double z) { - pointId++; points.insert(std::pair(pointId, Point3d(pointId, x, y, z))); + pointId++; } void addLine(const Points3d& points) { - lineId++; lines.insert(std::pair(lineId, LineString3d(lineId, points))); + lineId++; } void addLaneletVehicle(const LineString3d& left, const LineString3d& right) { - laneletId++; Lanelet ll{laneletId, left, right}; ll.setAttribute(AttributeName::Subtype, AttributeValueString::Road); lanelets.insert(std::make_pair(laneletId, ll)); - } - - void addLaneletPedestrian(const LineString3d& left, const LineString3d& right) { laneletId++; - Lanelet ll{laneletId, left, right}; - ll.setAttribute(AttributeName::Subtype, AttributeValueString::Crosswalk); - lanelets.insert(std::make_pair(laneletId, ll)); - } - - void addAreaPedestrian(const LineStrings3d& outerBound) { - Area area(areaId, outerBound); - area.setAttribute(AttributeName::Subtype, AttributeValueString::Crosswalk); - areas.emplace(areaId, area); - areaId++; } Id pointId{0}; Id lineId{1000}; Id laneletId{2000}; - Id areaId{3000}; std::unordered_map lanelets; std::unordered_map points; std::unordered_map lines; std::unordered_map areas; - MapGraphPtr vehicleGraph, pedestrianGraph, bicycleGraph; LaneletMapPtr laneletMap; private: void initPoints() { points.clear(); - addPoint(0., 1., 0.); // p1 - addPoint(1., 1., 0.); - addPoint(2., 1.5, 0.); - addPoint(0., 0., 0.); - addPoint(1., 0., 0.); - addPoint(2., 0.5, 0.); - addPoint(0., 2., 0.); - addPoint(1., 2., 0.); - addPoint(2., 2.5, 0.); // p9 - addPoint(5., 3.5, 0.); - addPoint(5., 1.5, 0.); // p11 - addPoint(7., 3.5, 0.); - addPoint(7., 1.5, 0.); // p13 - addPoint(10., 5., 0.); - addPoint(10., 3., 0.); - addPoint(10., 1., 0.); // p16 - addPoint(13., 7., 0.); - addPoint(13., 5., 0.); - addPoint(13., 3., 0.); - addPoint(13., 1., 0.); // p20 - addPoint(15., 7., 0.); - addPoint(15., 5., 0.); - addPoint(15., 3., 0.); - addPoint(15., 1., 0.); // p24 - addPoint(5., 14., 0.); - addPoint(5., 12., 0.); - addPoint(5., 10., 0.); - addPoint(7., 14., 0.); - addPoint(7., 12., 0.); - addPoint(7., 10., 0.); - addPoint(10., 13., 0.); // p31 - addPoint(10., 11., 0.); - addPoint(12., 13., 0.); - addPoint(12., 11., 0.); - addPoint(15., 14., 0.); - addPoint(15., 12., 0.); - addPoint(15., 10., 0.); - addPoint(17., 14., 0.); - addPoint(17., 12., 0.); - addPoint(17., 10., 0.); // p40 - addPoint(17., 8., 0.); - addPoint(19., 12., 0.); - addPoint(19., 10., 0.); - addPoint(19., 8., 0.); // p44 - addPoint(21., 12., 0.); - addPoint(21., 10., 0.); // p46 - addPoint(21., 8., 0.); - addPoint(23., 12., 0.); // p48 - addPoint(23., 10., 0.); // p49 - addPoint(23., 8., 0.); // p50 - addPoint(10.5, 14., 0.); // p51 - addPoint(11.5, 14., 0.); // p52 - addPoint(10.5, 10., 0.); // p53 - addPoint(11.5, 10., 0.); // p54 - addPoint(3., 5., 3.); // p55 - addPoint(4., 5., 3.); // p56 - addPoint(3., -1., 3.); // p57 - addPoint(4., -1., 3.); // p58 - addPoint(26., 10., 0.); // p59 - addPoint(28., 10., 0.); // p60 - addPoint(30., 10., 0.); // p61 - addPoint(25., 11., 0.); // p62 - addPoint(26., 12., 0.); // p63 - addPoint(28., 12., 0.); // p64 - addPoint(30., 12., 0.); // p65 - addPoint(25., 13., 0.); // p66 - addPoint(28., 14., 0.); // p67 - addPoint(30., 14., 0.); // p68 - addPoint(32., 10., 0.); // p69 - addPoint(33., 9., 0.); // p70 - addPoint(32., 8., 0.); // p71 - addPoint(33., 7., 0.); // p72 - addPoint(28., 8., 0.); // p73 - addPoint(30., 8., 0.); // p74 - addPoint(35., 10., 0.); // p75 - addPoint(35., 8., 0.); // p76 - addPoint(35., 6., 0.); // p77 - addPoint(37., 9., 0.); // p78 - addPoint(37., 7., 0.); // p79 - addPoint(37., 5., 0.); // p80 - addPoint(38., 10., 0.); // p81 - addPoint(38., 8., 0.); // p82 - addPoint(41., 10., 0.); // p83 - addPoint(41., 8., 0.); // p84 - addPoint(41., 6., 0.); // p85 - addPoint(43., 10., 0.); // p86 - addPoint(43., 8., 0.); // p87 - - // points around areas - pointId = 88; - addPoint(24, 4, 0); // p89 - addPoint(26, 4, 0); // p90 - addPoint(24, 2, 0); // p91 - addPoint(26, 2, 0); // p92 - addPoint(27, 5, 0); // p93 - addPoint(28, 5, 0); // p94 - addPoint(29, 4, 0); // p95 - addPoint(29, 2, 0); // p96 - addPoint(28, 1, 0); // p97 - addPoint(27, 1, 0); // p98 - addPoint(27, 0, 0); // p99 - addPoint(28, 0, 0); // p100 - addPoint(27, 6, 0); // p101 - addPoint(28, 6, 0); // p102 - addPoint(31, 4, 0); // p103 - addPoint(33, 4, 0); // p104 - addPoint(31, 2, 0); // p105 - addPoint(33, 2, 0); // p106 - addPoint(31, 0, 0); // p107 - - // points on the conflicting and circular section - pointId = 119; - addPoint(33, 11, 0); // p120 - addPoint(37, 16, 0); // p121 - addPoint(37, 14, 0); // p122 - addPoint(37, 12, 0); // p123 - addPoint(41, 12, 0); // p124 - addPoint(32, 16, 0); // p125 - addPoint(34, 16, 0); // p126 - addPoint(28, 18, 0); // p127 - addPoint(28, 20, 0); // p128 - addPoint(21, 14, 0); // p129 - addPoint(23, 14, 0); // p130 - addPoint(19, 14, 0); // p131 + addPoint(0.0, 0.0, 1.0); // p0 + addPoint(2.0, 0.0, 1.0); // p1 + addPoint(4.0, 0.0, 1.0); // p2 + addPoint(6.0, 0.0, 1.0); // p3 + addPoint(8.0, 0.0, 1.0); // p4 + addPoint(10.0, 0.0, 1.0); // p5 + addPoint(12.0, 0.0, 1.0); // p6 + addPoint(14.0, 0.0, 1.0); // p7 + addPoint(6.0, 1.0, 1.0); + addPoint(8.0, 1.0, 1.0); // p9 + addPoint(0.0, 2.0, 1.0); + addPoint(2.0, 2.0, 1.0); + addPoint(4.0, 2.0, 1.0); + addPoint(6.0, 2.0, 1.0); + addPoint(8.0, 2.0, 1.0); + addPoint(10.0, 2.0, 1.0); + addPoint(12.0, 2.0, 1.0); + addPoint(14.0, 2.0, 1.0); // p17 + addPoint(6.0, 3.0, 1.0); + addPoint(8.0, 3.0, 1.0); // p19 + addPoint(0.0, 4.0, 1.0); + addPoint(2.0, 4.0, 1.0); + addPoint(4.0, 4.0, 1.0); + addPoint(6.0, 4.0, 1.0); + addPoint(8.0, 4.0, 1.0); + addPoint(10.0, 4.0, 1.0); + addPoint(12.0, 4.0, 1.0); + addPoint(14.0, 4.0, 1.0); // p27 + addPoint(5.0, 5.0, 1.0); + addPoint(7.0, 5.0, 1.0); + addPoint(9.0, 5.0, 1.0); // p30 + addPoint(5.0, 7.0, 1.0); + addPoint(7.0, 7.0, 1.0); + addPoint(9.0, 7.0, 1.0); // p33 + addPoint(5.0, 9.0, 1.0); + addPoint(7.0, 9.0, 1.0); + addPoint(9.0, 9.0, 1.0); // p36 + addPoint(5.0, 11.0, 1.0); + addPoint(7.0, 11.0, 1.0); + addPoint(9.0, 11.0, 1.0); // p39 } void initLineStrings() { lines.clear(); - addLine(Points3d{points.at(1), points.at(2)}); // l1001 - lines.at(1001).setAttribute(AttributeNamesString::LaneChange, true); - addLine(Points3d{points.at(4), points.at(5)}); - addLine(Points3d{points.at(2), points.at(3)}); - addLine(Points3d{points.at(5), points.at(6)}); - addLine(Points3d{points.at(7), points.at(8)}); - addLine(Points3d{points.at(8), points.at(9)}); // ls1006 - addLine(Points3d{points.at(9), points.at(10)}); - addLine(Points3d{points.at(3), points.at(11)}); - lines.at(1008).setAttribute(AttributeName::Type, AttributeValueString::Virtual); - addLine(Points3d{points.at(3), points.at(10)}); - lines.at(1009).setAttribute(AttributeName::Type, AttributeValueString::Virtual); - addLine(Points3d{points.at(6), points.at(11)}); // ls1010 - addLine(Points3d{points.at(10), points.at(12)}); - addLine(Points3d{points.at(11), points.at(13)}); // ls1012 - addLine(Points3d{points.at(12), points.at(14)}); - addLine(Points3d{points.at(12), points.at(15)}); - addLine(Points3d{points.at(13), points.at(15)}); - lines.at(1015).setAttribute(AttributeName::Type, AttributeValueString::Virtual); - addLine(Points3d{points.at(13), points.at(16)}); // ls1016 - addLine(Points3d{points.at(14), points.at(17)}); - addLine(Points3d{points.at(14), points.at(18)}); - addLine(Points3d{points.at(15), points.at(18)}); + addLine(Points3d{points.at(7), points.at(6), points.at(5)}); // l1000 + lines.at(1000).setAttribute(AttributeName::Type, AttributeValueString::RoadBorder); + addLine(Points3d{points.at(5), points.at(4), points.at(3), points.at(2)}); // l1001 + lines.at(1001).setAttribute(AttributeName::Type, AttributeValueString::RoadBorder); + addLine(Points3d{points.at(2), points.at(1), points.at(0)}); // l1002 + lines.at(1002).setAttribute(AttributeName::Type, AttributeValueString::RoadBorder); + + addLine(Points3d{points.at(17), points.at(16), points.at(15)}); // l1003 + lines.at(1003).setAttribute(AttributeName::Type, AttributeValueString::LineThin); + lines.at(1003).setAttribute(AttributeName::Subtype, AttributeValueString::Solid); + addLine(Points3d{points.at(15), points.at(14), points.at(13), points.at(12)}); // l1004 + lines.at(1004).setAttribute(AttributeName::Type, AttributeValueString::LineThin); + lines.at(1004).setAttribute(AttributeName::Subtype, AttributeValueString::Dashed); + addLine(Points3d{points.at(12), points.at(11), points.at(10)}); // l1005 + lines.at(1005).setAttribute(AttributeName::Type, AttributeValueString::LineThin); + lines.at(1005).setAttribute(AttributeName::Subtype, AttributeValueString::Solid); + + addLine(Points3d{points.at(20), points.at(21), points.at(22)}); // l1006 + lines.at(1006).setAttribute(AttributeName::Type, AttributeValueString::RoadBorder); + addLine(Points3d{points.at(22), points.at(23), points.at(24), points.at(25)}); // l1007 + lines.at(1007).setAttribute(AttributeName::Type, AttributeValueString::LineThin); + lines.at(1007).setAttribute(AttributeName::Subtype, AttributeValueString::Dashed); + addLine(Points3d{points.at(25), points.at(26), points.at(27)}); // l1008 + lines.at(1008).setAttribute(AttributeName::Type, AttributeValueString::RoadBorder); + + addLine(Points3d{points.at(30), points.at(25)}); // l1009 + lines.at(1009).setAttribute(AttributeName::Type, AttributeValueString::RoadBorder); + addLine(Points3d{points.at(22), points.at(28)}); // l1010 + lines.at(1010).setAttribute(AttributeName::Type, AttributeValueString::RoadBorder); + + addLine(Points3d{points.at(28), points.at(31)}); // l1011 + lines.at(1011).setAttribute(AttributeName::Type, AttributeValueString::RoadBorder); + addLine(Points3d{points.at(29), points.at(32)}); // l1012 + lines.at(1012).setAttribute(AttributeName::Type, AttributeValueString::LineThin); + lines.at(1012).setAttribute(AttributeName::Subtype, AttributeValueString::Solid); + addLine(Points3d{points.at(33), points.at(30)}); // l1013 + lines.at(1013).setAttribute(AttributeName::Type, AttributeValueString::RoadBorder); + + addLine(Points3d{points.at(31), points.at(34), points.at(37)}); // l1014 + lines.at(1014).setAttribute(AttributeName::Type, AttributeValueString::RoadBorder); + addLine(Points3d{points.at(32), points.at(35), points.at(38)}); // l1015 + lines.at(1015).setAttribute(AttributeName::Type, AttributeValueString::LineThin); + lines.at(1015).setAttribute(AttributeName::Subtype, AttributeValueString::Dashed); + addLine(Points3d{points.at(39), points.at(36), points.at(33)}); // l1016 + lines.at(1016).setAttribute(AttributeName::Type, AttributeValueString::RoadBorder); + + addLine(Points3d{points.at(12), points.at(18), points.at(29)}); // l1017 + lines.at(1017).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine(Points3d{points.at(30), points.at(19), points.at(8), points.at(2)}); // l1018 + lines.at(1018).setAttribute(AttributeName::Type, AttributeValueString::Virtual); + addLine(Points3d{points.at(5), points.at(9), points.at(18), points.at(28)}); // l1019 lines.at(1019).setAttribute(AttributeName::Type, AttributeValueString::Virtual); - addLine(Points3d{points.at(15), points.at(19)}); - lines.at(1020).setAttribute(AttributeNamesString::LaneChange, true); - addLine(Points3d{points.at(16), points.at(20)}); // ls1021 - addLine(Points3d{points.at(17), points.at(21)}); - addLine(Points3d{points.at(18), points.at(22)}); - lines.at(1023).setAttribute(AttributeNamesString::LaneChange, true); - addLine(Points3d{points.at(19), points.at(23)}); - lines.at(1024).setAttribute(AttributeNamesString::LaneChange, true); - addLine(Points3d{points.at(20), points.at(24)}); - addLine(Points3d{points.at(25), points.at(28)}); // ls1026 - addLine(Points3d{points.at(26), points.at(29)}); - addLine(Points3d{points.at(27), points.at(30)}); - addLine(Points3d{points.at(28), points.at(31)}); - addLine(Points3d{points.at(29), points.at(31)}); // ls1030 - lines.at(1030).setAttribute(AttributeName::Type, AttributeValueString::Virtual); - addLine(Points3d{points.at(29), points.at(32)}); - lines.at(1031).setAttribute(AttributeName::Type, AttributeValueString::Virtual); - addLine(Points3d{points.at(30), points.at(32)}); - addLine(Points3d{points.at(31), points.at(33)}); - addLine(Points3d{points.at(32), points.at(34)}); - addLine(Points3d{points.at(33), points.at(35)}); - addLine(Points3d{points.at(33), points.at(36)}); - lines.at(1036).setAttribute(AttributeName::Type, AttributeValueString::Virtual); - addLine(Points3d{points.at(34), points.at(36)}); - lines.at(1037).setAttribute(AttributeName::Type, AttributeValueString::Virtual); - addLine(Points3d{points.at(34), points.at(37)}); - addLine(Points3d{points.at(35), points.at(38)}); - addLine(Points3d{points.at(36), points.at(39)}); - addLine(Points3d{points.at(37), points.at(40)}); // ls1041 - addLine(Points3d{points.at(39), points.at(42)}); - addLine(Points3d{points.at(40), points.at(43)}); - lines.at(1043).setAttribute(AttributeName::Type, AttributeValueString::LineThin); - lines.at(1043).setAttribute(AttributeName::Subtype, AttributeValueString::DashedSolid); - addLine(Points3d{points.at(41), points.at(44)}); // ls1044 - addLine(Points3d{points.at(42), points.at(45)}); - addLine(Points3d{points.at(43), points.at(46)}); // ls1046 - addLine(Points3d{points.at(44), points.at(47)}); - addLine(Points3d{points.at(45), points.at(48)}); - addLine(Points3d{points.at(46), points.at(49)}); - lines.at(1049).setAttribute(AttributeName::Type, AttributeValueString::LineThin); - lines.at(1049).setAttribute(AttributeName::Subtype, AttributeValueString::SolidDashed); - addLine(Points3d{points.at(47), points.at(50)}); // ls1050 - addLine(Points3d{points.at(51), points.at(53)}); // ls1051 - addLine(Points3d{points.at(52), points.at(54)}); // ls1052 - addLine(Points3d{points.at(55), points.at(57)}); // ls1053 - addLine(Points3d{points.at(56), points.at(58)}); // ls1054 - addLine(Points3d{points.at(49), points.at(59)}); // ls1055 - addLine(Points3d{points.at(59), points.at(60)}); // ls1056 - addLine(Points3d{points.at(60), points.at(61)}); // ls1057 - lines.at(1057).setAttribute(AttributeName::Type, AttributeValueString::LineThin); - lines.at(1057).setAttribute(AttributeName::Subtype, AttributeValueString::Solid); - addLine(Points3d{points.at(49), points.at(62)}); // ls1058 - lines.at(1058).setAttribute(AttributeName::Type, AttributeValueString::Virtual); - addLine(Points3d{points.at(62), points.at(64)}); // ls1059 - lines.at(1059).setAttribute(AttributeName::Type, AttributeValueString::Virtual); - addLine(Points3d{points.at(48), points.at(63)}); // ls1060 - lines.at(1060).setAttribute(AttributeName::Type, AttributeValueString::Virtual); - addLine(Points3d{points.at(63), points.at(64)}); // ls1061 - lines.at(1061).setAttribute(AttributeName::Type, AttributeValueString::Virtual); - addLine(Points3d{points.at(64), points.at(65)}); // ls1062 - lines.at(1062).setAttribute(AttributeNamesString::LaneChange, true); - addLine(Points3d{points.at(48), points.at(66)}); // ls1063 - addLine(Points3d{points.at(66), points.at(67)}); // ls1064 - addLine(Points3d{points.at(67), points.at(68)}); // ls1065 - addLine(Points3d{points.at(61), points.at(69)}); // ls1066 - addLine(Points3d{points.at(61), points.at(70)}); // ls1067 - addLine(Points3d{points.at(74), points.at(71)}); // ls1068 - addLine(Points3d{points.at(74), points.at(72)}); // ls1069 - addLine(Points3d{points.at(73), points.at(74)}); // ls1070 - addLine(Points3d{points.at(69), points.at(75)}); // ls1071 - addLine(Points3d{points.at(70), points.at(76)}); // ls1072 - addLine(Points3d{points.at(71), points.at(76)}); // ls1073 - addLine(Points3d{points.at(72), points.at(77)}); // ls1074 - addLine(Points3d{points.at(75), points.at(81)}); // ls1075 - addLine(Points3d{points.at(76), points.at(78)}); // ls1076 - addLine(Points3d{points.at(76), points.at(82)}); // ls1077 - addLine(Points3d{points.at(77), points.at(79)}); // ls1078 - addLine(Points3d{points.at(81), points.at(83)}); // ls1079 - addLine(Points3d{points.at(78), points.at(83)}); // ls1080 - addLine(Points3d{points.at(82), points.at(84)}); // ls1081 - addLine(Points3d{points.at(79), points.at(84)}); // ls1082 - lines.at(1082).setAttribute(AttributeName::Type, AttributeValueString::LineThin); - lines.at(1082).setAttribute(AttributeName::Subtype, AttributeValueString::Dashed); - addLine(Points3d{points.at(80), points.at(85)}); // ls1083 - addLine(Points3d{points.at(83), points.at(86)}); // ls1084 - addLine(Points3d{points.at(84), points.at(87)}); // ls1085 - - // linestrings around areas - addLine({points.at(89), points.at(90)}); // ls1086 - addLine({points.at(91), points.at(92)}); // ls1087 - addLine({points.at(90), points.at(93)}); // ls1088 - addLine({points.at(93), points.at(94)}); // ls1089 - addLine({points.at(94), points.at(95)}); // ls1090 - addLine({points.at(95), points.at(96)}); // ls1091 - lines.at(1091).setAttribute(AttributeName::Type, AttributeValueString::Virtual); - addLine({points.at(96), points.at(97)}); // ls1092 - lines.at(1092).setAttribute(AttributeName::Type, AttributeValueString::Curbstone); - lines.at(1092).setAttribute(AttributeName::Subtype, AttributeValueString::Low); - addLine({points.at(97), points.at(98)}); // ls1093 - lines.at(1093).setAttribute(AttributeName::Type, AttributeValueString::Curbstone); - lines.at(1093).setAttribute(AttributeName::Subtype, AttributeValueString::Low); - addLine({points.at(98), points.at(92)}); // ls1094 - addLine({points.at(95), points.at(103)}); // ls1095 - addLine({points.at(96), points.at(105)}); // ls1096 - lines.at(1096).setAttribute(AttributeName::Type, AttributeValueString::Wall); - addLine({points.at(103), points.at(105)}); // ls1097 - lines.at(1097).setAttribute(AttributeName::Type, AttributeValueString::Virtual); - addLine({points.at(103), points.at(104)}); // ls1098 - addLine({points.at(105), points.at(106)}); // ls1099 - addLine({points.at(99), points.at(100)}); // ls1100 - addLine({points.at(101), points.at(102)}); // ls1101 - addLine({points.at(92), points.at(90)}); // ls1102 - lines.at(1102).setAttribute(AttributeName::Type, AttributeValueString::Virtual); - addLine({points.at(97), points.at(107)}); // ls1103 - addLine({points.at(107), points.at(105)}); // ls1104 - - // lines on the conflicting and circular section - lineId = 1199; - addLine({points.at(61), points.at(120)}); // ls1200 - addLine({points.at(74), points.at(70)}); // ls1201 - addLine({points.at(120), points.at(122)}); // ls1202 - addLine({points.at(70), points.at(123)}); // ls1203 - addLine({points.at(121), points.at(124)}); // ls1204 - addLine({points.at(122), points.at(83)}); // ls1205 - addLine({points.at(123), points.at(84)}); // ls1206 - addLine({points.at(68), points.at(125)}); // ls1207 - addLine({points.at(65), points.at(126)}); // ls1208 - addLine({points.at(125), points.at(127)}); // ls1209 - addLine({points.at(126), points.at(128)}); // ls1210 - addLine({points.at(127), points.at(130)}); // ls1211 - addLine({points.at(128), points.at(129)}); // ls1212 - addLine({points.at(130), points.at(48)}); // ls1213 - addLine({points.at(129), points.at(49)}); // ls1214 - addLine({points.at(129), points.at(131)}); // ls1215 - addLine({points.at(130), points.at(42)}); // ls1216 - lines.at(1205).setAttribute(AttributeName::Type, AttributeValueString::LineThin); - lines.at(1205).setAttribute(AttributeName::Subtype, AttributeValueString::Dashed); + addLine(Points3d{points.at(15), points.at(19), points.at(29)}); // l1020 + lines.at(1020).setAttribute(AttributeName::Type, AttributeValueString::Virtual); } void initLanelets() { lanelets.clear(); - addLaneletVehicle(lines.at(1001), lines.at(1002)); // ll2001 - addLaneletVehicle(lines.at(1003), lines.at(1004)); - addLaneletVehicle(lines.at(1005), lines.at(1001)); - addLaneletVehicle(lines.at(1006), lines.at(1003)); // ll2004 - addLaneletVehicle(lines.at(1007), lines.at(1008)); - addLaneletVehicle(lines.at(1009), lines.at(1010)); // ll2006 - addLaneletVehicle(lines.at(1011), lines.at(1012)); // ll2007 - addLaneletVehicle(lines.at(1013), lines.at(1015)); // ll2008 - addLaneletVehicle(lines.at(1014), lines.at(1016)); // ll2009 - addLaneletVehicle(lines.at(1017), lines.at(1019)); // ll2010 - addLaneletVehicle(lines.at(1018), lines.at(1020)); // ll2011 - addLaneletVehicle(lines.at(1020), lines.at(1021)); // ll2012 - addLaneletVehicle(lines.at(1022), lines.at(1023)); // ll2013 - addLaneletVehicle(lines.at(1023), lines.at(1024)); // ll2014 - addLaneletVehicle(lines.at(1024), lines.at(1025)); // ll2015 - addLaneletVehicle(lines.at(1027).invert(), lines.at(1026).invert()); - addLaneletVehicle(lines.at(1027), lines.at(1028)); - addLaneletVehicle(lines.at(1031).invert(), lines.at(1029).invert()); - addLaneletVehicle(lines.at(1030), lines.at(1032)); - addLaneletVehicle(lines.at(1033), lines.at(1034)); - lanelets.at(2020).setAttribute(AttributeName::OneWay, false); - addLaneletVehicle(lines.at(1037).invert(), lines.at(1035).invert()); - addLaneletVehicle(lines.at(1036), lines.at(1038)); - lanelets.at(2022).setAttribute(AttributeNamesString::Subtype, AttributeValueString::Highway); - addLaneletVehicle(lines.at(1040).invert(), lines.at(1039).invert()); - addLaneletVehicle(lines.at(1040), lines.at(1041)); // ll2024 - addLaneletVehicle(lines.at(1042), lines.at(1043)); - addLaneletVehicle(lines.at(1043), lines.at(1044)); - addLaneletVehicle(lines.at(1045), lines.at(1046)); // ll2027 - addLaneletVehicle(lines.at(1046), lines.at(1047)); - addLaneletVehicle(lines.at(1048), lines.at(1049)); - addLaneletVehicle(lines.at(1049), lines.at(1050)); // ll2030 - addLaneletPedestrian(lines.at(1051), lines.at(1052)); // ll2031 - addLaneletVehicle(lines.at(1053), lines.at(1054)); // ll2032 - addLaneletVehicle(lines.at(1060), lines.at(1055)); // ll2033 - addLaneletVehicle(lines.at(1061), lines.at(1056)); // ll2034 - addLaneletVehicle(lines.at(1062), lines.at(1057)); // ll2035 - addLaneletVehicle(lines.at(1063), lines.at(1058)); // ll2036 - addLaneletVehicle(lines.at(1064), lines.at(1059)); // ll2037 - addLaneletVehicle(lines.at(1065), lines.at(1062)); // ll2038 - addLaneletVehicle(lines.at(1066), lines.at(1068)); // ll2039 - addLaneletVehicle(lines.at(1067), lines.at(1069)); // ll2040 - addLaneletVehicle(lines.at(1057), lines.at(1070)); // ll2041 - addLaneletVehicle(lines.at(1071), lines.at(1073)); // ll2042 - addLaneletVehicle(lines.at(1072), lines.at(1074)); // ll2043 - addLaneletVehicle(lines.at(1075), lines.at(1077)); // ll2044 - addLaneletVehicle(lines.at(1076), lines.at(1078)); // ll2045 - addLaneletVehicle(lines.at(1079), lines.at(1081)); // ll2046 - addLaneletVehicle(lines.at(1080), lines.at(1082)); // ll2047 - addLaneletVehicle(lines.at(1082), lines.at(1083)); // ll2048 - addLaneletVehicle(lines.at(1084), lines.at(1085)); // ll2049 - - // area - addLaneletPedestrian(lines.at(1086), lines.at(1087)); // ll2050 - lanelets.at(2050).setAttribute(AttributeName::OneWay, true); - addLaneletPedestrian(lines.at(1101), lines.at(1089)); // ll2051 - addLaneletPedestrian(lines.at(1093).invert(), lines.at(1100)); // ll2052 - lanelets.at(2052).setAttribute(AttributeName::OneWay, true); - addLaneletPedestrian(lines.at(1098), lines.at(1099)); // ll2053 - - // lanelets on conflicting section - laneletId = 2059; - addLaneletVehicle(lines.at(1200), lines.at(1201)); // ll2060 - addLaneletVehicle(lines.at(1202), lines.at(1203)); // ll2061 - addLaneletVehicle(lines.at(1204), lines.at(1205)); // ll2062 - addLaneletVehicle(lines.at(1205), lines.at(1206)); // ll2063 - addLaneletVehicle(lines.at(1207), lines.at(1208)); // ll2064 - addLaneletVehicle(lines.at(1209), lines.at(1210)); // ll2065 - addLaneletVehicle(lines.at(1211), lines.at(1212)); // ll2066 - addLaneletVehicle(lines.at(1213), lines.at(1214)); // ll2067 - addLaneletVehicle(lines.at(1216), lines.at(1215)); // ll2068 + addLaneletVehicle(lines.at(1005), lines.at(1002)); // ll2000 + addLaneletVehicle(lines.at(1004), lines.at(1001)); // ll2001 + addLaneletVehicle(lines.at(1003), lines.at(1000)); // ll2002 + addLaneletVehicle(lines.at(1005), lines.at(1006)); // ll2003 + addLaneletVehicle(lines.at(1004), lines.at(1007)); // ll2004 + addLaneletVehicle(lines.at(1003), lines.at(1008)); // ll2005 + addLaneletVehicle(lines.at(1012), lines.at(1011)); // ll2006 + addLaneletVehicle(lines.at(1012), lines.at(1013)); // ll2007 + addLaneletVehicle(lines.at(1015), lines.at(1014)); // ll2008 + addLaneletVehicle(lines.at(1015), lines.at(1016)); // ll2009 + addLaneletVehicle(lines.at(1017), lines.at(1010)); // ll2010 + addLaneletVehicle(lines.at(1017), lines.at(1018)); // ll2011 + addLaneletVehicle(lines.at(1020), lines.at(1019)); // ll2012 + addLaneletVehicle(lines.at(1020), lines.at(1009)); // ll2013 } - - void initAreas() { - addAreaPedestrian({lines.at(1102), lines.at(1088), lines.at(1089), lines.at(1090), lines.at(1091), lines.at(1092), - lines.at(1093), lines.at(1094)}); // ar3000 - addAreaPedestrian({lines.at(1095), lines.at(1097), lines.at(1096), lines.at(1091).invert()}); // ar3001 - // addAreaPedestrian({lines.at(1096).invert(), lines.at(1092), lines.at(1103), lines.at(1104)}); // ar3002 - addAreaPedestrian( - {lines.at(1096), lines.at(1104).invert(), lines.at(1103).invert(), lines.at(1092).invert()}); // ar3002 - areas.at(3002).setAttribute(AttributeName::Subtype, AttributeValueString::Walkway); - } -}; - -namespace { // NOLINT -static MapGraphTestData testData; // NOLINT -} // namespace - -class MapGraphTest : public ::testing::Test { - public: - const std::unordered_map& lanelets{testData.lanelets}; - const std::unordered_map& areas{testData.areas}; - const std::unordered_map& points{testData.points}; - const std::unordered_map& lines{testData.lines}; - const LaneletMapConstPtr laneletMap{testData.laneletMap}; -}; - -class GermanVehicleGraph : public MapGraphTest { - protected: - GermanVehicleGraph() { EXPECT_NO_THROW(graph->checkValidity()); } // NOLINT - - public: - MapGraphConstPtr graph{testData.vehicleGraph}; -}; - -class GermanPedestrianGraph : public MapGraphTest { - protected: - GermanPedestrianGraph() { EXPECT_NO_THROW(graph->checkValidity()); } // NOLINT - - public: - MapGraphConstPtr graph{testData.pedestrianGraph}; -}; - -class GermanBicycleGraph : public MapGraphTest { - protected: - GermanBicycleGraph() { EXPECT_NO_THROW(graph->checkValidity()); } // NOLINT - - public: - MapGraphConstPtr graph{testData.bicycleGraph}; }; -template -class AllGraphsTest : public T {}; - -using AllGraphs = testing::Types; - -#ifndef TYPED_TEST_SUITE -// backwards compability with old gtest versions -#define TYPED_TEST_SUITE TYPED_TEST_CASE -#endif - -TYPED_TEST_SUITE(AllGraphsTest, AllGraphs); } // namespace tests } // namespace map_learning } // namespace lanelet From d80f4618d3b3c285055d0f1850dbc63ba8915e9e Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Wed, 6 Dec 2023 20:58:35 +0100 Subject: [PATCH 23/64] add unit test map visualization file --- lanelet2_map_learning/test/LaneletTestMap.xml | 498 +++++++++++++++++- 1 file changed, 497 insertions(+), 1 deletion(-) diff --git a/lanelet2_map_learning/test/LaneletTestMap.xml b/lanelet2_map_learning/test/LaneletTestMap.xml index 2210728f..38aed00b 100644 --- a/lanelet2_map_learning/test/LaneletTestMap.xml +++ b/lanelet2_map_learning/test/LaneletTestMap.xml @@ -1 +1,497 @@ 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 \ No newline at end of file + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 025f23482840a9a4629ff16b6d1046b844ee384e Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 7 Dec 2023 19:10:46 +0100 Subject: [PATCH 24/64] add utils unit tests and fix bugs --- .../.vscode/c_cpp_properties.json | 36 ++ lanelet2_map_learning/.vscode/settings.json | 8 + .../include/lanelet2_map_learning/MapData.h | 11 +- .../lanelet2_map_learning/MapDataInterface.h | 6 +- .../lanelet2_map_learning/MapFeatures.h | 29 +- .../include/lanelet2_map_learning/Utils.h | 37 +- lanelet2_map_learning/src/MapData.cpp | 36 +- .../src/MapDataInterface.cpp | 19 +- lanelet2_map_learning/src/Utils.cpp | 55 +- lanelet2_map_learning/test/test_map.h | 21 +- .../test/test_map_features.cpp | 9 + .../test/test_map_graph_container.cpp | 76 --- .../test/test_map_visualization.cpp | 102 ---- lanelet2_map_learning/test/test_relations.cpp | 525 ------------------ lanelet2_map_learning/test/test_utils.cpp | 59 ++ 15 files changed, 252 insertions(+), 777 deletions(-) create mode 100644 lanelet2_map_learning/.vscode/c_cpp_properties.json create mode 100644 lanelet2_map_learning/.vscode/settings.json create mode 100644 lanelet2_map_learning/test/test_map_features.cpp delete mode 100644 lanelet2_map_learning/test/test_map_graph_container.cpp delete mode 100644 lanelet2_map_learning/test/test_map_visualization.cpp delete mode 100644 lanelet2_map_learning/test/test_relations.cpp create mode 100644 lanelet2_map_learning/test/test_utils.cpp diff --git a/lanelet2_map_learning/.vscode/c_cpp_properties.json b/lanelet2_map_learning/.vscode/c_cpp_properties.json new file mode 100644 index 00000000..d33cce5a --- /dev/null +++ b/lanelet2_map_learning/.vscode/c_cpp_properties.json @@ -0,0 +1,36 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/opt/mrtros/include/**", + "/opt/mrtsoftware/local/include/**", + "/home/immel/workspaces/av2_link_pred_ws/devel/include/**", + "/opt/mrtsoftware/release/include/**", + "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_core/include/**", + "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_examples/include/**", + "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_io/include/**", + "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_map_learning/include/**", + "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_maps/include/**", + "/home/immel/workspaces/av2_link_pred_ws/src/lanelet2_maps_mrt/include/**", + "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_matching/include/**", + "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_projection/include/**", + "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_python/include/**", + "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_routing/include/**", + "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_traffic_rules/include/**", + "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_validation/include/**", + "/home/immel/workspaces/av2_link_pred_ws/src/ll2_map_graph_converter/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/lanelet2_map_learning/.vscode/settings.json b/lanelet2_map_learning/.vscode/settings.json new file mode 100644 index 00000000..4f265cff --- /dev/null +++ b/lanelet2_map_learning/.vscode/settings.json @@ -0,0 +1,8 @@ +{ + "python.autoComplete.extraPaths": [ + "/opt/mrtros/lib/python3/dist-packages" + ], + "python.analysis.extraPaths": [ + "/opt/mrtros/lib/python3/dist-packages" + ] +} \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index ce2413c6..d742ba82 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -33,14 +33,13 @@ class LaneData { public: LaneData() noexcept {} static LaneData build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); + bool processAll(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints); private: - void processLeftBoundaries(LaneletSubmapConstPtr& localSubmap, - lanelet::routing::RoutingGraphConstPtr localSubmapGraph); - void processRightBoundaries(LaneletSubmapConstPtr& localSubmap, - lanelet::routing::RoutingGraphConstPtr localSubmapGraph); - void processCompoundFeatures(LaneletSubmapConstPtr& localSubmap, - lanelet::routing::RoutingGraphConstPtr localSubmapGraph); + void initLeftBoundaries(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); + void initRightBoundaries(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); + void initCompoundFeatures(LaneletSubmapConstPtr& localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph); LineStringType getLineStringTypeFromId(Id id); LaneLineStringFeature getLineStringFeatFromId(Id id); diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h index a62f3884..c941c3e5 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h @@ -41,9 +41,11 @@ class MapDataInterface { TEData laneTEDataBatch(const BasicPoints2d& pts, const std::vector& yaws); private: - LaneData getLaneData(LaneletSubmapConstPtr localSubmap, routing::RoutingGraphConstPtr localSubmapGraph); + LaneData getLaneData(LaneletSubmapConstPtr localSubmap, routing::RoutingGraphConstPtr localSubmapGraph, + bool processAll = false); - TEData getLaneTEData(routing::RoutingGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap); + TEData getLaneTEData(routing::RoutingGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, + bool processAll); LaneletMapConstPtr laneletMap_; LaneletSubmapConstPtr localSubmap_; diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index 362eca40..7fe64dd1 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -28,7 +28,7 @@ class MapFeature { bool valid_{true}; Optional mapID_; - MapFeature(); + MapFeature() = default; MapFeature(Id mapID) : mapID_{mapID}, initialized_{true} {} virtual ~MapFeature() noexcept = default; }; @@ -133,9 +133,6 @@ class CompoundLaneLineStringFeature : public LaneLineStringFeature { const std::vector& pathLengthsRaw() const { return pathLengthsRaw_; } const std::vector& pathLengthsProcessed() const { return pathLengthsProcessed_; } - bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; - Eigen::VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const override; - private: LaneLineStringFeatureList individualFeatures_; std::vector pathLengthsRaw_; @@ -154,5 +151,29 @@ Eigen::MatrixXd getFeatureVectorMatrix(const std::vector& mapFeatures, bool o std::vector getPointsMatrixList(const LaneLineStringFeatures& mapFeatures, bool pointsIn2d); std::vector getPointsMatrixList(const LaneLineStringFeatureList& mapFeatures, bool pointsIn2d); +template +bool processFeatureMap(std::map& featMap, const OrientedRect& bbox, const ParametrizationType& paramType, + int32_t nPoints) { + bool allValid = true; + for (auto& feat : featMap) { + if (!feat.second.process(bbox, paramType, nPoints)) { + allValid = false; + } + } + return allValid; +} + +template +bool processFeatureVec(std::vector& featVec, const OrientedRect& bbox, const ParametrizationType& paramType, + int32_t nPoints) { + bool allValid = true; + for (auto& feat : featVec) { + if (!feat.process(bbox, paramType, nPoints)) { + allValid = false; + } + } + return allValid; +} + } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h index 13209e37..3e558f83 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h @@ -17,12 +17,41 @@ namespace map_learning { boost::geometry::model::polygon getRotatedRect(const BasicPoint2d& center, double extentLongitudinal, double extentLateral, double yaw); -LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPoint2d& center, double yaw, +LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPoint2d& center, double extentLongitudinal, double extentLateral); -inline LineStringType bdSubtypeToEnum(ConstLineString3d lString); - -inline TEType teTypeToEnum(const ConstLineString3d& te); +inline LineStringType bdSubtypeToEnum(ConstLineString3d lString) { + std::string subtype = lString.attribute(AttributeName::Subtype).value(); + if (subtype == AttributeValueString::Dashed) + return LineStringType::Dashed; + else if (subtype == AttributeValueString::Solid) + return LineStringType::Solid; + else if (subtype == AttributeValueString::SolidSolid) + return LineStringType::Solid; + else if (subtype == AttributeValueString::SolidDashed) + return LineStringType::Solid; + else if (subtype == AttributeValueString::DashedSolid) + return LineStringType::Solid; + else if (subtype == AttributeValueString::Virtual) + return LineStringType::Virtual; + else { + throw std::runtime_error("Unexpected Line String Subtype!"); + return LineStringType::Unknown; + } +} + +inline TEType teTypeToEnum(const ConstLineString3d& te) { + std::string type = te.attribute(AttributeName::Type).value(); + std::string subtype = te.attribute(AttributeName::Subtype).value(); + if (type == AttributeValueString::TrafficLight) { + return TEType::TrafficLight; + } else if (type == AttributeValueString::TrafficSign) { + return TEType::TrafficSign; + } else { + throw std::runtime_error("Unexpected Traffic Element Type!"); + return TEType::Unknown; + } +} BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t nPoints); diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index dc7b4a8d..060e1223 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -1,5 +1,7 @@ #include "lanelet2_map_learning/MapData.h" +#include "lanelet2_map_learning/Utils.h" + namespace lanelet { namespace map_learning { @@ -12,15 +14,15 @@ bool isRoadBorder(const ConstLineString3d& lstring) { LaneData LaneData::build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { LaneData data; - data.processLeftBoundaries(localSubmap, localSubmapGraph); - data.processRightBoundaries(localSubmap, localSubmapGraph); - data.processCompoundFeatures(localSubmap, localSubmapGraph); + data.initLeftBoundaries(localSubmap, localSubmapGraph); + data.initRightBoundaries(localSubmap, localSubmapGraph); + data.initCompoundFeatures(localSubmap, localSubmapGraph); return data; } -void LaneData::processLeftBoundaries(LaneletSubmapConstPtr& localSubmap, - lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { +void LaneData::initLeftBoundaries(LaneletSubmapConstPtr& localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { for (const auto& ll : localSubmap->laneletLayer) { if (isRoadBorder(ll.leftBound3d())) { roadBorders_.insert( @@ -48,8 +50,8 @@ void LaneData::processLeftBoundaries(LaneletSubmapConstPtr& localSubmap, } } -void LaneData::processRightBoundaries(LaneletSubmapConstPtr& localSubmap, - lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { +void LaneData::initRightBoundaries(LaneletSubmapConstPtr& localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { for (const auto& ll : localSubmap->laneletLayer) { if (isRoadBorder(ll.rightBound3d())) { roadBorders_.insert( @@ -196,8 +198,8 @@ void insertAndCheckNewCompoundFeatures(std::vector>& compFeats, } } -void LaneData::processCompoundFeatures(LaneletSubmapConstPtr& localSubmap, - lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { +void LaneData::initCompoundFeatures(LaneletSubmapConstPtr& localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { std::vector> compoundedBordersAndDividers; std::map elInsertIdx; for (const auto& bd : roadBorders_) { @@ -250,5 +252,21 @@ void LaneData::processCompoundFeatures(LaneletSubmapConstPtr& localSubmap, } } +bool LaneData::processAll(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { + bool validRoadBorders = processFeatureMap(roadBorders_, bbox, paramType, nPoints); + bool validLaneDividers = processFeatureMap(laneDividers_, bbox, paramType, nPoints); + bool validLaneletFeatures = processFeatureMap(laneletFeatures_, bbox, paramType, nPoints); + bool validCompoundRoadBorders = processFeatureVec(compoundRoadBorders_, bbox, paramType, nPoints); + bool validCompoundLaneDividers = processFeatureVec(compoundLaneDividers_, bbox, paramType, nPoints); + bool validCompoundCenterlines = processFeatureVec(compoundCenterlines_, bbox, paramType, nPoints); + + if (validRoadBorders && validLaneDividers && validLaneletFeatures && validCompoundRoadBorders && + validCompoundLaneDividers && validCompoundCenterlines) { + return true; + } else { + return false; + } +} + } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapDataInterface.cpp b/lanelet2_map_learning/src/MapDataInterface.cpp index ede406a4..2f657aeb 100644 --- a/lanelet2_map_learning/src/MapDataInterface.cpp +++ b/lanelet2_map_learning/src/MapDataInterface.cpp @@ -3,7 +3,7 @@ #include #include #include -#include ; +#include #include #include @@ -17,8 +17,7 @@ namespace map_learning { void MapDataInterface::setCurrPosAndExtractSubmap(const BasicPoint2d& pt) { currPos_ = pt; - localSubmap_ = - extractSubmap(laneletMap_, *currPos_, *currYaw_, config_.submapAreaLongitudinal, config_.submapAreaLateral); + localSubmap_ = extractSubmap(laneletMap_, *currPos_, config_.submapAreaLongitudinal, config_.submapAreaLateral); localSubmapGraph_ = lanelet::routing::RoutingGraph::build(*laneletMap_, *trafficRules_); } @@ -29,8 +28,16 @@ MapDataInterface::MapDataInterface(LaneletMapConstPtr laneletMap, Configuration trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} LaneData MapDataInterface::getLaneData(LaneletSubmapConstPtr localSubmap, - lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { - return LaneData::build(localSubmap, localSubmapGraph); + lanelet::routing::RoutingGraphConstPtr localSubmapGraph, bool processAll) { + LaneData laneData = LaneData::build(localSubmap, localSubmapGraph); + OrientedRect bbox = getRotatedRect(*currPos_, config_.submapAreaLongitudinal, config_.submapAreaLateral, *currYaw_); + if (processAll) { + bool valid = laneData.processAll(bbox, config_.paramType, config_.nPoints); + if (!valid) { + throw std::runtime_error("Invalid result in processing lane data!"); + } + } + return laneData; } bool isTe(ConstLineString3d ls) { @@ -39,7 +46,7 @@ bool isTe(ConstLineString3d ls) { } TEData MapDataInterface::getLaneTEData(routing::RoutingGraphConstPtr localSubmapGraph, - LaneletSubmapConstPtr localSubmap) { + LaneletSubmapConstPtr localSubmap, bool processAll) { throw std::runtime_error("Not implemented yet!"); } diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index f639452b..e4b69a71 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -10,18 +10,25 @@ OrientedRect getRotatedRect(const BasicPoint2d& center, double extentLongitudina BasicPoints2d pts{BasicPoint2d{center.x() - extentLongitudinal, center.y() - extentLateral}, BasicPoint2d{center.x() - extentLongitudinal, center.y() + extentLateral}, BasicPoint2d{center.x() + extentLongitudinal, center.y() + extentLateral}, - BasicPoint2d{center.x() + extentLongitudinal, center.y() - extentLateral}}; + BasicPoint2d{center.x() + extentLongitudinal, center.y() - extentLateral}, + BasicPoint2d{center.x() - extentLongitudinal, center.y() - extentLateral}}; OrientedRect axisAlignedRect; boost::geometry::assign_points(axisAlignedRect, pts); - boost::geometry::strategy::transform::matrix_transformer trans( - cos(yaw), sin(yaw), center.x(), -sin(yaw), cos(yaw), center.y(), 0, 0, 1); + boost::geometry::strategy::transform::translate_transformer trans1(-center.x(), -center.y()); + boost::geometry::strategy::transform::rotate_transformer rotate(yaw); + boost::geometry::strategy::transform::translate_transformer trans2(center.x(), center.y()); + + OrientedRect trans1Rect; OrientedRect rotatedRect; - boost::geometry::transform(axisAlignedRect, rotatedRect, trans); - return rotatedRect; + OrientedRect trans2Rect; + boost::geometry::transform(axisAlignedRect, trans1Rect, trans1); + boost::geometry::transform(trans1Rect, rotatedRect, rotate); + boost::geometry::transform(rotatedRect, trans2Rect, trans2); + return trans2Rect; } -LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPoint2d& center, double yaw, +LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPoint2d& center, double extentLongitudinal, double extentLateral) { double maxExtent = std::max(extentLongitudinal, extentLateral); BasicPoint2d initRegionRear = {center.x() - 1.1 * maxExtent, center.y() - 1.1 * maxExtent}; @@ -31,44 +38,12 @@ LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPo return utils::createConstSubmap(initRegion, {}); } -inline LineStringType bdSubtypeToEnum(ConstLineString3d lString) { - std::string subtype = lString.attribute(AttributeName::Subtype).value(); - if (subtype == AttributeValueString::Dashed) - return LineStringType::Dashed; - else if (subtype == AttributeValueString::Solid) - return LineStringType::Solid; - else if (subtype == AttributeValueString::SolidSolid) - return LineStringType::Solid; - else if (subtype == AttributeValueString::SolidDashed) - return LineStringType::Solid; - else if (subtype == AttributeValueString::DashedSolid) - return LineStringType::Solid; - else if (subtype == AttributeValueString::Virtual) - return LineStringType::Virtual; - else { - throw std::runtime_error("Unexpected Line String Subtype!"); - return LineStringType::Unknown; - } -} - -inline TEType teTypeToEnum(const ConstLineString3d& te) { - std::string type = te.attribute(AttributeName::Type).value(); - std::string subtype = te.attribute(AttributeName::Subtype).value(); - if (type == AttributeValueString::TrafficLight) { - return TEType::TrafficLight; - } else if (type == AttributeValueString::TrafficSign) { - return TEType::TrafficSign; - } else { - throw std::runtime_error("Unexpected Traffic Element Type!"); - return TEType::Unknown; - } -} - BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t nPoints) { double length = boost::geometry::length(polyline, boost::geometry::strategy::distance::pythagoras()); - double dist = length / static_cast(nPoints); + double dist = length / static_cast(nPoints - 1); // to get all points of line boost::geometry::model::multi_point bdInterp; boost::geometry::line_interpolate(polyline, dist, bdInterp); + bdInterp.insert(bdInterp.begin(), polyline.front()); assert(bdInterp.size() == nPoints); return bdInterp; } diff --git a/lanelet2_map_learning/test/test_map.h b/lanelet2_map_learning/test/test_map.h index 9154ee1a..c6c1ac0d 100644 --- a/lanelet2_map_learning/test/test_map.h +++ b/lanelet2_map_learning/test/test_map.h @@ -15,13 +15,12 @@ namespace lanelet { namespace map_learning { namespace tests { -class MapGraphTestData { +class MapTestData { public: - MapGraphTestData() { + MapTestData() { initPoints(); initLineStrings(); initLanelets(); - initAreas(); laneletMap = std::make_shared(lanelets, areas, std::unordered_map(), std::unordered_map(), lines, points); } @@ -173,6 +172,22 @@ class MapGraphTestData { } }; +namespace { // NOLINT +static MapTestData testData; // NOLINT +} // namespace + +class MapLearningTest : public ::testing::Test { + public: + const std::unordered_map& lanelets{testData.lanelets}; + const std::unordered_map& points{testData.points}; + const std::unordered_map& lines{testData.lines}; + const LaneletMapConstPtr laneletMap{testData.laneletMap}; + const BasicPoint2d centerBbox{5, 5}; + const double extentLongitudinalBbox{15}; + const double extentLateralBbox{10}; + const double yawBbox{M_PI / 2.0}; +}; + } // namespace tests } // namespace map_learning } // namespace lanelet diff --git a/lanelet2_map_learning/test/test_map_features.cpp b/lanelet2_map_learning/test/test_map_features.cpp new file mode 100644 index 00000000..a0062ef5 --- /dev/null +++ b/lanelet2_map_learning/test/test_map_features.cpp @@ -0,0 +1,9 @@ +#include + +#include "lanelet2_map_learning/MapFeatures.h" + +using namespace lanelet; +using namespace lanelet::map_learning; + +TEST(LaneLineStringFeatureTest, LaneChangesLeft) { // NOLINT +} diff --git a/lanelet2_map_learning/test/test_map_graph_container.cpp b/lanelet2_map_learning/test/test_map_graph_container.cpp deleted file mode 100644 index 36bd9561..00000000 --- a/lanelet2_map_learning/test/test_map_graph_container.cpp +++ /dev/null @@ -1,76 +0,0 @@ -#include -#include - -#include "lanelet2_map_learning/Forward.h" -#include "lanelet2_map_learning/MapGraph.h" -#include "lanelet2_map_learning/MapGraphContainer.h" -#include "test_map.h" - -using namespace lanelet; -using namespace lanelet::map_learning; -using namespace lanelet::map_learning::tests; - -class MapGraphContainerTest : public MapGraphTest { - public: - MapGraphContainerTest() { - std::vector graphs{testData.vehicleGraph, testData.pedestrianGraph}; - container = std::make_unique(graphs); - } - - MapGraphContainerUPtr container; -}; - -TEST_F(MapGraphContainerTest, ConflictingInGraph) { // NOLINT - ConstLanelet pedestrianLanelet{*laneletMap->laneletLayer.find(2031)}; - ConstLanelets conflictingVehicle{container->conflictingInGraph(pedestrianLanelet, 0)}; - ASSERT_EQ(conflictingVehicle.size(), 1ul); - EXPECT_EQ(conflictingVehicle[0], *laneletMap->laneletLayer.find(2020)); - - ConstLanelets conflictingPedestrian{container->conflictingInGraph(pedestrianLanelet, 1)}; - EXPECT_EQ(conflictingPedestrian.size(), 0ul); -} - -TEST_F(MapGraphContainerTest, ConflictingInGraphs) { // NOLINT - ConstLanelet pedestrianLanelet{*laneletMap->laneletLayer.find(2031)}; - MapGraphContainer::ConflictingInGraphs result{container->conflictingInGraphs(pedestrianLanelet)}; - ASSERT_EQ(result.size(), 2ul); - - ConstLanelets conflictingVehicle{result[0].second}; - ASSERT_EQ(conflictingVehicle.size(), 1ul); - EXPECT_EQ(conflictingVehicle[0], *laneletMap->laneletLayer.find(2020)); - - ConstLanelets conflictingPedestrian{result[1].second}; - EXPECT_EQ(conflictingPedestrian.size(), 0ul); -} - -TEST_F(MapGraphContainerTest, ConflictingInGraph3dFits) { // NOLINT - ConstLanelet bridgeLanelet{*laneletMap->laneletLayer.find(2032)}; - ConstLanelets conflictingVehicle{container->conflictingInGraph(bridgeLanelet, 0, 2.)}; - EXPECT_EQ(conflictingVehicle.size(), 0ul); - - conflictingVehicle = container->conflictingInGraph(lanelets.find(2005)->second, 0, 2.); - ASSERT_EQ(conflictingVehicle.size(), 1ul); - EXPECT_TRUE(conflictingVehicle.front() == lanelets.find(2006)->second); - - conflictingVehicle = container->conflictingInGraph(lanelets.find(2006)->second, 0, 2.); - ASSERT_EQ(conflictingVehicle.size(), 1ul); - EXPECT_TRUE(conflictingVehicle.front() == lanelets.find(2005)->second); - - ConstLanelets conflictingPedestrian{container->conflictingInGraph(bridgeLanelet, 1, 2.)}; - EXPECT_EQ(conflictingPedestrian.size(), 0ul); -} - -TEST_F(MapGraphContainerTest, ConflictingInGraph3dDoesntFit) { // NOLINT - ConstLanelet bridgeLanelet{*laneletMap->laneletLayer.find(2032)}; - ConstLanelets conflictingVehicle{container->conflictingInGraph(bridgeLanelet, 0, 4.)}; - EXPECT_EQ(conflictingVehicle.size(), 2ul); - - conflictingVehicle = container->conflictingInGraph(lanelets.find(2005)->second, 0, 4.); - EXPECT_EQ(conflictingVehicle.size(), 2ul); - - conflictingVehicle = container->conflictingInGraph(lanelets.find(2006)->second, 0, 4.); - EXPECT_EQ(conflictingVehicle.size(), 2ul); - - ConstLanelets conflictingPedestrian{container->conflictingInGraph(bridgeLanelet, 1, 4.)}; - EXPECT_EQ(conflictingPedestrian.size(), 0ul); -} diff --git a/lanelet2_map_learning/test/test_map_visualization.cpp b/lanelet2_map_learning/test/test_map_visualization.cpp deleted file mode 100644 index c3932853..00000000 --- a/lanelet2_map_learning/test/test_map_visualization.cpp +++ /dev/null @@ -1,102 +0,0 @@ -#include - -#include - -#include "lanelet2_map_learning/Exceptions.h" -#include "lanelet2_map_learning/MapGraph.h" -#include "test_map.h" - -using namespace lanelet; -using namespace lanelet::map_learning; -using namespace lanelet::map_learning::tests; -namespace fs = boost::filesystem; - -class Tempfile { - public: - Tempfile() { - char path[] = {"/tmp/lanelet2_unittest.XXXXXX"}; - auto* res = mkdtemp(path); - if (res == nullptr) { - throw lanelet::LaneletError("Failed to crate temporary directory"); - } - path_ = path; - } - Tempfile(const Tempfile&) = delete; - Tempfile(Tempfile&&) = delete; - Tempfile& operator=(const Tempfile&) = delete; - Tempfile& operator=(Tempfile&&) = delete; - ~Tempfile() { fs::remove_all(fs::path(path_)); } - - auto operator()(const std::string& str) const noexcept -> std::string { return (fs::path(path_) / str).string(); } - - private: - std::string path_; -}; - -static Tempfile tempfile; - -TEST_F(GermanVehicleGraph, GraphVizExport) { // NOLINT - EXPECT_NO_THROW(graph->exportGraphViz(tempfile("graphexport.dot"))); // NOLINT - RelationType excluded{RelationType::Conflicting}; - EXPECT_NO_THROW(graph->exportGraphViz(tempfile("graphexportWithExcluded1.dot"), excluded)); // NOLINT - excluded |= RelationType::AdjacentLeft; - excluded |= RelationType::AdjacentRight; - EXPECT_NO_THROW(graph->exportGraphViz(tempfile("graphexportWithExcluded2.dot"), excluded)); // NOLINT -} - -TEST_F(GermanVehicleGraph, GraphVizExportError) { // NOLINT - EXPECT_THROW(graph->exportGraphViz("", RelationType::None), lanelet::InvalidInputError); // NOLINT - EXPECT_THROW(graph->exportGraphViz("/place/that/doesnt/exist"), lanelet::ExportError); // NOLINT -} - -TEST_F(GermanVehicleGraph, GraphMLExport) { // NOLINT - EXPECT_NO_THROW(graph->exportGraphML(tempfile("graphexport.graphml"))); // NOLINT - RelationType excluded{RelationType::Conflicting}; - EXPECT_NO_THROW(graph->exportGraphML(tempfile("graphexportWithExcluded1.graphml"), excluded)); // NOLINT - excluded |= RelationType::AdjacentLeft; - excluded |= RelationType::AdjacentLeft; - excluded |= RelationType::AdjacentRight; - EXPECT_NO_THROW(graph->exportGraphML(tempfile("graphexportWithExcluded2.graphml"), excluded)); // NOLINT -} - -TEST_F(GermanVehicleGraph, GraphMLExportError) { // NOLINT - EXPECT_THROW(graph->exportGraphML("", RelationType::None), lanelet::InvalidInputError); // NOLINT - EXPECT_THROW(graph->exportGraphML("/place/that/doesnt/exist", RelationType::None), lanelet::ExportError); // NOLINT -} - -TEST_F(GermanVehicleGraph, DebugLaneletMap) { // NOLINT - LaneletMapPtr map{graph->getDebugLaneletMap()}; - EXPECT_TRUE(map->pointLayer.exists(2007)); - EXPECT_TRUE(map->pointLayer.exists(2020)); - EXPECT_TRUE(map->pointLayer.exists(2032)); - EXPECT_GT(map->lineStringLayer.size(), map->pointLayer.size()); - EXPECT_FALSE(map->pointLayer.exists(2031)); -} - -TEST_F(GermanPedestrianGraph, DebugLaneletMap) { // NOLINT - LaneletMapPtr map{graph->getDebugLaneletMap()}; - EXPECT_FALSE(map->pointLayer.exists(2007)); - EXPECT_FALSE(map->pointLayer.exists(2020)); - EXPECT_FALSE(map->pointLayer.exists(2032)); - EXPECT_TRUE(map->pointLayer.exists(2031)); -} - -TEST_F(GermanBicycleGraph, DebugLaneletMap) { // NOLINT - LaneletMapPtr map{graph->getDebugLaneletMap()}; - EXPECT_TRUE(map->pointLayer.exists(2007)); - EXPECT_TRUE(map->pointLayer.exists(2020)); - EXPECT_TRUE(map->pointLayer.exists(2032)); - EXPECT_FALSE(map->pointLayer.exists(2022)); - EXPECT_FALSE(map->pointLayer.exists(2031)); -} - -TYPED_TEST(AllGraphsTest, CheckDebugLaneletMap) { - ASSERT_NO_THROW(this->graph->getDebugLaneletMap()); // NOLINT - const LaneletMapPtr map{this->graph->getDebugLaneletMap()}; - for (const auto& it : map->pointLayer) { - EXPECT_NO_THROW(it.attribute("id")); // NOLINT - } - for (const auto& it : map->lineStringLayer) { - EXPECT_NO_THROW(it.attribute("relation")); // NOLINT - } -} diff --git a/lanelet2_map_learning/test/test_relations.cpp b/lanelet2_map_learning/test/test_relations.cpp deleted file mode 100644 index ba7e899a..00000000 --- a/lanelet2_map_learning/test/test_relations.cpp +++ /dev/null @@ -1,525 +0,0 @@ -#include - -#include - -#include "lanelet2_map_learning/MapGraph.h" -#include "test_map.h" - -using namespace lanelet; -using namespace lanelet::map_learning::tests; - -TEST_F(GermanVehicleGraph, LaneChangesLeft) { // NOLINT - // Multiple1 - Optional left = graph->left(lanelets.at(2001)); - EXPECT_TRUE((!!left)); - EXPECT_TRUE(*left == lanelets.at(2003)); - - left = graph->left(lanelets.at(2002)); - EXPECT_FALSE((!!left)); -} - -TEST_F(GermanVehicleGraph, LaneChangesLeftMerging) { // NOLINT - Optional left = graph->left(lanelets.at(2005)); - EXPECT_FALSE((!!left)); - - left = graph->left(lanelets.at(2006)); - EXPECT_FALSE((!!left)); -} - -TEST_F(GermanVehicleGraph, LaneChangesLeftDiverging) { // NOLINT - Optional left = graph->left(lanelets.at(2008)); - EXPECT_FALSE((!!left)); - - left = graph->left(lanelets.at(2009)); - EXPECT_FALSE((!!left)); -} - -TEST_F(GermanVehicleGraph, LaneChangesLeftMaxHose) { // NOLINT - Optional left = graph->left(lanelets.at(2016)); - EXPECT_FALSE((!!left)); - - left = graph->left(lanelets.at(2020)); - EXPECT_FALSE((!!left)); -} - -TEST_F(GermanVehicleGraph, LaneChangesLeftInvalid) { // NOLINT - ConstLanelet invalidLanelet; - Optional left = graph->left(invalidLanelet); - EXPECT_FALSE((!!left)); -} - -TEST_F(GermanVehicleGraph, LaneChangesRight) { // NOLINT - // Multiple1 - Optional right = graph->right(lanelets.at(2003)); - EXPECT_TRUE((!!right)); - EXPECT_TRUE(*right == lanelets.at(2001)); - - right = graph->right(lanelets.at(2004)); - EXPECT_FALSE((!!right)); - - right = graph->right(lanelets.at(2001)); - EXPECT_FALSE((!!right)); - - right = graph->right(lanelets.at(2002)); - EXPECT_FALSE((!!right)); -} - -TEST_F(GermanVehicleGraph, LaneChangesRightMerging) { // NOLINT - Optional right = graph->right(lanelets.at(2006)); - EXPECT_FALSE((!!right)); - - right = graph->right(lanelets.at(2005)); - EXPECT_FALSE((!!right)); -} - -TEST_F(GermanVehicleGraph, LaneChangesRightDiverging) { // NOLINT - Optional right = graph->right(lanelets.at(2008)); - EXPECT_FALSE((!!right)); - - right = graph->right(lanelets.at(2009)); - EXPECT_FALSE((!!right)); -} - -TEST_F(GermanVehicleGraph, LaneChangesRightMaxHose) { // NOLINT - Optional right = graph->right(lanelets.at(2016)); - EXPECT_FALSE((!!right)); - - right = graph->right(lanelets.at(2020)); - EXPECT_FALSE((!!right)); -} - -TEST_F(GermanVehicleGraph, LaneChangesRightInvalid) { // NOLINT - // Invalid - ConstLanelet invalidLanelet; - Optional right = graph->right(invalidLanelet); - EXPECT_FALSE((!!right)); -} - -TEST_F(GermanVehicleGraph, AllLaneChangesLeft) { // NOLINT - // Multiple2 - ConstLanelets left; - left = graph->lefts(lanelets.at(2012)); - EXPECT_EQ(left.size(), 1ul); - EXPECT_TRUE(std::find(left.begin(), left.end(), lanelets.at(2011)) != left.end()); - - left = graph->lefts(lanelets.at(2011)); - EXPECT_TRUE(left.empty()); - - left = graph->lefts(lanelets.at(2014)); - EXPECT_EQ(left.size(), 1ul); - EXPECT_TRUE(std::find(left.begin(), left.end(), lanelets.at(2013)) != left.end()); - - left = graph->lefts(lanelets.at(2015)); - EXPECT_EQ(left.size(), 2ul); - EXPECT_TRUE(std::find(left.begin(), left.end(), lanelets.at(2014)) != left.end()); - EXPECT_TRUE(std::find(left.begin(), left.end(), lanelets.at(2013)) != left.end()); -} - -TEST_F(GermanVehicleGraph, AllLaneChangesRight) { // NOLINT - // Multiple2 - ConstLanelets right; - right = graph->rights(lanelets.at(2011)); - EXPECT_EQ(right.size(), 1ul); - EXPECT_TRUE(std::find(right.begin(), right.end(), lanelets.at(2012)) != right.end()); - - right = graph->rights(lanelets.at(2012)); - EXPECT_TRUE(right.empty()); - - right = graph->rights(lanelets.at(2013)); - EXPECT_EQ(right.size(), 2ul); - EXPECT_TRUE(std::find(right.begin(), right.end(), lanelets.at(2014)) != right.end()); - EXPECT_TRUE(std::find(right.begin(), right.end(), lanelets.at(2015)) != right.end()); - - right = graph->rights(lanelets.at(2014)); - EXPECT_EQ(right.size(), 1ul); - EXPECT_TRUE(std::find(right.begin(), right.end(), lanelets.at(2015)) != right.end()); -} - -TEST_F(GermanVehicleGraph, FollowingWithoutLaneChange) { // NOLINT - // Multiple1 - ConstLanelets following = graph->following(lanelets.at(2001), false); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2002)) != following.end()); - - following = graph->following(lanelets.at(2003), false); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2004)) != following.end()); - - following = graph->following(lanelets.at(2002), false); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2006)) != following.end()); - - following = graph->following(lanelets.at(2004), false); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2005)) != following.end()); -} - -TEST_F(GermanVehicleGraph, FollowingWithoutLaneChangeMerging) { // NOLINT - ConstLanelets following = graph->following(lanelets.at(2005), false); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2007)) != following.end()); -} - -TEST_F(GermanVehicleGraph, FollowingWithoutLaneChangeDiverging) { // NOLINT - // Single Lane -> Diverging - ConstLanelets following = graph->following(lanelets.at(2007), false); - EXPECT_EQ(following.size(), 2ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2008)) != following.end()); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2009)) != following.end()); -} - -TEST_F(GermanVehicleGraph, FollowingWithoutLaneChangeMaxHose) { // NOLINT - // Max Hose Problem - ConstLanelets following = graph->following(lanelets.at(2018), false); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2016)) != following.end()); - - following = graph->following(lanelets.at(2019), false); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2020)) != following.end()); - - following = graph->following(lanelets.at(2020), false); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2022)) != following.end()); - - following = graph->following(lanelets.at(2022), false); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2024)) != following.end()); - - following = graph->following(lanelets.at(2021), false); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2020).invert()) != following.end()); - - following = graph->following(lanelets.at(2020).invert(), false); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2018)) != following.end()); -} - -TEST_F(GermanVehicleGraph, FollowingWithoutLaneChangeInvalid) { // NOLINT - // Invalid - ConstLanelet invalidLanelet; - ConstLanelets following = graph->following(invalidLanelet, false); - EXPECT_TRUE(following.empty()); -} - -TEST_F(GermanVehicleGraph, FollowingWithLaneChange) { // NOLINT - // Multiple1 - ConstLanelets following = graph->following(lanelets.at(2001), true); - EXPECT_EQ(following.size(), 2ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2002)) != following.end()); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2003)) != following.end()); - - following = graph->following(lanelets.at(2003), true); - EXPECT_EQ(following.size(), 2ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2001)) != following.end()); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2004)) != following.end()); - - following = graph->following(lanelets.at(2002), true); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2006)) != following.end()); - - following = graph->following(lanelets.at(2004), true); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2005)) != following.end()); -} - -TEST_F(GermanVehicleGraph, FollowingWithLaneChangeMerging) { // NOLINT - ConstLanelets following = graph->following(lanelets.at(2005), true); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2007)) != following.end()); - - following = graph->following(lanelets.at(2006), true); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2007)) != following.end()); -} - -TEST_F(GermanVehicleGraph, FollowingWithLaneChangeDiverging) { // NOLINT - // Single Lane -> Diverging - ConstLanelets following = graph->following(lanelets.at(2007), true); - EXPECT_EQ(following.size(), 2ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2008)) != following.end()); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2009)) != following.end()); -} - -TEST_F(GermanVehicleGraph, FollowingWithLaneChangeMaxHose) { // NOLINT - // Max Hose Problem - ConstLanelets following = graph->following(lanelets.at(2018), true); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2016)) != following.end()); - - following = graph->following(lanelets.at(2019), true); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2020)) != following.end()); - - following = graph->following(lanelets.at(2020), true); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2022)) != following.end()); - - following = graph->following(lanelets.at(2022), true); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2024)) != following.end()); - - following = graph->following(lanelets.at(2021), true); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2020).invert()) != following.end()); - - following = graph->following(lanelets.at(2020).invert(), true); - EXPECT_EQ(following.size(), 1ul); - EXPECT_TRUE(std::find(following.begin(), following.end(), lanelets.at(2018)) != following.end()); -} - -TEST_F(GermanVehicleGraph, PreviousWithoutLaneChange) { // NOLINT - // Multiple1 - ConstLanelets previous; - previous = graph->previous(lanelets.at(2001), false); - EXPECT_EQ(previous.size(), 0ul); - - previous = graph->previous(lanelets.at(2003), false); - EXPECT_EQ(previous.size(), 0ul); - - previous = graph->previous(lanelets.at(2002), false); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2001)) != previous.end()); - - previous = graph->previous(lanelets.at(2004), false); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2003)) != previous.end()); -} - -TEST_F(GermanVehicleGraph, PreviousWithoutLaneChangeMerging) { // NOLINT - ConstLanelets previous = graph->previous(lanelets.at(2005), false); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2004)) != previous.end()); - - previous = graph->previous(lanelets.at(2006), false); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2002)) != previous.end()); -} - -TEST_F(GermanVehicleGraph, PreviousWithoutLaneChangeSingleLane) { // NOLINT - ConstLanelets previous = graph->previous(lanelets.at(2007), false); - EXPECT_EQ(previous.size(), 2ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2005)) != previous.end()); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2006)) != previous.end()); -} - -TEST_F(GermanVehicleGraph, PreviousWithoutLaneChangeDiverging) { // NOLINT - ConstLanelets previous = graph->previous(lanelets.at(2008), false); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2007)) != previous.end()); - - previous = graph->previous(lanelets.at(2009), false); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2007)) != previous.end()); -} - -TEST_F(GermanVehicleGraph, PreviousWithoutLaneChangeMaxHose) { // NOLINT - // Max Hose Problem - ConstLanelets previous = graph->previous(lanelets.at(2019), false); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2017)) != previous.end()); - - previous = graph->previous(lanelets.at(2020), false); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2019)) != previous.end()); - - previous = graph->previous(lanelets.at(2022), false); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2020)) != previous.end()); - - previous = graph->previous(lanelets.at(2024), false); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2022)) != previous.end()); - - previous = graph->previous(lanelets.at(2021), false); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2023)) != previous.end()); - - previous = graph->previous(lanelets.at(2020).invert(), false); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2021)) != previous.end()); - - previous = graph->previous(lanelets.at(2018), false); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2020).invert()) != previous.end()); - - previous = graph->previous(lanelets.at(2016), false); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2018)) != previous.end()); -} - -TEST_F(GermanVehicleGraph, PreviousWithoutLaneChangeInvalid) { // NOLINT - ConstLanelet invalidLanelet; - ConstLanelets previous = graph->previous(invalidLanelet, false); - EXPECT_TRUE(previous.empty()); -} - -TEST_F(GermanVehicleGraph, PreviousWithLaneChange) { // NOLINT - // Multiple1 - ConstLanelets previous = graph->previous(lanelets.at(2001), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2003)) != previous.end()); - - previous = graph->previous(lanelets.at(2003), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2001)) != previous.end()); - - previous = graph->previous(lanelets.at(2002), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2001)) != previous.end()); - - previous = graph->previous(lanelets.at(2004), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2003)) != previous.end()); -} - -TEST_F(GermanVehicleGraph, PreviousWithLaneChangeMerging) { // NOLINT - ConstLanelets previous = graph->previous(lanelets.at(2005), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2004)) != previous.end()); - - previous = graph->previous(lanelets.at(2006), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2002)) != previous.end()); -} - -TEST_F(GermanVehicleGraph, PreviousWithLaneChangeSingleLane) { // NOLINT - ConstLanelets previous = graph->previous(lanelets.at(2007), true); - EXPECT_EQ(previous.size(), 2ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2005)) != previous.end()); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2006)) != previous.end()); -} - -TEST_F(GermanVehicleGraph, PreviousWithLaneChangeDiverging) { // NOLINT - ConstLanelets previous = graph->previous(lanelets.at(2008), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2007)) != previous.end()); - - previous = graph->previous(lanelets.at(2009), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2007)) != previous.end()); -} - -TEST_F(GermanVehicleGraph, PreviousWithLaneChangeMaxHose) { // NOLINT - ConstLanelets previous = graph->previous(lanelets.at(2019), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2017)) != previous.end()); - - previous = graph->previous(lanelets.at(2020), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2019)) != previous.end()); - - previous = graph->previous(lanelets.at(2022), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2020)) != previous.end()); - - previous = graph->previous(lanelets.at(2024), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2022)) != previous.end()); - - previous = graph->previous(lanelets.at(2021), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2023)) != previous.end()); - - previous = graph->previous(lanelets.at(2020).invert(), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2021)) != previous.end()); - - previous = graph->previous(lanelets.at(2018), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2020).invert()) != previous.end()); - - previous = graph->previous(lanelets.at(2016), true); - EXPECT_EQ(previous.size(), 1ul); - EXPECT_TRUE(std::find(previous.begin(), previous.end(), lanelets.at(2018)) != previous.end()); -} - -TEST_F(GermanVehicleGraph, PreviousWithLaneChangeInvalid) { // NOLINT - ConstLanelet invalidLanelet; - ConstLanelets previous = graph->previous(invalidLanelet, true); - EXPECT_TRUE(previous.empty()); -} - -TEST_F(GermanVehicleGraph, GetBesides) { // NOLINT - // Diverging2 - ConstLanelets laneletSet; - laneletSet = graph->besides(lanelets.at(2010)); - ASSERT_EQ(1ul, laneletSet.size()); - EXPECT_EQ(laneletSet[0], lanelets.at(2010)); - - laneletSet = graph->besides(lanelets.at(2011)); - ASSERT_EQ(laneletSet.size(), 2ul); - EXPECT_EQ(laneletSet[0], lanelets.at(2011)); - EXPECT_EQ(laneletSet[1], lanelets.at(2012)); - - // Multiple2 - laneletSet = graph->besides(lanelets.at(2015)); - ASSERT_EQ(laneletSet.size(), 3ul); - EXPECT_EQ(laneletSet[0], lanelets.at(2013)); - EXPECT_EQ(laneletSet[1], lanelets.at(2014)); - EXPECT_EQ(laneletSet[2], lanelets.at(2015)); - - laneletSet = graph->besides(lanelets.at(2014)); - ASSERT_EQ(laneletSet.size(), 3ul); - EXPECT_EQ(laneletSet[0], lanelets.at(2013)); - EXPECT_EQ(laneletSet[1], lanelets.at(2014)); - EXPECT_EQ(laneletSet[2], lanelets.at(2015)); - - laneletSet = graph->besides(lanelets.at(2013)); - ASSERT_EQ(laneletSet.size(), 3ul); - EXPECT_EQ(laneletSet[0], lanelets.at(2013)); - EXPECT_EQ(laneletSet[1], lanelets.at(2014)); - EXPECT_EQ(laneletSet[2], lanelets.at(2015)); -} - -TEST_F(GermanVehicleGraph, conflicting) { // NOLINT - // Normal Lanelets - auto result = graph->conflicting(lanelets.at(2007)); - EXPECT_EQ(result.size(), 0ul); - - result = graph->conflicting(lanelets.at(2012)); - EXPECT_EQ(result.size(), 0ul); - - result = graph->conflicting(lanelets.at(2001)); - EXPECT_EQ(result.size(), 0ul); - - result = graph->conflicting(lanelets.at(2005)); - EXPECT_EQ(result.size(), 1ul); -} - -TEST_F(GermanVehicleGraph, conflicting3d) { // NOLINT - // Bridge lanelet - auto result = graph->conflicting(lanelets.at(2005)); - ASSERT_EQ(result.size(), 1ul); - EXPECT_TRUE(result.front() == lanelets.at(2006)); - - result = graph->conflicting(lanelets.at(2006)); - ASSERT_EQ(result.size(), 1ul); - EXPECT_TRUE(result.front() == lanelets.at(2005)); - - result = graph->conflicting(lanelets.at(2032)); - EXPECT_EQ(result.size(), 0ul); -} - -TEST_F(GermanVehicleGraph, conflictingBothWays) { // NOLINT - // Both ways Lanelet - auto result = graph->conflicting(lanelets.at(2020)); - ASSERT_EQ(result.size(), 1ul); - EXPECT_NE(result.front().lanelet()->inverted(), lanelets.at(2020).inverted()); - - result = graph->conflicting(lanelets.at(2020).invert()); - ASSERT_EQ(result.size(), 1ul); - EXPECT_EQ(result.front().lanelet()->inverted(), lanelets.at(2020).inverted()); -} - -TEST_F(GermanVehicleGraph, conflictingMerging) { // NOLINT - auto result = graph->conflicting(lanelets.at(2005)); - ASSERT_EQ(result.size(), 1ul); - EXPECT_EQ(result.front(), lanelets.at(2006)); -} - -TEST_F(GermanVehicleGraph, conflictingDiverging) { // NOLINT - auto result = graph->conflicting(lanelets.at(2008)); - ASSERT_EQ(result.size(), 1ul); - EXPECT_EQ(result.front(), lanelets.at(2009)); -} diff --git a/lanelet2_map_learning/test/test_utils.cpp b/lanelet2_map_learning/test/test_utils.cpp new file mode 100644 index 00000000..db4eb4eb --- /dev/null +++ b/lanelet2_map_learning/test/test_utils.cpp @@ -0,0 +1,59 @@ +#include + +#include "lanelet2_map_learning/Utils.h" +#include "test_map.h" + +using namespace lanelet; +using namespace lanelet::map_learning; +using namespace lanelet::map_learning::tests; + +TEST_F(MapLearningTest, GetRotatedRect) { // NOLINT + OrientedRect rotatedRect = getRotatedRect(centerBbox, extentLongitudinalBbox, extentLateralBbox, yawBbox); + + EXPECT_DOUBLE_EQ(rotatedRect.outer()[0].x(), -5); + EXPECT_DOUBLE_EQ(rotatedRect.outer()[0].y(), 20); + EXPECT_DOUBLE_EQ(rotatedRect.outer()[1].x(), 15); + EXPECT_DOUBLE_EQ(rotatedRect.outer()[1].y(), 20); + EXPECT_DOUBLE_EQ(rotatedRect.outer()[2].x(), 15); + EXPECT_DOUBLE_EQ(rotatedRect.outer()[2].y(), -10); + EXPECT_DOUBLE_EQ(rotatedRect.outer()[3].x(), -5); + EXPECT_DOUBLE_EQ(rotatedRect.outer()[3].y(), -10); +} + +TEST_F(MapLearningTest, ExtractSubmap) { // NOLINT + LaneletSubmapConstPtr submap = extractSubmap(laneletMap, centerBbox, extentLongitudinalBbox, extentLateralBbox); + EXPECT_TRUE(submap->laneletLayer.exists(2000)); + EXPECT_TRUE(submap->laneletLayer.exists(2001)); + EXPECT_TRUE(submap->laneletLayer.exists(2002)); + EXPECT_TRUE(submap->laneletLayer.exists(2003)); + EXPECT_TRUE(submap->laneletLayer.exists(2004)); + EXPECT_TRUE(submap->laneletLayer.exists(2005)); + EXPECT_TRUE(submap->laneletLayer.exists(2012)); + EXPECT_TRUE(submap->laneletLayer.exists(2013)); +} + +TEST(UtilsTest, ResampleLineString) { // NOLINT + BasicLineString3d polyline{BasicPoint3d{0, 0, 0}, BasicPoint3d{3, 0, 0}, BasicPoint3d{6, 0, 0}, + BasicPoint3d{9, 0, 0}}; + BasicLineString3d polylineResampled = resampleLineString(polyline, 10); + + EXPECT_EQ(polylineResampled.size(), 10); + EXPECT_NEAR(polylineResampled[3].x(), 3, 10e-5); + EXPECT_NEAR(polylineResampled[6].x(), 6, 10e-5); + EXPECT_NEAR(polylineResampled[9].x(), 9, 10e-5); +} + +TEST_F(MapLearningTest, CutLineString) { // NOLINT + BasicLineString3d polyline{BasicPoint3d{0, 0, 0}, BasicPoint3d{30, 0, 0}}; + OrientedRect bbox = getRotatedRect(centerBbox, extentLongitudinalBbox, extentLateralBbox, yawBbox); + BasicLineString3d polylineCut = cutLineString(bbox, polyline); + + // for (const auto& pt : polylineCut) { + // std::cerr << pt << '\n'; + // std::cerr << "---" << '\n'; + // } + // std::cerr << "---------------------------" << '\n'; + + EXPECT_EQ(polylineCut.size(), 2); + EXPECT_NEAR(polylineCut[1].x(), 15, 10e-5); +} \ No newline at end of file From 3517b2d7fb980da1eb69d74971952fdf959b25b8 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 7 Dec 2023 20:42:45 +0100 Subject: [PATCH 25/64] unit tests for map features and fixes --- .../lanelet2_map_learning/MapFeatures.h | 6 +- lanelet2_map_learning/src/MapFeatures.cpp | 49 +++++++--- lanelet2_map_learning/test/test_map.h | 3 + .../test/test_map_features.cpp | 98 ++++++++++++++++++- lanelet2_map_learning/test/test_utils.cpp | 19 ++-- 5 files changed, 151 insertions(+), 24 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index 7fe64dd1..e8f002af 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -118,7 +118,7 @@ class LaneletFeature : public MapFeature { LaneLineStringFeature leftBoundary_; LaneLineStringFeature rightBoundary_; LaneLineStringFeature centerline_; - LaneletRepresentationType reprType_; + Optional reprType_; }; class CompoundLaneLineStringFeature : public LaneLineStringFeature { @@ -128,15 +128,19 @@ class CompoundLaneLineStringFeature : public LaneLineStringFeature { CompoundLaneLineStringFeature(const LaneLineStringFeatureList& features, LineStringType compoundType); virtual ~CompoundLaneLineStringFeature() noexcept = default; + bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; const LaneLineStringFeatureList& features() const { return individualFeatures_; } const std::vector& pathLengthsRaw() const { return pathLengthsRaw_; } const std::vector& pathLengthsProcessed() const { return pathLengthsProcessed_; } + const std::vector& processedFeaturesValid() const { return processedFeaturesValid_; } private: LaneLineStringFeatureList individualFeatures_; std::vector pathLengthsRaw_; std::vector pathLengthsProcessed_; + std::vector processedFeaturesValid_; + double validLengthThresh_{0.5}; }; using CompoundLaneLineStringFeatureList = std::vector; diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index bf517e62..1123dff9 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -69,7 +69,7 @@ Eigen::VectorXd LaneLineStringFeature::computeFeatureVector(bool onlyPoints, boo vec[vec.size() - 1] = typeInt(); if (onlyPoints) { - return vec(Eigen::seq(0, vec.size() - 1)); + return vec(Eigen::seq(0, vec.size() - 2)); } else { return vec; } @@ -89,7 +89,7 @@ Eigen::VectorXd TEFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d } vec[vec.size() - 1] = static_cast(teType_); if (onlyPoints) { - return vec(Eigen::seq(0, vec.size() - 1)); + return vec(Eigen::seq(0, vec.size() - 2)); } else { return vec; } @@ -154,7 +154,10 @@ bool LaneletFeature::process(const OrientedRect& bbox, const ParametrizationType } Eigen::VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { - if (reprType_ == LaneletRepresentationType::Centerline) { + if (!reprType_.has_value()) { + throw std::runtime_error( + "You need to set a LaneletRepresentationType with setReprType() before computing the feature vector!"); + } else if (*reprType_ == LaneletRepresentationType::Centerline) { Eigen::VectorXd vecCenterlinePts = centerline_.computeFeatureVector(true, pointsIn2d); Eigen::VectorXd vec(vecCenterlinePts.size() + 2); // pts vec + left and right type vec(Eigen::seq(0, vecCenterlinePts.size() - 1)) = vecCenterlinePts; @@ -165,9 +168,10 @@ Eigen::VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool point } else { return vec; } - } else if (reprType_ == LaneletRepresentationType::Boundaries) { + } else if (*reprType_ == LaneletRepresentationType::Boundaries) { Eigen::VectorXd vecLeftBdPts = leftBoundary_.computeFeatureVector(true, pointsIn2d); Eigen::VectorXd vecRightBdPts = rightBoundary_.computeFeatureVector(true, pointsIn2d); + Eigen::VectorXd vec(vecLeftBdPts.size() + vecRightBdPts.size() + 2); // pts vec + left and right type vec(Eigen::seq(0, vecLeftBdPts.size() - 1)) = vecLeftBdPts; vec(Eigen::seq(vecLeftBdPts.size(), vecLeftBdPts.size() + vecRightBdPts.size() - 1)) = vecRightBdPts; @@ -186,7 +190,10 @@ Eigen::VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool point CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStringFeatureList& features, LineStringType compoundType) - : individualFeatures_{features}, pathLengthsRaw_{std::vector(features.size())} { + : individualFeatures_{features}, + pathLengthsRaw_{std::vector(features.size())}, + pathLengthsProcessed_{std::vector(features.size())}, + processedFeaturesValid_{std::vector(features.size())} { type_ = compoundType; for (size_t i = 0; i < features.size(); i++) { @@ -199,13 +206,33 @@ CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStrin double rawLength = boost::geometry::length(features[i].rawFeature(), boost::geometry::strategy::distance::pythagoras()); - if (features[i].cutFeature().size() > 1) - if (i > 0) { - pathLengthsRaw_[i] = pathLengthsRaw_[i - 1] + rawLength; - } else { - pathLengthsRaw_[i] = rawLength; - } + if (i > 0) { + pathLengthsRaw_[i] = pathLengthsRaw_[i - 1] + rawLength; + } else { + pathLengthsRaw_[i] = rawLength; + } + } +} + +bool CompoundLaneLineStringFeature::process(const OrientedRect& bbox, const ParametrizationType& paramType, + int32_t nPoints) { + bool valid = LaneLineStringFeature::process(bbox, paramType, nPoints); + for (size_t i = 0; i < individualFeatures_.size(); i++) { + individualFeatures_[i].process(bbox, paramType, nPoints); + double processedLength = boost::geometry::length(individualFeatures_[i].cutFeature(), + boost::geometry::strategy::distance::pythagoras()); + + if (processedLength > validLengthThresh_) { + processedFeaturesValid_[i] = true; + } + + if (i > 0) { + pathLengthsProcessed_[i] = pathLengthsProcessed_[i - 1] + processedLength; + } else { + pathLengthsProcessed_[i] = processedLength; + } } + return valid; } template diff --git a/lanelet2_map_learning/test/test_map.h b/lanelet2_map_learning/test/test_map.h index c6c1ac0d..36415e02 100644 --- a/lanelet2_map_learning/test/test_map.h +++ b/lanelet2_map_learning/test/test_map.h @@ -8,6 +8,7 @@ #include #include "lanelet2_map_learning/Forward.h" +#include "lanelet2_map_learning/Utils.h" /// The coordinates and relations for this test can be found in "LaneletTestMap.xml" which can be viewed in /// https://www.draw.io @@ -186,6 +187,8 @@ class MapLearningTest : public ::testing::Test { const double extentLongitudinalBbox{15}; const double extentLateralBbox{10}; const double yawBbox{M_PI / 2.0}; + OrientedRect bbox; + MapLearningTest() : bbox{getRotatedRect(centerBbox, extentLongitudinalBbox, extentLateralBbox, yawBbox)} {} }; } // namespace tests diff --git a/lanelet2_map_learning/test/test_map_features.cpp b/lanelet2_map_learning/test/test_map_features.cpp index a0062ef5..5d018a80 100644 --- a/lanelet2_map_learning/test/test_map_features.cpp +++ b/lanelet2_map_learning/test/test_map_features.cpp @@ -1,9 +1,105 @@ #include #include "lanelet2_map_learning/MapFeatures.h" +#include "test_map.h" using namespace lanelet; using namespace lanelet::map_learning; +using namespace lanelet::map_learning::tests; -TEST(LaneLineStringFeatureTest, LaneChangesLeft) { // NOLINT +TEST_F(MapLearningTest, LaneLineStringFeature) { // NOLINT + BasicLineString3d polyline{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}, + BasicPoint3d{20, 0, 0}}; + LaneLineStringFeature feat(polyline, Id(123), LineStringType::Solid); + + feat.process(bbox, ParametrizationType::LineString, 4); + EXPECT_EQ(feat.cutAndResampledFeature().size(), 4); + EXPECT_NEAR(feat.cutAndResampledFeature()[0].x(), 0, 10e-5); + EXPECT_NEAR(feat.cutAndResampledFeature()[1].x(), 5, 10e-5); + EXPECT_NEAR(feat.cutAndResampledFeature()[3].x(), 15, 10e-5); + + Eigen::VectorXd vec = feat.computeFeatureVector(false, true); + EXPECT_EQ(vec.size(), 9); + EXPECT_NEAR(vec[0], 0, 10e-5); + EXPECT_NEAR(vec[2], 5, 10e-5); + EXPECT_NEAR(vec[6], 15, 10e-5); + EXPECT_EQ(vec[8], 2); + + Eigen::MatrixXd pointMat = feat.pointMatrix(false); + EXPECT_EQ(pointMat.rows(), 4); + EXPECT_EQ(pointMat.cols(), 3); + EXPECT_NEAR(pointMat(0, 0), 0, 10e-5); + EXPECT_NEAR(pointMat(2, 0), 10, 10e-5); + EXPECT_NEAR(pointMat(3, 0), 15, 10e-5); } + +TEST_F(MapLearningTest, LaneletFeature) { // NOLINT + BasicLineString3d leftBd{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}, + BasicPoint3d{20, 0, 0}}; + LaneLineStringFeature leftBdFeat(leftBd, Id(123), LineStringType::Solid); + BasicLineString3d rightBd{BasicPoint3d{0, 4, 0}, BasicPoint3d{5, 4, 0}, BasicPoint3d{10, 4, 0}, + BasicPoint3d{20, 4, 0}}; + LaneLineStringFeature rightBdFeat(rightBd, Id(124), LineStringType::Dashed); + BasicLineString3d centerline{BasicPoint3d{0, 2, 0}, BasicPoint3d{5, 2, 0}, BasicPoint3d{10, 2, 0}, + BasicPoint3d{20, 2, 0}}; + LaneLineStringFeature centerlineFeat(centerline, Id(125), LineStringType::Centerline); + + LaneletFeature llFeat(leftBdFeat, rightBdFeat, centerlineFeat, Id(1234)); + llFeat.setReprType(LaneletRepresentationType::Boundaries); + + llFeat.process(bbox, ParametrizationType::LineString, 4); + EXPECT_EQ(llFeat.centerline().cutAndResampledFeature().size(), 4); + EXPECT_NEAR(llFeat.centerline().cutAndResampledFeature()[0].x(), 0, 10e-5); + EXPECT_NEAR(llFeat.centerline().cutAndResampledFeature()[1].x(), 5, 10e-5); + EXPECT_NEAR(llFeat.centerline().cutAndResampledFeature()[3].x(), 15, 10e-5); + + Eigen::VectorXd vec = llFeat.computeFeatureVector(false, true); + + EXPECT_EQ(vec.size(), 18); + EXPECT_NEAR(vec[8], 0, 10e-5); + EXPECT_NEAR(vec[10], 5, 10e-5); + EXPECT_NEAR(vec[14], 15, 10e-5); + EXPECT_EQ(vec[17], 1); +} + +TEST_F(MapLearningTest, CompoundLaneLineStringFeature) { // NOLINT + BasicLineString3d p1{BasicPoint3d{-10, 0, 0}, BasicPoint3d{-5, 0, 0}}; + LaneLineStringFeature feat1(p1, Id(123), LineStringType::Solid); + BasicLineString3d p2{BasicPoint3d{-5, 0, 0}, BasicPoint3d{0, 0, 0}}; + LaneLineStringFeature feat2(p2, Id(123), LineStringType::Solid); + BasicLineString3d p3{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}}; + LaneLineStringFeature feat3(p3, Id(123), LineStringType::Solid); + BasicLineString3d p4{BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}}; + LaneLineStringFeature feat4(p4, Id(124), LineStringType::Solid); + BasicLineString3d p5{BasicPoint3d{10, 0, 0}, BasicPoint3d{20, 0, 0}}; + LaneLineStringFeature feat5(p5, Id(125), LineStringType::Solid); + + CompoundLaneLineStringFeature cpdFeat(LaneLineStringFeatureList{feat1, feat2, feat3, feat4, feat5}, + LineStringType::Solid); + + cpdFeat.process(bbox, ParametrizationType::LineString, 5); + EXPECT_EQ(cpdFeat.cutAndResampledFeature().size(), 5); + EXPECT_NEAR(cpdFeat.cutAndResampledFeature()[0].x(), -5, 10e-5); + EXPECT_NEAR(cpdFeat.cutAndResampledFeature()[1].x(), 0, 10e-5); + EXPECT_NEAR(cpdFeat.cutAndResampledFeature()[3].x(), 10, 10e-5); + + EXPECT_NEAR(cpdFeat.pathLengthsProcessed().front(), 0, 10e-5); + EXPECT_EQ(cpdFeat.processedFeaturesValid().front(), false); + double cpdFeatLength = + boost::geometry::length(cpdFeat.cutFeature(), boost::geometry::strategy::distance::pythagoras()); + EXPECT_NEAR(cpdFeatLength, cpdFeat.pathLengthsProcessed().back(), 10e-5); + + Eigen::VectorXd vec = cpdFeat.computeFeatureVector(false, true); + EXPECT_EQ(vec.size(), 11); + EXPECT_NEAR(vec[0], -5, 10e-5); + EXPECT_NEAR(vec[2], 0, 10e-5); + EXPECT_NEAR(vec[6], 10, 10e-5); + EXPECT_EQ(vec[10], 2); + + Eigen::MatrixXd pointMat = cpdFeat.pointMatrix(false); + EXPECT_EQ(pointMat.rows(), 5); + EXPECT_EQ(pointMat.cols(), 3); + EXPECT_NEAR(pointMat(0, 0), -5, 10e-5); + EXPECT_NEAR(pointMat(2, 0), 5, 10e-5); + EXPECT_NEAR(pointMat(3, 0), 10, 10e-5); +} \ No newline at end of file diff --git a/lanelet2_map_learning/test/test_utils.cpp b/lanelet2_map_learning/test/test_utils.cpp index db4eb4eb..415d7373 100644 --- a/lanelet2_map_learning/test/test_utils.cpp +++ b/lanelet2_map_learning/test/test_utils.cpp @@ -8,16 +8,14 @@ using namespace lanelet::map_learning; using namespace lanelet::map_learning::tests; TEST_F(MapLearningTest, GetRotatedRect) { // NOLINT - OrientedRect rotatedRect = getRotatedRect(centerBbox, extentLongitudinalBbox, extentLateralBbox, yawBbox); - - EXPECT_DOUBLE_EQ(rotatedRect.outer()[0].x(), -5); - EXPECT_DOUBLE_EQ(rotatedRect.outer()[0].y(), 20); - EXPECT_DOUBLE_EQ(rotatedRect.outer()[1].x(), 15); - EXPECT_DOUBLE_EQ(rotatedRect.outer()[1].y(), 20); - EXPECT_DOUBLE_EQ(rotatedRect.outer()[2].x(), 15); - EXPECT_DOUBLE_EQ(rotatedRect.outer()[2].y(), -10); - EXPECT_DOUBLE_EQ(rotatedRect.outer()[3].x(), -5); - EXPECT_DOUBLE_EQ(rotatedRect.outer()[3].y(), -10); + EXPECT_DOUBLE_EQ(bbox.outer()[0].x(), -5); + EXPECT_DOUBLE_EQ(bbox.outer()[0].y(), 20); + EXPECT_DOUBLE_EQ(bbox.outer()[1].x(), 15); + EXPECT_DOUBLE_EQ(bbox.outer()[1].y(), 20); + EXPECT_DOUBLE_EQ(bbox.outer()[2].x(), 15); + EXPECT_DOUBLE_EQ(bbox.outer()[2].y(), -10); + EXPECT_DOUBLE_EQ(bbox.outer()[3].x(), -5); + EXPECT_DOUBLE_EQ(bbox.outer()[3].y(), -10); } TEST_F(MapLearningTest, ExtractSubmap) { // NOLINT @@ -45,7 +43,6 @@ TEST(UtilsTest, ResampleLineString) { // NOLINT TEST_F(MapLearningTest, CutLineString) { // NOLINT BasicLineString3d polyline{BasicPoint3d{0, 0, 0}, BasicPoint3d{30, 0, 0}}; - OrientedRect bbox = getRotatedRect(centerBbox, extentLongitudinalBbox, extentLateralBbox, yawBbox); BasicLineString3d polylineCut = cutLineString(bbox, polyline); // for (const auto& pt : polylineCut) { From 835184fafa2f59cd92deea345628828eee41bdc7 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Fri, 8 Dec 2023 21:40:31 +0100 Subject: [PATCH 26/64] wip MapData tests, add matplot++ for debug (remove later) --- .../include/lanelet2_map_learning/MapData.h | 10 ++ .../lanelet2_map_learning/MapDataInterface.h | 1 - .../lanelet2_map_learning/MapFeatures.h | 54 ++++++++++- lanelet2_map_learning/package.xml | 2 + lanelet2_map_learning/src/MapData.cpp | 6 +- lanelet2_map_learning/src/MapFeatures.cpp | 52 ---------- lanelet2_map_learning/test/map_data.png | Bin 0 -> 33862 bytes lanelet2_map_learning/test/test_map.h | 28 +++--- lanelet2_map_learning/test/test_map_data.cpp | 89 ++++++++++++++++++ 9 files changed, 169 insertions(+), 73 deletions(-) create mode 100644 lanelet2_map_learning/test/map_data.png create mode 100644 lanelet2_map_learning/test/test_map_data.cpp diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index d742ba82..7561b7b4 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -35,6 +35,16 @@ class LaneData { static LaneData build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); bool processAll(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints); + const LaneLineStringFeatures& roadBorders() { return roadBorders_; } + const LaneLineStringFeatures& laneDividers() { return laneDividers_; } + + const CompoundLaneLineStringFeatureList& compoundRoadBorders() { return compoundRoadBorders_; } + const CompoundLaneLineStringFeatureList& compoundLaneDividers() { return compoundLaneDividers_; } + const CompoundLaneLineStringFeatureList& compoundCenterlines() { return compoundCenterlines_; } + + const LaneletFeatures& laneletFeatures() { return laneletFeatures_; } + const LaneEdges& edgeList() { return edgeList_; } + private: void initLeftBoundaries(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); void initRightBoundaries(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h index c941c3e5..236a17a5 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h @@ -7,7 +7,6 @@ #include "lanelet2_map_learning/MapData.h" #include "lanelet2_map_learning/Types.h" #include "lanelet2_routing/RoutingGraph.h" -#include "lanelet2_routing/internal/Graph.h" namespace lanelet { class LaneletLayer; diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index e8f002af..b08305c8 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -148,12 +148,58 @@ using TEFeatures = std::map; using LaneletFeatures = std::map; template -Eigen::MatrixXd getFeatureVectorMatrix(const std::map& mapFeatures, bool onlyPoints, bool pointsIn2d); +Eigen::MatrixXd getFeatureVectorMatrix(const std::map& mapFeatures, bool onlyPoints, bool pointsIn2d) { + std::vector featList; + for (const auto& pair : mapFeatures) { + featList.push_back(pair.second); + } + return getFeatureVectorMatrix(featList, onlyPoints, pointsIn2d); +} + +template +Eigen::MatrixXd getFeatureVectorMatrix(const std::vector& mapFeatures, bool onlyPoints, bool pointsIn2d) { + assert(!mapFeatures.empty()); + std::vector featureVectors; + for (const auto& feat : mapFeatures) { + if (!feat.valid()) { + throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); + } + featureVectors.push_back(feat.computeFeatureVector(onlyPoints, pointsIn2d)); + } + if (std::adjacent_find(featureVectors.begin(), featureVectors.end(), + [](const Eigen::VectorXd& v1, const Eigen::VectorXd& v2) { return v1.size() != v2.size(); }) == + featureVectors.end()) { + throw std::runtime_error( + "Unequal length of feature vectors! To create a matrix all feature vectors must have the same length!"); + } + Eigen::MatrixXd featureMat(featureVectors.size(), featureVectors[0].size()); + for (size_t i = 0; i < featureVectors.size(); i++) { + featureMat.row(i) = featureVectors[i]; + } + return featureMat; +} + template -Eigen::MatrixXd getFeatureVectorMatrix(const std::vector& mapFeatures, bool onlyPoints, bool pointsIn2d); +std::vector getPointsMatrixList(const std::map& mapFeatures, bool pointsIn2d) { + std::vector featList; + for (const auto& pair : mapFeatures) { + featList.push_back(pair.second); + } + return getPointsMatrixList(featList, pointsIn2d); +} -std::vector getPointsMatrixList(const LaneLineStringFeatures& mapFeatures, bool pointsIn2d); -std::vector getPointsMatrixList(const LaneLineStringFeatureList& mapFeatures, bool pointsIn2d); +template +std::vector getPointsMatrixList(const std::vector& mapFeatures, bool pointsIn2d) { + assert(!mapFeatures.empty()); + std::vector pointMatrices; + for (const auto& feat : mapFeatures) { + if (!feat.valid()) { + throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); + } + pointMatrices.push_back(feat.pointMatrix(pointsIn2d)); + } + return pointMatrices; +} template bool processFeatureMap(std::map& featMap, const OrientedRect& bbox, const ParametrizationType& paramType, diff --git a/lanelet2_map_learning/package.xml b/lanelet2_map_learning/package.xml index 372104a9..2a4c5df4 100644 --- a/lanelet2_map_learning/package.xml +++ b/lanelet2_map_learning/package.xml @@ -20,6 +20,8 @@ lanelet2_routing lanelet2_traffic_rules + Matplot++ + catkin ament_cmake diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 060e1223..198ed6ce 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -104,7 +104,8 @@ LineStringType LaneData::getLineStringTypeFromId(Id id) { } else if (itRoadBd != roadBorders_.end()) { bdType = itRoadBd->second.type(); } else { - throw std::runtime_error("Lanelet boundary is neither a road border nor a lane divider!"); + throw std::runtime_error("Lanelet boundary " + std::to_string(id) + + " is neither a road border nor a lane divider!"); } return bdType; } @@ -118,7 +119,8 @@ LaneLineStringFeature LaneData::getLineStringFeatFromId(Id id) { } else if (itRoadBd != roadBorders_.end()) { lstringFeat = itRoadBd->second; } else { - throw std::runtime_error("Lanelet boundary is neither a road border nor a lane divider!"); + throw std::runtime_error("Lanelet boundary " + std::to_string(id) + + " is neither a road border nor a lane divider!"); } return lstringFeat; } diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index 1123dff9..5a1bca6b 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -235,57 +235,5 @@ bool CompoundLaneLineStringFeature::process(const OrientedRect& bbox, const Para return valid; } -template -Eigen::MatrixXd getFeatureVectorMatrix(const std::map& mapFeatures, bool onlyPoints, bool pointsIn2d) { - std::vector featList; - for (const auto& pair : mapFeatures) { - featList.push_back(pair.second); - } - return getFeatureVectorMatrix(featList, onlyPoints, pointsIn2d); -} - -template -Eigen::MatrixXd getFeatureVectorMatrix(const std::vector& mapFeatures, bool onlyPoints, bool pointsIn2d) { - assert(!mapFeatures.empty()); - std::vector featureVectors; - for (const auto& feat : mapFeatures) { - if (!feat.valid()) { - throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); - } - featureVectors.push_back(feat.computeFeatureVector(onlyPoints, pointsIn2d)); - } - if (std::adjacent_find(featureVectors.begin(), featureVectors.end(), - [](const Eigen::VectorXd& v1, const Eigen::VectorXd& v2) { return v1.size() != v2.size(); }) == - featureVectors.end()) { - throw std::runtime_error( - "Unequal length of feature vectors! To create a matrix all feature vectors must have the same length!"); - } - Eigen::MatrixXd featureMat(featureVectors.size(), featureVectors[0].size()); - for (size_t i = 0; i < featureVectors.size(); i++) { - featureMat.row(i) = featureVectors[i]; - } - return featureMat; -} - -std::vector getPointsMatrixList(const LaneLineStringFeatures& mapFeatures, bool pointsIn2d) { - LaneLineStringFeatureList featList; - for (const auto& pair : mapFeatures) { - featList.push_back(pair.second); - } - return getPointsMatrixList(featList, pointsIn2d); -} - -std::vector getPointsMatrixList(const LaneLineStringFeatureList& mapFeatures, bool pointsIn2d) { - assert(!mapFeatures.empty()); - std::vector pointMatrices; - for (const auto& feat : mapFeatures) { - if (!feat.valid()) { - throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); - } - pointMatrices.push_back(feat.pointMatrix(pointsIn2d)); - } - return pointMatrices; -} - } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/test/map_data.png b/lanelet2_map_learning/test/map_data.png new file mode 100644 index 0000000000000000000000000000000000000000..c5ed3de6c34c11c96c0e32b1e1b9d92803ce175c GIT binary patch literal 33862 zcmeFZcUV*3x-J?-ML|TQ2?!`sML?vNfG7x|ccddomm<9vk=~^DUP2GjJ1EknCxqU6 zfY3YSPW-L4&t7+}z0bM#Ip?2yS9o|vAY+a(<~PRoe(zUif)wN=@Np?{K_C!5L{dx% z1j1gs{=IVp_|MNDZ!3W2wt=*S7zlIylhFi?27#V{AY!5_&MBMIE-8Hu7|iY}xgqa| zz{;UP@!ni*#eBBtN|=A7!q+6ycsxDsm^V_srkO^!$J_N|O4VAvvMDNRn~540eEQH~ zm9Ttq#Ybbp&|AEka=dCueLOj3HH&obUv-~>q2ODS`|c~3o?b$_Fm+%Zpefeh_Sn~N zaNpVB0L_cn|8D+&DS`)9^;nWhlhu|ZBO|g-SRj!8*g|`VY|WhRO(%6pdHGd6We`ZA zM_fW8+3E)(;raEUdG9@U|KH939uZJWwp^W|8eR7HsK@IaEC$lR$=nvCR8*H8l-ABo zy2?$!mb9ARJofJH>2bY0M7ChA&Ko7_?bnqW9JdIBPxP0%B1fR|>D(4^Kki>|ANv*}5N0X=fs}Ya+_hPJP8=TPJp1yT=u>bQ?Xt)Iec{|+M7LAXL^Bj+@3JQ$WP z(W((%=0_B&?&CNC!_R%Zc|3QvRWnQNw)wrQtIK771p(8WwZERajN}J6u)Mt7^=Nav zP_@Vh8^_=8dkXLYs9nN_Ad9{Egcb~LGVBcZy4VdsOZwrGm?H{}s#$;zV}WzjV!#?F zTf8v9;M*d@oz)nUuOgUBSTBL91(;B-g)1;v;FVp ze^~@SWqCoM+<1w!k*1|s-k8*cpNt@oUnkY|-nQPj`5(>80Tvj#^AqLy5jo|=v4|J! zhyvBxIVS-E-TDwN2$FpEj!)dZF>#kWYcBc=1EYk)1`}9EL!+mgW27y&-ae%IBU9V& z9e)1$3tP+atk8Q#CdunqAl!vrSB*e3YxHu$iZMuAl%{)oj(Ip;!8J07GnXab zIwk>nLhw-W>R*FcdjUv4)Qh6VtFUG_%;-(1JYX9m1pm7K+Qs{z6>;Vsdp!U4-rw(1 zK>CDV1VIV$g(@SgkK!_Jj%eSkXew!hNX>qZA)hZB0HFNr%ODCl&%0WHIQy^)iIQ`ihFET}`I z9ss%Med@=2HW@hp%60zwb+9(H*y@ArUk_MA@98_+J{(q4wW!jD?GbsAJ~tb{no7sL zAm=h2JY7wn&6SQnJ4CU+FKbl^xIP&a4LvvCb%s-+o6omvW?Wg&KkzAe;04N1wGz!{ zy<{%4a?eXwqG>Ff1}5=Ds~=^~?a5YHpsDaV>POrbqcOyUgr84#K45<6G&rgjsr6wj z4mS|UQXPv7eo*N@o=Cze-!9alt58~_1c07$E-N!lZUOhm7mBj~ynLr9fc3n13qU9Z zxB2E|%>d>a@%x3(H{%}IMjs^G=b9nwxwdEuLz>DV&fT+SKxL-{^yuMr< zxZG|4Tw~N5XV?+SZ@)GeK;v2aq0PSm-8^XzkOxrN!yn+@632Kk?X$bsDN2w8m1I$4w%M=G5UYdBCqGBqNC%{vk0N# z3{l@(c%%m#2vnj;nnsgr)p{PFruy_>+#XoYHhZl0r$z|vb@`F$IwN(QF1M_`v?`3l z{zsowx9vKB1E+Erb^zcS|3ahOY$PvFHaW@aIu4*_5bpNC&(AN68Z5xWm6cUD>tU=FS z9n~aTH=izX7O57k5L^?J*6xJoL}DRi?82CfeKdf#J+UmjX2UrEzh`D<&PxUY58ko0 zi_~I?iHQKcztXFysNm$}#6QArty&;nWqBBDT1})(OR~-~9AjKf1hQe>uEkLVI25eq zO2ArNe=J-Or`(PsY+P9p8?-@bvPOi4pj-G$kXK0GLd**?r-j%3wtBt zDi_`ptL#{9Yx_o|_L-F8y3{P}a=eGIlcJ;ZmL3 zT4T974Ihhr%3e|l8k^g6zO&@wZ|uwtdq)ZKexv^{sWdLJ9H3AO98A2RK7(W1jg2i<)`vxEu&nDh_nsS2K%UCk3wj zznyj&fT<^_>m7QC7b^8R-uSC?LX9HjpK1QydFomjCi27njBq{oKB;-;e>ogk(fdE^ z0+H9uG9K=NL zg(`pNT%Z+8_`83&eLzmFjXdUte@6Jj%KS7<|I6XppO5}o7vKweyL3zO{0qAE5^$k@ zTNLIs8|11bZQTUTX#$;JTF0s-0u1ulEGR-7S`g2bwfk7sr48U`M z6zjUK=OVT3{)YTCfORnX_>qp0aUg*sTh#Y9J_Xm>2y_4dZW-Qdh+_e$w<*BONuH~i z!t*21$TYB9(iRR#n?>#W-z@$gm@ia^34aS(Nia*PB7D4pFh6 z{#YwuGAceB{WgDm%ZW;jO4Fgy5-l3flX)N%lV^X z(}g|gfZJIEJR4o$VrE}A0HH7bc&U8kKwu>+E9(^}Ctw;pMht0bX`ibVo7OXftl%oL z9_>;kT!2^FTIFnO{s0&w-}CaUEr4LQ$?X_0HBi}P#{uwhf)%beWy$dK*@*RbAkcO@ z)$yF`Vi+$V6Z=Wa8daF?We{Up?P9o~jML4(iH7CXtGzznS+2j#!5`K7uQg&p1-Pp9 z#=kY<|BOalE7;#p{y#&8x6Ain?mwCe92kAw@~D46slSDFt}ok9DlCA#FNif3#QIwl zLn~MH`l$bhOLYyWp}Spn_0M!&-p+($F?!wb%v}PI=qGSCfGz�Ef4d^@@yYVNSgR zJu~NsBwnn_`Wqe7kEW1WFPQDJO6C_K>a7{eynx`su{W$eMU9E`HI~Y4{^m2<%e|;x z_PzrksPwcJn`wHqc&Lp4we!JpoECy^vCau;6SzqB$y=C%5)yQ|l6^a|)%7BZ>RAOrjud7f}rZ12piDiBUN%vTJ z%*_4p;ngdVwN|XAW8FZ1$6o&kJTd{y;DCVi{CrfUd)?U~=Oi&GC3Nn7MkNK8B#qY# z2z5lB%n)mhWE8TfJ36d-SyIi`^tS81ER;#TpV|LDeqR?w>HKNqS@_IdMViGV+hRyX2A-AhWT3L=|* zsa+V%l!&;4RF;JL1hq_Xnj)NLhK91E>@N@N50oHDToBnLNu_uGz!sFwdAR_`>-q>7 z-G8hg6MD22o#T4QWtYd<02b1ouIFDpS(W>${gt*EQA2>5|02)&J>@a0<`SGr@c9dw z5-q`&=5wn%9qJMe7gBn;Ou5B9X9Q|0?fgjE3@=-T9@z@OT18f69{x3HHV(5cZbM#!4rc@Wo4FFqhh*W}(B+A7_PLk!-l_ zJ0WBim#&aPP1UW5WonqZJh~a>o~>0WIUqz7Yz)eHaxMDq5O(t!A!r((V!Yl}ygGZm zB<~CpgrDDQoaUvC?jYVYeCyqGt+Jr+=Sy00!BJ)kUbViYDErgpe*33>HbI4uE0GeR zlZ@x8XT8$A(=mb{OyNxQ0j4D{K}){Ct-;^dD!ldQbC}@{bU9s>mPyFiyx0yQYwJ97 z4)mAQ%%Wd3`ot4H;E!MDrSgdH_m$QT*nWd22K*0Eog8OX}>!ljGFR4 zc12_djF#f-Sc7byU$4GxrLP|2$rv(g^LD$`i?Mc<8c|3O@w&(y5XM_NnYi$#?otfT z88rvL#>U0WSWb-UOcQMAgU(M|Fm^zSqQ%{h{AKHS&lCzV_XF#(g4bFE>x9x~4JM!F z*xVX)J_IiS6hCLB&tW+h%J*Y&C_&k$VGJjBlmrwKQv1MdcJ1dkcwk>#P@5O{llz)+7~V z+`cLdsTay*bUy^)pNOT$H0G-7eh!vq2hK2KvnxWi4kz8CdMSI|vI(r%zqO@Ep(^1J zd~nq3;L^C@ak%d)EY6Iz){R+>VbdWdRBY`&o+*N+*=}UMjeQshNNEk&uYS|sIJ4H> z@udinQfe^nVQHhZinI`SR(+vY39O-DlE^hHxt?SZ7q%AbwUHiW8#AJj%4=>2IU6q* zPv+r29Fb2ydyK)^IYbJ$*V1L1`KcW!(&$ZVq`DQTbFl4Y={CyNK=yhjWv4wy2k1A< zW`+AwQSj5>(cKm&Hq)*}bS_OBqp~9L#eir?uSJ7Ix3DmDj2AV!*o844R7g%%+2xIV znus|M&3c%hfg8=WuG(b7DCG6`S^&+)nt0XmkM!x#eY#v`)^{8Msux_-H85qB(qg$( zIgkE|0f8crt?j|Zemk|4U{yua1PwwFqP`YPM^Azl_<)<7Nq(rRH;hHK)0e{RsWpX z?k*17n{XekEX^oJ7KGYXU6PjWGCjANVT4e7GrW^VP<*l)3WjR)SDDVk(T&QRnK4;0 zB@@pJ{LLGo$IlCte60CXHwuN-?L5@SGVs%jZdRZ#dzCJe^+?}+%u!NG*_!b>6`!u} zrYx)a2~v0jD7;C&Zqq>@ZSj~_qmQPigrO;rP zEh=H{lh4{N)6=IdoDyNS!8#2G2=#VK*@*S;PKQ=sY4a>x!3dTW)$j%-^>E}#VPR5M zOtruzGW^2hvPumWm(vsLfVmWRLzAd8t)I}~uM7-$48H(G*JV^z;CcGB@XSL$N7R(l0 zr+zJnG1!F)Q{MjT7p-jYiIg5V+vG;wqOhU>n6;>V1vE8Z)Ux9nb9Uk+2s5VDu2fCXvHS3MvRHw}H;_0etUMQWB z396mUckKPz?}SWj9J+P17m^!EyiaS?Yku@8)vK8e2Z{r8=gypHSh*PI$c-BL?PXUb9-EfF4)y((vqssV2YWkz9 z{^%*VQcN0`?{KRom6m5T@uupV2;sssaB97L`p*IA4etej*6=+m)`)g*J@)h}W+@PJPDwmYgHF+eGTJKUK43rXb{OTxiEz`GrQ* z>_Y_2;i9kWAPbs$%jK;#v1RB&kJ^z9{w&lZv1c*$LRi)h}X?4>^^r(_=e;}Ce;?a|2Sq;IRQNBGHRCnuKsAk+d-kZaiw3i zL^G<=4mwf8K6E7r`o<2Vk$RVx@}4cv;K})(B81 zlgk$c!G$?Yxk-g2E(r^T*#dT4(3Bnsz@I(ftMJujoj8hCv_lkI?du9-W+Ck;wPJdE=!5|LGh@)g;2#C z$c+g?+q|8Yk{U+SpFt)UJHBeg-*Rl1;knXtg0=JhQ`|SLn+t+&!ANd<8vuE;vhj1Y z#gZh{bDO{Y*G;3CJlRyYeu?m<{%FT&lLHyIs(84#j%Lx}UD$o{1!6LtoU_9ayGt!= zRYakj$6j>uky$!y4R%EZ&*a07c~5h=o+B6alek*KP0oGXtD#9pFxA6RL0}CjPawPo zZ*pCqwDx_4>pdJccfVB=HM;%-C$(;pcedCh9e?5dRo`3fy7W4<+OdjkYMvR1DhFEH z%nabrP_}Nm@N=|Nuy{yJ>|uPo?huPI_aWQ#VH9pt-^T}`$P-g>j07e~{X$Qd&z80| zIG#G*nJI@yBZ{C-v#N=&qAE=zZ{NSSZ3B`b2M#+PK4qTEK3(_X(669+A-bZcmlZFJ z`zXF+5{Yv|2LlD|CF08WJdz>I%>6WXbEc|5?MDkCa=Vk(XUp+Xx~yV=*IRA`nBY&_ z;Rm7)BeE1iV+DGi8Wko11`42D>>pI#mZWU({Q*G-%+Tr>tq*4@T%W9gN3REueHWioT z)&##p1hr-ZkJ#c3z7fDCzBk%8p7v@fub?k*u)wFG9v*6NI=-AuTWpH5f`_rM`F-P2 zAV!TiU0N@doL#(Aoklh|>6fmjvmJD=YVh;>)EPgZ5{v^|mMh~pXl&dSqLxj{k!Fw0 zc^hDl)afsF0x?|+%Iz_KR>C7jY#rwh+sf8&mIr;eUOo4NnAa;gxvQJR8K0)PFTF|Z zX%yJktkTW#NBztbf}hX&j0fnC7Kal^K6PJ`cCKT*dl%28W|1KJnV z@bPv$T0_5CmW$U~4Jy}PBMV}>F5YmnN0*{o;&X6GxU6-{c$S2lW)Nw~l#5d)X+rb$ zNrmJKw{c;`J;uUdxccf8rbTNoRQu(pIar6INeC%6zTP#E;x7J z3%W1gs0TaRy*w*UyOgAf-AOX!c8p?JAFAm!;ja1`e=jeL(t=Hwg_%V=y1}g0 ze*iJ1aBMRG4i;%HxY!?Hp{IxAgZL_d$otq_2)k|i*y%5#tGbn^qp$HhuI&1nr3a#O zJkDcMKIMe{e8eRgb+DAb*PBhh%mfXo&nY|+J!kPi=v~qm>|%bh&nyY9ZO|8X#^;tc zW*?32FU8h0)MIkeaozR2n@+N<8f2pybsbQ7>0b98utELTo^9#zIl3M#y!ttnRnzqM zBi|+!#n{5>2oZ01i)d}{W1%$}W+k-~j0flW4V@>s`joL$V}bk$5w}LJ>IhfKSR%Lf{9UPpEM`0InzRi>X^B66YW%w} zT(M59RgCaZc0fTsw_Z7`%I>wj3c5T`eJgO;BmgZFvs{kkRX5pwTNJn4L5fl zmv}bU&F>E)TC{s93+F-jU84{|n;NT<5cas7Cb#Ko)vi__xzGLl^&5ryVZ&8L&(-YB zy#k#=b{A7+N35Gv9Z*Tz3=u=wh{9MpfC$)`SWcPNKLy1B^vNbSRS?nK6iywR^M@D* zcFyKmas=}iWj;k79#5qnqaLt8i&(Byn{Y zygjXi|GYzh+#DCcxI?6*m1ROFue7mSmjP#Vit#U^qy`BN{jD=bR=yiMu1Mu%8jzz| zcxZijPRs~c03gzplPi6w+{H^S6ejAMVpL$%v$hW17vz-IsNiBrSLck0#JmSWLB!{8qNEJoKNCEiSfK*`qiUe`N0HEX1HxRzMw8N)UZ3ih#p zSFl!vOmPMHF(ui?ZP5E`I5OIMm-zMOw5@btXL_Dlkgf4b=r9w;_P6rRz^??M@kE3n zd^mTaqO%}?U;zD_>~LZAOk2tNP&bo=vGJ^Q#zPh62}8q%xRWcH(@_U#G;zzt!e+DS z7>yTSh%lHMQw~?w%vF^|957xU?L-uIJ5k(w*YAMVx3X{T_mx1k(5A0mQ?QjH2MCzh`G=Ts<)c4O<6l;X-K ziaf5owC)6dUbH_%4C^uaV_k=jpYELl;pd5%*)SH6mqsZvKaxQ{4$UF*R7E2eQ-ski z?WHW*UC)bL!o{;2c!&tS2Y9Q>$7?{O1BmLRtsqonA%d09)V?=Ev+`KLKxn{TQt2&s zN)|IU$xg&v^<;{{Ok-3f%^iMzKfO3jHKXWQ_q6`#N(nauVmmR3FVk~+G3>D88Rb-g zVZC?*?ACR{skiex^KC#r?0uQO50Jkh>unI#75lm}fYdM{H_P#%c!As_IQ(Fw%675| zge|GrAR(k?ZT|YlaS@qsQ{DPLe4yaUpW8@*8_ZL4c3{7K4k%XA(v^dj++j4ga=ol1 zCyheu+w!8C`R%4*M~D%wt+yVJaz^D)6Bc&qQL)FL4X;Cy#~Ig!)Vx+x&TaU-30604 zzBdy1&GI`4Zp3puVfyg0ns1TSi+cUuCOt7U!39Pdr-+2)Oj zLucM5+v9m0?`Ri789~QQpi+4Th$8`YDS94_G$Aj9{7XIaV@$Bo`o>QLUf_LRE# z(k!>Vx!m88?666q_i`B-VWsT&K9aIlOysnud&B%Eljom(s~A9aK?+nXTfi?)I3TC+m#FkPcd?KryNVHD8*flp_5aNVeQc z$Ic*r22{CJnvDoi52QNHxBw;W#l^)y{WcZ9ZH_^EFk|{()rag3Kigs>GmBr0c(ig6 z{Qxt{=;WamClBC~_+FYHjGF~x;4i_+xi*jwJubQfUtiC90(C65~wdPdH;E#LN0^7g=-p1s^Ev~(X*x}W*-|@ z!%ygusIvt<)Y0VOZfDfNW`*n3DTw<$6T~5hD$3qrzf7u^z|YpSB}u6S@9-QQb?`d6 zYSx2nVqVaGFoiz~UIXh0y}LCAdLZyZ4nIpV`(26PcDUfS^Tlo_WXs#_#o!H0+H-l- z`omnuWsNL>4{w+-Un*Uz95-z?vOM+PXLAEl&!)DD7!ueCO^7qHQ5`lFceKr_$w*)B zR&$ZOMOy3Ykx4b^zKjJ5<$`D0nSaeHic;^Znxvf}N&w?FM3VwkA`x z&`;;Rsy`iW)UW5!Hh!+)sZmUhGY29cd(^Wf(={8Tkrd-K&{LVrrv%8ihVFw$1Om)_3Nnn8~R#R z(@>8nz<-wMI{*jdi~Ve~<2}SfD63SISK1dSJFP2y<)t2-cXL2nZC%ODR;iyivTu!U zTOXF>;7{yaNAo{0UIqAdw`^^Nl8c+><=|dc!rutw8Ct?uBra>kwFC;3tIM%%H9$=K zKx9>E73Xp%;Od(SBz6XQ&aKM%66taB!OsseEJ3qgGke3X&z+u*ufkR+EvqluMLPmE zL;+b=y~1ybJkEV*&ZQ9z-jh6wxZ;!%A0wS3=AQ#%tPED@!D&N0j)qUiT zP0qO6jJCzA+93xMee1pPx*M>pF#@b`phN+{>sfem7=lk9*|bJmWyJ~^A5VH1%t|=x z+RF@t3V`z5F{iMh1m;esPmiIW)lnNna;8Pqmxx()0>mJ3F7aMZ7^UEdpUT1A@sZDWKbs0GYC4np0m(2KHQS8RdmwrsUZ6msQyijhrmF71dIG229T8cb$kOvd%a(K#PT(U()^-&c0qT;CwSj12+ zn~a}r^vsMBqFw^+m1fuJmLk|_torSv2_TJUzR+I(epr;sUD}%DgcNj#2k?<@%GKGr z?ubI~uRKKjm5Mu_)%(f|&}`>+q}mpLQZzW+=&c$DC}V1}-zhP*H{kZ8TSQ05M)Z9I~{)R3G-GV@LLi;c+7$~Ln zz5bU#5Vjsg2PAYYC-VXW=k5_r4dAxkl@vP#lG)yzvy-mp*d7nEeq0^#Uhbzy#l(^{ zy7e1&3WzUuh|6t7eMu5a=2qUCkh`3+6Q%6H;Dq(3`o>R?EDZoo9N=Nh-K&x5buBsu z*PD3XCiYa{d)VC|#?83%%xdgg!{icY=~PkK>MlKuMQv?o{>i;E>vX_iZp{6T#sZPi z0&&4;OXSh}cH`$GEa-=<-73aR?<{9$>hPOCX9Lg5%*CUT1I)CvH>#XmhKNrtO$MJ_ zXQlc1Db&7(&EZ*|*{|Y>E_MKf`U&vhft8pIj!lO^VfOLH%+>^3LA5#H3+_Fq3HIrY zS6}Uyqxwo;c(rAYE3=i@ZSuFEXHHiS;oVqs(Qze>KOO6X9Bj-Ruq|pBitS-9m`(j80NwT$MqtZl&~}Ptb6vg zryWcZQk^UJ#U4&)mm z*<{jIyq3LSWo^gA3Zsj483dg7z&}S-^Du3O<#dol^?UQi`G)64jf(U>f1S(zlxl^% zh|b=R{T&S9;?;S`PU{#5qBN{o63eB4ErMKqN)2cdP~l=@EFiI(l3PIs&cJaki1)d_ z$EJus*wLU>ZMts^?H9p^jc#-vB*3hSs+$mkPw+!WYK_eqn6E?TABw71>}+eS^2aMuCw`T~=P72SO6ZK3i+lw>4d)FWgEHXP`_DO@?FJsRNPJNa%1?lOKdvSrJ3rXlevjo z)qu&3%d!u+c+okVpMW^bx82-CPKP;fy30#6kXC+8#`>lFxu6zMH&nXk*$;YfUC(J! z>~DTjI#ZG#Z0tWEh%NY9T1Dh)jM>0I&P}emT(BS-EobwURw-uTy!UN}b_SlYnOf~n z`jWw56Z;8#W)_t$kB7lK}~M$Mu{V)Z#K(32;?X?ehPMPF#Q`kb}o zE1BE5rO`*pTddN6)j6-yQ%d-0Y`#KYpYwGwZS(f@%Qw&HbTV0}k&CA(iQndk1szQh zg%1fXS}PJH8}}V-)%0`? z9;|QZWCg8UyI=T}8;v8@--rsjay11ceV-Q$r`A`{JzL36KLf(rr5l=%*ggIdRTfoP z>480j;^h;kXT1il=a$gGFx8JAyGvD-A~uH52Rb>l0T#}WoPL15e(j{ppjT!JT3cId z43+`c;$hdbDEn^uvvsI}-pcW;!@Qpy*rmLlv@Ehd{%25&bE6iAne9r_YniMSn8r`ug(K3{z9>tM3% z7mHNRpOx&7f#49oTK%LV#t4Y^s!PIf7bvO141Akll@a z(;Zb(5wUHw*ntwTURfpKw|hw9(6lZlwkhk^5I8ou>Ms0+kbLXMJu6YX3KzC4R|gb$ zb?Kw=Kyx_JM^e;sk7D`KX#|_`?!J&0g_GrH`_y4cltjy3jNZW6KD9YK@Y{nT@LW`p zgs$33B|PvBE=m1ND790{qM}&IVh7nEAz|H}>*X3wj*SM%$)&U!3h@Mr<`L<9Xh3X_tX@cXE!Hq)2`{zX=HhGhq3m|{O$}3bMl0VmyrK9h4gxzblN{A?Dkiw4GGjqU9q=IGAY*34YwUh z+-@k^b69JYAsGZlpJx(DW-Zk=B7x{fT6QN6`ZL=lAtel z!=IHtVCei71HItNGkQ_EN1}&sY%?nH_U&D~iV384tx0Me#ax3(wUnYcPX(u^#~C|j zOWtcFNQ|ywcUt{5Frm5B2%?~)pdUvY^~HzUWg0v75ZJpCI;=y??I-s5(Dv z*x-~)`-D$oK9d6EQ4!2$TuJvDD{nV% zIT+?AP|Z@pHwQW7a|SDJhd5{7xRcOc{3_<|vrqiJzIS@gOjm3&*_&(YnG2!voF4Uv zqj|Fe4SpLL6Ic4@ScmyeEHz1TbhP6ExK4zG*6Z*fAs3M6Sr9&Dsq;w@OcTw$`n%h1 zAa#GRWUBrP32e{&@n`<-^`J5&E6r*1QZJ&IO?G{s+Vxn+B6wqcM7^fD|KjxGlDBoM zW`E9iTW*8drJGWEQ~x&m2Z-lha`>~qX8Io8a?)%6mGS|FR7gedZ}~*q_BKPql6Tqf z$P%LyL$uq-bz`!u?ya#IF+~RmksYlL$hQZpxgD$(@hPrI5Df^uy&r9{qFp4V63j5+ z6wm*V&~4ZvIB=|^p;0R`IDNlv{R!_AyT;B?@=`M>84=CE?xNacr;~IOLt^%k)22SM zw3oWxj*&$TwA3XEm80&br!~NSJ|+H(`qd~>uxBW)^5P4-OlO_$AxaC)Ps6|9DsgEr zL=rtPB-yjAXj@h zNFRVU@qD~{FmjX>=c>rar%}E3Zj%Ben6PLd5ry9>_UpwoK;`+&QnD(aHpNR z=NDG4Pm<&1APD4FXZrD6hNxP$*xeLW9--a}a=mJ(2fM2adyZDqz&Uw~eQwl5(kOn~ z*)u0ARHm>88=vdm$KhcRwaeEi`ld%CzB9|yHa~srho1#ab;|)D^~>{*C@rK$(of7D zM$6!QrU_V7QdWteFS(OEnMWj+f9-OEJvy>>XMPiT59w&26iEo0Z8IeNZb2yGg)q4O&%<+ z@SwF&_0xFN_%tlN>ZXDIlS$XcviPRgo_hl(SF%$d848n$bJrfP+$6kt@q=cdv4J&P zgg;TEqWp1ph?up8O8ZasG&I1UR|V^;3ipb_Be6-}fO{di>+ z9R|-a?l6&<{|and-eO>2-!-gp(Ahoif)}I++$w%AsuYFbHMrT%+#D4f@*bYBSw?N( zzimI;>W`}e(sgOym@4!=GzxZp3ziK`ijNErj zkeeq-TiRY8r_>3H!`UJ9Dkd?vk`YtfoNPDSiJmHln(r*n&zf&HaMU0;#FzfHHm_UBhku zk$Yj`33$5dsWMMe+;iFmwrea=&xxAw=V=&oh{@JjvyI7C_Y9vl+GYdlC2FZ#Ok||{ zA{RF&2R!wcB#S;@@P*)9wWfRhtaDQd3hdJG$-@rB*v{3$-%dQ7lGk2wd*YvmSpnQV|UcU$$k?6utVcbubC-W7x~%8s1K!>YL(`S#QkO zPK#c`Do0hS5Jpnw05bTMFF~_hJ?WhVy)RPz3xb5`!H=?+o0KKTwJfObGyY)iG)zy@ zEMq=IDSyve*QTXymh@{#SpCJH@I%AxG2df8>gAyjYQ5+ra-5Hs*}hxOJK{8b-#)jt z3AqnRpcK(Rd&dvw9`iT9I2G&QXzKWeo>hYF1j9@mXMgb8?ZYmlq}aTbybex39Ur{% z@DQq)Bk4f%#H)@YpkfQEf*1f7~10c~p0gWZPioxtd3Z34ovB1beNo(`{$gx`$(Sb6I+jsU^seqCp~7)$dKTQj zIN)`rJf|MUNiAX@Snpsq{B^{$axgrwi~iau?M87JGO2~C7HNa4%y-cP?KPx2b>6o` zPjLZ@E6a=ZY4ROZ?ju@WOj*fvJnC%#D45h}rg1REZJ>vN-qm6c+zcpHV>kh8LwzgFh_kQG0KcH_SXLoex21IV9o3TL2&lg8l?2ry~ zs`n&53yoFhdW~g5sN8ray);&>Vs@(gw9g0apTQku$$(V5BBK-9{TG+_3q;7 z5>XM@>hbZk)BIC;p&u=n;Vi!*lM#ksGN_&ijEEGuU7jqB2=4Ho%E>R4!&U07~iAvI1U;E!*J_oz;s3kf0G2-0b8#jqz z!m>F5s7DIl{CMq*{T!%7Q^U}IyDlJzh~4Ximh`Yl&uvvi!tL1cvf6Iv38ILIFp>kN zD)vk78ht-k1QB(oV^T!fVp%Gj*x8%nhNI%B)&>CzQMW0;?1xuj^R&(m`{IIgKxlFN zm#*E)J+Fb#+B(9BnSbInmicqHs^ne$WLM`yf{%BGB-GrF zK6QldYJWnn&!k$n+`4m*jweoVyY^xpV1yhZ>~Uj}p^c08d9016_-t1y@AJ9F829Dt zBxg_5Ns3)~{}I|Fu0X+U{SgLr`Na-8dP0(0J5Ommzi?CSgJ&Az-C*t=;Z*Ar2=n80 z-)Ua@MB!=fm&U*DP;k{Qa0(30i|hV2J%(buP6!5@1aSzITAER1G@cpyqB9=Itsr1y zLV!6UDXTtU(@X7)YUSi=fW?E$T)Cy=HPQH(R)VJW)LjL-NcNqdh&0y==M4n5q-?G# z%hgF+Q+Ja|+wFRXiixesk%nVcVQa2zlroJZV-q%&V>!_se4}Nlg@vwUq`>BH^0R|X z8FJ6_zO5l(O26wVQSnJqD#XV#z0DqK@~=TU4YqUM7I>Myk3WeQDW}{hR4}4dPY8Dj z&v;^BSi;?pxr`6I6BRvuO12YT++9(s>P7P9PEuQnRvn2X-FJM;;qRksL4i-0gSsRi zoA8Ft)3HMwqkS@|Q(uu+d0C`6bF1!Y069-t zFApZA=NWZH38T(O9U~7bzPO)iH?-+iWf6xxVC-zdLW!o8z^WR;UOxg*U%K;rPEJ8xPOu))4CDH#KsH_f`es`}80qURlYL!MiItitrmps9l06dqB zv}+P@(0*215l&p=_j~9R3+s2n=SWI>5_3fs{{)9(3(*ifni3^HD#1s5-&$W6tqwlj zZirxz)z(4;2Y$R}`C_Gg2^_>M9fhi;X=jJPH(A)WRZA-@2sXQYl_z`pq!>LpX z0SaDv{Q2#h_vK$>S?ZSKScH&1i?|ZnLtpQz6y07e_22F-FMU1R$bWh6cJ>9(E>~_& zlc?2Ao1CSOX`V?^v12W>BV+)&@&SPZTJ^Xzxf-h&_JKH7wTL#+k90vnyn76L+_?7{ z*uvlV%}rfOcqos8_0WycW+U2mlHkoC#*ortiedA@{-TnI2 zc|ib&-+Ab2PJdxhqb};pt&+4#E`I{3sp4qKzSsl6b5sCWs4aw1emLhYfzWG%ECkV` zuE9(|-*ctn-ADCr4}8pnzT*DWrDd(Lbv%1K1Uxr+d$*kobRQ?(-L@{-0h_d<_6CXY z%Az#YH~!-%`^QDTI0d??fp?UY!I6?;zbdYYwDyz)O{7SRs+V!Tc4Cv8%T`q{m?dtE z*M&2xOZPykopX7<2sWG-hVZ;{87)2RQZ1eQa;tGa9O1dety#7AOMHa`M&1`CxgmGV zU0nA&GFxK*9IaAp>alMB%1ihbZkcH$qis;2^S5h7+u!;FR>zP30jzB67_E#f$m;n% z-w+%loXbSM5AbDHu06N%jv>~pUK)HFp}i*X$MB8z91(zjeS1n>-=8hyN^lHf&f$Wd zdb07J9)t^yH70&uEJp13WJ{QhhE7xg{>@t~aj_|R04BTmZ{sfjT`Ic~Befh5h?uy} zd{8GJspv1o@Eglays7JH&!>Entr>*V1{J)g8_Z^B7;k$$7JMgmQz9G$P{3l1rLOr* z>176|$DeO8-`OSl3gxyJ8juH4GoRvNh z0On+?N#q;kxlG(48vI>Y?ww8^>u&APum0t>^!uyPyf6e{H$2W1^6s@hZx|{j_FBZoSiCK#G0;ar#d8^wie(G7X`lN4~c;wj!S; z3L+`Vnqo5FCh`uLd!0O0EpnYtBG?^#P&~wENS^%qCqf7XWM8GSEYy)VVg;R)GH*=R zyD$N%8@Fo>@ZziQJz z>IDUT4H&7fs*-*1;4zSC{ex&o$HMbEjD+W=Xhh~c2e+nK^$OgEuny%=X7Q@@*s zMc8(IwH_L*UK#S~zL#ULC(eNh&~qJoR8(LYnHoDb;L7q6Xwq>%>~oZ^rsM-2$|M_O zOlnt!a?byq%Src&JKnnU$uIbGmaX2N)|A)7r-sMo#{EmV$%SeE(se$yPBfw-4N^ev z&m#Xi);VR_8#(V*xRcy^V*tU=>b9O)j)3Pyi~^$L(sHBS@cS%SaOu=?mqKG5C*vkF zsCYRjSYf)DR8a>hyv~9WqA+R-`#5O!@q6PHzF|kxM5b9E$GiY6-D379CEYLOw9d(y zJl08x*zel%xE)GXSsrY!Di?m$5i9^Wo#XEIWUp41mF0A2m4%nG?dyYsLe)j}9+p>O zk(4jhW7y-T(}3?p3^bC&iOV6CY|{RM9~F%{|C=|DNPE)w6~Ec?(;}h;d*AOW2smko zRHLEkuE|)W#u#*&4!8MTe%hTBo~{}zmIqGroJU&Skds&z2 zJC;6Cw5giwqy+p)^FGyp)}JD`Hkc=L=P84&28jPGGNJbt@q2vx5U`%M{Co2KE4Rbf zUOdE{b3>1N@LlzgiG9gGwLNeVes}s^Gf^_S@MJxmRp0-T>0s&eSWDzr^0k2+SG!Z4 zC6vdRLsW&l``qcx{nO)K-+^;>WZW6%V3q3e_eF%D+|Cq=@+>&+1@zG*{OcQV4m_Eo z)}4rRZC{Uq(&kUaG6Clki^P_L&=T=RVxH6>$3H@+yULf9<7b0DGj4|u@Df-+O7xWM zbbfCjx?a*_>NM9y?9xeFr)PwiTjO+>giqq?qBsqBakhGMUZ-LMbQJde&>}jQ>mmRA zd^Dy}Jlgm9vEH$H9HUdCjk{OV`lWy>eYIn@flA$+>IZou_MY!-aQMbhYh~yF#>FD4 zwtKg^xEQEJv&&tC8mcRTOoX(NEslH6+dThk@~tqiluXT3Isygj|8(T4Y?=>%=2H;` z(r)-HKVL-DZm$t>96ySOEi_&X>W&SZ@Ql+Yu5eUv_nUo1VP`=#IQ#aMT;MVkml_Ed zFBoRquKg7iYvg+@`~EknCqQ=%ss z=NL=ks58UUve`cR$ZBE=5A?cRn0fg_pzw77{*SM(0}O(_;!I}k2Yiue9ck z6m|X}tfOC){z3JNei-0ow|V)KwPX-YKwe#_Xid6XCuKanXukT){tsfC&9N{>t-=#& zrc{T#;<->laHhL52xB=?d?vb|{%|2*9wqRR-p#dxm|l^(XzM4L)Xfa^5Lh!bWzMl* zgc4aAuInu1Q@&uwG3c#ISvO6ZvgojS^Rb&FW#^7mz4AEKo`!Q;J7O$Z4}Xf+adzP2 zrC0qjT9C;8IoZ0*Ju2Gm!i-Ky|HeD3TZD=inwgVm$gfi@-d3moxk%0u)gh9^#+=m0 zx!Ut{Jzb%YAceQ}ueo9BgnGu^bnnfigqJ3jHiLpULzw|vbC#2BR@Dca1!4xl5>;7X zds-Tw+dq7wKSDJEFG|Cu2F}F>T#NX3y(nG&^u*c#E0JO$L22ojeS+Y%LK5$$=A2H+bXvBTV2|#L~Z19om$XHdv7ed0Kh$9T%t84$}M_ za%n&%lX-sW5GkO20jr)j9wehvw7prq7|MI7l*`JrzyE>~s(n(4txGNPl14pz5{21W#maN$dh%+#^{KqE|QC4%~Xc)X>uE_q$|o z^L!EYAwCFev9#U+HXV6cR!96 zdJ|$X+7c4=^u&6eQ=cEI@tX_`RWlj&{P3z@XQZc?jE2lHH>C33H`ZB@hzlV#dN?e! za3{~YwpDx9hZ^}Ji$7wH{kAf>m3X+|*Wl)?7F%j?{#RjK*blYb@PTi8D{Z>%xYnHf z`h5&Du9ys8+_&uEv+rl-5JTBA;rfBAiN8F=rTA}ZKD&}##F5F zcxP^IZfIy2Ng?|5l{%Y#2MpB0?Mr0sa(sa^e>^DoyI3}vBZW)v_#P^&8%r{$LPO1| zzCT`5xRWr!Qlm6cNryzOph)hQJ z>u7XL2W12{*}%c_+#?E#s_eE|KMt!Y$Fu##=fZ)HU#VA!g%OeRII1@B)ico1E?El) zzAGdUL@aFP>!b?@{TcZwlziKG;_|AV^tGll2pt7=U{BlH7@ z0_8}~k`3%hopa-kzC2N+4Z?KCY;4WF!5XK4huQ~;OWH4AI_}D>59B()u!d^XYac(^ zB~vUj?2e+AHm(>iRAcqIM&QJ&A~U>v`I3i+CyNxUBN^FltZx&A3S& zO)ARtMpicTJbxfxvaDSmiHK{@2$25qDT;oa1YD5GS?bG=q6Ti0h%Q9g0W z6+h@R#N9o){%KSDlcJQj800L0)<6El{T4V4q2soSP6G)@EY4qyic?wS;yDt6t>h~TT#(w zlTkj`cR(_hTDoR~#&>5rIHW8t{(zSHMWh_hK5=tpjunxaiYKU+z?pA*`t=FDGKqj^ z4X8up0X_g!`PrSV2lHeD&k*St31AdFV#St$u%G7<8zS1Lh@-K?oi}efIuBcrJ33S~ z?!8j)7|Jk)yXoAglQBp_M*R_LWE0ho-|-k{`u*bW8&Hdn*Zr?)G>t47Pl3&RQ?76@ zTFVQu{?;iKJ-+1vg;`57=k@Vx| zT`Pm~o_ubHNS3ob(efA5n@i{2&!jCGO4}7gCBy&41t5q+wo0L;XnG5UL|I~NQsfz2 zKV?x5>hN_|^3sv+BA;)wN@*_btk5ZGE}N_kn*4sj^z(>NDSs*|4#JC5grOhiBMExm zN#Tl&5UiTY5?5%xUd0R#T{&Mba&Gn>`v!8#%w0F$*1@o|Jm3$T$QN97;5qJjujYjk zwtJo4$<8_Gz}uc$ZFQxfu&0J{TL@rKDy;kS6_8@-S=|nm_~~E3Y8_r`laMn!dCcFe;&I1FAzrw0_uF-b4177gH;zeocO5cMYc9bAA zr^_C^Or5t-xFk$tCKVAu!~;Xoz2*+*4NOq~9YQ_k_*7WiJy@v#>F(VGX1|e~lAVo8 zi=#OYWslBBG-vZ=r$3Ts*}om2H}@H?T{hV8gu~8W0@E6m+OOM*>WM_Me)bHZ#}od5 z)#UJc^6k-1wb{&>yWAn-$Z^-Q(&Kn3*#@|P0@mv%eWJ$NYpY6yEXm>`2*_M@S8LQ0ZG^2D#)q=gO%F!GVGj?VJ40h&*>E-+wyCGuVU5ovckL?7pliX42D* z+w4Fv5aG~c(jc&QiHi2)L&dWgc~UN!pu^%qxa4id&UU7S6zw0WJS0rAY$bY&?*;R} zM2iQ{8wh^-{v)Y-HcpC-vkV0)95G^3czmUwwS=?}JMnzbs}nF+wKg^!|FjIG0-Ols zu&QVFNo#gEK*Z1@W% zj2e`6zm`7ZP}-r;_XlH-v|O2Brl8<*fBaRG8+?0L9Hz_5Y0X@BG7e!8rb1DHRJ2VQ*D~l<1%7aRq!_oJc6gD8XSD$eK%;z+s4X zZ~(HC3c0qYvuTYp!dN(@R+Zur##3yA+5^?83mw&-7CZqkLkF-ssCZR7GV5!3Ki-4C z=~HV+;GD0R^12wx4dZ0w(M5VybS2`p%RN?d+9~Qai0S-0_k@zKj7+c*nPQ0)az0P0 z%K1aS?O2uwr^Xhtk#_mddsEVd{B`t2CDZ-$j!A2LQFl|3PU)_MT&|Jy^?8?c{0TMb z#u<7uRrE~KSTI!*M5}@F9i`RFBoX9GXyU$zm1bhB2&{uQT@iRVZ$97xJ`Sr%c%^uh z%SykrC*SkuNIr;GJoztiYmEh$f%!5R&+koi4^&miw(srw!dxLXP$@I*z<}d*tj zPs=cRi8t_!hTaWO+yWOD={BkJ?zxRg&5>NLZ2Kb8*(N;-)`>F`yhlMGkN_2P!J^4K zbzGJfH*#ANsH2Gq+rU91YcY?zc)P3G>o7$1cA9Y<@4 z!FqD6NsXNrggY*-v~jBCZX#3~kUDX$40l2%Oc>skJI!ggOs_TJhn)j4p zLiJoGIK-0|otJKWWE`z0m4ul+terp8+z+=mtAKe zywhgKQ?Z^SON?*;54)u=G3F(*$>~QP`wm;Gy@%wwPs@8gRMBg(3ZmiS*ikac>4LxL zQNx@57RjvADFW&a+^&J1Iw0;9w~gRUmKzcp-=@*jo5c zU)yDF2*qO){AzHEp}YT2Oi_<3ikp_3+xHIb{yZ-^#V%y+wih3nsKe`sMp>#f4#k15 zU&roqb^qO0q=SQ%0V^JOj*;xy(IDSZ=!TH-ww}D#CP?`VF66lgiOc|I0Bl0orS2x; z=Bm@?)WHW=5^|1~J|p&gYYwedJ3{9}U^$@1>9 z0C`vit*bu1w+pIBhZfKffT@(+tTA%wmwzl2<%gNkJJ=Ue10 zb`2nJaj%M+KLaYyErp8YEZ&C_CehgpHdqaW9v+x26E^=FFNY`6CFdW>gtkd9Nt?1o z(iiG66RH+A;~2!Ajt`{e>7<6a`yNoBy%75N_=5~BF3#=75U@fM4rFHd(n8qxLe>n3 z<8G{qYAr4~xi2B>3x!VGv@PX4%)KUy$~^h?tccKD=C5SObIXk?uB(Cw+Gpo7T{t)z z_FSoQa6zLY=sO?$RSiv5qArsuD_PaVGsvwy$pDakD-DAT>u9;mi+ z2ibJA$CM~y10KX-Rd|BV^uxJmn9X?GK*!45rzel;aLIiPl0*)GUWD2Kg3dGeaECM_ zBF`tKE0wFd+zU-uCXZDQ%Dhq-l$HdMQSeOJk`3B%C9nk4@1aOYW*A9hlCq?BPrpTz zq<$hnbFRHRN}n-5A}@T)Na(=#`W2K*={j3#AR=PBf+m#zp)XA~oFtdkZKR(Q@0q;m zix!}@EX2*fZ^&pnv2u3d*#I*%5ex~G==~SEJfB>+QvsJGnG<S$Puk_7+c$pGbvwtK)YT4z-bnzz# z09FC$etyE?G^#ICa$TsZX<>yBb%M0Xh$> z=Jg|4Gxy_ocx#g<;Q2hMeF3F9@DQgUwcTugbJcL=+^LeKqwnld{;|P&h@8yhQOm~4 zMqV&XcU}OV*oQWRWz?(^s;`u9&J%FP*eC{r$$yj9g5xwWQU{FdQUaE%-2Uj|#xe$$y(svOS;#akZ{uQe<2ViWaD zloqP5PT87Yxtot=o5;(hyiSr4=}>|6_(A4`>TW|c$d><{5`Ig-8feqJT3jzfdxUYk zS!zew9vDfFFJw-)+==tFv&<+rIvAD!xM-nvOAY@JIK-BR?l>b6d!{V$_4V|o+cq;U z?w`qMq{hc4IFCi-%~Dzj=dPWz7zKC?H{h-`O2fU0u9ySM2Xw1wEYH#yDC=Y&DQY?~kWXqFJendmDdkuWRyp@aUd|obfO0?@9*8=J0M2kp>X)1D-S~mrz$Y zn^n*5tg^f-7cnO;sq8?CSoEZ38s8)0&Z)}Z-}H_Vhv-gQBSpmG5k#`!g|`vTOveTl zOQ(lY)K3n^x7$Bu1^`WUF`}?z^5zO_tRtJbUQ=(aK_iBX)(=%fW==SkGyy7 z4VHi@Poj_MRE+;U_rE_lN_p%)d0%~d@8`57LyL!FCkRdaIJJT7!M^Qy(vs6wnI3X` zAP$KoTVnbH45MvTkj(i&U0?#_ay0Y5wLD?OeTi!O@?5c?b8{5hkeaN} zG~+ugAd&K>l@pryp;*pHI94d#4bcLM*YPiRv-Q<~h?)KEWTo`}Z!yc0z{!E19Lp`ueHr|EQL0dsU4yL|S)w>g%hK#PlahXgY-6U?rGIJ|o$P>Ue& z0ehMb8oS9jF(D*U3){*9&4@NXtRy*_wBd@E4<7veqi2=uwB%puJXGg&`>|1weX+qW z-q)&TB<6T+NzmFV79AWE;Q!j0VcxS{%SDZ!nB!f+Q^1=mI0hD+0K8zSYhYS|mCB#l zU?6{mYMOWNwBom7XBa?u$FFNBCD(la+Y)c@zJISg7@$BdBGL!pMAwULM#2Nrk>XUskrW*rDdqFZoteTQ{gf$8K@kXP zXF=P!x3d5SaD8nt_AV=M!*Hp9h)s7{3iWuY+jU?p zA&mHFt3*T`@3A3U;#mU-M#BZx@f$+JoOqopIL4mf958}OiIU|`l}Qo-Sb4fNlzy); zV}Jyb#xJWSGaY+iIFZ|b;&r}h#9PmTXt!mNoG!%w9*;9y zs*L5IHpxCz%%OZZclrtp=9&i9s9PJ-F9IeE_LF6v$4{7DiiJDNGSKns;xM!t z5<;G6InV+HCIb|hAthmf&m+RN4Y;kdL8|{fA`bA_vd_7)k`iCVu`IHRqiKsqL&l2P z!Co(e)JoJV&6+vq**eRaUqAF#dW^t!<$eAK3 ziwC|Q#{&|-rv87b434qQmmmDi;DC4bAx8!UueRaG>!uq}vbXm?BJHi^T@%NgXlN1@FnFC|CAlVm=s z+BmiOQX{UUiGn8Z?tSplwPMwCJq>OQni9dd%7aTk6$}3)6+Nf0cLGqz*kXGSIJon^ z(ft^lVmjU$2i~(jUV-xaN;=^{+Y2_s$+cWPVIb?pD)3z3wqylWvQKnJ|J@Ql-YnFz z;7No{Kyc3He6!$dfCzwR45act<8|(Hq`;YvE4^^?Ln$6D*%~!i77N6nRAB@NtF@{G znnBm$PW6ma{3yWof!|V2+Z90!O#R7MfRBJaj3wa6J_ZTb50PNp$jC^eo*13bw@Vgzy!?aVn*A_ z(PjfAYkka*MLWKnA0;xkJE9%jAx!59(G&`(N99hH3u=$7q-AG6&J?GirUtz{O)A>K z#X9eYbMVY$AVH2?UtKC>;~qdBA(ZOl-TUDr+)?MN0Ka+ogwAo8Qdn4+72O3#Fvlf9 zq=F|KrU}wYSl{1ewCVU<9q%>YJbm#FX#y1V$chj!=ZPr^2^VXw14{zaOfvRz8!D=v zQ_qu29nKO@=d#lWNDk;13EJrN%LL&96Ap!|yJ}ftVNx7z`?;)N)182KXh9^BBP?9 zpcs2X1%gPp?GwVNZnyes&F^m(b|s)0BH!tZ1EpA~>%vD=M$Zb%$+GNS39BKE@dW=0RgcXV6lTMmEXt1T=y) z+5DZ4a{C0Btl76vNL9fwOcyHu@gM(<(we0O%^ORk;~7T_RIsqHZb1GVVZPM*e_L1s zU%{KLbQ92I7&H%NNp1559ln5$4-5(#FHjj2TraVitz&0lDU*t&gSws}&W~if4!+O( z-0(9pG9F0_dP?%`VV*5YFS)sp(+j#OHSA;lQ9AgbSd$$8-%9~AX^+Rn=^j>Jx}YEh zMJ8xW28ukfrv55GAONW{npWO(g(G!rvQ6CX32ok5|<>rPHb6SJUN+SRDNtEjx>3(~B2YNFq%XxRf zlIWgqdNZ9i@DYywd)|X*?Z!Lp!*NjCRlU-Dq2(jBR17RMltCd^rphOPCjy$Nbzr!r znthr#9;0MuXZNM@o`!+`2^v+~!dU#j3sg#gO-{sIRk}Rg6Lj4NhljO*#XkYf-{Jtb z37#vPE9};OcCfrPT5!4=uVZ|2HZ8pr;LMlx~S&XeP(tiMunk<)HTvn7bc#%bh^1 zfUP$4oXnBNFL!I$8`H3PrB=2A=q2fHYl(nZ0_Ly)d@2}~Fj#%iJm_RG=-K}+}rO7Evs_^y?jQw&0gu*-r(ZM~nkMVxn-<7&BVEtdSV9|1-9R zJvZ_N^NqA0x1>MX1{DYC$h$yO|HagcM_rU}u)uI+!ll?65AkX-Bm3 z_b*%e;Eh|JazPFeO@?Kf_w2^TM!CsgXIB^4F0t-$Y~I(1+Qvq$7mwrO<8^>T3v?-H z`uAIy{Wnkt&jh{sKzpU8rfZ-cIBn({d~O7e-_0b88GM^TpfB>zbM6gAPpP zCZH z2ckuc<(rE&8P|`_0b>L3VaCiW!6yI*4-7%k!NI}8VhgkgM-O;gs!BpYpsKAc$FC?{ zR9L9l?Cs_1dUm+ltEHud^-MrhU0sS_kw{se@ra(;gnlYy{UZGaeNCT;1iJX@3rEc( z$oMxmHWHJP>KtEc*yM!#TpBOY@j;vokBmh1t_t{E&&zaZG*`fH9tN(tGU*idz&%z`*#eu{kw7jEiJ7{d{;73za>b06pJ~!T* z8ARW!t5195nb6VE!2>|UEV}q!psQt}$;gaND1T!0)8V{>zp<#O$j{GDQ0H4+eSP_n z12R~!=;-Li#zs&aFd;p?*?v`OGnj~OIZu8TJS{IT@ABfJSi6yjnK`l&OKp03dVPIe zI%i~NW~S0&ytA`2Ffi}~R%~djDRopYLYYro0KHif^d25QhuYNLBM05w$dMK?k&y17 zf&STOLZG>=5K +#include +#include + +#include "lanelet2_map_learning/MapData.h" +#include "lanelet2_map_learning/MapFeatures.h" +#include "lanelet2_routing/RoutingGraph.h" +#include "test_map.h" + +using namespace lanelet; +using namespace lanelet::map_learning; +using namespace lanelet::map_learning::tests; + +TEST_F(MapLearningTest, LaneDataPlot) { // NOLINT + traffic_rules::TrafficRulesPtr trafficRules{ + traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)}; + routing::RoutingGraphConstPtr laneletMapGraph = routing::RoutingGraph::build(*laneletMap, *trafficRules); + ConstLanelets lls; + lls.insert(lls.end(), laneletMap->laneletLayer.begin(), laneletMap->laneletLayer.end()); + LaneletSubmapConstPtr laneletSubmap = utils::createConstSubmap(lls, {}); + + matplot::hold(matplot::on); + matplot::xlim({-2, 16}); + matplot::ylim({-2, 13}); + matplot::gcf()->size(1000, 1000); + + for (const auto& ll : lls) { + BasicLineString2d leftBound = ll.leftBound2d().basicLineString(); + BasicLineString2d rightBound = ll.rightBound2d().basicLineString(); + BasicLineString2d centerline = ll.centerline2d().basicLineString(); + std::vector xl; + std::vector yl; + for (const auto& pt : leftBound) { + xl.push_back(pt.x()); + yl.push_back(pt.y()); + } + std::vector xr; + std::vector yr; + for (const auto& pt : rightBound) { + xr.push_back(pt.x()); + yr.push_back(pt.y()); + } + std::vector xc; + std::vector yc; + for (const auto& pt : centerline) { + xc.push_back(pt.x()); + yc.push_back(pt.y()); + } + matplot::plot(xl, yl, "b")->line_width(3); + matplot::plot(xr, yr, "r")->line_width(3); + matplot::plot(xc, yc, "--gs")->line_width(3).marker_color("g"); + } + + // LaneData laneData = LaneData::build(laneletSubmap, laneletMapGraph); + // bool valid = laneData.processAll(bbox, ParametrizationType::LineString, 10); + // + // std::vector compoundRoadBorders = getPointsMatrixList(laneData.compoundRoadBorders(), true); + // std::vector compoundLaneDividers = getPointsMatrixList(laneData.compoundLaneDividers(), true); + // std::vector compoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); + // + // for (const auto& mat : compoundRoadBorders) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "r")->line_width(3); + // } + // for (const auto& mat : compoundLaneDividers) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "y")->line_width(3); + // } + // for (const auto& mat : compoundCenterlines) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "--g")->line_width(3); + // } + matplot::save("map_data.png"); +} \ No newline at end of file From f314d4b64adb1cdc992bcca975722e2d1b0ada48 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Mon, 11 Dec 2023 20:32:09 +0100 Subject: [PATCH 27/64] fix MapData --- lanelet2_core/src/Lanelet.cpp | 1 + .../include/lanelet2_map_learning/MapData.h | 21 +- .../include/lanelet2_map_learning/Types.h | 10 +- .../include/lanelet2_map_learning/Utils.h | 21 +- lanelet2_map_learning/src/MapData.cpp | 168 +++++++------- lanelet2_map_learning/src/MapFeatures.cpp | 4 +- lanelet2_map_learning/src/Utils.cpp | 1 + lanelet2_map_learning/test/LaneletTestMap.xml | 214 ++++++++---------- lanelet2_map_learning/test/map_data.png | Bin 33862 -> 0 bytes lanelet2_map_learning/test/test_map.h | 120 +++++----- lanelet2_map_learning/test/test_map_data.cpp | 162 ++++++++----- lanelet2_map_learning/test/test_utils.cpp | 4 +- 12 files changed, 380 insertions(+), 346 deletions(-) delete mode 100644 lanelet2_map_learning/test/map_data.png diff --git a/lanelet2_core/src/Lanelet.cpp b/lanelet2_core/src/Lanelet.cpp index 58753ae0..0c8c71c8 100644 --- a/lanelet2_core/src/Lanelet.cpp +++ b/lanelet2_core/src/Lanelet.cpp @@ -210,6 +210,7 @@ std::shared_ptr calculateCenterline(const ConstLineString2d& // Determine right candidate std::tie(rightCandidate, rightCandidateDistance) = findClosestNonintersectingPoint(std::next(rightCurrent), leftCurrent, bounds, centerlinePoints.back(), false); + // Choose the better one if (leftCandidateDistance && (!rightCandidateDistance || leftCandidateDistance <= rightCandidateDistance)) { assert(leftCandidate != leftBound.end()); diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index 7561b7b4..38d657a0 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -21,13 +21,17 @@ struct Edge { Id el1_; Id el2_; }; -struct LaneEdge : Edge { - LaneEdge(Id el1, Id el2, lanelet::routing::RelationType type) : Edge(el1, el2), type_{type} {} - lanelet::routing::RelationType type_; + +namespace internal { +struct CompoundElsList { + CompoundElsList(const Id& start, LineStringType type) : ids{start}, type{type} {} + CompoundElsList(const std::vector& ids, LineStringType type) : ids{ids}, type{type} {} + std::vector ids; + LineStringType type; }; +} // namespace internal using Edges = std::vector; -using LaneEdges = std::vector; class LaneData { public: @@ -43,18 +47,19 @@ class LaneData { const CompoundLaneLineStringFeatureList& compoundCenterlines() { return compoundCenterlines_; } const LaneletFeatures& laneletFeatures() { return laneletFeatures_; } - const LaneEdges& edgeList() { return edgeList_; } + const Edges& edgeList() { return edgeList_; } private: void initLeftBoundaries(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); void initRightBoundaries(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); + void initLaneletFeatures(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); void initCompoundFeatures(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); LineStringType getLineStringTypeFromId(Id id); LaneLineStringFeature getLineStringFeatFromId(Id id); - std::vector> computeCompoundLeftBorders(const ConstLanelets& path); - std::vector> computeCompoundRightBorders(const ConstLanelets& path); + std::vector computeCompoundLeftBorders(const ConstLanelets& path); + std::vector computeCompoundRightBorders(const ConstLanelets& path); CompoundLaneLineStringFeature computeCompoundCenterline(const ConstLanelets& path); LaneLineStringFeatures roadBorders_; // auxilliary features @@ -65,7 +70,7 @@ class LaneData { CompoundLaneLineStringFeatureList compoundCenterlines_; LaneletFeatures laneletFeatures_; // node features - LaneEdges edgeList_; // edge list for centerlines + Edges edgeList_; // edge list for centerlines }; class TEData { diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h index 1e2f40ab..40446f0b 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h @@ -13,7 +13,15 @@ namespace map_learning { enum class LaneletRepresentationType { Centerline, Boundaries }; enum class ParametrizationType { Bezier, BezierEndpointFixed, LineString }; -enum class LineStringType { RoadBorder, Dashed, Solid, Virtual, Centerline, Unknown }; +enum class LineStringType { + RoadBorder, + Dashed, + Solid, + Mixed, + Virtual, + Centerline, + Unknown +}; // Mixed == DashedSolid or SolidDashed enum class TEType { TrafficLight, TrafficSign, Unknown }; using OrientedRect = boost::geometry::model::polygon; diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h index 3e558f83..a64db250 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h @@ -20,7 +20,20 @@ boost::geometry::model::polygon getRotatedRect(const BasicPoint2d& LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPoint2d& center, double extentLongitudinal, double extentLateral); -inline LineStringType bdSubtypeToEnum(ConstLineString3d lString) { +inline bool isRoadBorder(const ConstLineString3d& lstring) { + Attribute type = lstring.attribute(AttributeName::Type); + return type == AttributeValueString::RoadBorder || type == AttributeValueString::Curbstone || + type == AttributeValueString::Fence; +} + +inline LineStringType bdTypeToEnum(ConstLineString3d lString) { + Attribute type = lString.attribute(AttributeName::Type); + if (type == AttributeValueString::RoadBorder || type == AttributeValueString::Curbstone || + type == AttributeValueString::Fence) { + return LineStringType::RoadBorder; + } else if (type == AttributeValueString::Virtual) { + return LineStringType::Virtual; + } std::string subtype = lString.attribute(AttributeName::Subtype).value(); if (subtype == AttributeValueString::Dashed) return LineStringType::Dashed; @@ -29,11 +42,9 @@ inline LineStringType bdSubtypeToEnum(ConstLineString3d lString) { else if (subtype == AttributeValueString::SolidSolid) return LineStringType::Solid; else if (subtype == AttributeValueString::SolidDashed) - return LineStringType::Solid; + return LineStringType::Mixed; else if (subtype == AttributeValueString::DashedSolid) - return LineStringType::Solid; - else if (subtype == AttributeValueString::Virtual) - return LineStringType::Virtual; + return LineStringType::Mixed; else { throw std::runtime_error("Unexpected Line String Subtype!"); return LineStringType::Unknown; diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 198ed6ce..d24864d1 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -5,17 +5,17 @@ namespace lanelet { namespace map_learning { -bool isRoadBorder(const ConstLineString3d& lstring) { - Attribute type = lstring.attribute(AttributeName::Type); - return type == AttributeValueString::RoadBorder || type == AttributeValueString::Curbstone || - type == AttributeValueString::Fence; -} +using namespace internal; LaneData LaneData::build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { LaneData data; - + std::cerr << "Building Lane Data" << std::endl; data.initLeftBoundaries(localSubmap, localSubmapGraph); + std::cerr << "Left Boundaries Initialized" << std::endl; data.initRightBoundaries(localSubmap, localSubmapGraph); + std::cerr << "Right Boundaries Initialized" << std::endl; + data.initLaneletFeatures(localSubmap, localSubmapGraph); + std::cerr << "Lanelet Features Initialized" << std::endl; data.initCompoundFeatures(localSubmap, localSubmapGraph); return data; @@ -27,25 +27,18 @@ void LaneData::initLeftBoundaries(LaneletSubmapConstPtr& localSubmap, if (isRoadBorder(ll.leftBound3d())) { roadBorders_.insert( {ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), - LineStringType::RoadBorder)}); + bdTypeToEnum(ll.leftBound3d()))}); + } else { + laneDividers_.insert( + {ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), + bdTypeToEnum(ll.leftBound3d()))}); } Optional leftLL = localSubmapGraph->left(ll); Optional adjLeftLL = localSubmapGraph->adjacentLeft(ll); + if (leftLL) { - laneDividers_.insert( - {ll.leftBound3d().id(), - LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), LineStringType::Dashed)}); - edgeList_.push_back(LaneEdge(ll.id(), leftLL->id(), routing::RelationType::Left)); - } else if (adjLeftLL) { - laneDividers_.insert( - {ll.leftBound3d().id(), - LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), LineStringType::Solid)}); - edgeList_.push_back(LaneEdge(ll.id(), leftLL->id(), routing::RelationType::AdjacentLeft)); - } else if (ll.leftBound3d().attribute(AttributeName::Type) == AttributeValueString::Virtual) { - laneDividers_.insert( - {ll.leftBound3d().id(), - LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), LineStringType::Virtual)}); + edgeList_.push_back(Edge(ll.id(), leftLL->id())); } } } @@ -56,41 +49,43 @@ void LaneData::initRightBoundaries(LaneletSubmapConstPtr& localSubmap, if (isRoadBorder(ll.rightBound3d())) { roadBorders_.insert( {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), - LineStringType::RoadBorder)}); + bdTypeToEnum(ll.rightBound3d()))}); + } else { + laneDividers_.insert( + {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), + bdTypeToEnum(ll.rightBound3d()))}); } - Optional rightLL = localSubmapGraph->right(ll); Optional adjRightLL = localSubmapGraph->adjacentRight(ll); if (rightLL) { - laneDividers_.insert( - {ll.rightBound3d().id(), - LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), LineStringType::Dashed)}); - edgeList_.push_back(LaneEdge(ll.id(), rightLL->id(), routing::RelationType::Right)); - } else if (adjRightLL) { - laneDividers_.insert( - {ll.rightBound3d().id(), - LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), LineStringType::Solid)}); - edgeList_.push_back(LaneEdge(ll.id(), adjRightLL->id(), routing::RelationType::AdjacentRight)); - } else if (ll.rightBound3d().attribute(AttributeName::Type) == AttributeValueString::Virtual) { - laneDividers_.insert( - {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), - LineStringType::Virtual)}); + edgeList_.push_back(Edge(ll.id(), rightLL->id())); } } } +void LaneData::initLaneletFeatures(LaneletSubmapConstPtr& localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { + for (const auto& ll : localSubmap->laneletLayer) { + const LaneLineStringFeature& leftBoundary = getLineStringFeatFromId(ll.leftBound().id()); + const LaneLineStringFeature& rightBoundary = getLineStringFeatFromId(ll.rightBound().id()); + LaneLineStringFeature centerline{ll.centerline3d().basicLineString(), ll.centerline3d().id(), + LineStringType::Centerline}; + laneletFeatures_.insert({ll.id(), LaneletFeature(leftBoundary, rightBoundary, centerline, ll.id())}); + } +} + // Idea: algorithm for paths that starts with LLs with no previous, splits on junctions and terminates on LLs with // no successors void getPaths(lanelet::routing::RoutingGraphConstPtr localSubmapGraph, std::vector& paths, ConstLanelet start, ConstLanelets initPath = ConstLanelets()) { initPath.push_back(start); - ConstLanelets successorLLs = localSubmapGraph->following(start, true); + ConstLanelets successorLLs = localSubmapGraph->following(start, false); while (!successorLLs.empty()) { - initPath.push_back(successorLLs.front()); for (size_t i = 1; i != successorLLs.size(); i++) { getPaths(localSubmapGraph, paths, successorLLs[i], initPath); } - successorLLs = localSubmapGraph->following(successorLLs.front(), true); + initPath.push_back(successorLLs.front()); + successorLLs = localSubmapGraph->following(successorLLs.front(), false); } paths.push_back(initPath); } @@ -125,37 +120,39 @@ LaneLineStringFeature LaneData::getLineStringFeatFromId(Id id) { return lstringFeat; } -std::vector> LaneData::computeCompoundLeftBorders(const ConstLanelets& path) { - std::vector> compoundBorders; +std::vector LaneData::computeCompoundLeftBorders(const ConstLanelets& path) { + std::vector compoundBorders; ConstLanelet start = path.front(); LineStringType currType = getLineStringTypeFromId(start.leftBound3d().id()); - compoundBorders.push_back(std::vector{start.leftBound3d().id()}); + compoundBorders.push_back(CompoundElsList{start.leftBound3d().id(), currType}); for (size_t i = 1; i != path.size(); i++) { LineStringType newType = getLineStringTypeFromId(path[i].leftBound3d().id()); if (currType == newType) { - compoundBorders.back().push_back(path[i].leftBound3d().id()); + compoundBorders.back().ids.push_back(path[i].leftBound3d().id()); } else { - compoundBorders.push_back(std::vector{start.leftBound3d().id()}); + compoundBorders.push_back(CompoundElsList{path[i].leftBound3d().id(), newType}); + currType = newType; } } return compoundBorders; } -std::vector> LaneData::computeCompoundRightBorders(const ConstLanelets& path) { - std::vector> compoundBorders; +std::vector LaneData::computeCompoundRightBorders(const ConstLanelets& path) { + std::vector compoundBorders; ConstLanelet start = path.front(); LineStringType currType = getLineStringTypeFromId(start.rightBound3d().id()); - compoundBorders.push_back(std::vector{start.rightBound3d().id()}); + compoundBorders.push_back(CompoundElsList{start.rightBound3d().id(), currType}); for (size_t i = 1; i != path.size(); i++) { LineStringType newType = getLineStringTypeFromId(path[i].rightBound3d().id()); if (currType == newType) { - compoundBorders.back().push_back(path[i].rightBound3d().id()); + compoundBorders.back().ids.push_back(path[i].rightBound3d().id()); } else { - compoundBorders.push_back(std::vector{start.rightBound3d().id()}); + compoundBorders.push_back(CompoundElsList{path[i].rightBound3d().id(), newType}); + currType = newType; } } return compoundBorders; @@ -170,9 +167,9 @@ CompoundLaneLineStringFeature LaneData::computeCompoundCenterline(const ConstLan return CompoundLaneLineStringFeature(compoundCenterlines, LineStringType::Centerline); } -std::map::const_iterator findFirstOccElement(const std::vector& els, +std::map::const_iterator findFirstOccElement(const CompoundElsList& elsList, const std::map& searchMap) { - for (const auto& el : els) { + for (const auto& el : elsList.ids) { std::map::const_iterator it = searchMap.find(el); if (it != searchMap.end()) { return it; @@ -181,67 +178,78 @@ std::map::const_iterator findFirstOccElement(const std::vector& return searchMap.end(); } -void insertAndCheckNewCompoundFeatures(std::vector>& compFeats, - const std::vector>& newCompFeats, +void insertAndCheckNewCompoundFeatures(std::vector& compFeats, + const std::vector& newCompFeats, std::map& elInsertIdx) { for (const auto& compEl : newCompFeats) { std::map::const_iterator firstOccIt = findFirstOccElement(compEl, elInsertIdx); if (firstOccIt == elInsertIdx.end()) { compFeats.push_back(compEl); - for (const Id& el : compEl) { + for (const Id& el : compEl.ids) { elInsertIdx[el] = compFeats.size() - 1; } - } else if (compFeats[firstOccIt->second].size() < compEl.size()) { + } else if ((compFeats[firstOccIt->second].ids.size() < compEl.ids.size()) && + compFeats[firstOccIt->second].type == compEl.type) { compFeats[firstOccIt->second] = compEl; - for (const Id& el : compEl) { + for (const Id& el : compEl.ids) { elInsertIdx[el] = firstOccIt->second; } + } else { + compFeats.push_back(compEl); + for (const Id& el : compEl.ids) { + elInsertIdx[el] = compFeats.size() - 1; + } } } } void LaneData::initCompoundFeatures(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { - std::vector> compoundedBordersAndDividers; + std::vector compoundedBordersAndDividers; std::map elInsertIdx; - for (const auto& bd : roadBorders_) { - std::map::iterator it = elInsertIdx.find(bd.first); - if (it == elInsertIdx.end()) { - continue; - } - Optional prevEl; - Optional succEl; - for (const auto& bdComp : roadBorders_) { - if (bd.first == bdComp.first) { - continue; - } - if (bd.second.rawFeature().back() == bdComp.second.rawFeature().front()) { - succEl = bdComp.first; - } - if (bd.second.rawFeature().front() == bdComp.second.rawFeature().back()) { - prevEl = bdComp.first; - } - } - } std::vector paths; for (const auto& ll : localSubmap->laneletLayer) { - ConstLanelets previousLLs = localSubmapGraph->previous(ll, true); - ConstLanelets successorLLs = localSubmapGraph->following(ll, true); + ConstLanelets previousLLs = localSubmapGraph->previous(ll, false); + ConstLanelets successorLLs = localSubmapGraph->following(ll, false); if (previousLLs.empty() && !successorLLs.empty()) { getPaths(localSubmapGraph, paths, ll); } } + for (const auto& path : paths) { - std::vector> compoundedLeft = computeCompoundLeftBorders(path); + std::vector compoundedLeft = computeCompoundLeftBorders(path); insertAndCheckNewCompoundFeatures(compoundedBordersAndDividers, compoundedLeft, elInsertIdx); - std::vector> compoundedRight = computeCompoundRightBorders(path); + std::vector compoundedRight = computeCompoundRightBorders(path); insertAndCheckNewCompoundFeatures(compoundedBordersAndDividers, compoundedRight, elInsertIdx); compoundCenterlines_.push_back(computeCompoundCenterline(path)); + + std::cerr << "---------------------" << std::endl; + std::cerr << "Path Lanelets: " << std::endl; + for (const auto& ll : path) { + std::cerr << ll.id() << " "; + } + std::cerr << std::endl; + + for (const auto& cpd : compoundedLeft) { + std::cerr << "compoundedLeft : " << std::endl; + for (const auto& id : cpd.ids) { + std::cerr << id << " "; + } + std::cerr << std::endl; + } + + for (const auto& cpd : compoundedRight) { + std::cerr << "compoundedRight : " << std::endl; + for (const auto& id : cpd.ids) { + std::cerr << id << " "; + } + std::cerr << std::endl; + } } for (const auto& compFeat : compoundedBordersAndDividers) { LaneLineStringFeatureList toBeCompounded; - for (const auto& el : compFeat) { + for (const auto& el : compFeat.ids) { LaneLineStringFeature cmpdFeat = getLineStringFeatFromId(el); toBeCompounded.push_back(cmpdFeat); } diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index 5a1bca6b..18b9380e 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -133,9 +133,9 @@ LaneletFeature::LaneletFeature(const LaneLineStringFeature& leftBoundary, const LaneletFeature::LaneletFeature(const ConstLanelet& ll) : leftBoundary_{LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), - bdSubtypeToEnum(ll.leftBound3d()))}, + bdTypeToEnum(ll.leftBound3d()))}, rightBoundary_{LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), - bdSubtypeToEnum(ll.rightBound3d()))}, + bdTypeToEnum(ll.rightBound3d()))}, centerline_{LaneLineStringFeature(ll.centerline3d().basicLineString(), ll.centerline3d().id(), LineStringType::Centerline)} {} diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index e4b69a71..7f32aa8c 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -38,6 +38,7 @@ LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPo return utils::createConstSubmap(initRegion, {}); } +/// TODO: FURTHER INVESTIGATE THE WIERD BEHAVIOR OF BOOST LINE_INTERPOLATE BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t nPoints) { double length = boost::geometry::length(polyline, boost::geometry::strategy::distance::pythagoras()); double dist = length / static_cast(nPoints - 1); // to get all points of line diff --git a/lanelet2_map_learning/test/LaneletTestMap.xml b/lanelet2_map_learning/test/LaneletTestMap.xml index 38aed00b..7fd9fb07 100644 --- a/lanelet2_map_learning/test/LaneletTestMap.xml +++ b/lanelet2_map_learning/test/LaneletTestMap.xml @@ -1,27 +1,26 @@ - - + - + - - + + - - + + - + - + @@ -54,15 +53,9 @@ - - - - - - @@ -114,15 +107,9 @@ - - - - - - @@ -132,22 +119,16 @@ - - - - - - - + - + @@ -236,9 +217,6 @@ - - - @@ -248,24 +226,15 @@ - - - - - - - - - @@ -284,30 +253,12 @@ - - - - - - - - - - - - - - - - - - @@ -320,18 +271,6 @@ - - - - - - - - - - - - @@ -350,24 +289,6 @@ - - - - - - - - - - - - - - - - - - @@ -407,89 +328,134 @@ - + - + - - - - + - + - + - + - + - + - - - - + - - - - + - + - + - + - + - + - + - + - + - + - + - - + + - - + + + + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lanelet2_map_learning/test/map_data.png b/lanelet2_map_learning/test/map_data.png deleted file mode 100644 index c5ed3de6c34c11c96c0e32b1e1b9d92803ce175c..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 33862 zcmeFZcUV*3x-J?-ML|TQ2?!`sML?vNfG7x|ccddomm<9vk=~^DUP2GjJ1EknCxqU6 zfY3YSPW-L4&t7+}z0bM#Ip?2yS9o|vAY+a(<~PRoe(zUif)wN=@Np?{K_C!5L{dx% z1j1gs{=IVp_|MNDZ!3W2wt=*S7zlIylhFi?27#V{AY!5_&MBMIE-8Hu7|iY}xgqa| zz{;UP@!ni*#eBBtN|=A7!q+6ycsxDsm^V_srkO^!$J_N|O4VAvvMDNRn~540eEQH~ zm9Ttq#Ybbp&|AEka=dCueLOj3HH&obUv-~>q2ODS`|c~3o?b$_Fm+%Zpefeh_Sn~N zaNpVB0L_cn|8D+&DS`)9^;nWhlhu|ZBO|g-SRj!8*g|`VY|WhRO(%6pdHGd6We`ZA zM_fW8+3E)(;raEUdG9@U|KH939uZJWwp^W|8eR7HsK@IaEC$lR$=nvCR8*H8l-ABo zy2?$!mb9ARJofJH>2bY0M7ChA&Ko7_?bnqW9JdIBPxP0%B1fR|>D(4^Kki>|ANv*}5N0X=fs}Ya+_hPJP8=TPJp1yT=u>bQ?Xt)Iec{|+M7LAXL^Bj+@3JQ$WP z(W((%=0_B&?&CNC!_R%Zc|3QvRWnQNw)wrQtIK771p(8WwZERajN}J6u)Mt7^=Nav zP_@Vh8^_=8dkXLYs9nN_Ad9{Egcb~LGVBcZy4VdsOZwrGm?H{}s#$;zV}WzjV!#?F zTf8v9;M*d@oz)nUuOgUBSTBL91(;B-g)1;v;FVp ze^~@SWqCoM+<1w!k*1|s-k8*cpNt@oUnkY|-nQPj`5(>80Tvj#^AqLy5jo|=v4|J! zhyvBxIVS-E-TDwN2$FpEj!)dZF>#kWYcBc=1EYk)1`}9EL!+mgW27y&-ae%IBU9V& z9e)1$3tP+atk8Q#CdunqAl!vrSB*e3YxHu$iZMuAl%{)oj(Ip;!8J07GnXab zIwk>nLhw-W>R*FcdjUv4)Qh6VtFUG_%;-(1JYX9m1pm7K+Qs{z6>;Vsdp!U4-rw(1 zK>CDV1VIV$g(@SgkK!_Jj%eSkXew!hNX>qZA)hZB0HFNr%ODCl&%0WHIQy^)iIQ`ihFET}`I z9ss%Med@=2HW@hp%60zwb+9(H*y@ArUk_MA@98_+J{(q4wW!jD?GbsAJ~tb{no7sL zAm=h2JY7wn&6SQnJ4CU+FKbl^xIP&a4LvvCb%s-+o6omvW?Wg&KkzAe;04N1wGz!{ zy<{%4a?eXwqG>Ff1}5=Ds~=^~?a5YHpsDaV>POrbqcOyUgr84#K45<6G&rgjsr6wj z4mS|UQXPv7eo*N@o=Cze-!9alt58~_1c07$E-N!lZUOhm7mBj~ynLr9fc3n13qU9Z zxB2E|%>d>a@%x3(H{%}IMjs^G=b9nwxwdEuLz>DV&fT+SKxL-{^yuMr< zxZG|4Tw~N5XV?+SZ@)GeK;v2aq0PSm-8^XzkOxrN!yn+@632Kk?X$bsDN2w8m1I$4w%M=G5UYdBCqGBqNC%{vk0N# z3{l@(c%%m#2vnj;nnsgr)p{PFruy_>+#XoYHhZl0r$z|vb@`F$IwN(QF1M_`v?`3l z{zsowx9vKB1E+Erb^zcS|3ahOY$PvFHaW@aIu4*_5bpNC&(AN68Z5xWm6cUD>tU=FS z9n~aTH=izX7O57k5L^?J*6xJoL}DRi?82CfeKdf#J+UmjX2UrEzh`D<&PxUY58ko0 zi_~I?iHQKcztXFysNm$}#6QArty&;nWqBBDT1})(OR~-~9AjKf1hQe>uEkLVI25eq zO2ArNe=J-Or`(PsY+P9p8?-@bvPOi4pj-G$kXK0GLd**?r-j%3wtBt zDi_`ptL#{9Yx_o|_L-F8y3{P}a=eGIlcJ;ZmL3 zT4T974Ihhr%3e|l8k^g6zO&@wZ|uwtdq)ZKexv^{sWdLJ9H3AO98A2RK7(W1jg2i<)`vxEu&nDh_nsS2K%UCk3wj zznyj&fT<^_>m7QC7b^8R-uSC?LX9HjpK1QydFomjCi27njBq{oKB;-;e>ogk(fdE^ z0+H9uG9K=NL zg(`pNT%Z+8_`83&eLzmFjXdUte@6Jj%KS7<|I6XppO5}o7vKweyL3zO{0qAE5^$k@ zTNLIs8|11bZQTUTX#$;JTF0s-0u1ulEGR-7S`g2bwfk7sr48U`M z6zjUK=OVT3{)YTCfORnX_>qp0aUg*sTh#Y9J_Xm>2y_4dZW-Qdh+_e$w<*BONuH~i z!t*21$TYB9(iRR#n?>#W-z@$gm@ia^34aS(Nia*PB7D4pFh6 z{#YwuGAceB{WgDm%ZW;jO4Fgy5-l3flX)N%lV^X z(}g|gfZJIEJR4o$VrE}A0HH7bc&U8kKwu>+E9(^}Ctw;pMht0bX`ibVo7OXftl%oL z9_>;kT!2^FTIFnO{s0&w-}CaUEr4LQ$?X_0HBi}P#{uwhf)%beWy$dK*@*RbAkcO@ z)$yF`Vi+$V6Z=Wa8daF?We{Up?P9o~jML4(iH7CXtGzznS+2j#!5`K7uQg&p1-Pp9 z#=kY<|BOalE7;#p{y#&8x6Ain?mwCe92kAw@~D46slSDFt}ok9DlCA#FNif3#QIwl zLn~MH`l$bhOLYyWp}Spn_0M!&-p+($F?!wb%v}PI=qGSCfGz�Ef4d^@@yYVNSgR zJu~NsBwnn_`Wqe7kEW1WFPQDJO6C_K>a7{eynx`su{W$eMU9E`HI~Y4{^m2<%e|;x z_PzrksPwcJn`wHqc&Lp4we!JpoECy^vCau;6SzqB$y=C%5)yQ|l6^a|)%7BZ>RAOrjud7f}rZ12piDiBUN%vTJ z%*_4p;ngdVwN|XAW8FZ1$6o&kJTd{y;DCVi{CrfUd)?U~=Oi&GC3Nn7MkNK8B#qY# z2z5lB%n)mhWE8TfJ36d-SyIi`^tS81ER;#TpV|LDeqR?w>HKNqS@_IdMViGV+hRyX2A-AhWT3L=|* zsa+V%l!&;4RF;JL1hq_Xnj)NLhK91E>@N@N50oHDToBnLNu_uGz!sFwdAR_`>-q>7 z-G8hg6MD22o#T4QWtYd<02b1ouIFDpS(W>${gt*EQA2>5|02)&J>@a0<`SGr@c9dw z5-q`&=5wn%9qJMe7gBn;Ou5B9X9Q|0?fgjE3@=-T9@z@OT18f69{x3HHV(5cZbM#!4rc@Wo4FFqhh*W}(B+A7_PLk!-l_ zJ0WBim#&aPP1UW5WonqZJh~a>o~>0WIUqz7Yz)eHaxMDq5O(t!A!r((V!Yl}ygGZm zB<~CpgrDDQoaUvC?jYVYeCyqGt+Jr+=Sy00!BJ)kUbViYDErgpe*33>HbI4uE0GeR zlZ@x8XT8$A(=mb{OyNxQ0j4D{K}){Ct-;^dD!ldQbC}@{bU9s>mPyFiyx0yQYwJ97 z4)mAQ%%Wd3`ot4H;E!MDrSgdH_m$QT*nWd22K*0Eog8OX}>!ljGFR4 zc12_djF#f-Sc7byU$4GxrLP|2$rv(g^LD$`i?Mc<8c|3O@w&(y5XM_NnYi$#?otfT z88rvL#>U0WSWb-UOcQMAgU(M|Fm^zSqQ%{h{AKHS&lCzV_XF#(g4bFE>x9x~4JM!F z*xVX)J_IiS6hCLB&tW+h%J*Y&C_&k$VGJjBlmrwKQv1MdcJ1dkcwk>#P@5O{llz)+7~V z+`cLdsTay*bUy^)pNOT$H0G-7eh!vq2hK2KvnxWi4kz8CdMSI|vI(r%zqO@Ep(^1J zd~nq3;L^C@ak%d)EY6Iz){R+>VbdWdRBY`&o+*N+*=}UMjeQshNNEk&uYS|sIJ4H> z@udinQfe^nVQHhZinI`SR(+vY39O-DlE^hHxt?SZ7q%AbwUHiW8#AJj%4=>2IU6q* zPv+r29Fb2ydyK)^IYbJ$*V1L1`KcW!(&$ZVq`DQTbFl4Y={CyNK=yhjWv4wy2k1A< zW`+AwQSj5>(cKm&Hq)*}bS_OBqp~9L#eir?uSJ7Ix3DmDj2AV!*o844R7g%%+2xIV znus|M&3c%hfg8=WuG(b7DCG6`S^&+)nt0XmkM!x#eY#v`)^{8Msux_-H85qB(qg$( zIgkE|0f8crt?j|Zemk|4U{yua1PwwFqP`YPM^Azl_<)<7Nq(rRH;hHK)0e{RsWpX z?k*17n{XekEX^oJ7KGYXU6PjWGCjANVT4e7GrW^VP<*l)3WjR)SDDVk(T&QRnK4;0 zB@@pJ{LLGo$IlCte60CXHwuN-?L5@SGVs%jZdRZ#dzCJe^+?}+%u!NG*_!b>6`!u} zrYx)a2~v0jD7;C&Zqq>@ZSj~_qmQPigrO;rP zEh=H{lh4{N)6=IdoDyNS!8#2G2=#VK*@*S;PKQ=sY4a>x!3dTW)$j%-^>E}#VPR5M zOtruzGW^2hvPumWm(vsLfVmWRLzAd8t)I}~uM7-$48H(G*JV^z;CcGB@XSL$N7R(l0 zr+zJnG1!F)Q{MjT7p-jYiIg5V+vG;wqOhU>n6;>V1vE8Z)Ux9nb9Uk+2s5VDu2fCXvHS3MvRHw}H;_0etUMQWB z396mUckKPz?}SWj9J+P17m^!EyiaS?Yku@8)vK8e2Z{r8=gypHSh*PI$c-BL?PXUb9-EfF4)y((vqssV2YWkz9 z{^%*VQcN0`?{KRom6m5T@uupV2;sssaB97L`p*IA4etej*6=+m)`)g*J@)h}W+@PJPDwmYgHF+eGTJKUK43rXb{OTxiEz`GrQ* z>_Y_2;i9kWAPbs$%jK;#v1RB&kJ^z9{w&lZv1c*$LRi)h}X?4>^^r(_=e;}Ce;?a|2Sq;IRQNBGHRCnuKsAk+d-kZaiw3i zL^G<=4mwf8K6E7r`o<2Vk$RVx@}4cv;K})(B81 zlgk$c!G$?Yxk-g2E(r^T*#dT4(3Bnsz@I(ftMJujoj8hCv_lkI?du9-W+Ck;wPJdE=!5|LGh@)g;2#C z$c+g?+q|8Yk{U+SpFt)UJHBeg-*Rl1;knXtg0=JhQ`|SLn+t+&!ANd<8vuE;vhj1Y z#gZh{bDO{Y*G;3CJlRyYeu?m<{%FT&lLHyIs(84#j%Lx}UD$o{1!6LtoU_9ayGt!= zRYakj$6j>uky$!y4R%EZ&*a07c~5h=o+B6alek*KP0oGXtD#9pFxA6RL0}CjPawPo zZ*pCqwDx_4>pdJccfVB=HM;%-C$(;pcedCh9e?5dRo`3fy7W4<+OdjkYMvR1DhFEH z%nabrP_}Nm@N=|Nuy{yJ>|uPo?huPI_aWQ#VH9pt-^T}`$P-g>j07e~{X$Qd&z80| zIG#G*nJI@yBZ{C-v#N=&qAE=zZ{NSSZ3B`b2M#+PK4qTEK3(_X(669+A-bZcmlZFJ z`zXF+5{Yv|2LlD|CF08WJdz>I%>6WXbEc|5?MDkCa=Vk(XUp+Xx~yV=*IRA`nBY&_ z;Rm7)BeE1iV+DGi8Wko11`42D>>pI#mZWU({Q*G-%+Tr>tq*4@T%W9gN3REueHWioT z)&##p1hr-ZkJ#c3z7fDCzBk%8p7v@fub?k*u)wFG9v*6NI=-AuTWpH5f`_rM`F-P2 zAV!TiU0N@doL#(Aoklh|>6fmjvmJD=YVh;>)EPgZ5{v^|mMh~pXl&dSqLxj{k!Fw0 zc^hDl)afsF0x?|+%Iz_KR>C7jY#rwh+sf8&mIr;eUOo4NnAa;gxvQJR8K0)PFTF|Z zX%yJktkTW#NBztbf}hX&j0fnC7Kal^K6PJ`cCKT*dl%28W|1KJnV z@bPv$T0_5CmW$U~4Jy}PBMV}>F5YmnN0*{o;&X6GxU6-{c$S2lW)Nw~l#5d)X+rb$ zNrmJKw{c;`J;uUdxccf8rbTNoRQu(pIar6INeC%6zTP#E;x7J z3%W1gs0TaRy*w*UyOgAf-AOX!c8p?JAFAm!;ja1`e=jeL(t=Hwg_%V=y1}g0 ze*iJ1aBMRG4i;%HxY!?Hp{IxAgZL_d$otq_2)k|i*y%5#tGbn^qp$HhuI&1nr3a#O zJkDcMKIMe{e8eRgb+DAb*PBhh%mfXo&nY|+J!kPi=v~qm>|%bh&nyY9ZO|8X#^;tc zW*?32FU8h0)MIkeaozR2n@+N<8f2pybsbQ7>0b98utELTo^9#zIl3M#y!ttnRnzqM zBi|+!#n{5>2oZ01i)d}{W1%$}W+k-~j0flW4V@>s`joL$V}bk$5w}LJ>IhfKSR%Lf{9UPpEM`0InzRi>X^B66YW%w} zT(M59RgCaZc0fTsw_Z7`%I>wj3c5T`eJgO;BmgZFvs{kkRX5pwTNJn4L5fl zmv}bU&F>E)TC{s93+F-jU84{|n;NT<5cas7Cb#Ko)vi__xzGLl^&5ryVZ&8L&(-YB zy#k#=b{A7+N35Gv9Z*Tz3=u=wh{9MpfC$)`SWcPNKLy1B^vNbSRS?nK6iywR^M@D* zcFyKmas=}iWj;k79#5qnqaLt8i&(Byn{Y zygjXi|GYzh+#DCcxI?6*m1ROFue7mSmjP#Vit#U^qy`BN{jD=bR=yiMu1Mu%8jzz| zcxZijPRs~c03gzplPi6w+{H^S6ejAMVpL$%v$hW17vz-IsNiBrSLck0#JmSWLB!{8qNEJoKNCEiSfK*`qiUe`N0HEX1HxRzMw8N)UZ3ih#p zSFl!vOmPMHF(ui?ZP5E`I5OIMm-zMOw5@btXL_Dlkgf4b=r9w;_P6rRz^??M@kE3n zd^mTaqO%}?U;zD_>~LZAOk2tNP&bo=vGJ^Q#zPh62}8q%xRWcH(@_U#G;zzt!e+DS z7>yTSh%lHMQw~?w%vF^|957xU?L-uIJ5k(w*YAMVx3X{T_mx1k(5A0mQ?QjH2MCzh`G=Ts<)c4O<6l;X-K ziaf5owC)6dUbH_%4C^uaV_k=jpYELl;pd5%*)SH6mqsZvKaxQ{4$UF*R7E2eQ-ski z?WHW*UC)bL!o{;2c!&tS2Y9Q>$7?{O1BmLRtsqonA%d09)V?=Ev+`KLKxn{TQt2&s zN)|IU$xg&v^<;{{Ok-3f%^iMzKfO3jHKXWQ_q6`#N(nauVmmR3FVk~+G3>D88Rb-g zVZC?*?ACR{skiex^KC#r?0uQO50Jkh>unI#75lm}fYdM{H_P#%c!As_IQ(Fw%675| zge|GrAR(k?ZT|YlaS@qsQ{DPLe4yaUpW8@*8_ZL4c3{7K4k%XA(v^dj++j4ga=ol1 zCyheu+w!8C`R%4*M~D%wt+yVJaz^D)6Bc&qQL)FL4X;Cy#~Ig!)Vx+x&TaU-30604 zzBdy1&GI`4Zp3puVfyg0ns1TSi+cUuCOt7U!39Pdr-+2)Oj zLucM5+v9m0?`Ri789~QQpi+4Th$8`YDS94_G$Aj9{7XIaV@$Bo`o>QLUf_LRE# z(k!>Vx!m88?666q_i`B-VWsT&K9aIlOysnud&B%Eljom(s~A9aK?+nXTfi?)I3TC+m#FkPcd?KryNVHD8*flp_5aNVeQc z$Ic*r22{CJnvDoi52QNHxBw;W#l^)y{WcZ9ZH_^EFk|{()rag3Kigs>GmBr0c(ig6 z{Qxt{=;WamClBC~_+FYHjGF~x;4i_+xi*jwJubQfUtiC90(C65~wdPdH;E#LN0^7g=-p1s^Ev~(X*x}W*-|@ z!%ygusIvt<)Y0VOZfDfNW`*n3DTw<$6T~5hD$3qrzf7u^z|YpSB}u6S@9-QQb?`d6 zYSx2nVqVaGFoiz~UIXh0y}LCAdLZyZ4nIpV`(26PcDUfS^Tlo_WXs#_#o!H0+H-l- z`omnuWsNL>4{w+-Un*Uz95-z?vOM+PXLAEl&!)DD7!ueCO^7qHQ5`lFceKr_$w*)B zR&$ZOMOy3Ykx4b^zKjJ5<$`D0nSaeHic;^Znxvf}N&w?FM3VwkA`x z&`;;Rsy`iW)UW5!Hh!+)sZmUhGY29cd(^Wf(={8Tkrd-K&{LVrrv%8ihVFw$1Om)_3Nnn8~R#R z(@>8nz<-wMI{*jdi~Ve~<2}SfD63SISK1dSJFP2y<)t2-cXL2nZC%ODR;iyivTu!U zTOXF>;7{yaNAo{0UIqAdw`^^Nl8c+><=|dc!rutw8Ct?uBra>kwFC;3tIM%%H9$=K zKx9>E73Xp%;Od(SBz6XQ&aKM%66taB!OsseEJ3qgGke3X&z+u*ufkR+EvqluMLPmE zL;+b=y~1ybJkEV*&ZQ9z-jh6wxZ;!%A0wS3=AQ#%tPED@!D&N0j)qUiT zP0qO6jJCzA+93xMee1pPx*M>pF#@b`phN+{>sfem7=lk9*|bJmWyJ~^A5VH1%t|=x z+RF@t3V`z5F{iMh1m;esPmiIW)lnNna;8Pqmxx()0>mJ3F7aMZ7^UEdpUT1A@sZDWKbs0GYC4np0m(2KHQS8RdmwrsUZ6msQyijhrmF71dIG229T8cb$kOvd%a(K#PT(U()^-&c0qT;CwSj12+ zn~a}r^vsMBqFw^+m1fuJmLk|_torSv2_TJUzR+I(epr;sUD}%DgcNj#2k?<@%GKGr z?ubI~uRKKjm5Mu_)%(f|&}`>+q}mpLQZzW+=&c$DC}V1}-zhP*H{kZ8TSQ05M)Z9I~{)R3G-GV@LLi;c+7$~Ln zz5bU#5Vjsg2PAYYC-VXW=k5_r4dAxkl@vP#lG)yzvy-mp*d7nEeq0^#Uhbzy#l(^{ zy7e1&3WzUuh|6t7eMu5a=2qUCkh`3+6Q%6H;Dq(3`o>R?EDZoo9N=Nh-K&x5buBsu z*PD3XCiYa{d)VC|#?83%%xdgg!{icY=~PkK>MlKuMQv?o{>i;E>vX_iZp{6T#sZPi z0&&4;OXSh}cH`$GEa-=<-73aR?<{9$>hPOCX9Lg5%*CUT1I)CvH>#XmhKNrtO$MJ_ zXQlc1Db&7(&EZ*|*{|Y>E_MKf`U&vhft8pIj!lO^VfOLH%+>^3LA5#H3+_Fq3HIrY zS6}Uyqxwo;c(rAYE3=i@ZSuFEXHHiS;oVqs(Qze>KOO6X9Bj-Ruq|pBitS-9m`(j80NwT$MqtZl&~}Ptb6vg zryWcZQk^UJ#U4&)mm z*<{jIyq3LSWo^gA3Zsj483dg7z&}S-^Du3O<#dol^?UQi`G)64jf(U>f1S(zlxl^% zh|b=R{T&S9;?;S`PU{#5qBN{o63eB4ErMKqN)2cdP~l=@EFiI(l3PIs&cJaki1)d_ z$EJus*wLU>ZMts^?H9p^jc#-vB*3hSs+$mkPw+!WYK_eqn6E?TABw71>}+eS^2aMuCw`T~=P72SO6ZK3i+lw>4d)FWgEHXP`_DO@?FJsRNPJNa%1?lOKdvSrJ3rXlevjo z)qu&3%d!u+c+okVpMW^bx82-CPKP;fy30#6kXC+8#`>lFxu6zMH&nXk*$;YfUC(J! z>~DTjI#ZG#Z0tWEh%NY9T1Dh)jM>0I&P}emT(BS-EobwURw-uTy!UN}b_SlYnOf~n z`jWw56Z;8#W)_t$kB7lK}~M$Mu{V)Z#K(32;?X?ehPMPF#Q`kb}o zE1BE5rO`*pTddN6)j6-yQ%d-0Y`#KYpYwGwZS(f@%Qw&HbTV0}k&CA(iQndk1szQh zg%1fXS}PJH8}}V-)%0`? z9;|QZWCg8UyI=T}8;v8@--rsjay11ceV-Q$r`A`{JzL36KLf(rr5l=%*ggIdRTfoP z>480j;^h;kXT1il=a$gGFx8JAyGvD-A~uH52Rb>l0T#}WoPL15e(j{ppjT!JT3cId z43+`c;$hdbDEn^uvvsI}-pcW;!@Qpy*rmLlv@Ehd{%25&bE6iAne9r_YniMSn8r`ug(K3{z9>tM3% z7mHNRpOx&7f#49oTK%LV#t4Y^s!PIf7bvO141Akll@a z(;Zb(5wUHw*ntwTURfpKw|hw9(6lZlwkhk^5I8ou>Ms0+kbLXMJu6YX3KzC4R|gb$ zb?Kw=Kyx_JM^e;sk7D`KX#|_`?!J&0g_GrH`_y4cltjy3jNZW6KD9YK@Y{nT@LW`p zgs$33B|PvBE=m1ND790{qM}&IVh7nEAz|H}>*X3wj*SM%$)&U!3h@Mr<`L<9Xh3X_tX@cXE!Hq)2`{zX=HhGhq3m|{O$}3bMl0VmyrK9h4gxzblN{A?Dkiw4GGjqU9q=IGAY*34YwUh z+-@k^b69JYAsGZlpJx(DW-Zk=B7x{fT6QN6`ZL=lAtel z!=IHtVCei71HItNGkQ_EN1}&sY%?nH_U&D~iV384tx0Me#ax3(wUnYcPX(u^#~C|j zOWtcFNQ|ywcUt{5Frm5B2%?~)pdUvY^~HzUWg0v75ZJpCI;=y??I-s5(Dv z*x-~)`-D$oK9d6EQ4!2$TuJvDD{nV% zIT+?AP|Z@pHwQW7a|SDJhd5{7xRcOc{3_<|vrqiJzIS@gOjm3&*_&(YnG2!voF4Uv zqj|Fe4SpLL6Ic4@ScmyeEHz1TbhP6ExK4zG*6Z*fAs3M6Sr9&Dsq;w@OcTw$`n%h1 zAa#GRWUBrP32e{&@n`<-^`J5&E6r*1QZJ&IO?G{s+Vxn+B6wqcM7^fD|KjxGlDBoM zW`E9iTW*8drJGWEQ~x&m2Z-lha`>~qX8Io8a?)%6mGS|FR7gedZ}~*q_BKPql6Tqf z$P%LyL$uq-bz`!u?ya#IF+~RmksYlL$hQZpxgD$(@hPrI5Df^uy&r9{qFp4V63j5+ z6wm*V&~4ZvIB=|^p;0R`IDNlv{R!_AyT;B?@=`M>84=CE?xNacr;~IOLt^%k)22SM zw3oWxj*&$TwA3XEm80&br!~NSJ|+H(`qd~>uxBW)^5P4-OlO_$AxaC)Ps6|9DsgEr zL=rtPB-yjAXj@h zNFRVU@qD~{FmjX>=c>rar%}E3Zj%Ben6PLd5ry9>_UpwoK;`+&QnD(aHpNR z=NDG4Pm<&1APD4FXZrD6hNxP$*xeLW9--a}a=mJ(2fM2adyZDqz&Uw~eQwl5(kOn~ z*)u0ARHm>88=vdm$KhcRwaeEi`ld%CzB9|yHa~srho1#ab;|)D^~>{*C@rK$(of7D zM$6!QrU_V7QdWteFS(OEnMWj+f9-OEJvy>>XMPiT59w&26iEo0Z8IeNZb2yGg)q4O&%<+ z@SwF&_0xFN_%tlN>ZXDIlS$XcviPRgo_hl(SF%$d848n$bJrfP+$6kt@q=cdv4J&P zgg;TEqWp1ph?up8O8ZasG&I1UR|V^;3ipb_Be6-}fO{di>+ z9R|-a?l6&<{|and-eO>2-!-gp(Ahoif)}I++$w%AsuYFbHMrT%+#D4f@*bYBSw?N( zzimI;>W`}e(sgOym@4!=GzxZp3ziK`ijNErj zkeeq-TiRY8r_>3H!`UJ9Dkd?vk`YtfoNPDSiJmHln(r*n&zf&HaMU0;#FzfHHm_UBhku zk$Yj`33$5dsWMMe+;iFmwrea=&xxAw=V=&oh{@JjvyI7C_Y9vl+GYdlC2FZ#Ok||{ zA{RF&2R!wcB#S;@@P*)9wWfRhtaDQd3hdJG$-@rB*v{3$-%dQ7lGk2wd*YvmSpnQV|UcU$$k?6utVcbubC-W7x~%8s1K!>YL(`S#QkO zPK#c`Do0hS5Jpnw05bTMFF~_hJ?WhVy)RPz3xb5`!H=?+o0KKTwJfObGyY)iG)zy@ zEMq=IDSyve*QTXymh@{#SpCJH@I%AxG2df8>gAyjYQ5+ra-5Hs*}hxOJK{8b-#)jt z3AqnRpcK(Rd&dvw9`iT9I2G&QXzKWeo>hYF1j9@mXMgb8?ZYmlq}aTbybex39Ur{% z@DQq)Bk4f%#H)@YpkfQEf*1f7~10c~p0gWZPioxtd3Z34ovB1beNo(`{$gx`$(Sb6I+jsU^seqCp~7)$dKTQj zIN)`rJf|MUNiAX@Snpsq{B^{$axgrwi~iau?M87JGO2~C7HNa4%y-cP?KPx2b>6o` zPjLZ@E6a=ZY4ROZ?ju@WOj*fvJnC%#D45h}rg1REZJ>vN-qm6c+zcpHV>kh8LwzgFh_kQG0KcH_SXLoex21IV9o3TL2&lg8l?2ry~ zs`n&53yoFhdW~g5sN8ray);&>Vs@(gw9g0apTQku$$(V5BBK-9{TG+_3q;7 z5>XM@>hbZk)BIC;p&u=n;Vi!*lM#ksGN_&ijEEGuU7jqB2=4Ho%E>R4!&U07~iAvI1U;E!*J_oz;s3kf0G2-0b8#jqz z!m>F5s7DIl{CMq*{T!%7Q^U}IyDlJzh~4Ximh`Yl&uvvi!tL1cvf6Iv38ILIFp>kN zD)vk78ht-k1QB(oV^T!fVp%Gj*x8%nhNI%B)&>CzQMW0;?1xuj^R&(m`{IIgKxlFN zm#*E)J+Fb#+B(9BnSbInmicqHs^ne$WLM`yf{%BGB-GrF zK6QldYJWnn&!k$n+`4m*jweoVyY^xpV1yhZ>~Uj}p^c08d9016_-t1y@AJ9F829Dt zBxg_5Ns3)~{}I|Fu0X+U{SgLr`Na-8dP0(0J5Ommzi?CSgJ&Az-C*t=;Z*Ar2=n80 z-)Ua@MB!=fm&U*DP;k{Qa0(30i|hV2J%(buP6!5@1aSzITAER1G@cpyqB9=Itsr1y zLV!6UDXTtU(@X7)YUSi=fW?E$T)Cy=HPQH(R)VJW)LjL-NcNqdh&0y==M4n5q-?G# z%hgF+Q+Ja|+wFRXiixesk%nVcVQa2zlroJZV-q%&V>!_se4}Nlg@vwUq`>BH^0R|X z8FJ6_zO5l(O26wVQSnJqD#XV#z0DqK@~=TU4YqUM7I>Myk3WeQDW}{hR4}4dPY8Dj z&v;^BSi;?pxr`6I6BRvuO12YT++9(s>P7P9PEuQnRvn2X-FJM;;qRksL4i-0gSsRi zoA8Ft)3HMwqkS@|Q(uu+d0C`6bF1!Y069-t zFApZA=NWZH38T(O9U~7bzPO)iH?-+iWf6xxVC-zdLW!o8z^WR;UOxg*U%K;rPEJ8xPOu))4CDH#KsH_f`es`}80qURlYL!MiItitrmps9l06dqB zv}+P@(0*215l&p=_j~9R3+s2n=SWI>5_3fs{{)9(3(*ifni3^HD#1s5-&$W6tqwlj zZirxz)z(4;2Y$R}`C_Gg2^_>M9fhi;X=jJPH(A)WRZA-@2sXQYl_z`pq!>LpX z0SaDv{Q2#h_vK$>S?ZSKScH&1i?|ZnLtpQz6y07e_22F-FMU1R$bWh6cJ>9(E>~_& zlc?2Ao1CSOX`V?^v12W>BV+)&@&SPZTJ^Xzxf-h&_JKH7wTL#+k90vnyn76L+_?7{ z*uvlV%}rfOcqos8_0WycW+U2mlHkoC#*ortiedA@{-TnI2 zc|ib&-+Ab2PJdxhqb};pt&+4#E`I{3sp4qKzSsl6b5sCWs4aw1emLhYfzWG%ECkV` zuE9(|-*ctn-ADCr4}8pnzT*DWrDd(Lbv%1K1Uxr+d$*kobRQ?(-L@{-0h_d<_6CXY z%Az#YH~!-%`^QDTI0d??fp?UY!I6?;zbdYYwDyz)O{7SRs+V!Tc4Cv8%T`q{m?dtE z*M&2xOZPykopX7<2sWG-hVZ;{87)2RQZ1eQa;tGa9O1dety#7AOMHa`M&1`CxgmGV zU0nA&GFxK*9IaAp>alMB%1ihbZkcH$qis;2^S5h7+u!;FR>zP30jzB67_E#f$m;n% z-w+%loXbSM5AbDHu06N%jv>~pUK)HFp}i*X$MB8z91(zjeS1n>-=8hyN^lHf&f$Wd zdb07J9)t^yH70&uEJp13WJ{QhhE7xg{>@t~aj_|R04BTmZ{sfjT`Ic~Befh5h?uy} zd{8GJspv1o@Eglays7JH&!>Entr>*V1{J)g8_Z^B7;k$$7JMgmQz9G$P{3l1rLOr* z>176|$DeO8-`OSl3gxyJ8juH4GoRvNh z0On+?N#q;kxlG(48vI>Y?ww8^>u&APum0t>^!uyPyf6e{H$2W1^6s@hZx|{j_FBZoSiCK#G0;ar#d8^wie(G7X`lN4~c;wj!S; z3L+`Vnqo5FCh`uLd!0O0EpnYtBG?^#P&~wENS^%qCqf7XWM8GSEYy)VVg;R)GH*=R zyD$N%8@Fo>@ZziQJz z>IDUT4H&7fs*-*1;4zSC{ex&o$HMbEjD+W=Xhh~c2e+nK^$OgEuny%=X7Q@@*s zMc8(IwH_L*UK#S~zL#ULC(eNh&~qJoR8(LYnHoDb;L7q6Xwq>%>~oZ^rsM-2$|M_O zOlnt!a?byq%Src&JKnnU$uIbGmaX2N)|A)7r-sMo#{EmV$%SeE(se$yPBfw-4N^ev z&m#Xi);VR_8#(V*xRcy^V*tU=>b9O)j)3Pyi~^$L(sHBS@cS%SaOu=?mqKG5C*vkF zsCYRjSYf)DR8a>hyv~9WqA+R-`#5O!@q6PHzF|kxM5b9E$GiY6-D379CEYLOw9d(y zJl08x*zel%xE)GXSsrY!Di?m$5i9^Wo#XEIWUp41mF0A2m4%nG?dyYsLe)j}9+p>O zk(4jhW7y-T(}3?p3^bC&iOV6CY|{RM9~F%{|C=|DNPE)w6~Ec?(;}h;d*AOW2smko zRHLEkuE|)W#u#*&4!8MTe%hTBo~{}zmIqGroJU&Skds&z2 zJC;6Cw5giwqy+p)^FGyp)}JD`Hkc=L=P84&28jPGGNJbt@q2vx5U`%M{Co2KE4Rbf zUOdE{b3>1N@LlzgiG9gGwLNeVes}s^Gf^_S@MJxmRp0-T>0s&eSWDzr^0k2+SG!Z4 zC6vdRLsW&l``qcx{nO)K-+^;>WZW6%V3q3e_eF%D+|Cq=@+>&+1@zG*{OcQV4m_Eo z)}4rRZC{Uq(&kUaG6Clki^P_L&=T=RVxH6>$3H@+yULf9<7b0DGj4|u@Df-+O7xWM zbbfCjx?a*_>NM9y?9xeFr)PwiTjO+>giqq?qBsqBakhGMUZ-LMbQJde&>}jQ>mmRA zd^Dy}Jlgm9vEH$H9HUdCjk{OV`lWy>eYIn@flA$+>IZou_MY!-aQMbhYh~yF#>FD4 zwtKg^xEQEJv&&tC8mcRTOoX(NEslH6+dThk@~tqiluXT3Isygj|8(T4Y?=>%=2H;` z(r)-HKVL-DZm$t>96ySOEi_&X>W&SZ@Ql+Yu5eUv_nUo1VP`=#IQ#aMT;MVkml_Ed zFBoRquKg7iYvg+@`~EknCqQ=%ss z=NL=ks58UUve`cR$ZBE=5A?cRn0fg_pzw77{*SM(0}O(_;!I}k2Yiue9ck z6m|X}tfOC){z3JNei-0ow|V)KwPX-YKwe#_Xid6XCuKanXukT){tsfC&9N{>t-=#& zrc{T#;<->laHhL52xB=?d?vb|{%|2*9wqRR-p#dxm|l^(XzM4L)Xfa^5Lh!bWzMl* zgc4aAuInu1Q@&uwG3c#ISvO6ZvgojS^Rb&FW#^7mz4AEKo`!Q;J7O$Z4}Xf+adzP2 zrC0qjT9C;8IoZ0*Ju2Gm!i-Ky|HeD3TZD=inwgVm$gfi@-d3moxk%0u)gh9^#+=m0 zx!Ut{Jzb%YAceQ}ueo9BgnGu^bnnfigqJ3jHiLpULzw|vbC#2BR@Dca1!4xl5>;7X zds-Tw+dq7wKSDJEFG|Cu2F}F>T#NX3y(nG&^u*c#E0JO$L22ojeS+Y%LK5$$=A2H+bXvBTV2|#L~Z19om$XHdv7ed0Kh$9T%t84$}M_ za%n&%lX-sW5GkO20jr)j9wehvw7prq7|MI7l*`JrzyE>~s(n(4txGNPl14pz5{21W#maN$dh%+#^{KqE|QC4%~Xc)X>uE_q$|o z^L!EYAwCFev9#U+HXV6cR!96 zdJ|$X+7c4=^u&6eQ=cEI@tX_`RWlj&{P3z@XQZc?jE2lHH>C33H`ZB@hzlV#dN?e! za3{~YwpDx9hZ^}Ji$7wH{kAf>m3X+|*Wl)?7F%j?{#RjK*blYb@PTi8D{Z>%xYnHf z`h5&Du9ys8+_&uEv+rl-5JTBA;rfBAiN8F=rTA}ZKD&}##F5F zcxP^IZfIy2Ng?|5l{%Y#2MpB0?Mr0sa(sa^e>^DoyI3}vBZW)v_#P^&8%r{$LPO1| zzCT`5xRWr!Qlm6cNryzOph)hQJ z>u7XL2W12{*}%c_+#?E#s_eE|KMt!Y$Fu##=fZ)HU#VA!g%OeRII1@B)ico1E?El) zzAGdUL@aFP>!b?@{TcZwlziKG;_|AV^tGll2pt7=U{BlH7@ z0_8}~k`3%hopa-kzC2N+4Z?KCY;4WF!5XK4huQ~;OWH4AI_}D>59B()u!d^XYac(^ zB~vUj?2e+AHm(>iRAcqIM&QJ&A~U>v`I3i+CyNxUBN^FltZx&A3S& zO)ARtMpicTJbxfxvaDSmiHK{@2$25qDT;oa1YD5GS?bG=q6Ti0h%Q9g0W z6+h@R#N9o){%KSDlcJQj800L0)<6El{T4V4q2soSP6G)@EY4qyic?wS;yDt6t>h~TT#(w zlTkj`cR(_hTDoR~#&>5rIHW8t{(zSHMWh_hK5=tpjunxaiYKU+z?pA*`t=FDGKqj^ z4X8up0X_g!`PrSV2lHeD&k*St31AdFV#St$u%G7<8zS1Lh@-K?oi}efIuBcrJ33S~ z?!8j)7|Jk)yXoAglQBp_M*R_LWE0ho-|-k{`u*bW8&Hdn*Zr?)G>t47Pl3&RQ?76@ zTFVQu{?;iKJ-+1vg;`57=k@Vx| zT`Pm~o_ubHNS3ob(efA5n@i{2&!jCGO4}7gCBy&41t5q+wo0L;XnG5UL|I~NQsfz2 zKV?x5>hN_|^3sv+BA;)wN@*_btk5ZGE}N_kn*4sj^z(>NDSs*|4#JC5grOhiBMExm zN#Tl&5UiTY5?5%xUd0R#T{&Mba&Gn>`v!8#%w0F$*1@o|Jm3$T$QN97;5qJjujYjk zwtJo4$<8_Gz}uc$ZFQxfu&0J{TL@rKDy;kS6_8@-S=|nm_~~E3Y8_r`laMn!dCcFe;&I1FAzrw0_uF-b4177gH;zeocO5cMYc9bAA zr^_C^Or5t-xFk$tCKVAu!~;Xoz2*+*4NOq~9YQ_k_*7WiJy@v#>F(VGX1|e~lAVo8 zi=#OYWslBBG-vZ=r$3Ts*}om2H}@H?T{hV8gu~8W0@E6m+OOM*>WM_Me)bHZ#}od5 z)#UJc^6k-1wb{&>yWAn-$Z^-Q(&Kn3*#@|P0@mv%eWJ$NYpY6yEXm>`2*_M@S8LQ0ZG^2D#)q=gO%F!GVGj?VJ40h&*>E-+wyCGuVU5ovckL?7pliX42D* z+w4Fv5aG~c(jc&QiHi2)L&dWgc~UN!pu^%qxa4id&UU7S6zw0WJS0rAY$bY&?*;R} zM2iQ{8wh^-{v)Y-HcpC-vkV0)95G^3czmUwwS=?}JMnzbs}nF+wKg^!|FjIG0-Ols zu&QVFNo#gEK*Z1@W% zj2e`6zm`7ZP}-r;_XlH-v|O2Brl8<*fBaRG8+?0L9Hz_5Y0X@BG7e!8rb1DHRJ2VQ*D~l<1%7aRq!_oJc6gD8XSD$eK%;z+s4X zZ~(HC3c0qYvuTYp!dN(@R+Zur##3yA+5^?83mw&-7CZqkLkF-ssCZR7GV5!3Ki-4C z=~HV+;GD0R^12wx4dZ0w(M5VybS2`p%RN?d+9~Qai0S-0_k@zKj7+c*nPQ0)az0P0 z%K1aS?O2uwr^Xhtk#_mddsEVd{B`t2CDZ-$j!A2LQFl|3PU)_MT&|Jy^?8?c{0TMb z#u<7uRrE~KSTI!*M5}@F9i`RFBoX9GXyU$zm1bhB2&{uQT@iRVZ$97xJ`Sr%c%^uh z%SykrC*SkuNIr;GJoztiYmEh$f%!5R&+koi4^&miw(srw!dxLXP$@I*z<}d*tj zPs=cRi8t_!hTaWO+yWOD={BkJ?zxRg&5>NLZ2Kb8*(N;-)`>F`yhlMGkN_2P!J^4K zbzGJfH*#ANsH2Gq+rU91YcY?zc)P3G>o7$1cA9Y<@4 z!FqD6NsXNrggY*-v~jBCZX#3~kUDX$40l2%Oc>skJI!ggOs_TJhn)j4p zLiJoGIK-0|otJKWWE`z0m4ul+terp8+z+=mtAKe zywhgKQ?Z^SON?*;54)u=G3F(*$>~QP`wm;Gy@%wwPs@8gRMBg(3ZmiS*ikac>4LxL zQNx@57RjvADFW&a+^&J1Iw0;9w~gRUmKzcp-=@*jo5c zU)yDF2*qO){AzHEp}YT2Oi_<3ikp_3+xHIb{yZ-^#V%y+wih3nsKe`sMp>#f4#k15 zU&roqb^qO0q=SQ%0V^JOj*;xy(IDSZ=!TH-ww}D#CP?`VF66lgiOc|I0Bl0orS2x; z=Bm@?)WHW=5^|1~J|p&gYYwedJ3{9}U^$@1>9 z0C`vit*bu1w+pIBhZfKffT@(+tTA%wmwzl2<%gNkJJ=Ue10 zb`2nJaj%M+KLaYyErp8YEZ&C_CehgpHdqaW9v+x26E^=FFNY`6CFdW>gtkd9Nt?1o z(iiG66RH+A;~2!Ajt`{e>7<6a`yNoBy%75N_=5~BF3#=75U@fM4rFHd(n8qxLe>n3 z<8G{qYAr4~xi2B>3x!VGv@PX4%)KUy$~^h?tccKD=C5SObIXk?uB(Cw+Gpo7T{t)z z_FSoQa6zLY=sO?$RSiv5qArsuD_PaVGsvwy$pDakD-DAT>u9;mi+ z2ibJA$CM~y10KX-Rd|BV^uxJmn9X?GK*!45rzel;aLIiPl0*)GUWD2Kg3dGeaECM_ zBF`tKE0wFd+zU-uCXZDQ%Dhq-l$HdMQSeOJk`3B%C9nk4@1aOYW*A9hlCq?BPrpTz zq<$hnbFRHRN}n-5A}@T)Na(=#`W2K*={j3#AR=PBf+m#zp)XA~oFtdkZKR(Q@0q;m zix!}@EX2*fZ^&pnv2u3d*#I*%5ex~G==~SEJfB>+QvsJGnG<S$Puk_7+c$pGbvwtK)YT4z-bnzz# z09FC$etyE?G^#ICa$TsZX<>yBb%M0Xh$> z=Jg|4Gxy_ocx#g<;Q2hMeF3F9@DQgUwcTugbJcL=+^LeKqwnld{;|P&h@8yhQOm~4 zMqV&XcU}OV*oQWRWz?(^s;`u9&J%FP*eC{r$$yj9g5xwWQU{FdQUaE%-2Uj|#xe$$y(svOS;#akZ{uQe<2ViWaD zloqP5PT87Yxtot=o5;(hyiSr4=}>|6_(A4`>TW|c$d><{5`Ig-8feqJT3jzfdxUYk zS!zew9vDfFFJw-)+==tFv&<+rIvAD!xM-nvOAY@JIK-BR?l>b6d!{V$_4V|o+cq;U z?w`qMq{hc4IFCi-%~Dzj=dPWz7zKC?H{h-`O2fU0u9ySM2Xw1wEYH#yDC=Y&DQY?~kWXqFJendmDdkuWRyp@aUd|obfO0?@9*8=J0M2kp>X)1D-S~mrz$Y zn^n*5tg^f-7cnO;sq8?CSoEZ38s8)0&Z)}Z-}H_Vhv-gQBSpmG5k#`!g|`vTOveTl zOQ(lY)K3n^x7$Bu1^`WUF`}?z^5zO_tRtJbUQ=(aK_iBX)(=%fW==SkGyy7 z4VHi@Poj_MRE+;U_rE_lN_p%)d0%~d@8`57LyL!FCkRdaIJJT7!M^Qy(vs6wnI3X` zAP$KoTVnbH45MvTkj(i&U0?#_ay0Y5wLD?OeTi!O@?5c?b8{5hkeaN} zG~+ugAd&K>l@pryp;*pHI94d#4bcLM*YPiRv-Q<~h?)KEWTo`}Z!yc0z{!E19Lp`ueHr|EQL0dsU4yL|S)w>g%hK#PlahXgY-6U?rGIJ|o$P>Ue& z0ehMb8oS9jF(D*U3){*9&4@NXtRy*_wBd@E4<7veqi2=uwB%puJXGg&`>|1weX+qW z-q)&TB<6T+NzmFV79AWE;Q!j0VcxS{%SDZ!nB!f+Q^1=mI0hD+0K8zSYhYS|mCB#l zU?6{mYMOWNwBom7XBa?u$FFNBCD(la+Y)c@zJISg7@$BdBGL!pMAwULM#2Nrk>XUskrW*rDdqFZoteTQ{gf$8K@kXP zXF=P!x3d5SaD8nt_AV=M!*Hp9h)s7{3iWuY+jU?p zA&mHFt3*T`@3A3U;#mU-M#BZx@f$+JoOqopIL4mf958}OiIU|`l}Qo-Sb4fNlzy); zV}Jyb#xJWSGaY+iIFZ|b;&r}h#9PmTXt!mNoG!%w9*;9y zs*L5IHpxCz%%OZZclrtp=9&i9s9PJ-F9IeE_LF6v$4{7DiiJDNGSKns;xM!t z5<;G6InV+HCIb|hAthmf&m+RN4Y;kdL8|{fA`bA_vd_7)k`iCVu`IHRqiKsqL&l2P z!Co(e)JoJV&6+vq**eRaUqAF#dW^t!<$eAK3 ziwC|Q#{&|-rv87b434qQmmmDi;DC4bAx8!UueRaG>!uq}vbXm?BJHi^T@%NgXlN1@FnFC|CAlVm=s z+BmiOQX{UUiGn8Z?tSplwPMwCJq>OQni9dd%7aTk6$}3)6+Nf0cLGqz*kXGSIJon^ z(ft^lVmjU$2i~(jUV-xaN;=^{+Y2_s$+cWPVIb?pD)3z3wqylWvQKnJ|J@Ql-YnFz z;7No{Kyc3He6!$dfCzwR45act<8|(Hq`;YvE4^^?Ln$6D*%~!i77N6nRAB@NtF@{G znnBm$PW6ma{3yWof!|V2+Z90!O#R7MfRBJaj3wa6J_ZTb50PNp$jC^eo*13bw@Vgzy!?aVn*A_ z(PjfAYkka*MLWKnA0;xkJE9%jAx!59(G&`(N99hH3u=$7q-AG6&J?GirUtz{O)A>K z#X9eYbMVY$AVH2?UtKC>;~qdBA(ZOl-TUDr+)?MN0Ka+ogwAo8Qdn4+72O3#Fvlf9 zq=F|KrU}wYSl{1ewCVU<9q%>YJbm#FX#y1V$chj!=ZPr^2^VXw14{zaOfvRz8!D=v zQ_qu29nKO@=d#lWNDk;13EJrN%LL&96Ap!|yJ}ftVNx7z`?;)N)182KXh9^BBP?9 zpcs2X1%gPp?GwVNZnyes&F^m(b|s)0BH!tZ1EpA~>%vD=M$Zb%$+GNS39BKE@dW=0RgcXV6lTMmEXt1T=y) z+5DZ4a{C0Btl76vNL9fwOcyHu@gM(<(we0O%^ORk;~7T_RIsqHZb1GVVZPM*e_L1s zU%{KLbQ92I7&H%NNp1559ln5$4-5(#FHjj2TraVitz&0lDU*t&gSws}&W~if4!+O( z-0(9pG9F0_dP?%`VV*5YFS)sp(+j#OHSA;lQ9AgbSd$$8-%9~AX^+Rn=^j>Jx}YEh zMJ8xW28ukfrv55GAONW{npWO(g(G!rvQ6CX32ok5|<>rPHb6SJUN+SRDNtEjx>3(~B2YNFq%XxRf zlIWgqdNZ9i@DYywd)|X*?Z!Lp!*NjCRlU-Dq2(jBR17RMltCd^rphOPCjy$Nbzr!r znthr#9;0MuXZNM@o`!+`2^v+~!dU#j3sg#gO-{sIRk}Rg6Lj4NhljO*#XkYf-{Jtb z37#vPE9};OcCfrPT5!4=uVZ|2HZ8pr;LMlx~S&XeP(tiMunk<)HTvn7bc#%bh^1 zfUP$4oXnBNFL!I$8`H3PrB=2A=q2fHYl(nZ0_Ly)d@2}~Fj#%iJm_RG=-K}+}rO7Evs_^y?jQw&0gu*-r(ZM~nkMVxn-<7&BVEtdSV9|1-9R zJvZ_N^NqA0x1>MX1{DYC$h$yO|HagcM_rU}u)uI+!ll?65AkX-Bm3 z_b*%e;Eh|JazPFeO@?Kf_w2^TM!CsgXIB^4F0t-$Y~I(1+Qvq$7mwrO<8^>T3v?-H z`uAIy{Wnkt&jh{sKzpU8rfZ-cIBn({d~O7e-_0b88GM^TpfB>zbM6gAPpP zCZH z2ckuc<(rE&8P|`_0b>L3VaCiW!6yI*4-7%k!NI}8VhgkgM-O;gs!BpYpsKAc$FC?{ zR9L9l?Cs_1dUm+ltEHud^-MrhU0sS_kw{se@ra(;gnlYy{UZGaeNCT;1iJX@3rEc( z$oMxmHWHJP>KtEc*yM!#TpBOY@j;vokBmh1t_t{E&&zaZG*`fH9tN(tGU*idz&%z`*#eu{kw7jEiJ7{d{;73za>b06pJ~!T* z8ARW!t5195nb6VE!2>|UEV}q!psQt}$;gaND1T!0)8V{>zp<#O$j{GDQ0H4+eSP_n z12R~!=;-Li#zs&aFd;p?*?v`OGnj~OIZu8TJS{IT@ABfJSi6yjnK`l&OKp03dVPIe zI%i~NW~S0&ytA`2Ffi}~R%~djDRopYLYYro0KHif^d25QhuYNLBM05w$dMK?k&y17 zf&STOLZG>=5Ksize(1000, 1000); - for (const auto& ll : lls) { - BasicLineString2d leftBound = ll.leftBound2d().basicLineString(); - BasicLineString2d rightBound = ll.rightBound2d().basicLineString(); - BasicLineString2d centerline = ll.centerline2d().basicLineString(); - std::vector xl; - std::vector yl; - for (const auto& pt : leftBound) { - xl.push_back(pt.x()); - yl.push_back(pt.y()); - } - std::vector xr; - std::vector yr; - for (const auto& pt : rightBound) { - xr.push_back(pt.x()); - yr.push_back(pt.y()); - } - std::vector xc; - std::vector yc; - for (const auto& pt : centerline) { - xc.push_back(pt.x()); - yc.push_back(pt.y()); - } - matplot::plot(xl, yl, "b")->line_width(3); - matplot::plot(xr, yr, "r")->line_width(3); - matplot::plot(xc, yc, "--gs")->line_width(3).marker_color("g"); + // for (const auto& ll : lls) { + // BasicLineString2d leftBound = ll.leftBound2d().basicLineString(); + // BasicLineString2d rightBound = ll.rightBound2d().basicLineString(); + // BasicLineString2d centerline = ll.centerline2d().basicLineString(); + // std::vector xl; + // std::vector yl; + // for (const auto& pt : leftBound) { + // xl.push_back(pt.x()); + // yl.push_back(pt.y()); + // } + // std::vector xr; + // std::vector yr; + // for (const auto& pt : rightBound) { + // xr.push_back(pt.x()); + // yr.push_back(pt.y()); + // } + // std::vector xc; + // std::vector yc; + // for (const auto& pt : centerline) { + // xc.push_back(pt.x()); + // yc.push_back(pt.y()); + // } + // matplot::plot(xl, yl, "b")->line_width(3); + // matplot::plot(xr, yr, "r")->line_width(3); + // matplot::plot(xc, yc, "--gs")->line_width(3).marker_color("g"); + // } + + LaneData laneData = LaneData::build(laneletSubmap, laneletMapGraph); + + std::vector rawCompoundRoadBorders = getPointsMatrixList(laneData.compoundRoadBorders(), true); + std::vector rawCompoundLaneDividers = getPointsMatrixList(laneData.compoundLaneDividers(), true); + std::vector rawCompoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); + + for (const auto& mat : rawCompoundRoadBorders) { + std::vector x; + std::vector y; + x.resize(mat.rows()); + y.resize(mat.rows()); + Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + matplot::plot(x, y, "r")->line_width(3); + } + for (const auto& mat : rawCompoundLaneDividers) { + std::vector x; + std::vector y; + x.resize(mat.rows()); + y.resize(mat.rows()); + Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + matplot::plot(x, y, "b")->line_width(3); + } + for (const auto& mat : rawCompoundCenterlines) { + std::vector x; + std::vector y; + x.resize(mat.rows()); + y.resize(mat.rows()); + Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + matplot::plot(x, y, "--gs")->line_width(3).marker_color("g"); } - // LaneData laneData = LaneData::build(laneletSubmap, laneletMapGraph); - // bool valid = laneData.processAll(bbox, ParametrizationType::LineString, 10); - // - // std::vector compoundRoadBorders = getPointsMatrixList(laneData.compoundRoadBorders(), true); - // std::vector compoundLaneDividers = getPointsMatrixList(laneData.compoundLaneDividers(), true); - // std::vector compoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); - // - // for (const auto& mat : compoundRoadBorders) { - // std::vector x; - // std::vector y; - // x.resize(mat.rows()); - // y.resize(mat.rows()); - // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - // matplot::plot(x, y, "r")->line_width(3); - // } - // for (const auto& mat : compoundLaneDividers) { - // std::vector x; - // std::vector y; - // x.resize(mat.rows()); - // y.resize(mat.rows()); - // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - // matplot::plot(x, y, "y")->line_width(3); - // } - // for (const auto& mat : compoundCenterlines) { - // std::vector x; - // std::vector y; - // x.resize(mat.rows()); - // y.resize(mat.rows()); - // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - // matplot::plot(x, y, "--g")->line_width(3); - // } - matplot::save("map_data.png"); + matplot::save("map_data_raw.png"); + matplot::cla(); + matplot::hold(matplot::on); + matplot::xlim({-2, 16}); + matplot::ylim({-13, 2}); + matplot::gcf()->size(1000, 1000); + + bool valid = laneData.processAll(bbox, ParametrizationType::LineString, 10); + std::vector compoundRoadBorders = getPointsMatrixList(laneData.compoundRoadBorders(), true); + std::vector compoundLaneDividers = getPointsMatrixList(laneData.compoundLaneDividers(), true); + std::vector compoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); + + for (const auto& mat : compoundRoadBorders) { + std::vector x; + std::vector y; + x.resize(mat.rows()); + y.resize(mat.rows()); + Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + matplot::plot(x, y, "r")->line_width(3); + } + for (const auto& mat : compoundLaneDividers) { + std::vector x; + std::vector y; + x.resize(mat.rows()); + y.resize(mat.rows()); + Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + matplot::plot(x, y, "b")->line_width(3); + } + for (const auto& mat : compoundCenterlines) { + std::vector x; + std::vector y; + x.resize(mat.rows()); + y.resize(mat.rows()); + Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + matplot::plot(x, y, "--gs")->line_width(3).marker_color("g"); + } + + matplot::save("map_data_processed.png"); } \ No newline at end of file diff --git a/lanelet2_map_learning/test/test_utils.cpp b/lanelet2_map_learning/test/test_utils.cpp index 415d7373..f7d894a9 100644 --- a/lanelet2_map_learning/test/test_utils.cpp +++ b/lanelet2_map_learning/test/test_utils.cpp @@ -26,8 +26,8 @@ TEST_F(MapLearningTest, ExtractSubmap) { // NOLINT EXPECT_TRUE(submap->laneletLayer.exists(2003)); EXPECT_TRUE(submap->laneletLayer.exists(2004)); EXPECT_TRUE(submap->laneletLayer.exists(2005)); - EXPECT_TRUE(submap->laneletLayer.exists(2012)); - EXPECT_TRUE(submap->laneletLayer.exists(2013)); + EXPECT_TRUE(submap->laneletLayer.exists(2009)); + EXPECT_TRUE(submap->laneletLayer.exists(2010)); } TEST(UtilsTest, ResampleLineString) { // NOLINT From b809af76b635693a34b5d928f1a3c6d334b8e3c5 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Tue, 12 Dec 2023 17:52:08 +0100 Subject: [PATCH 28/64] add MapData Unit tests --- .../include/lanelet2_map_learning/MapData.h | 6 +- .../lanelet2_map_learning/MapFeatures.h | 6 +- lanelet2_map_learning/src/MapData.cpp | 54 ++++--- lanelet2_map_learning/src/Utils.cpp | 10 +- lanelet2_map_learning/test/test_map_data.cpp | 135 ++++++++++-------- lanelet2_map_learning/test/test_utils.cpp | 12 +- lanelet2_routing/src/RoutingGraphBuilder.cpp | 3 - 7 files changed, 117 insertions(+), 109 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index 38d657a0..1e9ca315 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -31,7 +31,7 @@ struct CompoundElsList { }; } // namespace internal -using Edges = std::vector; +using Edges = std::map; // key = id from class LaneData { public: @@ -47,7 +47,7 @@ class LaneData { const CompoundLaneLineStringFeatureList& compoundCenterlines() { return compoundCenterlines_; } const LaneletFeatures& laneletFeatures() { return laneletFeatures_; } - const Edges& edgeList() { return edgeList_; } + const Edges& edges() { return edges_; } private: void initLeftBoundaries(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); @@ -70,7 +70,7 @@ class LaneData { CompoundLaneLineStringFeatureList compoundCenterlines_; LaneletFeatures laneletFeatures_; // node features - Edges edgeList_; // edge list for centerlines + Edges edges_; // edge list for centerlines }; class TEData { diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index b08305c8..688fcedc 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -110,9 +110,9 @@ class LaneletFeature : public MapFeature { void setReprType(LaneletRepresentationType reprType) { reprType_ = reprType; } - const LaneLineStringFeature& leftBoundary() { return leftBoundary_; } - const LaneLineStringFeature& rightBoundary() { return rightBoundary_; } - const LaneLineStringFeature& centerline() { return centerline_; } + const LaneLineStringFeature& leftBoundary() const { return leftBoundary_; } + const LaneLineStringFeature& rightBoundary() const { return rightBoundary_; } + const LaneLineStringFeature& centerline() const { return centerline_; } private: LaneLineStringFeature leftBoundary_; diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index d24864d1..08f62345 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -9,13 +9,9 @@ using namespace internal; LaneData LaneData::build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { LaneData data; - std::cerr << "Building Lane Data" << std::endl; data.initLeftBoundaries(localSubmap, localSubmapGraph); - std::cerr << "Left Boundaries Initialized" << std::endl; data.initRightBoundaries(localSubmap, localSubmapGraph); - std::cerr << "Right Boundaries Initialized" << std::endl; data.initLaneletFeatures(localSubmap, localSubmapGraph); - std::cerr << "Lanelet Features Initialized" << std::endl; data.initCompoundFeatures(localSubmap, localSubmapGraph); return data; @@ -38,7 +34,7 @@ void LaneData::initLeftBoundaries(LaneletSubmapConstPtr& localSubmap, Optional adjLeftLL = localSubmapGraph->adjacentLeft(ll); if (leftLL) { - edgeList_.push_back(Edge(ll.id(), leftLL->id())); + edges_.insert({ll.id(), Edge(ll.id(), leftLL->id())}); } } } @@ -58,7 +54,7 @@ void LaneData::initRightBoundaries(LaneletSubmapConstPtr& localSubmap, Optional rightLL = localSubmapGraph->right(ll); Optional adjRightLL = localSubmapGraph->adjacentRight(ll); if (rightLL) { - edgeList_.push_back(Edge(ll.id(), rightLL->id())); + edges_.insert({ll.id(), Edge(ll.id(), rightLL->id())}); } } } @@ -194,7 +190,7 @@ void insertAndCheckNewCompoundFeatures(std::vector& compFeats, for (const Id& el : compEl.ids) { elInsertIdx[el] = firstOccIt->second; } - } else { + } else if (compFeats[firstOccIt->second].ids.size() != compEl.ids.size()) { compFeats.push_back(compEl); for (const Id& el : compEl.ids) { elInsertIdx[el] = compFeats.size() - 1; @@ -224,28 +220,28 @@ void LaneData::initCompoundFeatures(LaneletSubmapConstPtr& localSubmap, insertAndCheckNewCompoundFeatures(compoundedBordersAndDividers, compoundedRight, elInsertIdx); compoundCenterlines_.push_back(computeCompoundCenterline(path)); - std::cerr << "---------------------" << std::endl; - std::cerr << "Path Lanelets: " << std::endl; - for (const auto& ll : path) { - std::cerr << ll.id() << " "; - } - std::cerr << std::endl; - - for (const auto& cpd : compoundedLeft) { - std::cerr << "compoundedLeft : " << std::endl; - for (const auto& id : cpd.ids) { - std::cerr << id << " "; - } - std::cerr << std::endl; - } - - for (const auto& cpd : compoundedRight) { - std::cerr << "compoundedRight : " << std::endl; - for (const auto& id : cpd.ids) { - std::cerr << id << " "; - } - std::cerr << std::endl; - } + // std::cerr << "---------------------" << std::endl; + // std::cerr << "Path Lanelets: " << std::endl; + // for (const auto& ll : path) { + // std::cerr << ll.id() << " "; + // } + // std::cerr << std::endl; + // + // for (const auto& cpd : compoundedLeft) { + // std::cerr << "compoundedLeft : " << std::endl; + // for (const auto& id : cpd.ids) { + // std::cerr << id << " "; + // } + // std::cerr << std::endl; + // } + // + // for (const auto& cpd : compoundedRight) { + // std::cerr << "compoundedRight : " << std::endl; + // for (const auto& id : cpd.ids) { + // std::cerr << id << " "; + // } + // std::cerr << std::endl; + // } } for (const auto& compFeat : compoundedBordersAndDividers) { LaneLineStringFeatureList toBeCompounded; diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index 7f32aa8c..53b92368 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -38,14 +38,20 @@ LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPo return utils::createConstSubmap(initRegion, {}); } -/// TODO: FURTHER INVESTIGATE THE WIERD BEHAVIOR OF BOOST LINE_INTERPOLATE +/// TODO: FURTHER INVESTIGATE THE WEIRD BEHAVIOR OF BOOST LINE_INTERPOLATE BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t nPoints) { double length = boost::geometry::length(polyline, boost::geometry::strategy::distance::pythagoras()); double dist = length / static_cast(nPoints - 1); // to get all points of line boost::geometry::model::multi_point bdInterp; boost::geometry::line_interpolate(polyline, dist, bdInterp); bdInterp.insert(bdInterp.begin(), polyline.front()); - assert(bdInterp.size() == nPoints); + if (bdInterp.size() != nPoints && (bdInterp.back() - polyline.back()).norm() > 10e-5) { + bdInterp.insert(bdInterp.end(), polyline.back()); + } else if (bdInterp.size() == nPoints && (bdInterp.back() - polyline.back()).norm() > 10e-5) { + bdInterp.back() = polyline.back(); + } else if (bdInterp.size() != nPoints) { + throw std::runtime_error("LineString resampling failed to produce the desired number of points!"); + } return bdInterp; } diff --git a/lanelet2_map_learning/test/test_map_data.cpp b/lanelet2_map_learning/test/test_map_data.cpp index 70b8537f..a8b79941 100644 --- a/lanelet2_map_learning/test/test_map_data.cpp +++ b/lanelet2_map_learning/test/test_map_data.cpp @@ -11,7 +11,7 @@ using namespace lanelet; using namespace lanelet::map_learning; using namespace lanelet::map_learning::tests; -TEST_F(MapLearningTest, LaneDataPlot) { // NOLINT +TEST_F(MapLearningTest, LaneData) { // NOLINT traffic_rules::TrafficRulesPtr trafficRules{ traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)}; routing::RoutingGraphConstPtr laneletMapGraph = routing::RoutingGraph::build(*laneletMap, *trafficRules); @@ -57,73 +57,82 @@ TEST_F(MapLearningTest, LaneDataPlot) { // NOLINT std::vector rawCompoundLaneDividers = getPointsMatrixList(laneData.compoundLaneDividers(), true); std::vector rawCompoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); - for (const auto& mat : rawCompoundRoadBorders) { - std::vector x; - std::vector y; - x.resize(mat.rows()); - y.resize(mat.rows()); - Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - matplot::plot(x, y, "r")->line_width(3); - } - for (const auto& mat : rawCompoundLaneDividers) { - std::vector x; - std::vector y; - x.resize(mat.rows()); - y.resize(mat.rows()); - Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - matplot::plot(x, y, "b")->line_width(3); - } - for (const auto& mat : rawCompoundCenterlines) { - std::vector x; - std::vector y; - x.resize(mat.rows()); - y.resize(mat.rows()); - Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - matplot::plot(x, y, "--gs")->line_width(3).marker_color("g"); - } - - matplot::save("map_data_raw.png"); - matplot::cla(); - matplot::hold(matplot::on); - matplot::xlim({-2, 16}); - matplot::ylim({-13, 2}); - matplot::gcf()->size(1000, 1000); + // for (const auto& mat : rawCompoundRoadBorders) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "r")->line_width(3); + // } + // for (const auto& mat : rawCompoundLaneDividers) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "b")->line_width(3); + // } + // for (const auto& mat : rawCompoundCenterlines) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "--gs")->line_width(3).marker_color("g"); + // } + // + // matplot::save("map_data_raw.png"); + // matplot::cla(); + // matplot::hold(matplot::on); + // matplot::xlim({-2, 16}); + // matplot::ylim({-13, 2}); + // matplot::gcf()->size(1000, 1000); bool valid = laneData.processAll(bbox, ParametrizationType::LineString, 10); std::vector compoundRoadBorders = getPointsMatrixList(laneData.compoundRoadBorders(), true); std::vector compoundLaneDividers = getPointsMatrixList(laneData.compoundLaneDividers(), true); std::vector compoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); - for (const auto& mat : compoundRoadBorders) { - std::vector x; - std::vector y; - x.resize(mat.rows()); - y.resize(mat.rows()); - Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - matplot::plot(x, y, "r")->line_width(3); - } - for (const auto& mat : compoundLaneDividers) { - std::vector x; - std::vector y; - x.resize(mat.rows()); - y.resize(mat.rows()); - Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - matplot::plot(x, y, "b")->line_width(3); - } - for (const auto& mat : compoundCenterlines) { - std::vector x; - std::vector y; - x.resize(mat.rows()); - y.resize(mat.rows()); - Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - matplot::plot(x, y, "--gs")->line_width(3).marker_color("g"); - } + EXPECT_TRUE(laneData.laneletFeatures().find(2007) != laneData.laneletFeatures().end()); + EXPECT_EQ(laneData.laneletFeatures().find(2007)->second.leftBoundary().mapID().value(), 1012); + EXPECT_TRUE(laneData.roadBorders().find(1001) != laneData.roadBorders().end()); + + EXPECT_EQ(compoundRoadBorders.size(), 3); + EXPECT_EQ(compoundLaneDividers.size(), 8); + EXPECT_EQ(compoundCenterlines.size(), 4); + + // for (const auto& mat : compoundRoadBorders) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "r")->line_width(3); + // } - matplot::save("map_data_processed.png"); + // for (const auto& mat : compoundLaneDividers) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "b")->line_width(3); + // } + // for (const auto& mat : compoundCenterlines) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "--gs")->line_width(3).marker_color("g"); + // } + // + // matplot::save("map_data_processed.png"); } \ No newline at end of file diff --git a/lanelet2_map_learning/test/test_utils.cpp b/lanelet2_map_learning/test/test_utils.cpp index f7d894a9..d3d36ea6 100644 --- a/lanelet2_map_learning/test/test_utils.cpp +++ b/lanelet2_map_learning/test/test_utils.cpp @@ -32,13 +32,13 @@ TEST_F(MapLearningTest, ExtractSubmap) { // NOLINT TEST(UtilsTest, ResampleLineString) { // NOLINT BasicLineString3d polyline{BasicPoint3d{0, 0, 0}, BasicPoint3d{3, 0, 0}, BasicPoint3d{6, 0, 0}, - BasicPoint3d{9, 0, 0}}; - BasicLineString3d polylineResampled = resampleLineString(polyline, 10); + BasicPoint3d{15, 0, 0}}; + BasicLineString3d polylineResampled = resampleLineString(polyline, 11); - EXPECT_EQ(polylineResampled.size(), 10); - EXPECT_NEAR(polylineResampled[3].x(), 3, 10e-5); - EXPECT_NEAR(polylineResampled[6].x(), 6, 10e-5); - EXPECT_NEAR(polylineResampled[9].x(), 9, 10e-5); + EXPECT_EQ(polylineResampled.size(), 11); + EXPECT_NEAR(polylineResampled[3].x(), 4.5, 10e-5); + EXPECT_NEAR(polylineResampled[6].x(), 9, 10e-5); + EXPECT_NEAR(polylineResampled[10].x(), 15, 10e-5); } TEST_F(MapLearningTest, CutLineString) { // NOLINT diff --git a/lanelet2_routing/src/RoutingGraphBuilder.cpp b/lanelet2_routing/src/RoutingGraphBuilder.cpp index d03ebfc7..8d2c6066 100644 --- a/lanelet2_routing/src/RoutingGraphBuilder.cpp +++ b/lanelet2_routing/src/RoutingGraphBuilder.cpp @@ -207,7 +207,6 @@ void RoutingGraphBuilder::addSidewayEdge(LaneChangeLaneletsCollector& laneChange // we process lane changes later, when we know all lanelets that can participate in lane change laneChangeLanelets.add(ll, it->second); } else { - std::cerr << "Lanelet with id " << ll.id() << "cannot change lane to lanelet " << it->second << "\n"; assignCosts(ll, it->second, relation); } } @@ -358,8 +357,6 @@ void RoutingGraphBuilder::assignLaneChangeCosts(ConstLanelets froms, ConstLanele void RoutingGraphBuilder::assignCosts(const ConstLaneletOrArea& from, const ConstLaneletOrArea& to, const RelationType& relation) { - std::cerr << "Adding Edge from ll with id " << from.id() << " to lanelet " << to.id() << " with type " - << relationToString(relation) << "\n"; for (RoutingCostId rci = 0; rci < RoutingCostId(routingCosts_.size()); rci++) { EdgeInfo edgeInfo{}; edgeInfo.costId = rci; From 24cb1d490592331cda53ab30c080dbafd859c1f5 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Wed, 13 Dec 2023 19:35:01 +0100 Subject: [PATCH 29/64] bugfix extract submap --- lanelet2_map_learning/src/Utils.cpp | 2 +- lanelet2_map_learning/test/test_map_data.cpp | 11 +- .../test/test_map_data_interface.cpp | 140 ++++++++++++++++++ 3 files changed, 147 insertions(+), 6 deletions(-) create mode 100644 lanelet2_map_learning/test/test_map_data_interface.cpp diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index 53b92368..fd352ce3 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -30,7 +30,7 @@ OrientedRect getRotatedRect(const BasicPoint2d& center, double extentLongitudina LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPoint2d& center, double extentLongitudinal, double extentLateral) { - double maxExtent = std::max(extentLongitudinal, extentLateral); + double maxExtent = sqrt(extentLongitudinal * extentLongitudinal + extentLateral * extentLateral); BasicPoint2d initRegionRear = {center.x() - 1.1 * maxExtent, center.y() - 1.1 * maxExtent}; BasicPoint2d initRegionFront = {center.x() + 1.1 * maxExtent, center.y() + 1.1 * maxExtent}; BoundingBox2d initSearchRegion{initRegionRear, initRegionFront}; diff --git a/lanelet2_map_learning/test/test_map_data.cpp b/lanelet2_map_learning/test/test_map_data.cpp index a8b79941..af2849c3 100644 --- a/lanelet2_map_learning/test/test_map_data.cpp +++ b/lanelet2_map_learning/test/test_map_data.cpp @@ -1,6 +1,6 @@ #include #include -#include +// #include #include "lanelet2_map_learning/MapData.h" #include "lanelet2_map_learning/MapFeatures.h" @@ -19,15 +19,16 @@ TEST_F(MapLearningTest, LaneData) { // NOLINT lls.insert(lls.end(), laneletMap->laneletLayer.begin(), laneletMap->laneletLayer.end()); LaneletSubmapConstPtr laneletSubmap = utils::createConstSubmap(lls, {}); - matplot::hold(matplot::on); - matplot::xlim({-2, 16}); - matplot::ylim({-13, 2}); - matplot::gcf()->size(1000, 1000); + // matplot::hold(matplot::on); + // matplot::xlim({-2, 16}); + // matplot::ylim({-13, 2}); + // matplot::gcf()->size(1000, 1000); // for (const auto& ll : lls) { // BasicLineString2d leftBound = ll.leftBound2d().basicLineString(); // BasicLineString2d rightBound = ll.rightBound2d().basicLineString(); // BasicLineString2d centerline = ll.centerline2d().basicLineString(); + // BasicLineString2d centerline = ll.centerline2d().basicLineString(); // std::vector xl; // std::vector yl; // for (const auto& pt : leftBound) { diff --git a/lanelet2_map_learning/test/test_map_data_interface.cpp b/lanelet2_map_learning/test/test_map_data_interface.cpp new file mode 100644 index 00000000..411c09d7 --- /dev/null +++ b/lanelet2_map_learning/test/test_map_data_interface.cpp @@ -0,0 +1,140 @@ +#include +#include +// #include + +#include "lanelet2_map_learning/MapData.h" +#include "lanelet2_map_learning/MapDataInterface.h" +#include "lanelet2_map_learning/MapFeatures.h" +#include "lanelet2_routing/RoutingGraph.h" +#include "test_map.h" + +using namespace lanelet; +using namespace lanelet::map_learning; +using namespace lanelet::map_learning::tests; + +TEST_F(MapLearningTest, LaneData) { // NOLINT + traffic_rules::TrafficRulesPtr trafficRules{ + traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)}; + routing::RoutingGraphConstPtr laneletMapGraph = routing::RoutingGraph::build(*laneletMap, *trafficRules); + ConstLanelets lls; + lls.insert(lls.end(), laneletMap->laneletLayer.begin(), laneletMap->laneletLayer.end()); + LaneletSubmapConstPtr laneletSubmap = utils::createConstSubmap(lls, {}); + + // matplot::hold(matplot::on); + // matplot::xlim({-2, 16}); + // matplot::ylim({-13, 2}); + // matplot::gcf()->size(1000, 1000); + + // for (const auto& ll : lls) { + // BasicLineString2d leftBound = ll.leftBound2d().basicLineString(); + // BasicLineString2d rightBound = ll.rightBound2d().basicLineString(); + // BasicLineString2d centerline = ll.centerline2d().basicLineString(); + // BasicLineString2d centerline = ll.centerline2d().basicLineString(); + // std::vector xl; + // std::vector yl; + // for (const auto& pt : leftBound) { + // xl.push_back(pt.x()); + // yl.push_back(pt.y()); + // } + // std::vector xr; + // std::vector yr; + // for (const auto& pt : rightBound) { + // xr.push_back(pt.x()); + // yr.push_back(pt.y()); + // } + // std::vector xc; + // std::vector yc; + // for (const auto& pt : centerline) { + // xc.push_back(pt.x()); + // yc.push_back(pt.y()); + // } + // matplot::plot(xl, yl, "b")->line_width(3); + // matplot::plot(xr, yr, "r")->line_width(3); + // matplot::plot(xc, yc, "--gs")->line_width(3).marker_color("g"); + // } + + LaneData laneData = LaneData::build(laneletSubmap, laneletMapGraph); + + std::vector rawCompoundRoadBorders = getPointsMatrixList(laneData.compoundRoadBorders(), true); + std::vector rawCompoundLaneDividers = getPointsMatrixList(laneData.compoundLaneDividers(), true); + std::vector rawCompoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); + + // for (const auto& mat : rawCompoundRoadBorders) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "r")->line_width(3); + // } + // for (const auto& mat : rawCompoundLaneDividers) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "b")->line_width(3); + // } + // for (const auto& mat : rawCompoundCenterlines) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "--gs")->line_width(3).marker_color("g"); + // } + // + // matplot::save("map_data_raw.png"); + // matplot::cla(); + // matplot::hold(matplot::on); + // matplot::xlim({-2, 16}); + // matplot::ylim({-13, 2}); + // matplot::gcf()->size(1000, 1000); + + bool valid = laneData.processAll(bbox, ParametrizationType::LineString, 10); + std::vector compoundRoadBorders = getPointsMatrixList(laneData.compoundRoadBorders(), true); + std::vector compoundLaneDividers = getPointsMatrixList(laneData.compoundLaneDividers(), true); + std::vector compoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); + + EXPECT_TRUE(laneData.laneletFeatures().find(2007) != laneData.laneletFeatures().end()); + EXPECT_EQ(laneData.laneletFeatures().find(2007)->second.leftBoundary().mapID().value(), 1012); + EXPECT_TRUE(laneData.roadBorders().find(1001) != laneData.roadBorders().end()); + + EXPECT_EQ(compoundRoadBorders.size(), 3); + EXPECT_EQ(compoundLaneDividers.size(), 8); + EXPECT_EQ(compoundCenterlines.size(), 4); + + // for (const auto& mat : compoundRoadBorders) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "r")->line_width(3); + // } + + // for (const auto& mat : compoundLaneDividers) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "b")->line_width(3); + // } + // for (const auto& mat : compoundCenterlines) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "--gs")->line_width(3).marker_color("g"); + // } + // + // matplot::save("map_data_processed.png"); +} \ No newline at end of file From 742e47a8ab04de1c6bad31eea7c8dff19b7786d3 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 14 Dec 2023 15:39:45 +0100 Subject: [PATCH 30/64] add associated ll ids --- .../include/lanelet2_map_learning/MapData.h | 14 ++- .../lanelet2_map_learning/MapFeatures.h | 9 +- lanelet2_map_learning/src/MapData.cpp | 100 +++++++++++------- lanelet2_map_learning/src/MapFeatures.cpp | 6 +- lanelet2_map_learning/test/test_map_data.cpp | 19 +++- .../test/test_map_data_interface.cpp | 2 +- .../test/test_map_features.cpp | 18 ++-- 7 files changed, 111 insertions(+), 57 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index 1e9ca315..eb9892f2 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -46,6 +46,12 @@ class LaneData { const CompoundLaneLineStringFeatureList& compoundLaneDividers() { return compoundLaneDividers_; } const CompoundLaneLineStringFeatureList& compoundCenterlines() { return compoundCenterlines_; } + const std::map>& associatedCpdRoadBorderIndices() { return associatedCpdRoadBorderIndices_; } + const std::map>& associatedCpdLaneDividerIndices() { + return associatedCpdLaneDividerIndices_; + } + const std::map>& associatedCpdCenterlineIndices() { return associatedCpdCenterlineIndices_; } + const LaneletFeatures& laneletFeatures() { return laneletFeatures_; } const Edges& edges() { return edges_; } @@ -55,6 +61,7 @@ class LaneData { void initLaneletFeatures(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); void initCompoundFeatures(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); + void updateAssociatedCpdFeatureIndices(); LineStringType getLineStringTypeFromId(Id id); LaneLineStringFeature getLineStringFeatFromId(Id id); @@ -69,8 +76,11 @@ class LaneData { CompoundLaneLineStringFeatureList compoundLaneDividers_; // auxilliary features CompoundLaneLineStringFeatureList compoundCenterlines_; - LaneletFeatures laneletFeatures_; // node features - Edges edges_; // edge list for centerlines + LaneletFeatures laneletFeatures_; + std::map> associatedCpdRoadBorderIndices_; + std::map> associatedCpdLaneDividerIndices_; + std::map> associatedCpdCenterlineIndices_; + Edges edges_; // edge list for centerlines }; class TEData { diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index 688fcedc..dd337572 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -15,7 +15,7 @@ namespace map_learning { class MapFeature { public: - const Optional& mapID() const { return mapID_; } + const Id& mapID() const { return mapID_.get_value_or(InvalId); } const bool& initialized() const { return initialized_; } const bool& valid() const { return valid_; } @@ -52,8 +52,8 @@ class LineStringFeature : public MapFeature { class LaneLineStringFeature : public LineStringFeature { public: LaneLineStringFeature() {} - LaneLineStringFeature(const BasicLineString3d& feature, Id mapID, LineStringType type) - : LineStringFeature(feature, mapID), type_{type} {} + LaneLineStringFeature(const BasicLineString3d& feature, Id mapID, LineStringType type, Id laneletID) + : LineStringFeature(feature, mapID), type_{type}, laneletIDs_{laneletID} {} virtual ~LaneLineStringFeature() noexcept = default; virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; @@ -67,12 +67,15 @@ class LaneLineStringFeature : public LineStringFeature { const BasicLineString3d& cutAndResampledFeature() const { return cutAndResampledFeature_; } const LineStringType& type() const { return type_; } int typeInt() const { return static_cast(type_); } + const std::vector& laneletIDs() const { return laneletIDs_; } + void addLaneletID(Id id) { laneletIDs_.push_back(id); } protected: BasicLineString3d cutFeature_; BasicLineString3d cutAndResampledFeature_; bool wasCut_{false}; LineStringType type_; + std::vector laneletIDs_; }; using LaneLineStringFeatures = std::map; diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 08f62345..70f69bc8 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -13,7 +13,7 @@ LaneData LaneData::build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::R data.initRightBoundaries(localSubmap, localSubmapGraph); data.initLaneletFeatures(localSubmap, localSubmapGraph); data.initCompoundFeatures(localSubmap, localSubmapGraph); - + data.updateAssociatedCpdFeatureIndices(); return data; } @@ -21,13 +21,23 @@ void LaneData::initLeftBoundaries(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { for (const auto& ll : localSubmap->laneletLayer) { if (isRoadBorder(ll.leftBound3d())) { - roadBorders_.insert( - {ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), - bdTypeToEnum(ll.leftBound3d()))}); + LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(ll.leftBound3d().id()); + if (itRoadBd != roadBorders_.end()) { + itRoadBd->second.addLaneletID(ll.id()); + } else { + roadBorders_.insert( + {ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), + bdTypeToEnum(ll.leftBound3d()), ll.id())}); + } } else { - laneDividers_.insert( - {ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), - bdTypeToEnum(ll.leftBound3d()))}); + LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(ll.leftBound3d().id()); + if (itLaneBd != laneDividers_.end()) { + itLaneBd->second.addLaneletID(ll.id()); + } else { + laneDividers_.insert( + {ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), + bdTypeToEnum(ll.leftBound3d()), ll.id())}); + } } Optional leftLL = localSubmapGraph->left(ll); @@ -43,13 +53,23 @@ void LaneData::initRightBoundaries(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { for (const auto& ll : localSubmap->laneletLayer) { if (isRoadBorder(ll.rightBound3d())) { - roadBorders_.insert( - {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), - bdTypeToEnum(ll.rightBound3d()))}); + LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(ll.rightBound3d().id()); + if (itRoadBd != roadBorders_.end()) { + itRoadBd->second.addLaneletID(ll.id()); + } else { + roadBorders_.insert( + {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), + bdTypeToEnum(ll.rightBound3d()), ll.id())}); + } } else { - laneDividers_.insert( - {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), - bdTypeToEnum(ll.rightBound3d()))}); + LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(ll.rightBound3d().id()); + if (itLaneBd != laneDividers_.end()) { + itLaneBd->second.addLaneletID(ll.id()); + } else { + laneDividers_.insert( + {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), + bdTypeToEnum(ll.rightBound3d()), ll.id())}); + } } Optional rightLL = localSubmapGraph->right(ll); Optional adjRightLL = localSubmapGraph->adjacentRight(ll); @@ -65,7 +85,7 @@ void LaneData::initLaneletFeatures(LaneletSubmapConstPtr& localSubmap, const LaneLineStringFeature& leftBoundary = getLineStringFeatFromId(ll.leftBound().id()); const LaneLineStringFeature& rightBoundary = getLineStringFeatFromId(ll.rightBound().id()); LaneLineStringFeature centerline{ll.centerline3d().basicLineString(), ll.centerline3d().id(), - LineStringType::Centerline}; + LineStringType::Centerline, ll.id()}; laneletFeatures_.insert({ll.id(), LaneletFeature(leftBoundary, rightBoundary, centerline, ll.id())}); } } @@ -158,7 +178,7 @@ CompoundLaneLineStringFeature LaneData::computeCompoundCenterline(const ConstLan LaneLineStringFeatureList compoundCenterlines; for (const auto& ll : path) { compoundCenterlines.push_back( - LaneLineStringFeature(ll.centerline3d().basicLineString(), ll.id(), LineStringType::Centerline)); + LaneLineStringFeature(ll.centerline3d().basicLineString(), ll.id(), LineStringType::Centerline, ll.id())); } return CompoundLaneLineStringFeature(compoundCenterlines, LineStringType::Centerline); } @@ -219,29 +239,6 @@ void LaneData::initCompoundFeatures(LaneletSubmapConstPtr& localSubmap, std::vector compoundedRight = computeCompoundRightBorders(path); insertAndCheckNewCompoundFeatures(compoundedBordersAndDividers, compoundedRight, elInsertIdx); compoundCenterlines_.push_back(computeCompoundCenterline(path)); - - // std::cerr << "---------------------" << std::endl; - // std::cerr << "Path Lanelets: " << std::endl; - // for (const auto& ll : path) { - // std::cerr << ll.id() << " "; - // } - // std::cerr << std::endl; - // - // for (const auto& cpd : compoundedLeft) { - // std::cerr << "compoundedLeft : " << std::endl; - // for (const auto& id : cpd.ids) { - // std::cerr << id << " "; - // } - // std::cerr << std::endl; - // } - // - // for (const auto& cpd : compoundedRight) { - // std::cerr << "compoundedRight : " << std::endl; - // for (const auto& id : cpd.ids) { - // std::cerr << id << " "; - // } - // std::cerr << std::endl; - // } } for (const auto& compFeat : compoundedBordersAndDividers) { LaneLineStringFeatureList toBeCompounded; @@ -258,6 +255,33 @@ void LaneData::initCompoundFeatures(LaneletSubmapConstPtr& localSubmap, } } +void LaneData::updateAssociatedCpdFeatureIndices() { + for (size_t i = 0; i < compoundRoadBorders_.size(); i++) { + const auto& cpdFeat = compoundRoadBorders_[i]; + for (const auto& indFeat : cpdFeat.features()) { + for (const auto& id : indFeat.laneletIDs()) { + associatedCpdRoadBorderIndices_[id].push_back(i); + } + } + } + for (size_t i = 0; i < compoundLaneDividers_.size(); i++) { + const auto& cpdFeat = compoundLaneDividers_[i]; + for (const auto& indFeat : cpdFeat.features()) { + for (const auto& id : indFeat.laneletIDs()) { + associatedCpdLaneDividerIndices_[id].push_back(i); + } + } + } + for (size_t i = 0; i < compoundCenterlines_.size(); i++) { + const auto& cpdFeat = compoundCenterlines_[i]; + for (const auto& indFeat : cpdFeat.features()) { + for (const auto& id : indFeat.laneletIDs()) { + associatedCpdCenterlineIndices_[id].push_back(i); + } + } + } +} + bool LaneData::processAll(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { bool validRoadBorders = processFeatureMap(roadBorders_, bbox, paramType, nPoints); bool validLaneDividers = processFeatureMap(laneDividers_, bbox, paramType, nPoints); diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index 18b9380e..11a84d7b 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -133,11 +133,11 @@ LaneletFeature::LaneletFeature(const LaneLineStringFeature& leftBoundary, const LaneletFeature::LaneletFeature(const ConstLanelet& ll) : leftBoundary_{LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), - bdTypeToEnum(ll.leftBound3d()))}, + bdTypeToEnum(ll.leftBound3d()), ll.id())}, rightBoundary_{LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), - bdTypeToEnum(ll.rightBound3d()))}, + bdTypeToEnum(ll.rightBound3d()), ll.id())}, centerline_{LaneLineStringFeature(ll.centerline3d().basicLineString(), ll.centerline3d().id(), - LineStringType::Centerline)} {} + LineStringType::Centerline, ll.id())} {} bool LaneletFeature::process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { leftBoundary_.process(bbox, paramType, nPoints); diff --git a/lanelet2_map_learning/test/test_map_data.cpp b/lanelet2_map_learning/test/test_map_data.cpp index af2849c3..d1ac7a53 100644 --- a/lanelet2_map_learning/test/test_map_data.cpp +++ b/lanelet2_map_learning/test/test_map_data.cpp @@ -99,13 +99,30 @@ TEST_F(MapLearningTest, LaneData) { // NOLINT std::vector compoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); EXPECT_TRUE(laneData.laneletFeatures().find(2007) != laneData.laneletFeatures().end()); - EXPECT_EQ(laneData.laneletFeatures().find(2007)->second.leftBoundary().mapID().value(), 1012); + EXPECT_EQ(laneData.laneletFeatures().find(2007)->second.leftBoundary().mapID(), 1012); EXPECT_TRUE(laneData.roadBorders().find(1001) != laneData.roadBorders().end()); EXPECT_EQ(compoundRoadBorders.size(), 3); EXPECT_EQ(compoundLaneDividers.size(), 8); EXPECT_EQ(compoundCenterlines.size(), 4); + EXPECT_EQ(laneData.associatedCpdRoadBorderIndices().at(2001).size(), 1); + size_t assoBorderIdx = laneData.associatedCpdRoadBorderIndices().at(2001).front(); + EXPECT_EQ(laneData.compoundRoadBorders()[assoBorderIdx].features().back().mapID(), 1000); + EXPECT_EQ(laneData.compoundRoadBorders()[assoBorderIdx].features().back().laneletIDs().front(), 2002); + + EXPECT_EQ(laneData.associatedCpdLaneDividerIndices().at(2004).size(), 2); + EXPECT_EQ(laneData.associatedCpdLaneDividerIndices().at(2009).size(), 1); + size_t assoDividerIdx = laneData.associatedCpdLaneDividerIndices().at(2009).front(); + EXPECT_EQ(laneData.compoundLaneDividers()[assoDividerIdx].features().front().mapID(), 1015); + EXPECT_EQ(laneData.compoundLaneDividers()[assoDividerIdx].features().front().laneletIDs().size(), 2); + + EXPECT_EQ(laneData.associatedCpdCenterlineIndices().at(2003).size(), 2); + EXPECT_EQ(laneData.associatedCpdCenterlineIndices().at(2000).size(), 1); + size_t assoCenterlineIdx = laneData.associatedCpdCenterlineIndices().at(2000).front(); + EXPECT_EQ(laneData.compoundCenterlines()[assoCenterlineIdx].features().front().mapID(), 2000); + EXPECT_EQ(laneData.compoundCenterlines()[assoCenterlineIdx].features().front().laneletIDs().front(), 2000); + // for (const auto& mat : compoundRoadBorders) { // std::vector x; // std::vector y; diff --git a/lanelet2_map_learning/test/test_map_data_interface.cpp b/lanelet2_map_learning/test/test_map_data_interface.cpp index 411c09d7..3a8f5872 100644 --- a/lanelet2_map_learning/test/test_map_data_interface.cpp +++ b/lanelet2_map_learning/test/test_map_data_interface.cpp @@ -100,7 +100,7 @@ TEST_F(MapLearningTest, LaneData) { // NOLINT std::vector compoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); EXPECT_TRUE(laneData.laneletFeatures().find(2007) != laneData.laneletFeatures().end()); - EXPECT_EQ(laneData.laneletFeatures().find(2007)->second.leftBoundary().mapID().value(), 1012); + EXPECT_EQ(laneData.laneletFeatures().find(2007)->second.leftBoundary().mapID(), 1012); EXPECT_TRUE(laneData.roadBorders().find(1001) != laneData.roadBorders().end()); EXPECT_EQ(compoundRoadBorders.size(), 3); diff --git a/lanelet2_map_learning/test/test_map_features.cpp b/lanelet2_map_learning/test/test_map_features.cpp index 5d018a80..fe597143 100644 --- a/lanelet2_map_learning/test/test_map_features.cpp +++ b/lanelet2_map_learning/test/test_map_features.cpp @@ -10,7 +10,7 @@ using namespace lanelet::map_learning::tests; TEST_F(MapLearningTest, LaneLineStringFeature) { // NOLINT BasicLineString3d polyline{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}, BasicPoint3d{20, 0, 0}}; - LaneLineStringFeature feat(polyline, Id(123), LineStringType::Solid); + LaneLineStringFeature feat(polyline, Id(123), LineStringType::Solid, Id(1234)); feat.process(bbox, ParametrizationType::LineString, 4); EXPECT_EQ(feat.cutAndResampledFeature().size(), 4); @@ -36,13 +36,13 @@ TEST_F(MapLearningTest, LaneLineStringFeature) { // NOLINT TEST_F(MapLearningTest, LaneletFeature) { // NOLINT BasicLineString3d leftBd{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}, BasicPoint3d{20, 0, 0}}; - LaneLineStringFeature leftBdFeat(leftBd, Id(123), LineStringType::Solid); + LaneLineStringFeature leftBdFeat(leftBd, Id(123), LineStringType::Solid, Id(1234)); BasicLineString3d rightBd{BasicPoint3d{0, 4, 0}, BasicPoint3d{5, 4, 0}, BasicPoint3d{10, 4, 0}, BasicPoint3d{20, 4, 0}}; - LaneLineStringFeature rightBdFeat(rightBd, Id(124), LineStringType::Dashed); + LaneLineStringFeature rightBdFeat(rightBd, Id(124), LineStringType::Dashed, Id(1234)); BasicLineString3d centerline{BasicPoint3d{0, 2, 0}, BasicPoint3d{5, 2, 0}, BasicPoint3d{10, 2, 0}, BasicPoint3d{20, 2, 0}}; - LaneLineStringFeature centerlineFeat(centerline, Id(125), LineStringType::Centerline); + LaneLineStringFeature centerlineFeat(centerline, Id(125), LineStringType::Centerline, Id(1234)); LaneletFeature llFeat(leftBdFeat, rightBdFeat, centerlineFeat, Id(1234)); llFeat.setReprType(LaneletRepresentationType::Boundaries); @@ -64,15 +64,15 @@ TEST_F(MapLearningTest, LaneletFeature) { // NOLINT TEST_F(MapLearningTest, CompoundLaneLineStringFeature) { // NOLINT BasicLineString3d p1{BasicPoint3d{-10, 0, 0}, BasicPoint3d{-5, 0, 0}}; - LaneLineStringFeature feat1(p1, Id(123), LineStringType::Solid); + LaneLineStringFeature feat1(p1, Id(123), LineStringType::Solid, Id(1234)); BasicLineString3d p2{BasicPoint3d{-5, 0, 0}, BasicPoint3d{0, 0, 0}}; - LaneLineStringFeature feat2(p2, Id(123), LineStringType::Solid); + LaneLineStringFeature feat2(p2, Id(123), LineStringType::Solid, Id(1234)); BasicLineString3d p3{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}}; - LaneLineStringFeature feat3(p3, Id(123), LineStringType::Solid); + LaneLineStringFeature feat3(p3, Id(123), LineStringType::Solid, Id(1234)); BasicLineString3d p4{BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}}; - LaneLineStringFeature feat4(p4, Id(124), LineStringType::Solid); + LaneLineStringFeature feat4(p4, Id(124), LineStringType::Solid, Id(1234)); BasicLineString3d p5{BasicPoint3d{10, 0, 0}, BasicPoint3d{20, 0, 0}}; - LaneLineStringFeature feat5(p5, Id(125), LineStringType::Solid); + LaneLineStringFeature feat5(p5, Id(125), LineStringType::Solid, Id(1234)); CompoundLaneLineStringFeature cpdFeat(LaneLineStringFeatureList{feat1, feat2, feat3, feat4, feat5}, LineStringType::Solid); From 2f185fbc43f3fcb038a6e5b725adcfffbeaf9d3f Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 14 Dec 2023 18:45:38 +0100 Subject: [PATCH 31/64] finish MapDataInterface unit tests --- .../include/lanelet2_map_learning/MapData.h | 24 ++- .../lanelet2_map_learning/MapDataInterface.h | 27 ++- lanelet2_map_learning/package.xml | 2 +- lanelet2_map_learning/src/MapData.cpp | 8 +- .../src/MapDataInterface.cpp | 46 +++-- lanelet2_map_learning/src/MapFeatures.cpp | 4 +- lanelet2_map_learning/test/test_map_data.cpp | 122 +++++------ .../test/test_map_data_interface.cpp | 191 +++++++----------- 8 files changed, 194 insertions(+), 230 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index eb9892f2..d5efa098 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -64,7 +64,7 @@ class LaneData { void updateAssociatedCpdFeatureIndices(); LineStringType getLineStringTypeFromId(Id id); - LaneLineStringFeature getLineStringFeatFromId(Id id); + const LaneLineStringFeature& getLineStringFeatFromId(Id id); std::vector computeCompoundLeftBorders(const ConstLanelets& path); std::vector computeCompoundRightBorders(const ConstLanelets& path); CompoundLaneLineStringFeature computeCompoundCenterline(const ConstLanelets& path); @@ -77,10 +77,10 @@ class LaneData { CompoundLaneLineStringFeatureList compoundCenterlines_; LaneletFeatures laneletFeatures_; - std::map> associatedCpdRoadBorderIndices_; - std::map> associatedCpdLaneDividerIndices_; - std::map> associatedCpdCenterlineIndices_; - Edges edges_; // edge list for centerlines + std::map> associatedCpdRoadBorderIndices_; // related to the "unfiltered" compound features! + std::map> associatedCpdLaneDividerIndices_; // related to the "unfiltered" compound features! + std::map> associatedCpdCenterlineIndices_; // related to the "unfiltered" compound features! + Edges edges_; // edge list for centerlines }; class TEData { @@ -96,5 +96,19 @@ class TEData { /// xLane and xTE stacked, with xLane being first }; +template +std::vector getValidElements(const std::vector& vec) { + std::vector res; + std::copy_if(vec.begin(), vec.end(), std::back_inserter(res), [](T el) { return el.valid(); }); + return res; +} + +template +std::map getValidElements(const std::map& map) { + std::map res; + std::copy_if(map.begin(), map.end(), std::back_inserter(res), [](const auto& pair) { return pair.second.valid(); }); + return res; +} + } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h index 236a17a5..8ae5129f 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h @@ -21,30 +21,35 @@ class MapDataInterface { struct Configuration { Configuration() noexcept {} + Configuration(LaneletRepresentationType reprType, ParametrizationType paramType, double submapAreaLongitudinal, + double submapAreaLateral, int nPoints) noexcept + : reprType{reprType}, + paramType{paramType}, + submapExtentLongitudinal{submapExtentLongitudinal}, + submapExtentLateral{submapExtentLateral} {} LaneletRepresentationType reprType{LaneletRepresentationType::Boundaries}; ParametrizationType paramType{ParametrizationType::LineString}; - bool includeLaneletBdTypes{false}; - double submapAreaLongitudinal{30}; // in driving direction - double submapAreaLateral{15}; // in lateral direction - int nPoints{11}; + double submapExtentLongitudinal{30}; // in driving direction + double submapExtentLateral{15}; // in lateral direction + int nPoints{20}; }; MapDataInterface(LaneletMapConstPtr laneletMap, Configuration config = Configuration(), Optional currPos = boost::none); - void setCurrPosAndExtractSubmap(const BasicPoint2d& pt); + void setCurrPosAndExtractSubmap(const BasicPoint2d& pt, double yaw); LaneData laneData(); TEData teData(); - LaneData laneDataBatch(const BasicPoints2d& pts, const std::vector& yaws); - TEData laneTEDataBatch(const BasicPoints2d& pts, const std::vector& yaws); + std::vector laneDataBatch(const BasicPoints2d& pts, const std::vector& yaws); + std::vector laneTEDataBatch(const BasicPoints2d& pts, const std::vector& yaws); private: - LaneData getLaneData(LaneletSubmapConstPtr localSubmap, routing::RoutingGraphConstPtr localSubmapGraph, - bool processAll = false); + LaneData getLaneData(LaneletSubmapConstPtr localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph, + const BasicPoint2d& pos, double yaw, bool processAll = true); - TEData getLaneTEData(routing::RoutingGraphConstPtr localSubmapGraph, LaneletSubmapConstPtr localSubmap, - bool processAll); + TEData getLaneTEData(LaneletSubmapConstPtr localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph, + const BasicPoint2d& pos, double yaw, bool processAll = true); LaneletMapConstPtr laneletMap_; LaneletSubmapConstPtr localSubmap_; diff --git a/lanelet2_map_learning/package.xml b/lanelet2_map_learning/package.xml index 2a4c5df4..df18b481 100644 --- a/lanelet2_map_learning/package.xml +++ b/lanelet2_map_learning/package.xml @@ -20,7 +20,7 @@ lanelet2_routing lanelet2_traffic_rules - Matplot++ + catkin diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 70f69bc8..b2eb0e6f 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -121,19 +121,17 @@ LineStringType LaneData::getLineStringTypeFromId(Id id) { return bdType; } -LaneLineStringFeature LaneData::getLineStringFeatFromId(Id id) { - LaneLineStringFeature lstringFeat; +const LaneLineStringFeature& LaneData::getLineStringFeatFromId(Id id) { LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(id); LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(id); if (itLaneBd != laneDividers_.end()) { - lstringFeat = itLaneBd->second; + return itLaneBd->second; } else if (itRoadBd != roadBorders_.end()) { - lstringFeat = itRoadBd->second; + return itRoadBd->second; } else { throw std::runtime_error("Lanelet boundary " + std::to_string(id) + " is neither a road border nor a lane divider!"); } - return lstringFeat; } std::vector LaneData::computeCompoundLeftBorders(const ConstLanelets& path) { diff --git a/lanelet2_map_learning/src/MapDataInterface.cpp b/lanelet2_map_learning/src/MapDataInterface.cpp index 2f657aeb..80f799ba 100644 --- a/lanelet2_map_learning/src/MapDataInterface.cpp +++ b/lanelet2_map_learning/src/MapDataInterface.cpp @@ -15,9 +15,10 @@ namespace lanelet { namespace map_learning { -void MapDataInterface::setCurrPosAndExtractSubmap(const BasicPoint2d& pt) { +void MapDataInterface::setCurrPosAndExtractSubmap(const BasicPoint2d& pt, double yaw) { currPos_ = pt; - localSubmap_ = extractSubmap(laneletMap_, *currPos_, config_.submapAreaLongitudinal, config_.submapAreaLateral); + currYaw_ = yaw; + localSubmap_ = extractSubmap(laneletMap_, *currPos_, config_.submapExtentLongitudinal, config_.submapExtentLateral); localSubmapGraph_ = lanelet::routing::RoutingGraph::build(*laneletMap_, *trafficRules_); } @@ -28,25 +29,43 @@ MapDataInterface::MapDataInterface(LaneletMapConstPtr laneletMap, Configuration trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} LaneData MapDataInterface::getLaneData(LaneletSubmapConstPtr localSubmap, - lanelet::routing::RoutingGraphConstPtr localSubmapGraph, bool processAll) { + lanelet::routing::RoutingGraphConstPtr localSubmapGraph, const BasicPoint2d& pos, + double yaw, bool processAll) { LaneData laneData = LaneData::build(localSubmap, localSubmapGraph); - OrientedRect bbox = getRotatedRect(*currPos_, config_.submapAreaLongitudinal, config_.submapAreaLateral, *currYaw_); + OrientedRect bbox = getRotatedRect(pos, config_.submapExtentLongitudinal, config_.submapExtentLateral, yaw); if (processAll) { - bool valid = laneData.processAll(bbox, config_.paramType, config_.nPoints); - if (!valid) { - throw std::runtime_error("Invalid result in processing lane data!"); - } + laneData.processAll(bbox, config_.paramType, config_.nPoints); } return laneData; } +std::vector MapDataInterface::laneDataBatch(const BasicPoints2d& pts, const std::vector& yaws) { + if (pts.size() != yaws.size()) { + throw std::runtime_error("Unequal sizes of pts and yaws!"); + } + std::vector lDataVec; + for (size_t i = 0; i < pts.size(); i++) { + LaneletSubmapConstPtr localSubmap = + extractSubmap(laneletMap_, pts[i], config_.submapExtentLongitudinal, config_.submapExtentLateral); + routing::RoutingGraphConstPtr localSubmapGraph = + lanelet::routing::RoutingGraph::build(*localSubmap, *trafficRules_); + lDataVec.push_back(getLaneData(localSubmap, localSubmapGraph, pts[i], yaws[i])); + } + return lDataVec; +} + +std::vector MapDataInterface::laneTEDataBatch(const BasicPoints2d& pts, const std::vector& yaws) { + throw std::runtime_error("Not implemented yet!"); +} + bool isTe(ConstLineString3d ls) { std::string type = ls.attribute(AttributeName::Type).value(); return type == AttributeValueString::TrafficLight || type == AttributeValueString::TrafficSign; } -TEData MapDataInterface::getLaneTEData(routing::RoutingGraphConstPtr localSubmapGraph, - LaneletSubmapConstPtr localSubmap, bool processAll) { +TEData MapDataInterface::getLaneTEData(LaneletSubmapConstPtr localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph, const BasicPoint2d& pos, + double yaw, bool processAll) { throw std::runtime_error("Not implemented yet!"); } @@ -55,8 +74,11 @@ LaneData MapDataInterface::laneData() { throw InvalidObjectStateError( "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); } - - return getLaneData(localSubmap_, localSubmapGraph_); + if (!currYaw_) { + throw InvalidObjectStateError( + "Your current yaw angle is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); + } + return getLaneData(localSubmap_, localSubmapGraph_, *currPos_, *currYaw_); } TEData MapDataInterface::teData() { diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index 11a84d7b..a14d533e 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -216,7 +216,8 @@ CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStrin bool CompoundLaneLineStringFeature::process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { - bool valid = LaneLineStringFeature::process(bbox, paramType, nPoints); + bool valid = false; + LaneLineStringFeature::process(bbox, paramType, nPoints); for (size_t i = 0; i < individualFeatures_.size(); i++) { individualFeatures_[i].process(bbox, paramType, nPoints); double processedLength = boost::geometry::length(individualFeatures_[i].cutFeature(), @@ -224,6 +225,7 @@ bool CompoundLaneLineStringFeature::process(const OrientedRect& bbox, const Para if (processedLength > validLengthThresh_) { processedFeaturesValid_[i] = true; + valid = true; } if (i > 0) { diff --git a/lanelet2_map_learning/test/test_map_data.cpp b/lanelet2_map_learning/test/test_map_data.cpp index d1ac7a53..094a95b1 100644 --- a/lanelet2_map_learning/test/test_map_data.cpp +++ b/lanelet2_map_learning/test/test_map_data.cpp @@ -19,45 +19,47 @@ TEST_F(MapLearningTest, LaneData) { // NOLINT lls.insert(lls.end(), laneletMap->laneletLayer.begin(), laneletMap->laneletLayer.end()); LaneletSubmapConstPtr laneletSubmap = utils::createConstSubmap(lls, {}); - // matplot::hold(matplot::on); - // matplot::xlim({-2, 16}); - // matplot::ylim({-13, 2}); - // matplot::gcf()->size(1000, 1000); - - // for (const auto& ll : lls) { - // BasicLineString2d leftBound = ll.leftBound2d().basicLineString(); - // BasicLineString2d rightBound = ll.rightBound2d().basicLineString(); - // BasicLineString2d centerline = ll.centerline2d().basicLineString(); - // BasicLineString2d centerline = ll.centerline2d().basicLineString(); - // std::vector xl; - // std::vector yl; - // for (const auto& pt : leftBound) { - // xl.push_back(pt.x()); - // yl.push_back(pt.y()); - // } - // std::vector xr; - // std::vector yr; - // for (const auto& pt : rightBound) { - // xr.push_back(pt.x()); - // yr.push_back(pt.y()); - // } - // std::vector xc; - // std::vector yc; - // for (const auto& pt : centerline) { - // xc.push_back(pt.x()); - // yc.push_back(pt.y()); - // } - // matplot::plot(xl, yl, "b")->line_width(3); - // matplot::plot(xr, yr, "r")->line_width(3); - // matplot::plot(xc, yc, "--gs")->line_width(3).marker_color("g"); - // } - LaneData laneData = LaneData::build(laneletSubmap, laneletMapGraph); std::vector rawCompoundRoadBorders = getPointsMatrixList(laneData.compoundRoadBorders(), true); std::vector rawCompoundLaneDividers = getPointsMatrixList(laneData.compoundLaneDividers(), true); std::vector rawCompoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); + bool valid = laneData.processAll(bbox, ParametrizationType::LineString, 20); + std::vector compoundRoadBorders = getPointsMatrixList(laneData.compoundRoadBorders(), true); + std::vector compoundLaneDividers = getPointsMatrixList(laneData.compoundLaneDividers(), true); + std::vector compoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); + + EXPECT_TRUE(laneData.laneletFeatures().find(2007) != laneData.laneletFeatures().end()); + EXPECT_EQ(laneData.laneletFeatures().find(2007)->second.leftBoundary().mapID(), 1012); + EXPECT_TRUE(laneData.roadBorders().find(1001) != laneData.roadBorders().end()); + + EXPECT_EQ(compoundRoadBorders.size(), 3); + EXPECT_EQ(compoundLaneDividers.size(), 8); + EXPECT_EQ(compoundCenterlines.size(), 4); + + EXPECT_EQ(laneData.associatedCpdRoadBorderIndices().at(2001).size(), 1); + size_t assoBorderIdx = laneData.associatedCpdRoadBorderIndices().at(2001).front(); + EXPECT_EQ(laneData.compoundRoadBorders()[assoBorderIdx].features().back().mapID(), 1000); + EXPECT_EQ(laneData.compoundRoadBorders()[assoBorderIdx].features().back().laneletIDs().front(), 2002); + + EXPECT_EQ(laneData.associatedCpdLaneDividerIndices().at(2004).size(), 2); + EXPECT_EQ(laneData.associatedCpdLaneDividerIndices().at(2009).size(), 1); + size_t assoDividerIdx = laneData.associatedCpdLaneDividerIndices().at(2009).front(); + EXPECT_EQ(laneData.compoundLaneDividers()[assoDividerIdx].features().front().mapID(), 1015); + EXPECT_EQ(laneData.compoundLaneDividers()[assoDividerIdx].features().front().laneletIDs().size(), 2); + + EXPECT_EQ(laneData.associatedCpdCenterlineIndices().at(2003).size(), 2); + EXPECT_EQ(laneData.associatedCpdCenterlineIndices().at(2000).size(), 1); + size_t assoCenterlineIdx = laneData.associatedCpdCenterlineIndices().at(2000).front(); + EXPECT_EQ(laneData.compoundCenterlines()[assoCenterlineIdx].features().front().mapID(), 2000); + EXPECT_EQ(laneData.compoundCenterlines()[assoCenterlineIdx].features().front().laneletIDs().front(), 2000); + + // matplot::hold(matplot::on); + // matplot::xlim({-2, 16}); + // matplot::ylim({-13, 2}); + // matplot::gcf()->size(1000, 1000); + // // for (const auto& mat : rawCompoundRoadBorders) { // std::vector x; // std::vector y; @@ -68,9 +70,9 @@ TEST_F(MapLearningTest, LaneData) { // NOLINT // matplot::plot(x, y, "r")->line_width(3); // } // for (const auto& mat : rawCompoundLaneDividers) { - // std::vector x; - // std::vector y; - // x.resize(mat.rows()); + // std::vector x#include + // std::vector y#include + // x.resize(mat.rows());#include // y.resize(mat.rows()); // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); @@ -92,37 +94,7 @@ TEST_F(MapLearningTest, LaneData) { // NOLINT // matplot::xlim({-2, 16}); // matplot::ylim({-13, 2}); // matplot::gcf()->size(1000, 1000); - - bool valid = laneData.processAll(bbox, ParametrizationType::LineString, 10); - std::vector compoundRoadBorders = getPointsMatrixList(laneData.compoundRoadBorders(), true); - std::vector compoundLaneDividers = getPointsMatrixList(laneData.compoundLaneDividers(), true); - std::vector compoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); - - EXPECT_TRUE(laneData.laneletFeatures().find(2007) != laneData.laneletFeatures().end()); - EXPECT_EQ(laneData.laneletFeatures().find(2007)->second.leftBoundary().mapID(), 1012); - EXPECT_TRUE(laneData.roadBorders().find(1001) != laneData.roadBorders().end()); - - EXPECT_EQ(compoundRoadBorders.size(), 3); - EXPECT_EQ(compoundLaneDividers.size(), 8); - EXPECT_EQ(compoundCenterlines.size(), 4); - - EXPECT_EQ(laneData.associatedCpdRoadBorderIndices().at(2001).size(), 1); - size_t assoBorderIdx = laneData.associatedCpdRoadBorderIndices().at(2001).front(); - EXPECT_EQ(laneData.compoundRoadBorders()[assoBorderIdx].features().back().mapID(), 1000); - EXPECT_EQ(laneData.compoundRoadBorders()[assoBorderIdx].features().back().laneletIDs().front(), 2002); - - EXPECT_EQ(laneData.associatedCpdLaneDividerIndices().at(2004).size(), 2); - EXPECT_EQ(laneData.associatedCpdLaneDividerIndices().at(2009).size(), 1); - size_t assoDividerIdx = laneData.associatedCpdLaneDividerIndices().at(2009).front(); - EXPECT_EQ(laneData.compoundLaneDividers()[assoDividerIdx].features().front().mapID(), 1015); - EXPECT_EQ(laneData.compoundLaneDividers()[assoDividerIdx].features().front().laneletIDs().size(), 2); - - EXPECT_EQ(laneData.associatedCpdCenterlineIndices().at(2003).size(), 2); - EXPECT_EQ(laneData.associatedCpdCenterlineIndices().at(2000).size(), 1); - size_t assoCenterlineIdx = laneData.associatedCpdCenterlineIndices().at(2000).front(); - EXPECT_EQ(laneData.compoundCenterlines()[assoCenterlineIdx].features().front().mapID(), 2000); - EXPECT_EQ(laneData.compoundCenterlines()[assoCenterlineIdx].features().front().laneletIDs().front(), 2000); - + // // for (const auto& mat : compoundRoadBorders) { // std::vector x; // std::vector y; @@ -132,15 +104,14 @@ TEST_F(MapLearningTest, LaneData) { // NOLINT // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); // matplot::plot(x, y, "r")->line_width(3); // } - // for (const auto& mat : compoundLaneDividers) { - // std::vector x; - // std::vector y; - // x.resize(mat.rows()); - // y.resize(mat.rows()); - // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - // matplot::plot(x, y, "b")->line_width(3); + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "b")->line_width(3); // } // for (const auto& mat : compoundCenterlines) { // std::vector x; @@ -151,6 +122,5 @@ TEST_F(MapLearningTest, LaneData) { // NOLINT // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); // matplot::plot(x, y, "--gs")->line_width(3).marker_color("g"); // } - // // matplot::save("map_data_processed.png"); } \ No newline at end of file diff --git a/lanelet2_map_learning/test/test_map_data_interface.cpp b/lanelet2_map_learning/test/test_map_data_interface.cpp index 3a8f5872..9e37708c 100644 --- a/lanelet2_map_learning/test/test_map_data_interface.cpp +++ b/lanelet2_map_learning/test/test_map_data_interface.cpp @@ -12,129 +12,82 @@ using namespace lanelet; using namespace lanelet::map_learning; using namespace lanelet::map_learning::tests; -TEST_F(MapLearningTest, LaneData) { // NOLINT +TEST_F(MapLearningTest, MapDataInterface) { // NOLINT traffic_rules::TrafficRulesPtr trafficRules{ traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)}; - routing::RoutingGraphConstPtr laneletMapGraph = routing::RoutingGraph::build(*laneletMap, *trafficRules); - ConstLanelets lls; - lls.insert(lls.end(), laneletMap->laneletLayer.begin(), laneletMap->laneletLayer.end()); - LaneletSubmapConstPtr laneletSubmap = utils::createConstSubmap(lls, {}); + MapDataInterface::Configuration config{}; + config.reprType = LaneletRepresentationType::Centerline; + config.paramType = ParametrizationType::LineString; + config.submapExtentLongitudinal = 5; + config.submapExtentLateral = 3; + config.nPoints = 20; - // matplot::hold(matplot::on); - // matplot::xlim({-2, 16}); - // matplot::ylim({-13, 2}); - // matplot::gcf()->size(1000, 1000); + MapDataInterface parser(laneletMap, config); + BasicPoints2d pts{BasicPoint2d(0, -3), BasicPoint2d(3, -3), BasicPoint2d(5, -3), + BasicPoint2d(6, -5), BasicPoint2d(6, -8), BasicPoint2d(6, -11)}; + std::vector yaws{0, 0, M_PI / 4, M_PI / 3, M_PI / 2, M_PI / 2}; + std::vector lDataVec = parser.laneDataBatch(pts, yaws); - // for (const auto& ll : lls) { - // BasicLineString2d leftBound = ll.leftBound2d().basicLineString(); - // BasicLineString2d rightBound = ll.rightBound2d().basicLineString(); - // BasicLineString2d centerline = ll.centerline2d().basicLineString(); - // BasicLineString2d centerline = ll.centerline2d().basicLineString(); - // std::vector xl; - // std::vector yl; - // for (const auto& pt : leftBound) { - // xl.push_back(pt.x()); - // yl.push_back(pt.y()); - // } - // std::vector xr; - // std::vector yr; - // for (const auto& pt : rightBound) { - // xr.push_back(pt.x()); - // yr.push_back(pt.y()); - // } - // std::vector xc; - // std::vector yc; - // for (const auto& pt : centerline) { - // xc.push_back(pt.x()); - // yc.push_back(pt.y()); - // } - // matplot::plot(xl, yl, "b")->line_width(3); - // matplot::plot(xr, yr, "r")->line_width(3); - // matplot::plot(xc, yc, "--gs")->line_width(3).marker_color("g"); - // } + for (size_t i = 0; i < lDataVec.size(); i++) { + std::vector compoundRoadBorders = + getPointsMatrixList(getValidElements(lDataVec[i].compoundRoadBorders()), true); + std::vector compoundLaneDividers = + getPointsMatrixList(getValidElements(lDataVec[i].compoundLaneDividers()), true); + std::vector compoundCenterlines = + getPointsMatrixList(getValidElements(lDataVec[i].compoundCenterlines()), true); - LaneData laneData = LaneData::build(laneletSubmap, laneletMapGraph); + switch (i) { + case 0: { + LaneletFeatures::const_iterator itLL0 = lDataVec[i].laneletFeatures().find(2000); + EXPECT_TRUE(itLL0 != lDataVec[i].laneletFeatures().end()); + LaneletFeatures::const_iterator itLL1 = lDataVec[i].laneletFeatures().find(2002); + EXPECT_TRUE(itLL1 == lDataVec[i].laneletFeatures().end()); + break; + } + case 1: { + LaneletFeatures::const_iterator itLL = lDataVec[i].laneletFeatures().find(2011); + EXPECT_TRUE(itLL != lDataVec[i].laneletFeatures().end()); + break; + } + case 5: { + LaneletFeatures::const_iterator itLL = lDataVec[i].laneletFeatures().find(2004); + EXPECT_TRUE(itLL == lDataVec[i].laneletFeatures().end()); + break; + } + } - std::vector rawCompoundRoadBorders = getPointsMatrixList(laneData.compoundRoadBorders(), true); - std::vector rawCompoundLaneDividers = getPointsMatrixList(laneData.compoundLaneDividers(), true); - std::vector rawCompoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); - - // for (const auto& mat : rawCompoundRoadBorders) { - // std::vector x; - // std::vector y; - // x.resize(mat.rows()); - // y.resize(mat.rows()); - // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - // matplot::plot(x, y, "r")->line_width(3); - // } - // for (const auto& mat : rawCompoundLaneDividers) { - // std::vector x; - // std::vector y; - // x.resize(mat.rows()); - // y.resize(mat.rows()); - // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - // matplot::plot(x, y, "b")->line_width(3); - // } - // for (const auto& mat : rawCompoundCenterlines) { - // std::vector x; - // std::vector y; - // x.resize(mat.rows()); - // y.resize(mat.rows()); - // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - // matplot::plot(x, y, "--gs")->line_width(3).marker_color("g"); - // } - // - // matplot::save("map_data_raw.png"); - // matplot::cla(); - // matplot::hold(matplot::on); - // matplot::xlim({-2, 16}); - // matplot::ylim({-13, 2}); - // matplot::gcf()->size(1000, 1000); - - bool valid = laneData.processAll(bbox, ParametrizationType::LineString, 10); - std::vector compoundRoadBorders = getPointsMatrixList(laneData.compoundRoadBorders(), true); - std::vector compoundLaneDividers = getPointsMatrixList(laneData.compoundLaneDividers(), true); - std::vector compoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); - - EXPECT_TRUE(laneData.laneletFeatures().find(2007) != laneData.laneletFeatures().end()); - EXPECT_EQ(laneData.laneletFeatures().find(2007)->second.leftBoundary().mapID(), 1012); - EXPECT_TRUE(laneData.roadBorders().find(1001) != laneData.roadBorders().end()); - - EXPECT_EQ(compoundRoadBorders.size(), 3); - EXPECT_EQ(compoundLaneDividers.size(), 8); - EXPECT_EQ(compoundCenterlines.size(), 4); - - // for (const auto& mat : compoundRoadBorders) { - // std::vector x; - // std::vector y; - // x.resize(mat.rows()); - // y.resize(mat.rows()); - // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - // matplot::plot(x, y, "r")->line_width(3); - // } - - // for (const auto& mat : compoundLaneDividers) { - // std::vector x; - // std::vector y; - // x.resize(mat.rows()); - // y.resize(mat.rows()); - // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - // matplot::plot(x, y, "b")->line_width(3); - // } - // for (const auto& mat : compoundCenterlines) { - // std::vector x; - // std::vector y; - // x.resize(mat.rows()); - // y.resize(mat.rows()); - // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); - // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - // matplot::plot(x, y, "--gs")->line_width(3).marker_color("g"); - // } - // - // matplot::save("map_data_processed.png"); + // matplot::hold(matplot::on); + // matplot::cla(); + // matplot::xlim({-2, 16}); + // matplot::ylim({-13, 2}); + // matplot::gcf()->size(1000, 1000); + // for (const auto& mat : compoundRoadBorders) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "r")->line_width(3); + // } + // for (const auto& mat : compoundLaneDividers) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "b")->line_width(3); + // } + // for (const auto& mat : compoundCenterlines) { + // std::vector x; + // std::vector y; + // x.resize(mat.rows()); + // y.resize(mat.rows()); + // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); + // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); + // matplot::plot(x, y, "--gs")->line_width(3).marker_color("g"); + // } + // matplot::save("map_data_processed_" + std::to_string(i) + ".png"); + } } \ No newline at end of file From b04797aeb756fc27ba80706782c885ec6191ef43 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Fri, 15 Dec 2023 18:44:23 +0100 Subject: [PATCH 32/64] wip serialization --- .../include/lanelet2_map_learning/Forward.h | 31 +++++ .../include/lanelet2_map_learning/MapData.h | 5 + .../lanelet2_map_learning/MapFeatures.h | 26 +++- .../include/lanelet2_map_learning/Serialize.h | 122 ++++++++++++++++++ .../include/lanelet2_map_learning/Utils.h | 11 +- lanelet2_map_learning/src/Utils.cpp | 30 +++++ .../test/test_map_data_interface.cpp | 29 +++++ 7 files changed, 248 insertions(+), 6 deletions(-) create mode 100644 lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h index 75fcab26..00c1a100 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h @@ -33,5 +33,36 @@ inline int relationToInt(lanelet::routing::RelationType type) { enum class LaneletRepresentationType; enum class ParametrizationType; +class MapFeature; +class LineStringFeature; +class LaneLineStringFeature; +class CompoundLaneLineStringFeature; +class LaneletFeature; +class LaneData; + } // namespace map_learning } // namespace lanelet + +namespace boost { +namespace serialization { + +template +void serialize(Archive& ar, lanelet::map_learning::MapFeature& feat, const unsigned int /*version*/); + +template +void serialize(Archive& ar, lanelet::map_learning::LineStringFeature& feat, const unsigned int /*version*/); + +template +void serialize(Archive& ar, lanelet::map_learning::LaneLineStringFeature& feat, const unsigned int /*version*/); + +template +void serialize(Archive& ar, lanelet::map_learning::CompoundLaneLineStringFeature& feat, const unsigned int /*version*/); + +template +void serialize(Archive& ar, lanelet::map_learning::LaneletFeature& feat, const unsigned int /*version*/); + +template +void serialize(Archive& ar, lanelet::map_learning::LaneData& feat, const unsigned int /*version*/); + +} // namespace serialization +} // namespace boost \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index d5efa098..9907fc06 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -17,6 +17,7 @@ namespace lanelet { namespace map_learning { struct Edge { + Edge() = default; Edge(Id el1, Id el2) : el1_{el1}, el2_{el2} {} Id el1_; Id el2_; @@ -55,6 +56,10 @@ class LaneData { const LaneletFeatures& laneletFeatures() { return laneletFeatures_; } const Edges& edges() { return edges_; } + template + friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::LaneData& feat, + const unsigned int /*version*/); + private: void initLeftBoundaries(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); void initRightBoundaries(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index dd337572..e86a560f 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -22,6 +22,12 @@ class MapFeature { virtual Eigen::VectorXd computeFeatureVector(bool /*unused*/, bool /*unused*/) const = 0; virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) = 0; + template + friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::MapFeature& feat, + const unsigned int /*version*/); + + virtual ~MapFeature() noexcept = default; + protected: bool initialized_{false}; bool wasCut_{false}; @@ -30,7 +36,6 @@ class MapFeature { MapFeature() = default; MapFeature(Id mapID) : mapID_{mapID}, initialized_{true} {} - virtual ~MapFeature() noexcept = default; }; class LineStringFeature : public MapFeature { @@ -41,12 +46,17 @@ class LineStringFeature : public MapFeature { virtual Eigen::MatrixXd pointMatrix(bool pointsIn2d) const = 0; virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) = 0; + template + friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::LineStringFeature& feat, + const unsigned int /*version*/); + + virtual ~LineStringFeature() noexcept = default; + protected: BasicLineString3d rawFeature_; LineStringFeature() {} LineStringFeature(const BasicLineString3d& feature, Id mapID) : MapFeature(mapID), rawFeature_{feature} {} - virtual ~LineStringFeature() noexcept = default; }; class LaneLineStringFeature : public LineStringFeature { @@ -70,6 +80,10 @@ class LaneLineStringFeature : public LineStringFeature { const std::vector& laneletIDs() const { return laneletIDs_; } void addLaneletID(Id id) { laneletIDs_.push_back(id); } + template + friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::LaneLineStringFeature& feat, + const unsigned int /*version*/); + protected: BasicLineString3d cutFeature_; BasicLineString3d cutAndResampledFeature_; @@ -117,6 +131,10 @@ class LaneletFeature : public MapFeature { const LaneLineStringFeature& rightBoundary() const { return rightBoundary_; } const LaneLineStringFeature& centerline() const { return centerline_; } + template + friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::LaneletFeature& feat, + const unsigned int /*version*/); + private: LaneLineStringFeature leftBoundary_; LaneLineStringFeature rightBoundary_; @@ -138,6 +156,10 @@ class CompoundLaneLineStringFeature : public LaneLineStringFeature { const std::vector& pathLengthsProcessed() const { return pathLengthsProcessed_; } const std::vector& processedFeaturesValid() const { return processedFeaturesValid_; } + template + friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::CompoundLaneLineStringFeature& feat, + const unsigned int /*version*/); + private: LaneLineStringFeatureList individualFeatures_; std::vector pathLengthsRaw_; diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h b/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h new file mode 100644 index 00000000..2d641aa5 --- /dev/null +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h @@ -0,0 +1,122 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "MapData.h" + +namespace boost { +namespace serialization { + +template +inline void save(Archive& ar, const Eigen::Matrix& g, + const unsigned int /*version*/) { + int rows = g.rows(); + int cols = g.cols(); + ar & rows; + ar & cols; + std::cerr << "Trying to save Eigen Mat!" << std::endl; + ar& boost::serialization::make_array(g.data(), rows * cols); + std::cerr << "Saved Eigen Mat!" << std::endl; +} + +template +inline void load(Archive& ar, Eigen::Matrix& g, + const unsigned int /*version*/) { + int rows, cols; + ar & rows; + ar & cols; + g.resize(rows, cols); + ar& boost::serialization::make_array(g.data(), rows * cols); +} + +template +inline void serialize(Archive& ar, Eigen::Matrix& g, + const unsigned int version) { + split_free(ar, g, version); +} + +template +void serialize(Archive& ar, lanelet::map_learning::LaneletRepresentationType& type, const unsigned int /*version*/) { + ar & type; +} + +template +void serialize(Archive& ar, lanelet::map_learning::ParametrizationType& type, const unsigned int /*version*/) { + ar & type; +} + +template +void serialize(Archive& ar, lanelet::map_learning::LineStringType& type, const unsigned int /*version*/) { + ar & type; +} + +template +void serialize(Archive& ar, lanelet::map_learning::MapFeature& feat, const unsigned int /*version*/) { + ar & feat.initialized_; + ar & feat.wasCut_; + ar & feat.valid_; + ar & feat.mapID_; +} + +template +void serialize(Archive& ar, lanelet::map_learning::LineStringFeature& feat, const unsigned int /*version*/) { + ar& boost::serialization::base_object(feat); + ar & feat.rawFeature_; +} + +template +void serialize(Archive& ar, lanelet::map_learning::LaneLineStringFeature& feat, const unsigned int /*version*/) { + ar& boost::serialization::base_object(feat); + ar & feat.cutFeature_; + ar & feat.cutAndResampledFeature_; + ar & feat.wasCut_; + ar & feat.type_; + ar & feat.laneletIDs_; +} + +template +void serialize(Archive& ar, lanelet::map_learning::CompoundLaneLineStringFeature& feat, + const unsigned int /*version*/) { + ar& boost::serialization::base_object(feat); + ar & feat.individualFeatures_; + ar & feat.pathLengthsRaw_; + ar & feat.pathLengthsProcessed_; + ar & feat.processedFeaturesValid_; + ar & feat.validLengthThresh_; +} + +template +void serialize(Archive& ar, lanelet::map_learning::LaneletFeature& feat, const unsigned int /*version*/) { + ar& boost::serialization::base_object(feat); + ar & feat.leftBoundary_; + ar & feat.rightBoundary_; + ar & feat.centerline_; + ar & feat.reprType_; +} + +template +void serialize(Archive& ar, lanelet::map_learning::Edge& edge, const unsigned int /*version*/) { + ar & edge.el1_; + ar & edge.el2_; +} + +template +void serialize(Archive& ar, lanelet::map_learning::LaneData& lData, const unsigned int /*version*/) { + ar & lData.roadBorders_; + ar & lData.laneDividers_; + ar & lData.compoundRoadBorders_; + ar & lData.compoundLaneDividers_; + ar & lData.compoundCenterlines_; + ar & lData.laneletFeatures_; + ar & lData.associatedCpdRoadBorderIndices_; + ar & lData.associatedCpdLaneDividerIndices_; + ar & lData.associatedCpdCenterlineIndices_; + ar & lData.edges_; +} + +} // namespace serialization +} // namespace boost \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h index a64db250..accc58ec 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h @@ -6,11 +6,10 @@ #include #include -#include - -#include "lanelet2_map_learning/Forward.h" -#include "lanelet2_map_learning/Types.h" +#include "Forward.h" +#include "Serialize.h" +#include "Types.h" namespace lanelet { namespace map_learning { @@ -68,5 +67,9 @@ BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t BasicLineString3d cutLineString(const OrientedRect& bbox, const BasicLineString3d& polyline); +void saveLaneData(const std::string& filename, const std::vector& lDataVec); + +std::vector loadLaneData(const std::string& filename); + } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index fd352ce3..a62ef70a 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -3,6 +3,11 @@ #include #include +#include +#include +#include + +namespace fs = boost::filesystem; namespace lanelet { namespace map_learning { @@ -69,6 +74,8 @@ BasicLineString3d cutLineString(const OrientedRect& bbox, const BasicLineString3 for (const auto& pt2d : cut2d[0]) { double lastDist = std::numeric_limits::max(); for (const auto& pt : polyline) { + std::cerr << "Point: " << pt.x() << ", " << pt.y() << std::endl; + double currDist = (pt2d - BasicPoint2d(pt.x(), pt.y())).norm(); if (currDist < lastDist) { lastDist = currDist; @@ -84,5 +91,28 @@ BasicLineString3d cutLineString(const OrientedRect& bbox, const BasicLineString3 return cut3d; } +void saveLaneData(const std::string& filename, const std::vector& lDataVec) { + std::ofstream fs(filename, std::ofstream::binary); + if (!fs.good()) { + throw std::runtime_error("Failed to open archive " + filename); + } + boost::archive::binary_oarchive oa(fs); + oa << lDataVec; +} + +std::vector loadLaneData(const std::string& filename) { + if (!fs::exists(fs::path(filename))) { + throw std::runtime_error("Could not find file under " + filename); + } + std::ifstream fs(filename, std::ifstream::binary); + if (!fs.good()) { + throw std::runtime_error("Failed to open archive " + filename); + } + boost::archive::binary_iarchive ia(fs); + std::vector lDataVec; + ia >> lDataVec; + return lDataVec; +} + } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/test/test_map_data_interface.cpp b/lanelet2_map_learning/test/test_map_data_interface.cpp index 9e37708c..14a7ce03 100644 --- a/lanelet2_map_learning/test/test_map_data_interface.cpp +++ b/lanelet2_map_learning/test/test_map_data_interface.cpp @@ -90,4 +90,33 @@ TEST_F(MapLearningTest, MapDataInterface) { // NOLINT // } // matplot::save("map_data_processed_" + std::to_string(i) + ".png"); } +} + +TEST_F(MapLearningTest, MapDataSaveLoad) { + traffic_rules::TrafficRulesPtr trafficRules{ + traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)}; + MapDataInterface::Configuration config{}; + config.reprType = LaneletRepresentationType::Centerline; + config.paramType = ParametrizationType::LineString; + config.submapExtentLongitudinal = 5; + config.submapExtentLateral = 3; + config.nPoints = 20; + MapDataInterface parser(laneletMap, config); + + BasicPoints2d pts{BasicPoint2d(0, -3), BasicPoint2d(3, -3), BasicPoint2d(5, -3), + BasicPoint2d(6, -5), BasicPoint2d(6, -8), BasicPoint2d(6, -11)}; + std::vector yaws{0, 0, M_PI / 4, M_PI / 3, M_PI / 2, M_PI / 2}; + + std::cerr << "Generated MapDataInterface!" << std::endl; + + std::vector lDataVec = parser.laneDataBatch(pts, yaws); + std::cerr << "Generated LaneData!" << std::endl; + + saveLaneData("/tmp/lane_data_save_test.bin", lDataVec); + std::vector lDataLoaded = loadLaneData("/tmp/lane_data_save_test.bin"); + + EXPECT_EQ(lDataVec.size(), lDataLoaded.size()); + EXPECT_EQ(lDataVec.front().compoundCenterlines().size(), lDataLoaded.front().compoundCenterlines().size()); + EXPECT_EQ(lDataVec.front().compoundRoadBorders().size(), lDataLoaded.front().compoundRoadBorders().size()); + EXPECT_EQ(lDataVec.back().compoundLaneDividers().size(), lDataLoaded.back().compoundLaneDividers().size()); } \ No newline at end of file From 03a8854cb7ba848c4e4148da73a0decd28efc70d Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Mon, 18 Dec 2023 18:00:29 +0100 Subject: [PATCH 33/64] finish serialization, bugfixes --- .../lanelet2_map_learning/MapDataInterface.h | 6 +- .../lanelet2_map_learning/MapFeatures.h | 8 +- .../include/lanelet2_map_learning/Serialize.h | 146 ++++++++++-------- .../include/lanelet2_map_learning/Utils.h | 4 +- .../src/MapDataInterface.cpp | 8 +- lanelet2_map_learning/src/MapFeatures.cpp | 5 +- lanelet2_map_learning/src/Utils.cpp | 67 +++++--- .../test/test_map_data_interface.cpp | 24 ++- lanelet2_map_learning/test/test_utils.cpp | 9 +- lanelet2_python/package.xml | 1 + 10 files changed, 168 insertions(+), 110 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h index 8ae5129f..805f456e 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h @@ -41,8 +41,10 @@ class MapDataInterface { LaneData laneData(); TEData teData(); - std::vector laneDataBatch(const BasicPoints2d& pts, const std::vector& yaws); - std::vector laneTEDataBatch(const BasicPoints2d& pts, const std::vector& yaws); + std::vector laneDataBatch(const BasicPoints2d& pts, const std::vector& yaws, + bool processAll = true); + std::vector laneTEDataBatch(const BasicPoints2d& pts, const std::vector& yaws, + bool processAll = true); private: LaneData getLaneData(LaneletSubmapConstPtr localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph, diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index e86a560f..c7f4301d 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -183,7 +183,9 @@ Eigen::MatrixXd getFeatureVectorMatrix(const std::map& mapFeatures, bool template Eigen::MatrixXd getFeatureVectorMatrix(const std::vector& mapFeatures, bool onlyPoints, bool pointsIn2d) { - assert(!mapFeatures.empty()); + if (mapFeatures.empty()) { + throw std::runtime_error("Empty mapFeatures vector supplied!"); + } std::vector featureVectors; for (const auto& feat : mapFeatures) { if (!feat.valid()) { @@ -215,7 +217,9 @@ std::vector getPointsMatrixList(const std::map& mapFeatu template std::vector getPointsMatrixList(const std::vector& mapFeatures, bool pointsIn2d) { - assert(!mapFeatures.empty()); + if (mapFeatures.empty()) { + throw std::runtime_error("Empty mapFeatures vector supplied!"); + } std::vector pointMatrices; for (const auto& feat : mapFeatures) { if (!feat.valid()) { diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h b/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h index 2d641aa5..7ac4f1eb 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h @@ -1,9 +1,9 @@ #pragma once +#include #include #include #include -#include #include #include "MapData.h" @@ -11,112 +11,130 @@ namespace boost { namespace serialization { +// from https://stackoverflow.com/a/35074759 template -inline void save(Archive& ar, const Eigen::Matrix& g, - const unsigned int /*version*/) { - int rows = g.rows(); - int cols = g.cols(); - ar & rows; - ar & cols; - std::cerr << "Trying to save Eigen Mat!" << std::endl; - ar& boost::serialization::make_array(g.data(), rows * cols); - std::cerr << "Saved Eigen Mat!" << std::endl; -} - -template -inline void load(Archive& ar, Eigen::Matrix& g, - const unsigned int /*version*/) { - int rows, cols; - ar & rows; - ar & cols; - g.resize(rows, cols); - ar& boost::serialization::make_array(g.data(), rows * cols); -} - -template -inline void serialize(Archive& ar, Eigen::Matrix& g, +inline void serialize(Archive& ar, Eigen::Matrix& matrix, const unsigned int version) { - split_free(ar, g, version); + int rows = matrix.rows(); + int cols = matrix.cols(); + ar& make_nvp("rows", rows); + ar& make_nvp("cols", cols); + matrix.resize(rows, cols); // no-op if size does not change! + + // always save/load row-major + for (int r = 0; r < rows; ++r) + for (int c = 0; c < cols; ++c) ar& make_nvp("val", matrix(r, c)); } +// prevent unneccessary boost serialization xml tags +// template +// struct implementation_level_impl> { +// typedef mpl::integral_c_tag tag; +// typedef mpl::int_ type; +// BOOST_STATIC_CONSTANT(int, value = implementation_level_impl::type::value); +// }; + template void serialize(Archive& ar, lanelet::map_learning::LaneletRepresentationType& type, const unsigned int /*version*/) { - ar & type; + ar& BOOST_SERIALIZATION_NVP(type); } template void serialize(Archive& ar, lanelet::map_learning::ParametrizationType& type, const unsigned int /*version*/) { - ar & type; + ar& BOOST_SERIALIZATION_NVP(type); } template void serialize(Archive& ar, lanelet::map_learning::LineStringType& type, const unsigned int /*version*/) { - ar & type; + ar& BOOST_SERIALIZATION_NVP(type); } template void serialize(Archive& ar, lanelet::map_learning::MapFeature& feat, const unsigned int /*version*/) { - ar & feat.initialized_; - ar & feat.wasCut_; - ar & feat.valid_; - ar & feat.mapID_; + ar& BOOST_SERIALIZATION_NVP(feat.initialized_); + ar& BOOST_SERIALIZATION_NVP(feat.wasCut_); + ar& BOOST_SERIALIZATION_NVP(feat.valid_); + ar& BOOST_SERIALIZATION_NVP(feat.mapID_); } template void serialize(Archive& ar, lanelet::map_learning::LineStringFeature& feat, const unsigned int /*version*/) { - ar& boost::serialization::base_object(feat); - ar & feat.rawFeature_; + ar& make_nvp("MapFeature", boost::serialization::base_object(feat)); + ar& BOOST_SERIALIZATION_NVP(feat.rawFeature_); } template void serialize(Archive& ar, lanelet::map_learning::LaneLineStringFeature& feat, const unsigned int /*version*/) { - ar& boost::serialization::base_object(feat); - ar & feat.cutFeature_; - ar & feat.cutAndResampledFeature_; - ar & feat.wasCut_; - ar & feat.type_; - ar & feat.laneletIDs_; + ar& make_nvp("LineStringFeature", boost::serialization::base_object(feat)); + ar& BOOST_SERIALIZATION_NVP(feat.cutFeature_); + ar& BOOST_SERIALIZATION_NVP(feat.cutAndResampledFeature_); + ar& BOOST_SERIALIZATION_NVP(feat.wasCut_); + ar& BOOST_SERIALIZATION_NVP(feat.type_); + ar& BOOST_SERIALIZATION_NVP(feat.laneletIDs_); } template void serialize(Archive& ar, lanelet::map_learning::CompoundLaneLineStringFeature& feat, const unsigned int /*version*/) { - ar& boost::serialization::base_object(feat); - ar & feat.individualFeatures_; - ar & feat.pathLengthsRaw_; - ar & feat.pathLengthsProcessed_; - ar & feat.processedFeaturesValid_; - ar & feat.validLengthThresh_; + ar& make_nvp("LaneLineStringFeature", + boost::serialization::base_object(feat)); + ar& BOOST_SERIALIZATION_NVP(feat.individualFeatures_); + ar& BOOST_SERIALIZATION_NVP(feat.pathLengthsRaw_); + ar& BOOST_SERIALIZATION_NVP(feat.pathLengthsProcessed_); + ar& BOOST_SERIALIZATION_NVP(feat.processedFeaturesValid_); + ar& BOOST_SERIALIZATION_NVP(feat.validLengthThresh_); } template void serialize(Archive& ar, lanelet::map_learning::LaneletFeature& feat, const unsigned int /*version*/) { - ar& boost::serialization::base_object(feat); - ar & feat.leftBoundary_; - ar & feat.rightBoundary_; - ar & feat.centerline_; - ar & feat.reprType_; + ar& make_nvp("MapFeature", boost::serialization::base_object(feat)); + ar& BOOST_SERIALIZATION_NVP(feat.leftBoundary_); + ar& BOOST_SERIALIZATION_NVP(feat.rightBoundary_); + ar& BOOST_SERIALIZATION_NVP(feat.centerline_); + ar& BOOST_SERIALIZATION_NVP(feat.reprType_); } template void serialize(Archive& ar, lanelet::map_learning::Edge& edge, const unsigned int /*version*/) { - ar & edge.el1_; - ar & edge.el2_; + ar& BOOST_SERIALIZATION_NVP(edge.el1_); + ar& BOOST_SERIALIZATION_NVP(edge.el2_); } template void serialize(Archive& ar, lanelet::map_learning::LaneData& lData, const unsigned int /*version*/) { - ar & lData.roadBorders_; - ar & lData.laneDividers_; - ar & lData.compoundRoadBorders_; - ar & lData.compoundLaneDividers_; - ar & lData.compoundCenterlines_; - ar & lData.laneletFeatures_; - ar & lData.associatedCpdRoadBorderIndices_; - ar & lData.associatedCpdLaneDividerIndices_; - ar & lData.associatedCpdCenterlineIndices_; - ar & lData.edges_; + ar& BOOST_SERIALIZATION_NVP(lData.roadBorders_); + ar& BOOST_SERIALIZATION_NVP(lData.laneDividers_); + ar& BOOST_SERIALIZATION_NVP(lData.compoundRoadBorders_); + ar& BOOST_SERIALIZATION_NVP(lData.compoundLaneDividers_); + ar& BOOST_SERIALIZATION_NVP(lData.compoundCenterlines_); + ar& BOOST_SERIALIZATION_NVP(lData.laneletFeatures_); + ar& BOOST_SERIALIZATION_NVP(lData.associatedCpdRoadBorderIndices_); + ar& BOOST_SERIALIZATION_NVP(lData.associatedCpdLaneDividerIndices_); + ar& BOOST_SERIALIZATION_NVP(lData.associatedCpdCenterlineIndices_); + ar& BOOST_SERIALIZATION_NVP(lData.edges_); } } // namespace serialization -} // namespace boost \ No newline at end of file +} // namespace boost + +// prevent unneccessary boost serialization xml tags +// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LaneData, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::Edge, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LaneletFeature, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LaneLineStringFeature, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::CompoundLaneLineStringFeature, +// boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LineStringFeature, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::MapFeature, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LineStringType, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::ParametrizationType, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LaneletRepresentationType, +// boost::serialization::object_serializable); +// +// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LaneLineStringFeatures, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LaneLineStringFeatureList, +// boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::CompoundLaneLineStringFeatureList, +// boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LaneletFeatures, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::BasicLineString3d, boost::serialization::object_serializable); diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h index accc58ec..2be08c60 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h @@ -67,9 +67,9 @@ BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t BasicLineString3d cutLineString(const OrientedRect& bbox, const BasicLineString3d& polyline); -void saveLaneData(const std::string& filename, const std::vector& lDataVec); +void saveLaneData(const std::string& filename, const std::vector& lDataVec, bool binary = false); -std::vector loadLaneData(const std::string& filename); +std::vector loadLaneData(const std::string& filename, bool binary = false); } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapDataInterface.cpp b/lanelet2_map_learning/src/MapDataInterface.cpp index 80f799ba..a161d495 100644 --- a/lanelet2_map_learning/src/MapDataInterface.cpp +++ b/lanelet2_map_learning/src/MapDataInterface.cpp @@ -39,7 +39,8 @@ LaneData MapDataInterface::getLaneData(LaneletSubmapConstPtr localSubmap, return laneData; } -std::vector MapDataInterface::laneDataBatch(const BasicPoints2d& pts, const std::vector& yaws) { +std::vector MapDataInterface::laneDataBatch(const BasicPoints2d& pts, const std::vector& yaws, + bool processAll) { if (pts.size() != yaws.size()) { throw std::runtime_error("Unequal sizes of pts and yaws!"); } @@ -49,12 +50,13 @@ std::vector MapDataInterface::laneDataBatch(const BasicPoints2d& pts, extractSubmap(laneletMap_, pts[i], config_.submapExtentLongitudinal, config_.submapExtentLateral); routing::RoutingGraphConstPtr localSubmapGraph = lanelet::routing::RoutingGraph::build(*localSubmap, *trafficRules_); - lDataVec.push_back(getLaneData(localSubmap, localSubmapGraph, pts[i], yaws[i])); + lDataVec.push_back(getLaneData(localSubmap, localSubmapGraph, pts[i], yaws[i], processAll)); } return lDataVec; } -std::vector MapDataInterface::laneTEDataBatch(const BasicPoints2d& pts, const std::vector& yaws) { +std::vector MapDataInterface::laneTEDataBatch(const BasicPoints2d& pts, const std::vector& yaws, + bool processAll) { throw std::runtime_error("Not implemented yet!"); } diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index a14d533e..894c7590 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -33,7 +33,6 @@ LStringProcessResult processLineStringImpl(const BasicLineString3d& lstring, con double lengthProcessed = boost::geometry::length(result.cutAndResampledFeature_, boost::geometry::strategy::distance::pythagoras()); - assert(lengthOriginal - lengthProcessed > -1e-2); if (lengthOriginal - lengthProcessed > 1e-2) { result.wasCut_ = true; } @@ -197,7 +196,9 @@ CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStrin type_ = compoundType; for (size_t i = 0; i < features.size(); i++) { - assert(features[i].rawFeature().size() > 1); + if (features[i].rawFeature().empty()) { + throw std::runtime_error("Feature with empty rawFeature() supplied!"); + } if (i == features.size() - 1) { rawFeature_.insert(rawFeature_.end(), features[i].rawFeature().begin(), features[i].rawFeature().end()); } else { diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index a62ef70a..b73209ad 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -5,6 +5,8 @@ #include #include +#include +#include #include namespace fs = boost::filesystem; @@ -67,50 +69,69 @@ BasicLineString3d cutLineString(const OrientedRect& bbox, const BasicLineString3 } std::deque cut2d; boost::geometry::intersection(bbox, polyline2d, cut2d); - assert(cut2d.size() == 1); - // restore z value from closest point on the original linestring BasicLineString3d cut3d; + if (cut2d.empty()) { + return cut3d; + } else if (cut2d.size() > 1) { + throw std::runtime_error("More than one cut line!"); + } + + // restore z value from closest point on the original linestring and remove double points of boost intersection for (const auto& pt2d : cut2d[0]) { double lastDist = std::numeric_limits::max(); + double bestZ; for (const auto& pt : polyline) { - std::cerr << "Point: " << pt.x() << ", " << pt.y() << std::endl; - double currDist = (pt2d - BasicPoint2d(pt.x(), pt.y())).norm(); if (currDist < lastDist) { lastDist = currDist; - if (pt == polyline.back()) { - cut3d.push_back(BasicPoint3d(pt2d.x(), pt2d.y(), pt.z())); - } - } else { - cut3d.push_back(BasicPoint3d(pt2d.x(), pt2d.y(), pt.z())); + bestZ = pt.z(); } } + cut3d.push_back(BasicPoint3d(pt2d.x(), pt2d.y(), bestZ)); } - return cut3d; } -void saveLaneData(const std::string& filename, const std::vector& lDataVec) { - std::ofstream fs(filename, std::ofstream::binary); - if (!fs.good()) { - throw std::runtime_error("Failed to open archive " + filename); +void saveLaneData(const std::string& filename, const std::vector& lDataVec, bool binary) { + if (binary) { + std::ofstream fs(filename, std::ofstream::binary); + if (!fs.good()) { + throw std::runtime_error("Failed to open archive " + filename); + } + boost::archive::binary_oarchive oa(fs); + oa << lDataVec; + } else { + std::ofstream fs(filename); + if (!fs.good()) { + throw std::runtime_error("Failed to open archive " + filename); + } + boost::archive::xml_oarchive oa(fs, boost::archive::no_header); + oa << BOOST_SERIALIZATION_NVP(lDataVec); } - boost::archive::binary_oarchive oa(fs); - oa << lDataVec; } -std::vector loadLaneData(const std::string& filename) { +std::vector loadLaneData(const std::string& filename, bool binary) { if (!fs::exists(fs::path(filename))) { throw std::runtime_error("Could not find file under " + filename); } - std::ifstream fs(filename, std::ifstream::binary); - if (!fs.good()) { - throw std::runtime_error("Failed to open archive " + filename); - } - boost::archive::binary_iarchive ia(fs); std::vector lDataVec; - ia >> lDataVec; + if (binary) { + std::ifstream fs(filename, std::ifstream::binary); + if (!fs.good()) { + throw std::runtime_error("Failed to open archive " + filename); + } + boost::archive::binary_iarchive ia(fs); + ia >> lDataVec; + } else { + std::ifstream fs(filename); + if (!fs.good()) { + throw std::runtime_error("Failed to open archive " + filename); + } + boost::archive::xml_iarchive ia(fs, boost::archive::no_header); + ia >> BOOST_SERIALIZATION_NVP(lDataVec); + } + return lDataVec; } diff --git a/lanelet2_map_learning/test/test_map_data_interface.cpp b/lanelet2_map_learning/test/test_map_data_interface.cpp index 14a7ce03..a51c5261 100644 --- a/lanelet2_map_learning/test/test_map_data_interface.cpp +++ b/lanelet2_map_learning/test/test_map_data_interface.cpp @@ -1,5 +1,7 @@ #include #include + +#include // #include #include "lanelet2_map_learning/MapData.h" @@ -12,6 +14,12 @@ using namespace lanelet; using namespace lanelet::map_learning; using namespace lanelet::map_learning::tests; +// template +// auto since(std::chrono::time_point const& start) { +// return std::chrono::duration_cast(clock_t::now() - start); +// } + TEST_F(MapLearningTest, MapDataInterface) { // NOLINT traffic_rules::TrafficRulesPtr trafficRules{ traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)}; @@ -26,7 +34,10 @@ TEST_F(MapLearningTest, MapDataInterface) { // NOLINT BasicPoints2d pts{BasicPoint2d(0, -3), BasicPoint2d(3, -3), BasicPoint2d(5, -3), BasicPoint2d(6, -5), BasicPoint2d(6, -8), BasicPoint2d(6, -11)}; std::vector yaws{0, 0, M_PI / 4, M_PI / 3, M_PI / 2, M_PI / 2}; + + // auto start = std::chrono::steady_clock::now(); std::vector lDataVec = parser.laneDataBatch(pts, yaws); + // std::cerr << "Elapsed(micros)=" << since(start).count() << std::endl; for (size_t i = 0; i < lDataVec.size(); i++) { std::vector compoundRoadBorders = @@ -107,14 +118,17 @@ TEST_F(MapLearningTest, MapDataSaveLoad) { BasicPoint2d(6, -5), BasicPoint2d(6, -8), BasicPoint2d(6, -11)}; std::vector yaws{0, 0, M_PI / 4, M_PI / 3, M_PI / 2, M_PI / 2}; - std::cerr << "Generated MapDataInterface!" << std::endl; - std::vector lDataVec = parser.laneDataBatch(pts, yaws); - std::cerr << "Generated LaneData!" << std::endl; - saveLaneData("/tmp/lane_data_save_test.bin", lDataVec); - std::vector lDataLoaded = loadLaneData("/tmp/lane_data_save_test.bin"); + saveLaneData("/tmp/lane_data_save_test.xml", lDataVec); + std::vector lDataLoaded = loadLaneData("/tmp/lane_data_save_test.xml"); + EXPECT_EQ(lDataVec.size(), lDataLoaded.size()); + EXPECT_EQ(lDataVec.front().compoundCenterlines().size(), lDataLoaded.front().compoundCenterlines().size()); + EXPECT_EQ(lDataVec.front().compoundRoadBorders().size(), lDataLoaded.front().compoundRoadBorders().size()); + EXPECT_EQ(lDataVec.back().compoundLaneDividers().size(), lDataLoaded.back().compoundLaneDividers().size()); + saveLaneData("/tmp/lane_data_save_test.bin", lDataVec, true); + lDataLoaded = loadLaneData("/tmp/lane_data_save_test.bin", true); EXPECT_EQ(lDataVec.size(), lDataLoaded.size()); EXPECT_EQ(lDataVec.front().compoundCenterlines().size(), lDataLoaded.front().compoundCenterlines().size()); EXPECT_EQ(lDataVec.front().compoundRoadBorders().size(), lDataLoaded.front().compoundRoadBorders().size()); diff --git a/lanelet2_map_learning/test/test_utils.cpp b/lanelet2_map_learning/test/test_utils.cpp index d3d36ea6..c0024b95 100644 --- a/lanelet2_map_learning/test/test_utils.cpp +++ b/lanelet2_map_learning/test/test_utils.cpp @@ -42,15 +42,10 @@ TEST(UtilsTest, ResampleLineString) { // NOLINT } TEST_F(MapLearningTest, CutLineString) { // NOLINT - BasicLineString3d polyline{BasicPoint3d{0, 0, 0}, BasicPoint3d{30, 0, 0}}; + BasicLineString3d polyline{BasicPoint3d{0, 0, 0}, BasicPoint3d{30, 0, 0}, BasicPoint3d{40, 0, 0}, + BasicPoint3d{50, 0, 0}}; BasicLineString3d polylineCut = cutLineString(bbox, polyline); - // for (const auto& pt : polylineCut) { - // std::cerr << pt << '\n'; - // std::cerr << "---" << '\n'; - // } - // std::cerr << "---------------------------" << '\n'; - EXPECT_EQ(polylineCut.size(), 2); EXPECT_NEAR(polylineCut[1].x(), 15, 10e-5); } \ No newline at end of file diff --git a/lanelet2_python/package.xml b/lanelet2_python/package.xml index 57d6d3ed..2b4f4f7c 100644 --- a/lanelet2_python/package.xml +++ b/lanelet2_python/package.xml @@ -18,6 +18,7 @@ lanelet2_core lanelet2_io lanelet2_routing + lanelet2_map_learning lanelet2_traffic_rules lanelet2_projection lanelet2_matching From 6bb3a84c0760902dccd12fa4873aad368f3a40ca Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Mon, 18 Dec 2023 19:58:08 +0100 Subject: [PATCH 34/64] wip python bindings --- .../lanelet2_map_learning/MapFeatures.h | 1 - .../test/test_map_data_interface.cpp | 4 +- lanelet2_python/python_api/map_learning.cpp | 51 +++++++++++++++++++ 3 files changed, 53 insertions(+), 3 deletions(-) create mode 100644 lanelet2_python/python_api/map_learning.cpp diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index c7f4301d..296e40b5 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -30,7 +30,6 @@ class MapFeature { protected: bool initialized_{false}; - bool wasCut_{false}; bool valid_{true}; Optional mapID_; diff --git a/lanelet2_map_learning/test/test_map_data_interface.cpp b/lanelet2_map_learning/test/test_map_data_interface.cpp index a51c5261..04275136 100644 --- a/lanelet2_map_learning/test/test_map_data_interface.cpp +++ b/lanelet2_map_learning/test/test_map_data_interface.cpp @@ -1,13 +1,13 @@ #include -#include -#include +// #include // #include #include "lanelet2_map_learning/MapData.h" #include "lanelet2_map_learning/MapDataInterface.h" #include "lanelet2_map_learning/MapFeatures.h" #include "lanelet2_routing/RoutingGraph.h" +#include "lanelet2_traffic_rules/TrafficRulesFactory.h" #include "test_map.h" using namespace lanelet; diff --git a/lanelet2_python/python_api/map_learning.cpp b/lanelet2_python/python_api/map_learning.cpp new file mode 100644 index 00000000..8ac39f67 --- /dev/null +++ b/lanelet2_python/python_api/map_learning.cpp @@ -0,0 +1,51 @@ +#include "lanelet2_map_learning/MapData.h" +#include "lanelet2_map_learning/MapDataInterface.h" +#include "lanelet2_map_learning/MapFeatures.h" +#include "lanelet2_python/internal/converter.h" +#include "lanelet2_python/internal/eigen_converter.h" + +using namespace boost::python; +using namespace lanelet; + +/// TODO: ADD VIRTUAL FUNCTIONS, CONSTRUCTORS + +BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT + class_("MapFeature", "Abstract base map feature class", no_init) + .add_property("initialized", &map_learning::MapFeature::initialized) + .add_property("valid", &map_learning::MapFeature::valid) + .add_property("mapID", &map_learning::MapFeature::mapID); + + class_>( + "LineStringFeature", "Abstract line string feature class", no_init) + .add_property("rawFeature", &map_learning::LineStringFeature::rawFeature); + + class_>( + "LaneLineStringFeature", "Lane line string feature class", + init()) + .add_property("wasCut", &map_learning::LaneLineStringFeature::wasCut) + .add_property("cutFeature", &map_learning::LaneLineStringFeature::cutFeature) + .add_property("cutAndResampledFeature", &map_learning::LaneLineStringFeature::cutAndResampledFeature) + .add_property("type", &map_learning::LaneLineStringFeature::type) + .add_property("typeInt", &map_learning::LaneLineStringFeature::typeInt) + .add_property("laneletIDs", &map_learning::LaneLineStringFeature::laneletIDs) + .add_property("addLaneletID", &map_learning::LaneLineStringFeature::addLaneletID); + + class_>( + "LaneletFeature", "Lanelet feature class that contains lower level LaneLineStringFeatures", + init()) + .def(init()) + .add_property("leftBoundary", &map_learning::LaneletFeature::leftBoundary) + .add_property("rightBoundary", &map_learning::LaneletFeature::rightBoundary) + .add_property("centerline", &map_learning::LaneletFeature::centerline) + .add_property("setReprType", &map_learning::LaneletFeature::setReprType); + + class_>( + "CompoundLaneLineStringFeature", + "Compound lane line string feature class that can trace back the individual features", + init()); + + class_("Edge"); + + class_("MapFeature"); +} \ No newline at end of file From eeb1f1d5843d3f4e06207d1a4b922d144d66e9b0 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Wed, 20 Dec 2023 20:20:23 +0100 Subject: [PATCH 35/64] cont wip python bindings --- .../lanelet2_map_learning/MapFeatures.h | 20 +-- .../include/lanelet2_map_learning/Serialize.h | 1 - lanelet2_map_learning/test/test_map_data.cpp | 12 +- .../test/test_map_data_interface.cpp | 6 +- lanelet2_python/python_api/map_learning.cpp | 148 ++++++++++++++---- 5 files changed, 134 insertions(+), 53 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index 296e40b5..9b427b51 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -15,9 +15,10 @@ namespace map_learning { class MapFeature { public: - const Id& mapID() const { return mapID_.get_value_or(InvalId); } - const bool& initialized() const { return initialized_; } - const bool& valid() const { return valid_; } + const Id mapID() const { return mapID_.get_value_or(InvalId); } + bool initialized() const { return initialized_; } + bool valid() const { return valid_; } + bool wasCut() const { return wasCut_; } virtual Eigen::VectorXd computeFeatureVector(bool /*unused*/, bool /*unused*/) const = 0; virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) = 0; @@ -31,6 +32,7 @@ class MapFeature { protected: bool initialized_{false}; bool valid_{true}; + bool wasCut_{false}; Optional mapID_; MapFeature() = default; @@ -71,10 +73,9 @@ class LaneLineStringFeature : public LineStringFeature { bool pointsIn2d) const override; // uses processedFeature_ when available virtual Eigen::MatrixXd pointMatrix(bool pointsIn2d) const override; // uses processedFeature_ when available - const bool& wasCut() const { return wasCut_; } const BasicLineString3d& cutFeature() const { return cutFeature_; } const BasicLineString3d& cutAndResampledFeature() const { return cutAndResampledFeature_; } - const LineStringType& type() const { return type_; } + LineStringType type() const { return type_; } int typeInt() const { return static_cast(type_); } const std::vector& laneletIDs() const { return laneletIDs_; } void addLaneletID(Id id) { laneletIDs_.push_back(id); } @@ -86,7 +87,6 @@ class LaneLineStringFeature : public LineStringFeature { protected: BasicLineString3d cutFeature_; BasicLineString3d cutAndResampledFeature_; - bool wasCut_{false}; LineStringType type_; std::vector laneletIDs_; }; @@ -164,7 +164,7 @@ class CompoundLaneLineStringFeature : public LaneLineStringFeature { std::vector pathLengthsRaw_; std::vector pathLengthsProcessed_; std::vector processedFeaturesValid_; - double validLengthThresh_{0.5}; + double validLengthThresh_{0.3}; }; using CompoundLaneLineStringFeatureList = std::vector; @@ -206,16 +206,16 @@ Eigen::MatrixXd getFeatureVectorMatrix(const std::vector& mapFeatures, bool o } template -std::vector getPointsMatrixList(const std::map& mapFeatures, bool pointsIn2d) { +std::vector getPointsMatrices(const std::map& mapFeatures, bool pointsIn2d) { std::vector featList; for (const auto& pair : mapFeatures) { featList.push_back(pair.second); } - return getPointsMatrixList(featList, pointsIn2d); + return getPointsMatrices(featList, pointsIn2d); } template -std::vector getPointsMatrixList(const std::vector& mapFeatures, bool pointsIn2d) { +std::vector getPointsMatrices(const std::vector& mapFeatures, bool pointsIn2d) { if (mapFeatures.empty()) { throw std::runtime_error("Empty mapFeatures vector supplied!"); } diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h b/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h index 7ac4f1eb..bc163eb8 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h @@ -68,7 +68,6 @@ void serialize(Archive& ar, lanelet::map_learning::LaneLineStringFeature& feat, ar& make_nvp("LineStringFeature", boost::serialization::base_object(feat)); ar& BOOST_SERIALIZATION_NVP(feat.cutFeature_); ar& BOOST_SERIALIZATION_NVP(feat.cutAndResampledFeature_); - ar& BOOST_SERIALIZATION_NVP(feat.wasCut_); ar& BOOST_SERIALIZATION_NVP(feat.type_); ar& BOOST_SERIALIZATION_NVP(feat.laneletIDs_); } diff --git a/lanelet2_map_learning/test/test_map_data.cpp b/lanelet2_map_learning/test/test_map_data.cpp index 094a95b1..8eb5c7eb 100644 --- a/lanelet2_map_learning/test/test_map_data.cpp +++ b/lanelet2_map_learning/test/test_map_data.cpp @@ -21,14 +21,14 @@ TEST_F(MapLearningTest, LaneData) { // NOLINT LaneData laneData = LaneData::build(laneletSubmap, laneletMapGraph); - std::vector rawCompoundRoadBorders = getPointsMatrixList(laneData.compoundRoadBorders(), true); - std::vector rawCompoundLaneDividers = getPointsMatrixList(laneData.compoundLaneDividers(), true); - std::vector rawCompoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); + std::vector rawCompoundRoadBorders = getPointsMatrices(laneData.compoundRoadBorders(), true); + std::vector rawCompoundLaneDividers = getPointsMatrices(laneData.compoundLaneDividers(), true); + std::vector rawCompoundCenterlines = getPointsMatrices(laneData.compoundCenterlines(), true); bool valid = laneData.processAll(bbox, ParametrizationType::LineString, 20); - std::vector compoundRoadBorders = getPointsMatrixList(laneData.compoundRoadBorders(), true); - std::vector compoundLaneDividers = getPointsMatrixList(laneData.compoundLaneDividers(), true); - std::vector compoundCenterlines = getPointsMatrixList(laneData.compoundCenterlines(), true); + std::vector compoundRoadBorders = getPointsMatrices(laneData.compoundRoadBorders(), true); + std::vector compoundLaneDividers = getPointsMatrices(laneData.compoundLaneDividers(), true); + std::vector compoundCenterlines = getPointsMatrices(laneData.compoundCenterlines(), true); EXPECT_TRUE(laneData.laneletFeatures().find(2007) != laneData.laneletFeatures().end()); EXPECT_EQ(laneData.laneletFeatures().find(2007)->second.leftBoundary().mapID(), 1012); diff --git a/lanelet2_map_learning/test/test_map_data_interface.cpp b/lanelet2_map_learning/test/test_map_data_interface.cpp index 04275136..2380e902 100644 --- a/lanelet2_map_learning/test/test_map_data_interface.cpp +++ b/lanelet2_map_learning/test/test_map_data_interface.cpp @@ -41,11 +41,11 @@ TEST_F(MapLearningTest, MapDataInterface) { // NOLINT for (size_t i = 0; i < lDataVec.size(); i++) { std::vector compoundRoadBorders = - getPointsMatrixList(getValidElements(lDataVec[i].compoundRoadBorders()), true); + getPointsMatrices(getValidElements(lDataVec[i].compoundRoadBorders()), true); std::vector compoundLaneDividers = - getPointsMatrixList(getValidElements(lDataVec[i].compoundLaneDividers()), true); + getPointsMatrices(getValidElements(lDataVec[i].compoundLaneDividers()), true); std::vector compoundCenterlines = - getPointsMatrixList(getValidElements(lDataVec[i].compoundCenterlines()), true); + getPointsMatrices(getValidElements(lDataVec[i].compoundCenterlines()), true); switch (i) { case 0: { diff --git a/lanelet2_python/python_api/map_learning.cpp b/lanelet2_python/python_api/map_learning.cpp index 8ac39f67..627b8cb5 100644 --- a/lanelet2_python/python_api/map_learning.cpp +++ b/lanelet2_python/python_api/map_learning.cpp @@ -1,3 +1,8 @@ +#include +#include +#include +#include + #include "lanelet2_map_learning/MapData.h" #include "lanelet2_map_learning/MapDataInterface.h" #include "lanelet2_map_learning/MapFeatures.h" @@ -6,46 +11,123 @@ using namespace boost::python; using namespace lanelet; +using namespace lanelet::map_learning; + +class GetPointsMatricesWrapper { + public: + template + static std::vector getPointsMatricesMap(const std::map& mapFeatures, bool pointsIn2d) { + return getPointsMatrices(mapFeatures, pointsIn2d); + } + + template + static std::vector getPointsMatricesVector(const std::vector& mapFeatures, bool pointsIn2d) { + return getPointsMatrices(mapFeatures, pointsIn2d); + } + + static void wrap() { + // Expose the templated functions to Python + def("getPointsMatricesMap", &GetPointsMatricesWrapper::getPointsMatricesMap); + def("getPointsMatricesVector", &GetPointsMatricesWrapper::getPointsMatricesVector); + def("getPointsMatricesMap", &GetPointsMatricesWrapper::getPointsMatricesMap); + def("getPointsMatricesVector", &GetPointsMatricesWrapper::getPointsMatricesVector); + } +}; -/// TODO: ADD VIRTUAL FUNCTIONS, CONSTRUCTORS +/// TODO: ADD VIRTUAL FUNCTIONS, CONSTRUCTORS, EIGEN TO NUMPY BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT - class_("MapFeature", "Abstract base map feature class", no_init) - .add_property("initialized", &map_learning::MapFeature::initialized) - .add_property("valid", &map_learning::MapFeature::valid) - .add_property("mapID", &map_learning::MapFeature::mapID); - - class_>( - "LineStringFeature", "Abstract line string feature class", no_init) - .add_property("rawFeature", &map_learning::LineStringFeature::rawFeature); - - class_>( - "LaneLineStringFeature", "Lane line string feature class", - init()) - .add_property("wasCut", &map_learning::LaneLineStringFeature::wasCut) - .add_property("cutFeature", &map_learning::LaneLineStringFeature::cutFeature) - .add_property("cutAndResampledFeature", &map_learning::LaneLineStringFeature::cutAndResampledFeature) - .add_property("type", &map_learning::LaneLineStringFeature::type) - .add_property("typeInt", &map_learning::LaneLineStringFeature::typeInt) - .add_property("laneletIDs", &map_learning::LaneLineStringFeature::laneletIDs) - .add_property("addLaneletID", &map_learning::LaneLineStringFeature::addLaneletID); - - class_>( + + enum_("LaneletRepresentationType") + .value("Centerline", LaneletRepresentationType::Centerline) + .value("Boundaries", LaneletRepresentationType::Boundaries); + + enum_("ParametrizationType") + .value("Bezier", ParametrizationType::Bezier) + .value("BezierEndpointFixed", ParametrizationType::BezierEndpointFixed) + .value("LineString", ParametrizationType::LineString); + + enum_("LineStringType") + .value("RoadBorder", LineStringType::RoadBorder) + .value("Dashed", LineStringType::Dashed) + .value("Solid", LineStringType::Solid) + .value("Mixed", LineStringType::Mixed) + .value("Virtual", LineStringType::Virtual) + .value("Centerline", LineStringType::Centerline) + .value("Unknown", LineStringType::Unknown); + + class_("MapFeature", "Abstract base map feature class", no_init) + .add_property("wasCut", &MapFeature::wasCut) + .add_property("mapID", &MapFeature::mapID) + .add_property("initialized", &MapFeature::initialized) + .add_property("valid", &MapFeature::valid) + .def("computeFeatureVector", pure_virtual(&MapFeature::computeFeatureVector)) + .def("process", pure_virtual(&MapFeature::process)); + + class_, boost::noncopyable>("LineStringFeature", + "Abstract line string feature class", no_init) + .add_property("rawFeature", + make_function(&LineStringFeature::rawFeature, return_value_policy())) + .def("computeFeatureVector", pure_virtual(&LineStringFeature::computeFeatureVector)) + .def("process", pure_virtual(&LineStringFeature::process)) + .def("pointMatrix", pure_virtual(&LineStringFeature::pointMatrix)); + + class_>("LaneLineStringFeature", "Lane line string feature class", + init()) + .def(init<>()) + .add_property("cutFeature", + make_function(&LaneLineStringFeature::cutFeature, return_value_policy())) + .add_property("cutAndResampledFeature", make_function(&LaneLineStringFeature::cutAndResampledFeature, + return_value_policy())) + .add_property("type", &LaneLineStringFeature::type) + .add_property("typeInt", &LaneLineStringFeature::typeInt) + .add_property("laneletIDs", + make_function(&LaneLineStringFeature::laneletIDs, return_value_policy())) + .add_property("addLaneletID", &LaneLineStringFeature::addLaneletID); + + class_>( "LaneletFeature", "Lanelet feature class that contains lower level LaneLineStringFeatures", - init()) + init()) .def(init()) - .add_property("leftBoundary", &map_learning::LaneletFeature::leftBoundary) - .add_property("rightBoundary", &map_learning::LaneletFeature::rightBoundary) - .add_property("centerline", &map_learning::LaneletFeature::centerline) - .add_property("setReprType", &map_learning::LaneletFeature::setReprType); + .def(init<>()) + .add_property("leftBoundary", + make_function(&LaneletFeature::leftBoundary, return_value_policy())) + .add_property("rightBoundary", + make_function(&LaneletFeature::rightBoundary, return_value_policy())) + .add_property("centerline", + make_function(&LaneletFeature::centerline, return_value_policy())) + .add_property("setReprType", &LaneletFeature::setReprType); - class_>( + class_>( "CompoundLaneLineStringFeature", "Compound lane line string feature class that can trace back the individual features", - init()); + init()) + .def(init<>()) + .add_property("features", make_function(&CompoundLaneLineStringFeature::features, + return_value_policy())) + .add_property("pathLengthsRaw", make_function(&CompoundLaneLineStringFeature::pathLengthsRaw, + return_value_policy())) + .add_property("pathLengthsProcessed", make_function(&CompoundLaneLineStringFeature::pathLengthsProcessed, + return_value_policy())) + .add_property("processedFeaturesValid", make_function(&CompoundLaneLineStringFeature::processedFeaturesValid, + return_value_policy())); + + class_("Edge"); + + class_("LaneData"); - class_("Edge"); + GetPointsMatricesWrapper::wrap(); - class_("MapFeature"); -} \ No newline at end of file + converters::numpy_array_to_eigen_matrix>(); + converters::python_list_to_eigen_matrix>(); + py::to_python_converter< + Eigen::Matrix, + converters::eigen_matrix_to_numpy_array>>(); + converters::VectorToListConverter>(); + converters::IterableConverter().fromPython>().fromPython(); + class_>("IdToLaneLineStringFeatureMap") + .def(map_indexing_suite>()); + class_>("IdToLaneletFeatureMap").def(map_indexing_suite>()); + class_>("IdToCompoundLaneLineStringFeatureMap") + .def(map_indexing_suite>()); +} From dd8bb732a8e96eac794eac2fb7890330aab81d1d Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Wed, 20 Dec 2023 20:36:19 +0100 Subject: [PATCH 36/64] fix const ref properties --- lanelet2_python/python_api/map_learning.cpp | 40 +++++++++++---------- 1 file changed, 22 insertions(+), 18 deletions(-) diff --git a/lanelet2_python/python_api/map_learning.cpp b/lanelet2_python/python_api/map_learning.cpp index 627b8cb5..11673fbd 100644 --- a/lanelet2_python/python_api/map_learning.cpp +++ b/lanelet2_python/python_api/map_learning.cpp @@ -29,8 +29,8 @@ class GetPointsMatricesWrapper { // Expose the templated functions to Python def("getPointsMatricesMap", &GetPointsMatricesWrapper::getPointsMatricesMap); def("getPointsMatricesVector", &GetPointsMatricesWrapper::getPointsMatricesVector); - def("getPointsMatricesMap", &GetPointsMatricesWrapper::getPointsMatricesMap); - def("getPointsMatricesVector", &GetPointsMatricesWrapper::getPointsMatricesVector); + // def("getPointsMatricesVector", + // &GetPointsMatricesWrapper::getPointsMatricesVector); } }; @@ -67,7 +67,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT class_, boost::noncopyable>("LineStringFeature", "Abstract line string feature class", no_init) .add_property("rawFeature", - make_function(&LineStringFeature::rawFeature, return_value_policy())) + make_function(&LineStringFeature::rawFeature, return_value_policy())) .def("computeFeatureVector", pure_virtual(&LineStringFeature::computeFeatureVector)) .def("process", pure_virtual(&LineStringFeature::process)) .def("pointMatrix", pure_virtual(&LineStringFeature::pointMatrix)); @@ -76,13 +76,13 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT init()) .def(init<>()) .add_property("cutFeature", - make_function(&LaneLineStringFeature::cutFeature, return_value_policy())) + make_function(&LaneLineStringFeature::cutFeature, return_value_policy())) .add_property("cutAndResampledFeature", make_function(&LaneLineStringFeature::cutAndResampledFeature, - return_value_policy())) + return_value_policy())) .add_property("type", &LaneLineStringFeature::type) .add_property("typeInt", &LaneLineStringFeature::typeInt) .add_property("laneletIDs", - make_function(&LaneLineStringFeature::laneletIDs, return_value_policy())) + make_function(&LaneLineStringFeature::laneletIDs, return_value_policy())) .add_property("addLaneletID", &LaneLineStringFeature::addLaneletID); class_>( @@ -91,11 +91,11 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .def(init()) .def(init<>()) .add_property("leftBoundary", - make_function(&LaneletFeature::leftBoundary, return_value_policy())) + make_function(&LaneletFeature::leftBoundary, return_value_policy())) .add_property("rightBoundary", - make_function(&LaneletFeature::rightBoundary, return_value_policy())) + make_function(&LaneletFeature::rightBoundary, return_value_policy())) .add_property("centerline", - make_function(&LaneletFeature::centerline, return_value_policy())) + make_function(&LaneletFeature::centerline, return_value_policy())) .add_property("setReprType", &LaneletFeature::setReprType); class_>( @@ -104,13 +104,13 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT init()) .def(init<>()) .add_property("features", make_function(&CompoundLaneLineStringFeature::features, - return_value_policy())) + return_value_policy())) .add_property("pathLengthsRaw", make_function(&CompoundLaneLineStringFeature::pathLengthsRaw, - return_value_policy())) + return_value_policy())) .add_property("pathLengthsProcessed", make_function(&CompoundLaneLineStringFeature::pathLengthsProcessed, - return_value_policy())) + return_value_policy())) .add_property("processedFeaturesValid", make_function(&CompoundLaneLineStringFeature::processedFeaturesValid, - return_value_policy())); + return_value_policy())); class_("Edge"); @@ -124,10 +124,14 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT Eigen::Matrix, converters::eigen_matrix_to_numpy_array>>(); converters::VectorToListConverter>(); - converters::IterableConverter().fromPython>().fromPython(); - class_>("IdToLaneLineStringFeatureMap") + converters::VectorToListConverter(); + converters::VectorToListConverter(); + converters::IterableConverter() + .fromPython>() + .fromPython() + .fromPython() + .fromPython(); + class_>("LaneLineStringFeatures") .def(map_indexing_suite>()); - class_>("IdToLaneletFeatureMap").def(map_indexing_suite>()); - class_>("IdToCompoundLaneLineStringFeatureMap") - .def(map_indexing_suite>()); + class_>("LaneletFeatures").def(map_indexing_suite()); } From 7ca574b6a2d35825cc60e15f4cb458769956f425 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 21 Dec 2023 20:32:00 +0100 Subject: [PATCH 37/64] wip python bindings --- .../lanelet2_map_learning/MapFeatures.h | 55 ++--- lanelet2_map_learning/src/MapData.cpp | 12 +- lanelet2_map_learning/src/MapFeatures.cpp | 38 ++-- lanelet2_python/python_api/map_learning.cpp | 200 +++++++++++++++--- 4 files changed, 222 insertions(+), 83 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index 9b427b51..46d30259 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -13,6 +13,9 @@ namespace lanelet { namespace map_learning { +using VectorXd = Eigen::Matrix; +using MatrixXd = Eigen::Matrix; + class MapFeature { public: const Id mapID() const { return mapID_.get_value_or(InvalId); } @@ -20,8 +23,8 @@ class MapFeature { bool valid() const { return valid_; } bool wasCut() const { return wasCut_; } - virtual Eigen::VectorXd computeFeatureVector(bool /*unused*/, bool /*unused*/) const = 0; - virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) = 0; + virtual VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const = 0; + virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) = 0; template friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::MapFeature& feat, @@ -43,9 +46,9 @@ class LineStringFeature : public MapFeature { public: const BasicLineString3d& rawFeature() const { return rawFeature_; } - virtual Eigen::VectorXd computeFeatureVector(bool /*unused*/, bool /*unused*/) const = 0; - virtual Eigen::MatrixXd pointMatrix(bool pointsIn2d) const = 0; - virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) = 0; + virtual VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const = 0; + virtual MatrixXd pointMatrix(bool pointsIn2d) const = 0; + virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) = 0; template friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::LineStringFeature& feat, @@ -68,10 +71,9 @@ class LaneLineStringFeature : public LineStringFeature { virtual ~LaneLineStringFeature() noexcept = default; virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; - virtual Eigen::VectorXd computeFeatureVector( - bool onlyPoints, - bool pointsIn2d) const override; // uses processedFeature_ when available - virtual Eigen::MatrixXd pointMatrix(bool pointsIn2d) const override; // uses processedFeature_ when available + virtual VectorXd computeFeatureVector(bool onlyPoints, + bool pointsIn2d) const override; // uses processedFeature_ when available + virtual MatrixXd pointMatrix(bool pointsIn2d) const override; // uses processedFeature_ when available const BasicLineString3d& cutFeature() const { return cutFeature_; } const BasicLineString3d& cutAndResampledFeature() const { return cutAndResampledFeature_; } @@ -103,9 +105,9 @@ class TEFeature : public LineStringFeature { bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) override; // not implemented yet - Eigen::VectorXd computeFeatureVector(bool onlyPoints, - bool pointsIn2d) const override; // currently uses raw feature only - virtual Eigen::MatrixXd pointMatrix(bool pointsIn2d) const override; + VectorXd computeFeatureVector(bool onlyPoints, + bool pointsIn2d) const override; // currently uses raw feature only + virtual MatrixXd pointMatrix(bool pointsIn2d) const override; const TEType& teType() { return teType_; } @@ -122,7 +124,7 @@ class LaneletFeature : public MapFeature { virtual ~LaneletFeature() noexcept = default; bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; - Eigen::VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const override; + VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const override; void setReprType(LaneletRepresentationType reprType) { reprType_ = reprType; } @@ -172,7 +174,7 @@ using TEFeatures = std::map; using LaneletFeatures = std::map; template -Eigen::MatrixXd getFeatureVectorMatrix(const std::map& mapFeatures, bool onlyPoints, bool pointsIn2d) { +MatrixXd getFeatureVectorMatrix(const std::map& mapFeatures, bool onlyPoints, bool pointsIn2d) { std::vector featList; for (const auto& pair : mapFeatures) { featList.push_back(pair.second); @@ -181,11 +183,11 @@ Eigen::MatrixXd getFeatureVectorMatrix(const std::map& mapFeatures, bool } template -Eigen::MatrixXd getFeatureVectorMatrix(const std::vector& mapFeatures, bool onlyPoints, bool pointsIn2d) { +MatrixXd getFeatureVectorMatrix(const std::vector& mapFeatures, bool onlyPoints, bool pointsIn2d) { if (mapFeatures.empty()) { throw std::runtime_error("Empty mapFeatures vector supplied!"); } - std::vector featureVectors; + std::vector featureVectors; for (const auto& feat : mapFeatures) { if (!feat.valid()) { throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); @@ -193,12 +195,13 @@ Eigen::MatrixXd getFeatureVectorMatrix(const std::vector& mapFeatures, bool o featureVectors.push_back(feat.computeFeatureVector(onlyPoints, pointsIn2d)); } if (std::adjacent_find(featureVectors.begin(), featureVectors.end(), - [](const Eigen::VectorXd& v1, const Eigen::VectorXd& v2) { return v1.size() != v2.size(); }) == - featureVectors.end()) { + [](const VectorXd& v1, const VectorXd& v2) { return v1.size() != v2.size(); }) == + featureVectors.end() && + featureVectors.size() > 1) { throw std::runtime_error( "Unequal length of feature vectors! To create a matrix all feature vectors must have the same length!"); } - Eigen::MatrixXd featureMat(featureVectors.size(), featureVectors[0].size()); + MatrixXd featureMat(featureVectors.size(), featureVectors[0].size()); for (size_t i = 0; i < featureVectors.size(); i++) { featureMat.row(i) = featureVectors[i]; } @@ -206,7 +209,7 @@ Eigen::MatrixXd getFeatureVectorMatrix(const std::vector& mapFeatures, bool o } template -std::vector getPointsMatrices(const std::map& mapFeatures, bool pointsIn2d) { +std::vector getPointsMatrices(const std::map& mapFeatures, bool pointsIn2d) { std::vector featList; for (const auto& pair : mapFeatures) { featList.push_back(pair.second); @@ -215,11 +218,11 @@ std::vector getPointsMatrices(const std::map& mapFeature } template -std::vector getPointsMatrices(const std::vector& mapFeatures, bool pointsIn2d) { +std::vector getPointsMatrices(const std::vector& mapFeatures, bool pointsIn2d) { if (mapFeatures.empty()) { throw std::runtime_error("Empty mapFeatures vector supplied!"); } - std::vector pointMatrices; + std::vector pointMatrices; for (const auto& feat : mapFeatures) { if (!feat.valid()) { throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); @@ -230,8 +233,8 @@ std::vector getPointsMatrices(const std::vector& mapFeatures } template -bool processFeatureMap(std::map& featMap, const OrientedRect& bbox, const ParametrizationType& paramType, - int32_t nPoints) { +bool processFeatures(std::map& featMap, const OrientedRect& bbox, const ParametrizationType& paramType, + int32_t nPoints) { bool allValid = true; for (auto& feat : featMap) { if (!feat.second.process(bbox, paramType, nPoints)) { @@ -242,8 +245,8 @@ bool processFeatureMap(std::map& featMap, const OrientedRect& bbox, const } template -bool processFeatureVec(std::vector& featVec, const OrientedRect& bbox, const ParametrizationType& paramType, - int32_t nPoints) { +bool processFeatures(std::vector& featVec, const OrientedRect& bbox, const ParametrizationType& paramType, + int32_t nPoints) { bool allValid = true; for (auto& feat : featVec) { if (!feat.process(bbox, paramType, nPoints)) { diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index b2eb0e6f..1e59e8c8 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -281,12 +281,12 @@ void LaneData::updateAssociatedCpdFeatureIndices() { } bool LaneData::processAll(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { - bool validRoadBorders = processFeatureMap(roadBorders_, bbox, paramType, nPoints); - bool validLaneDividers = processFeatureMap(laneDividers_, bbox, paramType, nPoints); - bool validLaneletFeatures = processFeatureMap(laneletFeatures_, bbox, paramType, nPoints); - bool validCompoundRoadBorders = processFeatureVec(compoundRoadBorders_, bbox, paramType, nPoints); - bool validCompoundLaneDividers = processFeatureVec(compoundLaneDividers_, bbox, paramType, nPoints); - bool validCompoundCenterlines = processFeatureVec(compoundCenterlines_, bbox, paramType, nPoints); + bool validRoadBorders = processFeatures(roadBorders_, bbox, paramType, nPoints); + bool validLaneDividers = processFeatures(laneDividers_, bbox, paramType, nPoints); + bool validLaneletFeatures = processFeatures(laneletFeatures_, bbox, paramType, nPoints); + bool validCompoundRoadBorders = processFeatures(compoundRoadBorders_, bbox, paramType, nPoints); + bool validCompoundLaneDividers = processFeatures(compoundLaneDividers_, bbox, paramType, nPoints); + bool validCompoundCenterlines = processFeatures(compoundCenterlines_, bbox, paramType, nPoints); if (validRoadBorders && validLaneDividers && validLaneletFeatures && validCompoundRoadBorders && validCompoundLaneDividers && validCompoundCenterlines) { diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index 894c7590..e0290ddf 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -51,11 +51,11 @@ bool LaneLineStringFeature::process(const OrientedRect& bbox, const Parametrizat return result.valid_; } -Eigen::VectorXd LaneLineStringFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { +VectorXd LaneLineStringFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { const BasicLineString3d& selectedFeature = (cutAndResampledFeature_.size() > 0) ? cutAndResampledFeature_ : rawFeature_; - Eigen::VectorXd vec = pointsIn2d ? Eigen::VectorXd(2 * selectedFeature.size() + 1) - : Eigen::VectorXd(3 * selectedFeature.size() + 1); // n points with 2/3 dims + type + VectorXd vec = pointsIn2d ? VectorXd(2 * selectedFeature.size() + 1) + : VectorXd(3 * selectedFeature.size() + 1); // n points with 2/3 dims + type if (pointsIn2d == true) { for (size_t i = 0; i < selectedFeature.size(); i++) { vec(Eigen::seq(2 * i, 2 * i + 1)) = selectedFeature[i](Eigen::seq(0, 1)); @@ -74,9 +74,9 @@ Eigen::VectorXd LaneLineStringFeature::computeFeatureVector(bool onlyPoints, boo } } -Eigen::VectorXd TEFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { - Eigen::VectorXd vec = pointsIn2d ? Eigen::VectorXd(2 * rawFeature_.size() + 1) - : Eigen::VectorXd(3 * rawFeature_.size() + 1); // n points with 2/3 dims + type +VectorXd TEFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { + VectorXd vec = pointsIn2d ? VectorXd(2 * rawFeature_.size() + 1) + : VectorXd(3 * rawFeature_.size() + 1); // n points with 2/3 dims + type if (pointsIn2d == true) { for (size_t i = 0; i < rawFeature_.size(); i++) { vec(Eigen::seq(2 * i, 2 * i + 1)) = rawFeature_[i](Eigen::seq(0, 1)); @@ -94,11 +94,11 @@ Eigen::VectorXd TEFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d } } -Eigen::MatrixXd LaneLineStringFeature::pointMatrix(bool pointsIn2d) const { +MatrixXd LaneLineStringFeature::pointMatrix(bool pointsIn2d) const { const BasicLineString3d& selectedFeature = (cutAndResampledFeature_.size() > 0) ? cutAndResampledFeature_ : rawFeature_; - Eigen::MatrixXd mat = pointsIn2d ? Eigen::MatrixXd(selectedFeature.size(), 2) - : Eigen::MatrixXd(selectedFeature.size(), 3); // n points with 2/3 dims + type + MatrixXd mat = pointsIn2d ? MatrixXd(selectedFeature.size(), 2) + : MatrixXd(selectedFeature.size(), 3); // n points with 2/3 dims + type if (pointsIn2d == true) { for (size_t i = 0; i < selectedFeature.size(); i++) { mat.row(i) = selectedFeature[i](Eigen::seq(0, 1)); @@ -111,9 +111,9 @@ Eigen::MatrixXd LaneLineStringFeature::pointMatrix(bool pointsIn2d) const { return mat; } -Eigen::MatrixXd TEFeature::pointMatrix(bool pointsIn2d) const { - Eigen::MatrixXd mat = pointsIn2d ? Eigen::MatrixXd(rawFeature_.size(), 2) - : Eigen::MatrixXd(rawFeature_.size(), 3); // n points with 2/3 dims + type +MatrixXd TEFeature::pointMatrix(bool pointsIn2d) const { + MatrixXd mat = + pointsIn2d ? MatrixXd(rawFeature_.size(), 2) : MatrixXd(rawFeature_.size(), 3); // n points with 2/3 dims + type if (pointsIn2d == true) { for (size_t i = 0; i < rawFeature_.size(); i++) { mat.row(i) = rawFeature_[i](Eigen::seq(0, 1)); @@ -152,13 +152,13 @@ bool LaneletFeature::process(const OrientedRect& bbox, const ParametrizationType return valid_; } -Eigen::VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { +VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { if (!reprType_.has_value()) { throw std::runtime_error( "You need to set a LaneletRepresentationType with setReprType() before computing the feature vector!"); } else if (*reprType_ == LaneletRepresentationType::Centerline) { - Eigen::VectorXd vecCenterlinePts = centerline_.computeFeatureVector(true, pointsIn2d); - Eigen::VectorXd vec(vecCenterlinePts.size() + 2); // pts vec + left and right type + VectorXd vecCenterlinePts = centerline_.computeFeatureVector(true, pointsIn2d); + VectorXd vec(vecCenterlinePts.size() + 2); // pts vec + left and right type vec(Eigen::seq(0, vecCenterlinePts.size() - 1)) = vecCenterlinePts; vec[vec.size() - 2] = leftBoundary_.typeInt(); vec[vec.size() - 1] = rightBoundary_.typeInt(); @@ -168,10 +168,10 @@ Eigen::VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool point return vec; } } else if (*reprType_ == LaneletRepresentationType::Boundaries) { - Eigen::VectorXd vecLeftBdPts = leftBoundary_.computeFeatureVector(true, pointsIn2d); - Eigen::VectorXd vecRightBdPts = rightBoundary_.computeFeatureVector(true, pointsIn2d); + VectorXd vecLeftBdPts = leftBoundary_.computeFeatureVector(true, pointsIn2d); + VectorXd vecRightBdPts = rightBoundary_.computeFeatureVector(true, pointsIn2d); - Eigen::VectorXd vec(vecLeftBdPts.size() + vecRightBdPts.size() + 2); // pts vec + left and right type + VectorXd vec(vecLeftBdPts.size() + vecRightBdPts.size() + 2); // pts vec + left and right type vec(Eigen::seq(0, vecLeftBdPts.size() - 1)) = vecLeftBdPts; vec(Eigen::seq(vecLeftBdPts.size(), vecLeftBdPts.size() + vecRightBdPts.size() - 1)) = vecRightBdPts; vec[vec.size() - 2] = leftBoundary_.typeInt(); @@ -183,7 +183,7 @@ Eigen::VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool point } } else { throw std::runtime_error("Unknown LaneletRepresentationType!"); - return Eigen::VectorXd(); + return VectorXd(); } } diff --git a/lanelet2_python/python_api/map_learning.cpp b/lanelet2_python/python_api/map_learning.cpp index 11673fbd..ccc24160 100644 --- a/lanelet2_python/python_api/map_learning.cpp +++ b/lanelet2_python/python_api/map_learning.cpp @@ -13,28 +13,113 @@ using namespace boost::python; using namespace lanelet; using namespace lanelet::map_learning; -class GetPointsMatricesWrapper { +/// TODO: ADD VIRTUAL FUNCTIONS, CONSTRUCTORS, EIGEN TO NUMPY +/// TODO: DEBUG EIGEN SEGFAULTS FROM PYTHON + +class MapFeatureWrap : public MapFeature, public wrapper { public: - template - static std::vector getPointsMatricesMap(const std::map& mapFeatures, bool pointsIn2d) { - return getPointsMatrices(mapFeatures, pointsIn2d); + VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { + return this->get_override("computeFeatureVector")(onlyPoints, pointsIn2d); + } + bool process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { + return this->get_override("process")(bbox, paramType, nPoints); } +}; - template - static std::vector getPointsMatricesVector(const std::vector& mapFeatures, bool pointsIn2d) { - return getPointsMatrices(mapFeatures, pointsIn2d); +class LineStringFeatureWrap : public LineStringFeature, public wrapper { + public: + VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { + return this->get_override("computeFeatureVector")(onlyPoints, pointsIn2d); } + bool process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { + return this->get_override("process")(bbox, paramType, nPoints); + } + MatrixXd pointMatrix(bool pointsIn2d) const { return this->get_override("pointMatrix")(pointsIn2d); } +}; + +class LaneLineStringFeatureWrap : public LaneLineStringFeature, public wrapper { + public: + LaneLineStringFeatureWrap() {} + + LaneLineStringFeatureWrap(const BasicLineString3d &feature, Id mapID, LineStringType type, Id laneletID) + : LaneLineStringFeature(feature, mapID, type, laneletID) {} - static void wrap() { - // Expose the templated functions to Python - def("getPointsMatricesMap", &GetPointsMatricesWrapper::getPointsMatricesMap); - def("getPointsMatricesVector", &GetPointsMatricesWrapper::getPointsMatricesVector); - // def("getPointsMatricesVector", - // &GetPointsMatricesWrapper::getPointsMatricesVector); + VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { + if (override f = this->get_override("computeFeatureVector")) return f(onlyPoints, pointsIn2d); + return LaneLineStringFeature::computeFeatureVector(onlyPoints, pointsIn2d); } + VectorXd default_computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { + return this->LaneLineStringFeature::computeFeatureVector(onlyPoints, pointsIn2d); + } + bool process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { + if (override f = this->get_override("process")) return f(bbox, paramType, nPoints); + return LaneLineStringFeature::process(bbox, paramType, nPoints); + } + bool default_process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { + return this->LaneLineStringFeature::process(bbox, paramType, nPoints); + } + MatrixXd pointMatrix(bool pointsIn2d) const { + if (override f = this->get_override("pointMatrix")) return f(pointsIn2d); + return LaneLineStringFeature::pointMatrix(pointsIn2d); + } + MatrixXd default_pointMatrix(bool pointsIn2d) const { return this->LaneLineStringFeature::pointMatrix(pointsIn2d); } }; -/// TODO: ADD VIRTUAL FUNCTIONS, CONSTRUCTORS, EIGEN TO NUMPY +class CompoundLaneLineStringFeatureWrap : public CompoundLaneLineStringFeature, + public wrapper { + public: + CompoundLaneLineStringFeatureWrap() {} + + CompoundLaneLineStringFeatureWrap(const LaneLineStringFeatureList &features, LineStringType compoundType) + : CompoundLaneLineStringFeature(features, compoundType) {} + + VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { + if (override f = this->get_override("computeFeatureVector")) return f(onlyPoints, pointsIn2d); + return CompoundLaneLineStringFeature::computeFeatureVector(onlyPoints, pointsIn2d); + } + VectorXd default_computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { + return this->CompoundLaneLineStringFeature::computeFeatureVector(onlyPoints, pointsIn2d); + } + bool process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { + if (override f = this->get_override("process")) return f(bbox, paramType, nPoints); + return CompoundLaneLineStringFeature::process(bbox, paramType, nPoints); + } + bool default_process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { + return this->CompoundLaneLineStringFeature::process(bbox, paramType, nPoints); + } + MatrixXd pointMatrix(bool pointsIn2d) const { + if (override f = this->get_override("pointMatrix")) return f(pointsIn2d); + return CompoundLaneLineStringFeature::pointMatrix(pointsIn2d); + } + MatrixXd default_pointMatrix(bool pointsIn2d) const { + return this->CompoundLaneLineStringFeature::pointMatrix(pointsIn2d); + } +}; + +class LaneletFeatureWrap : public LaneletFeature, public wrapper { + public: + LaneletFeatureWrap() {} + + LaneletFeatureWrap(const LaneLineStringFeature &leftBoundary, const LaneLineStringFeature &rightBoundary, + const LaneLineStringFeature ¢erline, Id mapID) + : LaneletFeature(leftBoundary, rightBoundary, centerline, mapID) {} + LaneletFeatureWrap(const ConstLanelet &ll) : LaneletFeature(ll) {} + + VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { + if (override f = this->get_override("computeFeatureVector")) return f(onlyPoints, pointsIn2d); + return LaneletFeature::computeFeatureVector(onlyPoints, pointsIn2d); + } + VectorXd default_computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { + return this->LaneletFeature::computeFeatureVector(onlyPoints, pointsIn2d); + } + bool process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { + if (override f = this->get_override("process")) return f(bbox, paramType, nPoints); + return LaneletFeature::process(bbox, paramType, nPoints); + } + bool default_process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { + return this->LaneletFeature::process(bbox, paramType, nPoints); + } +}; BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT @@ -56,7 +141,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .value("Centerline", LineStringType::Centerline) .value("Unknown", LineStringType::Unknown); - class_("MapFeature", "Abstract base map feature class", no_init) + class_("MapFeature", "Abstract base map feature class", no_init) .add_property("wasCut", &MapFeature::wasCut) .add_property("mapID", &MapFeature::mapID) .add_property("initialized", &MapFeature::initialized) @@ -64,16 +149,16 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .def("computeFeatureVector", pure_virtual(&MapFeature::computeFeatureVector)) .def("process", pure_virtual(&MapFeature::process)); - class_, boost::noncopyable>("LineStringFeature", - "Abstract line string feature class", no_init) + class_, boost::noncopyable>("LineStringFeature", + "Abstract line string feature class", no_init) .add_property("rawFeature", make_function(&LineStringFeature::rawFeature, return_value_policy())) .def("computeFeatureVector", pure_virtual(&LineStringFeature::computeFeatureVector)) .def("process", pure_virtual(&LineStringFeature::process)) .def("pointMatrix", pure_virtual(&LineStringFeature::pointMatrix)); - class_>("LaneLineStringFeature", "Lane line string feature class", - init()) + class_, boost::noncopyable>( + "LaneLineStringFeature", "Lane line string feature class", init()) .def(init<>()) .add_property("cutFeature", make_function(&LaneLineStringFeature::cutFeature, return_value_policy())) @@ -83,9 +168,13 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .add_property("typeInt", &LaneLineStringFeature::typeInt) .add_property("laneletIDs", make_function(&LaneLineStringFeature::laneletIDs, return_value_policy())) - .add_property("addLaneletID", &LaneLineStringFeature::addLaneletID); + .add_property("addLaneletID", &LaneLineStringFeature::addLaneletID) + .def("computeFeatureVector", &LaneLineStringFeature::computeFeatureVector, + &LaneLineStringFeatureWrap::default_computeFeatureVector) + .def("process", &LaneLineStringFeature::process, &LaneLineStringFeatureWrap::default_process) + .def("pointMatrix", &LaneLineStringFeature::pointMatrix, &LaneLineStringFeatureWrap::default_pointMatrix); - class_>( + class_, boost::noncopyable>( "LaneletFeature", "Lanelet feature class that contains lower level LaneLineStringFeatures", init()) .def(init()) @@ -96,9 +185,12 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT make_function(&LaneletFeature::rightBoundary, return_value_policy())) .add_property("centerline", make_function(&LaneletFeature::centerline, return_value_policy())) - .add_property("setReprType", &LaneletFeature::setReprType); + .add_property("setReprType", &LaneletFeature::setReprType) + .def("computeFeatureVector", &LaneletFeature::computeFeatureVector, + &LaneletFeatureWrap::default_computeFeatureVector) + .def("process", &LaneletFeature::process, &LaneletFeatureWrap::default_process); - class_>( + class_, boost::noncopyable>( "CompoundLaneLineStringFeature", "Compound lane line string feature class that can trace back the individual features", init()) @@ -110,28 +202,72 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .add_property("pathLengthsProcessed", make_function(&CompoundLaneLineStringFeature::pathLengthsProcessed, return_value_policy())) .add_property("processedFeaturesValid", make_function(&CompoundLaneLineStringFeature::processedFeaturesValid, - return_value_policy())); + return_value_policy())) + .def("computeFeatureVector", &CompoundLaneLineStringFeature::computeFeatureVector, + &CompoundLaneLineStringFeatureWrap::default_computeFeatureVector) + .def("process", &CompoundLaneLineStringFeature::process, &CompoundLaneLineStringFeatureWrap::default_process) + .def("pointMatrix", &CompoundLaneLineStringFeature::pointMatrix, + &CompoundLaneLineStringFeatureWrap::default_pointMatrix); class_("Edge"); class_("LaneData"); - GetPointsMatricesWrapper::wrap(); + // Eigen and stl converters + converters::numpy_array_to_eigen_matrix(); + converters::python_list_to_eigen_matrix(); + py::to_python_converter>(); - converters::numpy_array_to_eigen_matrix>(); - converters::python_list_to_eigen_matrix>(); - py::to_python_converter< - Eigen::Matrix, - converters::eigen_matrix_to_numpy_array>>(); - converters::VectorToListConverter>(); + converters::numpy_array_to_eigen_vector(); + converters::python_list_to_eigen_vector(); + py::to_python_converter>(); + + converters::VectorToListConverter>(); converters::VectorToListConverter(); converters::VectorToListConverter(); converters::IterableConverter() - .fromPython>() + .fromPython>() .fromPython() .fromPython() .fromPython(); class_>("LaneLineStringFeatures") .def(map_indexing_suite>()); class_>("LaneletFeatures").def(map_indexing_suite()); + converters::VectorToListConverter>(); + class_>("BoolList").def(vector_indexing_suite>()); + + // overloaded template function instantiations + def (*)(const std::map &, bool)>("getPointsMatrices", + &getPointsMatrices); + def (*)(const std::vector &, bool)>("getPointsMatrices", + &getPointsMatrices); + def (*)(const std::map &, bool)>("getPointsMatrices", + &getPointsMatrices); + def (*)(const std::vector &, bool)>("getPointsMatrices", + &getPointsMatrices); + + def &, bool, bool)>("getFeatureVectorMatrix", + &getFeatureVectorMatrix); + def &, bool, bool)>("getFeatureVectorMatrix", + &getFeatureVectorMatrix); + def &, bool, bool)>("getFeatureVectorMatrix", + &getFeatureVectorMatrix); + def &, bool, bool)>("getFeatureVectorMatrix", + &getFeatureVectorMatrix); + def &, bool, bool)>("getFeatureVectorMatrix", + &getFeatureVectorMatrix); + def &, bool, bool)>("getFeatureVectorMatrix", &getFeatureVectorMatrix); + + def &, const OrientedRect &, const ParametrizationType &, int32_t)>( + "processFeatures", &processFeatures); + def &, const OrientedRect &, const ParametrizationType &, int32_t)>( + "processFeatures", &processFeatures); + def &, const OrientedRect &, const ParametrizationType &, + int32_t)>("processFeatures", &processFeatures); + def &, const OrientedRect &, const ParametrizationType &, + int32_t)>("processFeatures", &processFeatures); + def &, const OrientedRect &, const ParametrizationType &, int32_t)>( + "processFeatures", &processFeatures); + def &, const OrientedRect &, const ParametrizationType &, int32_t)>( + "processFeatures", &processFeatures); } From 7db76cd3e97dcfe838a676bf93a5acd93952bf38 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Mon, 8 Jan 2024 20:42:41 +0100 Subject: [PATCH 38/64] wip python bindings --- .../include/lanelet2_map_learning/MapData.h | 1 + .../lanelet2_map_learning/MapDataInterface.h | 16 +-- .../lanelet2_map_learning/MapFeatures.h | 2 +- .../include/lanelet2_map_learning/Types.h | 7 +- .../include/lanelet2_map_learning/Utils.h | 3 +- lanelet2_map_learning/src/MapData.cpp | 2 +- .../src/MapDataInterface.cpp | 8 +- lanelet2_map_learning/src/MapFeatures.cpp | 7 +- lanelet2_map_learning/src/Utils.cpp | 10 +- lanelet2_map_learning/test/test_utils.cpp | 16 +-- lanelet2_python/python_api/map_learning.cpp | 133 +++++++++++++----- .../test/test_lanelet2_map_learning.py | 32 +++++ 12 files changed, 168 insertions(+), 69 deletions(-) create mode 100644 lanelet2_python/test/test_lanelet2_map_learning.py diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index 9907fc06..a06d0d1d 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -88,6 +88,7 @@ class LaneData { Edges edges_; // edge list for centerlines }; +/// TODO: finish TE support class TEData { public: TEData() noexcept {} diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h index 805f456e..18b3bad4 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h @@ -15,27 +15,25 @@ namespace map_learning { class MapDataInterface { public: - template - using FeatureBuffer = - std::unordered_map, std::equal_to, Eigen::aligned_allocator>>; - struct Configuration { Configuration() noexcept {} - Configuration(LaneletRepresentationType reprType, ParametrizationType paramType, double submapAreaLongitudinal, - double submapAreaLateral, int nPoints) noexcept + Configuration(LaneletRepresentationType reprType, ParametrizationType paramType, double submapExtentLongitudinal, + double submapExtentLateral, int nPoints) noexcept : reprType{reprType}, paramType{paramType}, submapExtentLongitudinal{submapExtentLongitudinal}, - submapExtentLateral{submapExtentLateral} {} + submapExtentLateral{submapExtentLateral}, + nPoints{nPoints} {} LaneletRepresentationType reprType{LaneletRepresentationType::Boundaries}; ParametrizationType paramType{ParametrizationType::LineString}; double submapExtentLongitudinal{30}; // in driving direction double submapExtentLateral{15}; // in lateral direction int nPoints{20}; }; + MapDataInterface(LaneletMapConstPtr laneletMap); + MapDataInterface(LaneletMapConstPtr laneletMap, Configuration config); - MapDataInterface(LaneletMapConstPtr laneletMap, Configuration config = Configuration(), - Optional currPos = boost::none); + const Configuration& config() { return config_; } void setCurrPosAndExtractSubmap(const BasicPoint2d& pt, double yaw); LaneData laneData(); diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index 46d30259..ebfa3328 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -140,7 +140,7 @@ class LaneletFeature : public MapFeature { LaneLineStringFeature leftBoundary_; LaneLineStringFeature rightBoundary_; LaneLineStringFeature centerline_; - Optional reprType_; + LaneletRepresentationType reprType_{LaneletRepresentationType::Centerline}; }; class CompoundLaneLineStringFeature : public LaneLineStringFeature { diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h index 40446f0b..a95640f0 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h @@ -24,7 +24,12 @@ enum class LineStringType { }; // Mixed == DashedSolid or SolidDashed enum class TEType { TrafficLight, TrafficSign, Unknown }; -using OrientedRect = boost::geometry::model::polygon; +// wrapper for boost::python reasons +struct OrientedRect { + boost::geometry::model::polygon bg_poly; + boost::geometry::model::polygon::ring_type& bounds() { return bg_poly.outer(); } + const boost::geometry::model::polygon::ring_type& bounds_const() { return bg_poly.outer(); } +}; } // namespace map_learning } // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h index 2be08c60..31a7f750 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h @@ -13,8 +13,7 @@ namespace lanelet { namespace map_learning { -boost::geometry::model::polygon getRotatedRect(const BasicPoint2d& center, double extentLongitudinal, - double extentLateral, double yaw); +OrientedRect getRotatedRect(const BasicPoint2d& center, double extentLongitudinal, double extentLateral, double yaw); LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPoint2d& center, double extentLongitudinal, double extentLateral); diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 1e59e8c8..21585327 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -226,7 +226,7 @@ void LaneData::initCompoundFeatures(LaneletSubmapConstPtr& localSubmap, for (const auto& ll : localSubmap->laneletLayer) { ConstLanelets previousLLs = localSubmapGraph->previous(ll, false); ConstLanelets successorLLs = localSubmapGraph->following(ll, false); - if (previousLLs.empty() && !successorLLs.empty()) { + if (previousLLs.empty()) { getPaths(localSubmapGraph, paths, ll); } } diff --git a/lanelet2_map_learning/src/MapDataInterface.cpp b/lanelet2_map_learning/src/MapDataInterface.cpp index a161d495..2cd3784b 100644 --- a/lanelet2_map_learning/src/MapDataInterface.cpp +++ b/lanelet2_map_learning/src/MapDataInterface.cpp @@ -22,10 +22,14 @@ void MapDataInterface::setCurrPosAndExtractSubmap(const BasicPoint2d& pt, double localSubmapGraph_ = lanelet::routing::RoutingGraph::build(*laneletMap_, *trafficRules_); } -MapDataInterface::MapDataInterface(LaneletMapConstPtr laneletMap, Configuration config, Optional currPos) +MapDataInterface::MapDataInterface(LaneletMapConstPtr laneletMap) + : laneletMap_{laneletMap}, + config_{}, + trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} + +MapDataInterface::MapDataInterface(LaneletMapConstPtr laneletMap, Configuration config) : laneletMap_{laneletMap}, config_{config}, - currPos_{currPos}, trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} LaneData MapDataInterface::getLaneData(LaneletSubmapConstPtr localSubmap, diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index e0290ddf..22d6ea7d 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -153,10 +153,7 @@ bool LaneletFeature::process(const OrientedRect& bbox, const ParametrizationType } VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { - if (!reprType_.has_value()) { - throw std::runtime_error( - "You need to set a LaneletRepresentationType with setReprType() before computing the feature vector!"); - } else if (*reprType_ == LaneletRepresentationType::Centerline) { + if (reprType_ == LaneletRepresentationType::Centerline) { VectorXd vecCenterlinePts = centerline_.computeFeatureVector(true, pointsIn2d); VectorXd vec(vecCenterlinePts.size() + 2); // pts vec + left and right type vec(Eigen::seq(0, vecCenterlinePts.size() - 1)) = vecCenterlinePts; @@ -167,7 +164,7 @@ VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) } else { return vec; } - } else if (*reprType_ == LaneletRepresentationType::Boundaries) { + } else if (reprType_ == LaneletRepresentationType::Boundaries) { VectorXd vecLeftBdPts = leftBoundary_.computeFeatureVector(true, pointsIn2d); VectorXd vecRightBdPts = rightBoundary_.computeFeatureVector(true, pointsIn2d); diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index b73209ad..bc2ddac1 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -20,7 +20,7 @@ OrientedRect getRotatedRect(const BasicPoint2d& center, double extentLongitudina BasicPoint2d{center.x() + extentLongitudinal, center.y() - extentLateral}, BasicPoint2d{center.x() - extentLongitudinal, center.y() - extentLateral}}; OrientedRect axisAlignedRect; - boost::geometry::assign_points(axisAlignedRect, pts); + boost::geometry::assign_points(axisAlignedRect.bg_poly, pts); boost::geometry::strategy::transform::translate_transformer trans1(-center.x(), -center.y()); boost::geometry::strategy::transform::rotate_transformer rotate(yaw); @@ -29,9 +29,9 @@ OrientedRect getRotatedRect(const BasicPoint2d& center, double extentLongitudina OrientedRect trans1Rect; OrientedRect rotatedRect; OrientedRect trans2Rect; - boost::geometry::transform(axisAlignedRect, trans1Rect, trans1); - boost::geometry::transform(trans1Rect, rotatedRect, rotate); - boost::geometry::transform(rotatedRect, trans2Rect, trans2); + boost::geometry::transform(axisAlignedRect.bg_poly, trans1Rect.bg_poly, trans1); + boost::geometry::transform(trans1Rect.bg_poly, rotatedRect.bg_poly, rotate); + boost::geometry::transform(rotatedRect.bg_poly, trans2Rect.bg_poly, trans2); return trans2Rect; } @@ -68,7 +68,7 @@ BasicLineString3d cutLineString(const OrientedRect& bbox, const BasicLineString3 polyline2d.push_back(BasicPoint2d(pt.x(), pt.y())); } std::deque cut2d; - boost::geometry::intersection(bbox, polyline2d, cut2d); + boost::geometry::intersection(bbox.bg_poly, polyline2d, cut2d); BasicLineString3d cut3d; if (cut2d.empty()) { diff --git a/lanelet2_map_learning/test/test_utils.cpp b/lanelet2_map_learning/test/test_utils.cpp index c0024b95..5fb38e93 100644 --- a/lanelet2_map_learning/test/test_utils.cpp +++ b/lanelet2_map_learning/test/test_utils.cpp @@ -8,14 +8,14 @@ using namespace lanelet::map_learning; using namespace lanelet::map_learning::tests; TEST_F(MapLearningTest, GetRotatedRect) { // NOLINT - EXPECT_DOUBLE_EQ(bbox.outer()[0].x(), -5); - EXPECT_DOUBLE_EQ(bbox.outer()[0].y(), 20); - EXPECT_DOUBLE_EQ(bbox.outer()[1].x(), 15); - EXPECT_DOUBLE_EQ(bbox.outer()[1].y(), 20); - EXPECT_DOUBLE_EQ(bbox.outer()[2].x(), 15); - EXPECT_DOUBLE_EQ(bbox.outer()[2].y(), -10); - EXPECT_DOUBLE_EQ(bbox.outer()[3].x(), -5); - EXPECT_DOUBLE_EQ(bbox.outer()[3].y(), -10); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[0].x(), -5); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[0].y(), 20); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[1].x(), 15); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[1].y(), 20); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[2].x(), 15); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[2].y(), -10); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[3].x(), -5); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[3].y(), -10); } TEST_F(MapLearningTest, ExtractSubmap) { // NOLINT diff --git a/lanelet2_python/python_api/map_learning.cpp b/lanelet2_python/python_api/map_learning.cpp index ccc24160..cd3f865d 100644 --- a/lanelet2_python/python_api/map_learning.cpp +++ b/lanelet2_python/python_api/map_learning.cpp @@ -6,6 +6,7 @@ #include "lanelet2_map_learning/MapData.h" #include "lanelet2_map_learning/MapDataInterface.h" #include "lanelet2_map_learning/MapFeatures.h" +#include "lanelet2_map_learning/Utils.h" #include "lanelet2_python/internal/converter.h" #include "lanelet2_python/internal/eigen_converter.h" @@ -123,6 +124,9 @@ class LaneletFeatureWrap : public LaneletFeature, public wrapper BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT + Py_Initialize(); + np::initialize(); + enum_("LaneletRepresentationType") .value("Centerline", LaneletRepresentationType::Centerline) .value("Boundaries", LaneletRepresentationType::Boundaries); @@ -141,6 +145,19 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .value("Centerline", LineStringType::Centerline) .value("Unknown", LineStringType::Unknown); + class_("OrientedRect", "Oriented rectangle for feature crop area", init<>()) + .add_property("bounds", make_function(&OrientedRect::bounds_const, return_value_policy())); + + def("getRotatedRect", &getRotatedRect); + def("extractSubmap", &extractSubmap); + def("isRoadBorder", &isRoadBorder); + def("bdTypeToEnum", &bdTypeToEnum); + def("teTypeToEnum", &teTypeToEnum); + def("resampleLineString", &resampleLineString); + def("cutLineString", &cutLineString); + def("saveLaneData", &saveLaneData); + def("loadLaneData", &loadLaneData); + class_("MapFeature", "Abstract base map feature class", no_init) .add_property("wasCut", &MapFeature::wasCut) .add_property("mapID", &MapFeature::mapID) @@ -185,7 +202,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT make_function(&LaneletFeature::rightBoundary, return_value_policy())) .add_property("centerline", make_function(&LaneletFeature::centerline, return_value_policy())) - .add_property("setReprType", &LaneletFeature::setReprType) + .def("setReprType", &LaneletFeature::setReprType) .def("computeFeatureVector", &LaneletFeature::computeFeatureVector, &LaneletFeatureWrap::default_computeFeatureVector) .def("process", &LaneletFeature::process, &LaneletFeatureWrap::default_process); @@ -209,22 +226,65 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .def("pointMatrix", &CompoundLaneLineStringFeature::pointMatrix, &CompoundLaneLineStringFeatureWrap::default_pointMatrix); - class_("Edge"); + class_("Edge", "Struct of a lane graph edge", init()) + .def(init<>()) + .add_property("el1", &Edge::el1_) + .add_property("el2", &Edge::el2_); - class_("LaneData"); + class_("LaneData", "Class for holding, accessing and processing of lane data") + .def(init<>()) + .def("build", &LaneData::build) + .staticmethod("build") + .def("processAll", &LaneData::processAll) + .add_property("roadBorders", make_function(&LaneData::roadBorders, return_value_policy())) + .add_property("laneDividers", make_function(&LaneData::laneDividers, return_value_policy())) + .add_property("compoundRoadBorders", + make_function(&LaneData::compoundRoadBorders, return_value_policy())) + .add_property("compoundLaneDividers", + make_function(&LaneData::compoundLaneDividers, return_value_policy())) + .add_property("compoundCenterlines", + make_function(&LaneData::compoundCenterlines, return_value_policy())) + .add_property("associatedCpdRoadBorderIndices", make_function(&LaneData::associatedCpdRoadBorderIndices, + return_value_policy())) + .add_property("associatedCpdLaneDividerIndices", make_function(&LaneData::associatedCpdLaneDividerIndices, + return_value_policy())) + .add_property("associatedCpdCenterlineIndices", make_function(&LaneData::associatedCpdCenterlineIndices, + return_value_policy())) + .add_property("laneletFeatures", + make_function(&LaneData::laneletFeatures, return_value_policy())) + .add_property("edges", make_function(&LaneData::edges, return_value_policy())); - // Eigen and stl converters - converters::numpy_array_to_eigen_matrix(); - converters::python_list_to_eigen_matrix(); - py::to_python_converter>(); + { + scope inMapDataInterface = + class_("MapDataInterface", "Main Interface Class for processing of Lanelet maps", + init()) + .def(init()) + .add_property("config", + make_function(&MapDataInterface::config, return_value_policy())) + .def("setCurrPosAndExtractSubmap", &MapDataInterface::setCurrPosAndExtractSubmap) + .def("laneData", &MapDataInterface::laneData) + .def("teData", &MapDataInterface::teData) + .def("laneDataBatch", &MapDataInterface::laneDataBatch) + .def("laneTEDataBatch", &MapDataInterface::laneTEDataBatch); - converters::numpy_array_to_eigen_vector(); - converters::python_list_to_eigen_vector(); - py::to_python_converter>(); + class_("Configuration", "Configuration class for MapDataInterface", init<>()) + .def(init()) + .def_readwrite("reprType", &MapDataInterface::Configuration::reprType) + .def_readwrite("paramType", &MapDataInterface::Configuration::paramType) + .def_readwrite("submapExtentLongitudinal", &MapDataInterface::Configuration::submapExtentLongitudinal) + .def_readwrite("submapExtentLateral", &MapDataInterface::Configuration::submapExtentLateral) + .def_readwrite("nPoints", &MapDataInterface::Configuration::nPoints); + } + + // Eigen, stl etc. converters + converters::convertMatrix(true); + converters::convertVector(true); converters::VectorToListConverter>(); converters::VectorToListConverter(); converters::VectorToListConverter(); + converters::VectorToListConverter< + boost::geometry::model::ring>(); converters::IterableConverter() .fromPython>() .fromPython() @@ -236,38 +296,41 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT converters::VectorToListConverter>(); class_>("BoolList").def(vector_indexing_suite>()); + implicitly_convertible(); + // overloaded template function instantiations - def (*)(const std::map &, bool)>("getPointsMatrices", - &getPointsMatrices); - def (*)(const std::vector &, bool)>("getPointsMatrices", - &getPointsMatrices); - def (*)(const std::map &, bool)>("getPointsMatrices", - &getPointsMatrices); - def (*)(const std::vector &, bool)>("getPointsMatrices", - &getPointsMatrices); - - def &, bool, bool)>("getFeatureVectorMatrix", - &getFeatureVectorMatrix); - def &, bool, bool)>("getFeatureVectorMatrix", - &getFeatureVectorMatrix); - def &, bool, bool)>("getFeatureVectorMatrix", - &getFeatureVectorMatrix); - def &, bool, bool)>("getFeatureVectorMatrix", - &getFeatureVectorMatrix); + def (*)(const std::map &, bool)>( + "getPointsMatrices", &getPointsMatrices); + def (*)(const std::vector &, bool)>( + "getPointsMatrices", &getPointsMatrices); + def (*)(const std::map &, bool)>( + "getPointsMatrices", &getPointsMatrices); + def (*)(const std::vector &, bool)>( + "getPointsMatrices", &getPointsMatrices); + + def &, bool, bool)>( + "getFeatureVectorMatrix", &getFeatureVectorMatrix); + def &, bool, bool)>( + "getFeatureVectorMatrix", &getFeatureVectorMatrix); + def &, bool, bool)>( + "getFeatureVectorMatrix", &getFeatureVectorMatrix); + def &, bool, bool)>( + "getFeatureVectorMatrix", &getFeatureVectorMatrix); def &, bool, bool)>("getFeatureVectorMatrix", - &getFeatureVectorMatrix); - def &, bool, bool)>("getFeatureVectorMatrix", &getFeatureVectorMatrix); + &getFeatureVectorMatrix); + def &, bool, bool)>("getFeatureVectorMatrix", + &getFeatureVectorMatrix); def &, const OrientedRect &, const ParametrizationType &, int32_t)>( - "processFeatures", &processFeatures); + "processFeatures", &processFeatures); def &, const OrientedRect &, const ParametrizationType &, int32_t)>( - "processFeatures", &processFeatures); + "processFeatures", &processFeatures); def &, const OrientedRect &, const ParametrizationType &, - int32_t)>("processFeatures", &processFeatures); + int32_t)>("processFeatures", &processFeatures); def &, const OrientedRect &, const ParametrizationType &, - int32_t)>("processFeatures", &processFeatures); + int32_t)>("processFeatures", &processFeatures); def &, const OrientedRect &, const ParametrizationType &, int32_t)>( - "processFeatures", &processFeatures); + "processFeatures", &processFeatures); def &, const OrientedRect &, const ParametrizationType &, int32_t)>( - "processFeatures", &processFeatures); + "processFeatures", &processFeatures); } diff --git a/lanelet2_python/test/test_lanelet2_map_learning.py b/lanelet2_python/test/test_lanelet2_map_learning.py new file mode 100644 index 00000000..334a4080 --- /dev/null +++ b/lanelet2_python/test/test_lanelet2_map_learning.py @@ -0,0 +1,32 @@ +import unittest +import lanelet2 # if we fail here, there is something wrong with lanelet2 registration +from lanelet2.core import getId, Point3d, BasicPoint2d, LineString3d, Lanelet, LaneletMap +from lanelet2.map_learning import MapDataInterface, getFeatureVectorMatrix + +def get_sample_lanelet_map(): + mymap = LaneletMap() + x_left = 4 + x_middle = 2 + x_right = 0 + ls_left = LineString3d(1234, [Point3d(getId(), x_left, i, 0) for i in range(0, 3)], {"type": "road_border"}) + ls_middle = LineString3d(getId(), [Point3d(getId(), x_middle, i, 0) for i in range(0, 3)], {"type": "line", "subtype": "dashed"}) + ls_right = LineString3d(getId(), [Point3d(getId(), x_right, i, 0) for i in range(0, 3)], {"type": "road_border"}) + llet = Lanelet(getId(), ls_left, ls_middle) + llet_2 = Lanelet(getId(), ls_middle, ls_right) + mymap.add(llet) + mymap.add(llet_2) + return mymap + +class MapDataInterfaceTestCase(unittest.TestCase): + def test_map_data_interface(self): + mymap = get_sample_lanelet_map() + pos = BasicPoint2d(1, 1) + mDataIf = MapDataInterface(mymap) + mDataIf.setCurrPosAndExtractSubmap(pos, 0) + lData = mDataIf.laneData() + fmat = lData.compoundRoadBorders[0].pointMatrix(True) + print(fmat) + + +if __name__ == '__main__': + unittest.main() From 8a486e95a68dea480e8c49ba7b71fba925c302c3 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Tue, 9 Jan 2024 17:45:51 +0100 Subject: [PATCH 39/64] bugfixes python bindings --- .../include/lanelet2_map_learning/MapData.h | 16 ++-- .../lanelet2_map_learning/MapFeatures.h | 64 +++++++-------- .../include/lanelet2_map_learning/Serialize.h | 1 + lanelet2_map_learning/src/MapData.cpp | 77 ++++++++++--------- lanelet2_map_learning/src/MapFeatures.cpp | 52 ++++++------- lanelet2_map_learning/test/test_map_data.cpp | 14 ++-- .../test/test_map_features.cpp | 32 +++++--- lanelet2_python/python_api/map_learning.cpp | 75 +++++++++--------- .../test/test_lanelet2_map_learning.py | 2 +- 9 files changed, 173 insertions(+), 160 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index a06d0d1d..4f3481b8 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -69,10 +69,10 @@ class LaneData { void updateAssociatedCpdFeatureIndices(); LineStringType getLineStringTypeFromId(Id id); - const LaneLineStringFeature& getLineStringFeatFromId(Id id); + LaneLineStringFeaturePtr getLineStringFeatFromId(Id id); std::vector computeCompoundLeftBorders(const ConstLanelets& path); std::vector computeCompoundRightBorders(const ConstLanelets& path); - CompoundLaneLineStringFeature computeCompoundCenterline(const ConstLanelets& path); + CompoundLaneLineStringFeaturePtr computeCompoundCenterline(const ConstLanelets& path); LaneLineStringFeatures roadBorders_; // auxilliary features LaneLineStringFeatures laneDividers_; // auxilliary features @@ -103,16 +103,16 @@ class TEData { }; template -std::vector getValidElements(const std::vector& vec) { - std::vector res; - std::copy_if(vec.begin(), vec.end(), std::back_inserter(res), [](T el) { return el.valid(); }); +std::vector> getValidElements(const std::vector>& vec) { + std::vector> res; + std::copy_if(vec.begin(), vec.end(), std::back_inserter(res), [](std::shared_ptr el) { return el->valid(); }); return res; } template -std::map getValidElements(const std::map& map) { - std::map res; - std::copy_if(map.begin(), map.end(), std::back_inserter(res), [](const auto& pair) { return pair.second.valid(); }); +std::map> getValidElements(const std::map>& map) { + std::map> res; + std::copy_if(map.begin(), map.end(), std::back_inserter(res), [](const auto& pair) { return pair.second->valid(); }); return res; } diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index ebfa3328..176ab5c6 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -93,8 +93,9 @@ class LaneLineStringFeature : public LineStringFeature { std::vector laneletIDs_; }; -using LaneLineStringFeatures = std::map; -using LaneLineStringFeatureList = std::vector; +using LaneLineStringFeaturePtr = std::shared_ptr; +using LaneLineStringFeatures = std::map; +using LaneLineStringFeatureList = std::vector; class TEFeature : public LineStringFeature { public: @@ -118,8 +119,8 @@ class TEFeature : public LineStringFeature { class LaneletFeature : public MapFeature { public: LaneletFeature() {} - LaneletFeature(const LaneLineStringFeature& leftBoundary, const LaneLineStringFeature& rightBoundary, - const LaneLineStringFeature& centerline, Id mapID); + LaneletFeature(LaneLineStringFeaturePtr leftBoundary, LaneLineStringFeaturePtr rightBoundary, + LaneLineStringFeaturePtr centerline, Id mapID); LaneletFeature(const ConstLanelet& ll); virtual ~LaneletFeature() noexcept = default; @@ -128,18 +129,18 @@ class LaneletFeature : public MapFeature { void setReprType(LaneletRepresentationType reprType) { reprType_ = reprType; } - const LaneLineStringFeature& leftBoundary() const { return leftBoundary_; } - const LaneLineStringFeature& rightBoundary() const { return rightBoundary_; } - const LaneLineStringFeature& centerline() const { return centerline_; } + LaneLineStringFeaturePtr leftBoundary() const { return leftBoundary_; } + LaneLineStringFeaturePtr rightBoundary() const { return rightBoundary_; } + LaneLineStringFeaturePtr centerline() const { return centerline_; } template friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::LaneletFeature& feat, const unsigned int /*version*/); private: - LaneLineStringFeature leftBoundary_; - LaneLineStringFeature rightBoundary_; - LaneLineStringFeature centerline_; + LaneLineStringFeaturePtr leftBoundary_; + LaneLineStringFeaturePtr rightBoundary_; + LaneLineStringFeaturePtr centerline_; LaneletRepresentationType reprType_{LaneletRepresentationType::Centerline}; }; @@ -152,7 +153,7 @@ class CompoundLaneLineStringFeature : public LaneLineStringFeature { virtual ~CompoundLaneLineStringFeature() noexcept = default; bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; - const LaneLineStringFeatureList& features() const { return individualFeatures_; } + LaneLineStringFeatureList features() const { return individualFeatures_; } const std::vector& pathLengthsRaw() const { return pathLengthsRaw_; } const std::vector& pathLengthsProcessed() const { return pathLengthsProcessed_; } const std::vector& processedFeaturesValid() const { return processedFeaturesValid_; } @@ -169,13 +170,16 @@ class CompoundLaneLineStringFeature : public LaneLineStringFeature { double validLengthThresh_{0.3}; }; -using CompoundLaneLineStringFeatureList = std::vector; -using TEFeatures = std::map; -using LaneletFeatures = std::map; +using CompoundLaneLineStringFeaturePtr = std::shared_ptr; +using TEFeaturePtr = std::shared_ptr; +using LaneletFeaturePtr = std::shared_ptr; +using CompoundLaneLineStringFeatureList = std::vector; +using TEFeatures = std::map; +using LaneletFeatures = std::map; template -MatrixXd getFeatureVectorMatrix(const std::map& mapFeatures, bool onlyPoints, bool pointsIn2d) { - std::vector featList; +MatrixXd getFeatureVectorMatrix(const std::map>& mapFeatures, bool onlyPoints, bool pointsIn2d) { + std::vector> featList; for (const auto& pair : mapFeatures) { featList.push_back(pair.second); } @@ -183,16 +187,16 @@ MatrixXd getFeatureVectorMatrix(const std::map& mapFeatures, bool onlyPoi } template -MatrixXd getFeatureVectorMatrix(const std::vector& mapFeatures, bool onlyPoints, bool pointsIn2d) { +MatrixXd getFeatureVectorMatrix(const std::vector>& mapFeatures, bool onlyPoints, bool pointsIn2d) { if (mapFeatures.empty()) { throw std::runtime_error("Empty mapFeatures vector supplied!"); } std::vector featureVectors; for (const auto& feat : mapFeatures) { - if (!feat.valid()) { + if (!feat->valid()) { throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); } - featureVectors.push_back(feat.computeFeatureVector(onlyPoints, pointsIn2d)); + featureVectors.push_back(feat->computeFeatureVector(onlyPoints, pointsIn2d)); } if (std::adjacent_find(featureVectors.begin(), featureVectors.end(), [](const VectorXd& v1, const VectorXd& v2) { return v1.size() != v2.size(); }) == @@ -209,8 +213,8 @@ MatrixXd getFeatureVectorMatrix(const std::vector& mapFeatures, bool onlyPoin } template -std::vector getPointsMatrices(const std::map& mapFeatures, bool pointsIn2d) { - std::vector featList; +std::vector getPointsMatrices(const std::map>& mapFeatures, bool pointsIn2d) { + std::vector> featList; for (const auto& pair : mapFeatures) { featList.push_back(pair.second); } @@ -218,26 +222,26 @@ std::vector getPointsMatrices(const std::map& mapFeatures, bool } template -std::vector getPointsMatrices(const std::vector& mapFeatures, bool pointsIn2d) { +std::vector getPointsMatrices(const std::vector>& mapFeatures, bool pointsIn2d) { if (mapFeatures.empty()) { throw std::runtime_error("Empty mapFeatures vector supplied!"); } std::vector pointMatrices; for (const auto& feat : mapFeatures) { - if (!feat.valid()) { + if (!feat->valid()) { throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); } - pointMatrices.push_back(feat.pointMatrix(pointsIn2d)); + pointMatrices.push_back(feat->pointMatrix(pointsIn2d)); } return pointMatrices; } template -bool processFeatures(std::map& featMap, const OrientedRect& bbox, const ParametrizationType& paramType, - int32_t nPoints) { +bool processFeatures(std::map>& featMap, const OrientedRect& bbox, + const ParametrizationType& paramType, int32_t nPoints) { bool allValid = true; for (auto& feat : featMap) { - if (!feat.second.process(bbox, paramType, nPoints)) { + if (!feat.second->process(bbox, paramType, nPoints)) { allValid = false; } } @@ -245,11 +249,11 @@ bool processFeatures(std::map& featMap, const OrientedRect& bbox, const P } template -bool processFeatures(std::vector& featVec, const OrientedRect& bbox, const ParametrizationType& paramType, - int32_t nPoints) { +bool processFeatures(std::vector>& featVec, const OrientedRect& bbox, + const ParametrizationType& paramType, int32_t nPoints) { bool allValid = true; for (auto& feat : featVec) { - if (!feat.process(bbox, paramType, nPoints)) { + if (!feat->process(bbox, paramType, nPoints)) { allValid = false; } } diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h b/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h index bc163eb8..19590167 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include "MapData.h" diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 21585327..63049075 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -23,20 +23,20 @@ void LaneData::initLeftBoundaries(LaneletSubmapConstPtr& localSubmap, if (isRoadBorder(ll.leftBound3d())) { LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(ll.leftBound3d().id()); if (itRoadBd != roadBorders_.end()) { - itRoadBd->second.addLaneletID(ll.id()); + itRoadBd->second->addLaneletID(ll.id()); } else { - roadBorders_.insert( - {ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), - bdTypeToEnum(ll.leftBound3d()), ll.id())}); + roadBorders_.insert({ll.leftBound3d().id(), std::make_shared( + ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), + bdTypeToEnum(ll.leftBound3d()), ll.id())}); } } else { LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(ll.leftBound3d().id()); if (itLaneBd != laneDividers_.end()) { - itLaneBd->second.addLaneletID(ll.id()); + itLaneBd->second->addLaneletID(ll.id()); } else { - laneDividers_.insert( - {ll.leftBound3d().id(), LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), - bdTypeToEnum(ll.leftBound3d()), ll.id())}); + laneDividers_.insert({ll.leftBound3d().id(), std::make_shared( + ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), + bdTypeToEnum(ll.leftBound3d()), ll.id())}); } } @@ -55,20 +55,20 @@ void LaneData::initRightBoundaries(LaneletSubmapConstPtr& localSubmap, if (isRoadBorder(ll.rightBound3d())) { LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(ll.rightBound3d().id()); if (itRoadBd != roadBorders_.end()) { - itRoadBd->second.addLaneletID(ll.id()); + itRoadBd->second->addLaneletID(ll.id()); } else { - roadBorders_.insert( - {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), - bdTypeToEnum(ll.rightBound3d()), ll.id())}); + roadBorders_.insert({ll.rightBound3d().id(), std::make_shared( + ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), + bdTypeToEnum(ll.rightBound3d()), ll.id())}); } } else { LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(ll.rightBound3d().id()); if (itLaneBd != laneDividers_.end()) { - itLaneBd->second.addLaneletID(ll.id()); + itLaneBd->second->addLaneletID(ll.id()); } else { - laneDividers_.insert( - {ll.rightBound3d().id(), LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), - bdTypeToEnum(ll.rightBound3d()), ll.id())}); + laneDividers_.insert({ll.rightBound3d().id(), std::make_shared( + ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), + bdTypeToEnum(ll.rightBound3d()), ll.id())}); } } Optional rightLL = localSubmapGraph->right(ll); @@ -82,11 +82,12 @@ void LaneData::initRightBoundaries(LaneletSubmapConstPtr& localSubmap, void LaneData::initLaneletFeatures(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { for (const auto& ll : localSubmap->laneletLayer) { - const LaneLineStringFeature& leftBoundary = getLineStringFeatFromId(ll.leftBound().id()); - const LaneLineStringFeature& rightBoundary = getLineStringFeatFromId(ll.rightBound().id()); - LaneLineStringFeature centerline{ll.centerline3d().basicLineString(), ll.centerline3d().id(), - LineStringType::Centerline, ll.id()}; - laneletFeatures_.insert({ll.id(), LaneletFeature(leftBoundary, rightBoundary, centerline, ll.id())}); + LaneLineStringFeaturePtr leftBoundary = getLineStringFeatFromId(ll.leftBound().id()); + LaneLineStringFeaturePtr rightBoundary = getLineStringFeatFromId(ll.rightBound().id()); + LaneLineStringFeaturePtr centerline = std::make_shared( + ll.centerline3d().basicLineString(), ll.centerline3d().id(), LineStringType::Centerline, ll.id()); + laneletFeatures_.insert( + {ll.id(), std::make_shared(leftBoundary, rightBoundary, centerline, ll.id())}); } } @@ -111,9 +112,9 @@ LineStringType LaneData::getLineStringTypeFromId(Id id) { LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(id); LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(id); if (itLaneBd != laneDividers_.end()) { - bdType = itLaneBd->second.type(); + bdType = itLaneBd->second->type(); } else if (itRoadBd != roadBorders_.end()) { - bdType = itRoadBd->second.type(); + bdType = itRoadBd->second->type(); } else { throw std::runtime_error("Lanelet boundary " + std::to_string(id) + " is neither a road border nor a lane divider!"); @@ -121,7 +122,7 @@ LineStringType LaneData::getLineStringTypeFromId(Id id) { return bdType; } -const LaneLineStringFeature& LaneData::getLineStringFeatFromId(Id id) { +LaneLineStringFeaturePtr LaneData::getLineStringFeatFromId(Id id) { LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(id); LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(id); if (itLaneBd != laneDividers_.end()) { @@ -172,13 +173,13 @@ std::vector LaneData::computeCompoundRightBorders(const ConstLa return compoundBorders; } -CompoundLaneLineStringFeature LaneData::computeCompoundCenterline(const ConstLanelets& path) { +CompoundLaneLineStringFeaturePtr LaneData::computeCompoundCenterline(const ConstLanelets& path) { LaneLineStringFeatureList compoundCenterlines; for (const auto& ll : path) { - compoundCenterlines.push_back( - LaneLineStringFeature(ll.centerline3d().basicLineString(), ll.id(), LineStringType::Centerline, ll.id())); + compoundCenterlines.push_back(std::make_shared(ll.centerline3d().basicLineString(), ll.id(), + LineStringType::Centerline, ll.id())); } - return CompoundLaneLineStringFeature(compoundCenterlines, LineStringType::Centerline); + return std::make_shared(compoundCenterlines, LineStringType::Centerline); } std::map::const_iterator findFirstOccElement(const CompoundElsList& elsList, @@ -241,14 +242,14 @@ void LaneData::initCompoundFeatures(LaneletSubmapConstPtr& localSubmap, for (const auto& compFeat : compoundedBordersAndDividers) { LaneLineStringFeatureList toBeCompounded; for (const auto& el : compFeat.ids) { - LaneLineStringFeature cmpdFeat = getLineStringFeatFromId(el); + LaneLineStringFeaturePtr cmpdFeat = getLineStringFeatFromId(el); toBeCompounded.push_back(cmpdFeat); } - LineStringType cmpdType = toBeCompounded.front().type(); + LineStringType cmpdType = toBeCompounded.front()->type(); if (cmpdType == LineStringType::RoadBorder) { - compoundRoadBorders_.push_back(CompoundLaneLineStringFeature(toBeCompounded, cmpdType)); + compoundRoadBorders_.push_back(std::make_shared(toBeCompounded, cmpdType)); } else { - compoundLaneDividers_.push_back(CompoundLaneLineStringFeature(toBeCompounded, cmpdType)); + compoundLaneDividers_.push_back(std::make_shared(toBeCompounded, cmpdType)); } } } @@ -256,24 +257,24 @@ void LaneData::initCompoundFeatures(LaneletSubmapConstPtr& localSubmap, void LaneData::updateAssociatedCpdFeatureIndices() { for (size_t i = 0; i < compoundRoadBorders_.size(); i++) { const auto& cpdFeat = compoundRoadBorders_[i]; - for (const auto& indFeat : cpdFeat.features()) { - for (const auto& id : indFeat.laneletIDs()) { + for (const auto& indFeat : cpdFeat->features()) { + for (const auto& id : indFeat->laneletIDs()) { associatedCpdRoadBorderIndices_[id].push_back(i); } } } for (size_t i = 0; i < compoundLaneDividers_.size(); i++) { const auto& cpdFeat = compoundLaneDividers_[i]; - for (const auto& indFeat : cpdFeat.features()) { - for (const auto& id : indFeat.laneletIDs()) { + for (const auto& indFeat : cpdFeat->features()) { + for (const auto& id : indFeat->laneletIDs()) { associatedCpdLaneDividerIndices_[id].push_back(i); } } } for (size_t i = 0; i < compoundCenterlines_.size(); i++) { const auto& cpdFeat = compoundCenterlines_[i]; - for (const auto& indFeat : cpdFeat.features()) { - for (const auto& id : indFeat.laneletIDs()) { + for (const auto& indFeat : cpdFeat->features()) { + for (const auto& id : indFeat->laneletIDs()) { associatedCpdCenterlineIndices_[id].push_back(i); } } diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index 22d6ea7d..5496689b 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -126,27 +126,27 @@ MatrixXd TEFeature::pointMatrix(bool pointsIn2d) const { return mat; } -LaneletFeature::LaneletFeature(const LaneLineStringFeature& leftBoundary, const LaneLineStringFeature& rightBoundary, - const LaneLineStringFeature& centerline, Id mapID) +LaneletFeature::LaneletFeature(LaneLineStringFeaturePtr leftBoundary, LaneLineStringFeaturePtr rightBoundary, + LaneLineStringFeaturePtr centerline, Id mapID) : MapFeature(mapID), leftBoundary_{leftBoundary}, rightBoundary_{rightBoundary}, centerline_{centerline} {} LaneletFeature::LaneletFeature(const ConstLanelet& ll) - : leftBoundary_{LaneLineStringFeature(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), - bdTypeToEnum(ll.leftBound3d()), ll.id())}, - rightBoundary_{LaneLineStringFeature(ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), - bdTypeToEnum(ll.rightBound3d()), ll.id())}, - centerline_{LaneLineStringFeature(ll.centerline3d().basicLineString(), ll.centerline3d().id(), - LineStringType::Centerline, ll.id())} {} + : leftBoundary_{std::make_shared(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), + bdTypeToEnum(ll.leftBound3d()), ll.id())}, + rightBoundary_{std::make_shared( + ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), bdTypeToEnum(ll.rightBound3d()), ll.id())}, + centerline_{std::make_shared(ll.centerline3d().basicLineString(), ll.centerline3d().id(), + LineStringType::Centerline, ll.id())} {} bool LaneletFeature::process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { - leftBoundary_.process(bbox, paramType, nPoints); - rightBoundary_.process(bbox, paramType, nPoints); - centerline_.process(bbox, paramType, nPoints); + leftBoundary_->process(bbox, paramType, nPoints); + rightBoundary_->process(bbox, paramType, nPoints); + centerline_->process(bbox, paramType, nPoints); - if (leftBoundary_.wasCut() || rightBoundary_.wasCut() || centerline_.wasCut()) { + if (leftBoundary_->wasCut() || rightBoundary_->wasCut() || centerline_->wasCut()) { wasCut_ = true; } - if (!leftBoundary_.valid() || !rightBoundary_.valid() || !centerline_.valid()) { + if (!leftBoundary_->valid() || !rightBoundary_->valid() || !centerline_->valid()) { valid_ = false; } return valid_; @@ -154,25 +154,25 @@ bool LaneletFeature::process(const OrientedRect& bbox, const ParametrizationType VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { if (reprType_ == LaneletRepresentationType::Centerline) { - VectorXd vecCenterlinePts = centerline_.computeFeatureVector(true, pointsIn2d); + VectorXd vecCenterlinePts = centerline_->computeFeatureVector(true, pointsIn2d); VectorXd vec(vecCenterlinePts.size() + 2); // pts vec + left and right type vec(Eigen::seq(0, vecCenterlinePts.size() - 1)) = vecCenterlinePts; - vec[vec.size() - 2] = leftBoundary_.typeInt(); - vec[vec.size() - 1] = rightBoundary_.typeInt(); + vec[vec.size() - 2] = leftBoundary_->typeInt(); + vec[vec.size() - 1] = rightBoundary_->typeInt(); if (onlyPoints) { return vec(Eigen::seq(0, vec.size() - 2)); } else { return vec; } } else if (reprType_ == LaneletRepresentationType::Boundaries) { - VectorXd vecLeftBdPts = leftBoundary_.computeFeatureVector(true, pointsIn2d); - VectorXd vecRightBdPts = rightBoundary_.computeFeatureVector(true, pointsIn2d); + VectorXd vecLeftBdPts = leftBoundary_->computeFeatureVector(true, pointsIn2d); + VectorXd vecRightBdPts = rightBoundary_->computeFeatureVector(true, pointsIn2d); VectorXd vec(vecLeftBdPts.size() + vecRightBdPts.size() + 2); // pts vec + left and right type vec(Eigen::seq(0, vecLeftBdPts.size() - 1)) = vecLeftBdPts; vec(Eigen::seq(vecLeftBdPts.size(), vecLeftBdPts.size() + vecRightBdPts.size() - 1)) = vecRightBdPts; - vec[vec.size() - 2] = leftBoundary_.typeInt(); - vec[vec.size() - 1] = rightBoundary_.typeInt(); + vec[vec.size() - 2] = leftBoundary_->typeInt(); + vec[vec.size() - 1] = rightBoundary_->typeInt(); if (onlyPoints) { return vec(Eigen::seq(0, vec.size() - 2)); } else { @@ -193,17 +193,17 @@ CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStrin type_ = compoundType; for (size_t i = 0; i < features.size(); i++) { - if (features[i].rawFeature().empty()) { + if (features[i]->rawFeature().empty()) { throw std::runtime_error("Feature with empty rawFeature() supplied!"); } if (i == features.size() - 1) { - rawFeature_.insert(rawFeature_.end(), features[i].rawFeature().begin(), features[i].rawFeature().end()); + rawFeature_.insert(rawFeature_.end(), features[i]->rawFeature().begin(), features[i]->rawFeature().end()); } else { - rawFeature_.insert(rawFeature_.end(), features[i].rawFeature().begin(), features[i].rawFeature().end() - 1); + rawFeature_.insert(rawFeature_.end(), features[i]->rawFeature().begin(), features[i]->rawFeature().end() - 1); } double rawLength = - boost::geometry::length(features[i].rawFeature(), boost::geometry::strategy::distance::pythagoras()); + boost::geometry::length(features[i]->rawFeature(), boost::geometry::strategy::distance::pythagoras()); if (i > 0) { pathLengthsRaw_[i] = pathLengthsRaw_[i - 1] + rawLength; } else { @@ -217,8 +217,8 @@ bool CompoundLaneLineStringFeature::process(const OrientedRect& bbox, const Para bool valid = false; LaneLineStringFeature::process(bbox, paramType, nPoints); for (size_t i = 0; i < individualFeatures_.size(); i++) { - individualFeatures_[i].process(bbox, paramType, nPoints); - double processedLength = boost::geometry::length(individualFeatures_[i].cutFeature(), + individualFeatures_[i]->process(bbox, paramType, nPoints); + double processedLength = boost::geometry::length(individualFeatures_[i]->cutFeature(), boost::geometry::strategy::distance::pythagoras()); if (processedLength > validLengthThresh_) { diff --git a/lanelet2_map_learning/test/test_map_data.cpp b/lanelet2_map_learning/test/test_map_data.cpp index 8eb5c7eb..0ba16de1 100644 --- a/lanelet2_map_learning/test/test_map_data.cpp +++ b/lanelet2_map_learning/test/test_map_data.cpp @@ -31,7 +31,7 @@ TEST_F(MapLearningTest, LaneData) { // NOLINT std::vector compoundCenterlines = getPointsMatrices(laneData.compoundCenterlines(), true); EXPECT_TRUE(laneData.laneletFeatures().find(2007) != laneData.laneletFeatures().end()); - EXPECT_EQ(laneData.laneletFeatures().find(2007)->second.leftBoundary().mapID(), 1012); + EXPECT_EQ(laneData.laneletFeatures().find(2007)->second->leftBoundary()->mapID(), 1012); EXPECT_TRUE(laneData.roadBorders().find(1001) != laneData.roadBorders().end()); EXPECT_EQ(compoundRoadBorders.size(), 3); @@ -40,20 +40,20 @@ TEST_F(MapLearningTest, LaneData) { // NOLINT EXPECT_EQ(laneData.associatedCpdRoadBorderIndices().at(2001).size(), 1); size_t assoBorderIdx = laneData.associatedCpdRoadBorderIndices().at(2001).front(); - EXPECT_EQ(laneData.compoundRoadBorders()[assoBorderIdx].features().back().mapID(), 1000); - EXPECT_EQ(laneData.compoundRoadBorders()[assoBorderIdx].features().back().laneletIDs().front(), 2002); + EXPECT_EQ(laneData.compoundRoadBorders()[assoBorderIdx]->features().back()->mapID(), 1000); + EXPECT_EQ(laneData.compoundRoadBorders()[assoBorderIdx]->features().back()->laneletIDs().front(), 2002); EXPECT_EQ(laneData.associatedCpdLaneDividerIndices().at(2004).size(), 2); EXPECT_EQ(laneData.associatedCpdLaneDividerIndices().at(2009).size(), 1); size_t assoDividerIdx = laneData.associatedCpdLaneDividerIndices().at(2009).front(); - EXPECT_EQ(laneData.compoundLaneDividers()[assoDividerIdx].features().front().mapID(), 1015); - EXPECT_EQ(laneData.compoundLaneDividers()[assoDividerIdx].features().front().laneletIDs().size(), 2); + EXPECT_EQ(laneData.compoundLaneDividers()[assoDividerIdx]->features().front()->mapID(), 1015); + EXPECT_EQ(laneData.compoundLaneDividers()[assoDividerIdx]->features().front()->laneletIDs().size(), 2); EXPECT_EQ(laneData.associatedCpdCenterlineIndices().at(2003).size(), 2); EXPECT_EQ(laneData.associatedCpdCenterlineIndices().at(2000).size(), 1); size_t assoCenterlineIdx = laneData.associatedCpdCenterlineIndices().at(2000).front(); - EXPECT_EQ(laneData.compoundCenterlines()[assoCenterlineIdx].features().front().mapID(), 2000); - EXPECT_EQ(laneData.compoundCenterlines()[assoCenterlineIdx].features().front().laneletIDs().front(), 2000); + EXPECT_EQ(laneData.compoundCenterlines()[assoCenterlineIdx]->features().front()->mapID(), 2000); + EXPECT_EQ(laneData.compoundCenterlines()[assoCenterlineIdx]->features().front()->laneletIDs().front(), 2000); // matplot::hold(matplot::on); // matplot::xlim({-2, 16}); diff --git a/lanelet2_map_learning/test/test_map_features.cpp b/lanelet2_map_learning/test/test_map_features.cpp index fe597143..d2e0e349 100644 --- a/lanelet2_map_learning/test/test_map_features.cpp +++ b/lanelet2_map_learning/test/test_map_features.cpp @@ -36,22 +36,25 @@ TEST_F(MapLearningTest, LaneLineStringFeature) { // NOLINT TEST_F(MapLearningTest, LaneletFeature) { // NOLINT BasicLineString3d leftBd{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}, BasicPoint3d{20, 0, 0}}; - LaneLineStringFeature leftBdFeat(leftBd, Id(123), LineStringType::Solid, Id(1234)); + LaneLineStringFeaturePtr leftBdFeat = + std::make_shared(leftBd, Id(123), LineStringType::Solid, Id(1234)); BasicLineString3d rightBd{BasicPoint3d{0, 4, 0}, BasicPoint3d{5, 4, 0}, BasicPoint3d{10, 4, 0}, BasicPoint3d{20, 4, 0}}; - LaneLineStringFeature rightBdFeat(rightBd, Id(124), LineStringType::Dashed, Id(1234)); + LaneLineStringFeaturePtr rightBdFeat = + std::make_shared(rightBd, Id(124), LineStringType::Dashed, Id(1234)); BasicLineString3d centerline{BasicPoint3d{0, 2, 0}, BasicPoint3d{5, 2, 0}, BasicPoint3d{10, 2, 0}, BasicPoint3d{20, 2, 0}}; - LaneLineStringFeature centerlineFeat(centerline, Id(125), LineStringType::Centerline, Id(1234)); + LaneLineStringFeaturePtr centerlineFeat = + std::make_shared(centerline, Id(125), LineStringType::Centerline, Id(1234)); LaneletFeature llFeat(leftBdFeat, rightBdFeat, centerlineFeat, Id(1234)); llFeat.setReprType(LaneletRepresentationType::Boundaries); llFeat.process(bbox, ParametrizationType::LineString, 4); - EXPECT_EQ(llFeat.centerline().cutAndResampledFeature().size(), 4); - EXPECT_NEAR(llFeat.centerline().cutAndResampledFeature()[0].x(), 0, 10e-5); - EXPECT_NEAR(llFeat.centerline().cutAndResampledFeature()[1].x(), 5, 10e-5); - EXPECT_NEAR(llFeat.centerline().cutAndResampledFeature()[3].x(), 15, 10e-5); + EXPECT_EQ(llFeat.centerline()->cutAndResampledFeature().size(), 4); + EXPECT_NEAR(llFeat.centerline()->cutAndResampledFeature()[0].x(), 0, 10e-5); + EXPECT_NEAR(llFeat.centerline()->cutAndResampledFeature()[1].x(), 5, 10e-5); + EXPECT_NEAR(llFeat.centerline()->cutAndResampledFeature()[3].x(), 15, 10e-5); Eigen::VectorXd vec = llFeat.computeFeatureVector(false, true); @@ -64,15 +67,20 @@ TEST_F(MapLearningTest, LaneletFeature) { // NOLINT TEST_F(MapLearningTest, CompoundLaneLineStringFeature) { // NOLINT BasicLineString3d p1{BasicPoint3d{-10, 0, 0}, BasicPoint3d{-5, 0, 0}}; - LaneLineStringFeature feat1(p1, Id(123), LineStringType::Solid, Id(1234)); + LaneLineStringFeaturePtr feat1 = + std::make_shared(p1, Id(123), LineStringType::Solid, Id(1234)); BasicLineString3d p2{BasicPoint3d{-5, 0, 0}, BasicPoint3d{0, 0, 0}}; - LaneLineStringFeature feat2(p2, Id(123), LineStringType::Solid, Id(1234)); + LaneLineStringFeaturePtr feat2 = + std::make_shared(p2, Id(123), LineStringType::Solid, Id(1234)); BasicLineString3d p3{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}}; - LaneLineStringFeature feat3(p3, Id(123), LineStringType::Solid, Id(1234)); + LaneLineStringFeaturePtr feat3 = + std::make_shared(p3, Id(123), LineStringType::Solid, Id(1234)); BasicLineString3d p4{BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}}; - LaneLineStringFeature feat4(p4, Id(124), LineStringType::Solid, Id(1234)); + LaneLineStringFeaturePtr feat4 = + std::make_shared(p4, Id(124), LineStringType::Solid, Id(1234)); BasicLineString3d p5{BasicPoint3d{10, 0, 0}, BasicPoint3d{20, 0, 0}}; - LaneLineStringFeature feat5(p5, Id(125), LineStringType::Solid, Id(1234)); + LaneLineStringFeaturePtr feat5 = + std::make_shared(p5, Id(125), LineStringType::Solid, Id(1234)); CompoundLaneLineStringFeature cpdFeat(LaneLineStringFeatureList{feat1, feat2, feat3, feat4, feat5}, LineStringType::Solid); diff --git a/lanelet2_python/python_api/map_learning.cpp b/lanelet2_python/python_api/map_learning.cpp index cd3f865d..d8fb0f8c 100644 --- a/lanelet2_python/python_api/map_learning.cpp +++ b/lanelet2_python/python_api/map_learning.cpp @@ -101,8 +101,8 @@ class LaneletFeatureWrap : public LaneletFeature, public wrapper public: LaneletFeatureWrap() {} - LaneletFeatureWrap(const LaneLineStringFeature &leftBoundary, const LaneLineStringFeature &rightBoundary, - const LaneLineStringFeature ¢erline, Id mapID) + LaneletFeatureWrap(LaneLineStringFeaturePtr leftBoundary, LaneLineStringFeaturePtr rightBoundary, + LaneLineStringFeaturePtr centerline, Id mapID) : LaneletFeature(leftBoundary, rightBoundary, centerline, mapID) {} LaneletFeatureWrap(const ConstLanelet &ll) : LaneletFeature(ll) {} @@ -174,7 +174,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .def("process", pure_virtual(&LineStringFeature::process)) .def("pointMatrix", pure_virtual(&LineStringFeature::pointMatrix)); - class_, boost::noncopyable>( + class_, LaneLineStringFeaturePtr, boost::noncopyable>( "LaneLineStringFeature", "Lane line string feature class", init()) .def(init<>()) .add_property("cutFeature", @@ -191,29 +191,25 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .def("process", &LaneLineStringFeature::process, &LaneLineStringFeatureWrap::default_process) .def("pointMatrix", &LaneLineStringFeature::pointMatrix, &LaneLineStringFeatureWrap::default_pointMatrix); - class_, boost::noncopyable>( + class_, LaneletFeaturePtr, boost::noncopyable>( "LaneletFeature", "Lanelet feature class that contains lower level LaneLineStringFeatures", - init()) + init()) .def(init()) .def(init<>()) - .add_property("leftBoundary", - make_function(&LaneletFeature::leftBoundary, return_value_policy())) - .add_property("rightBoundary", - make_function(&LaneletFeature::rightBoundary, return_value_policy())) - .add_property("centerline", - make_function(&LaneletFeature::centerline, return_value_policy())) + .add_property("leftBoundary", make_function(&LaneletFeature::leftBoundary)) + .add_property("rightBoundary", make_function(&LaneletFeature::rightBoundary)) + .add_property("centerline", make_function(&LaneletFeature::centerline)) .def("setReprType", &LaneletFeature::setReprType) .def("computeFeatureVector", &LaneletFeature::computeFeatureVector, &LaneletFeatureWrap::default_computeFeatureVector) .def("process", &LaneletFeature::process, &LaneletFeatureWrap::default_process); - class_, boost::noncopyable>( - "CompoundLaneLineStringFeature", - "Compound lane line string feature class that can trace back the individual features", - init()) + class_, CompoundLaneLineStringFeaturePtr, + boost::noncopyable>("CompoundLaneLineStringFeature", + "Compound lane line string feature class that can trace back the individual features", + init()) .def(init<>()) - .add_property("features", make_function(&CompoundLaneLineStringFeature::features, - return_value_policy())) + .add_property("features", make_function(&CompoundLaneLineStringFeature::features)) .add_property("pathLengthsRaw", make_function(&CompoundLaneLineStringFeature::pathLengthsRaw, return_value_policy())) .add_property("pathLengthsProcessed", make_function(&CompoundLaneLineStringFeature::pathLengthsProcessed, @@ -290,47 +286,50 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .fromPython() .fromPython() .fromPython(); - class_>("LaneLineStringFeatures") - .def(map_indexing_suite>()); - class_>("LaneletFeatures").def(map_indexing_suite()); + class_("LaneLineStringFeatures").def(map_indexing_suite()); + class_("LaneletFeatures").def(map_indexing_suite()); converters::VectorToListConverter>(); class_>("BoolList").def(vector_indexing_suite>()); + // class_("LaneLineStringFeatureList") + // .def(vector_indexing_suite()); + // class_("CompoundLaneLineStringFeatureList") + // .def(vector_indexing_suite()); implicitly_convertible(); // overloaded template function instantiations - def (*)(const std::map &, bool)>( + def (*)(const std::map &, bool)>( "getPointsMatrices", &getPointsMatrices); - def (*)(const std::vector &, bool)>( + def (*)(const std::vector &, bool)>( "getPointsMatrices", &getPointsMatrices); - def (*)(const std::map &, bool)>( + def (*)(const std::map &, bool)>( "getPointsMatrices", &getPointsMatrices); - def (*)(const std::vector &, bool)>( + def (*)(const std::vector &, bool)>( "getPointsMatrices", &getPointsMatrices); - def &, bool, bool)>( + def &, bool, bool)>( "getFeatureVectorMatrix", &getFeatureVectorMatrix); - def &, bool, bool)>( + def &, bool, bool)>( "getFeatureVectorMatrix", &getFeatureVectorMatrix); - def &, bool, bool)>( + def &, bool, bool)>( "getFeatureVectorMatrix", &getFeatureVectorMatrix); - def &, bool, bool)>( + def &, bool, bool)>( "getFeatureVectorMatrix", &getFeatureVectorMatrix); - def &, bool, bool)>("getFeatureVectorMatrix", - &getFeatureVectorMatrix); - def &, bool, bool)>("getFeatureVectorMatrix", - &getFeatureVectorMatrix); + def &, bool, bool)>("getFeatureVectorMatrix", + &getFeatureVectorMatrix); + def &, bool, bool)>("getFeatureVectorMatrix", + &getFeatureVectorMatrix); - def &, const OrientedRect &, const ParametrizationType &, int32_t)>( + def &, const OrientedRect &, const ParametrizationType &, int32_t)>( "processFeatures", &processFeatures); - def &, const OrientedRect &, const ParametrizationType &, int32_t)>( + def &, const OrientedRect &, const ParametrizationType &, int32_t)>( "processFeatures", &processFeatures); - def &, const OrientedRect &, const ParametrizationType &, + def &, const OrientedRect &, const ParametrizationType &, int32_t)>("processFeatures", &processFeatures); - def &, const OrientedRect &, const ParametrizationType &, + def &, const OrientedRect &, const ParametrizationType &, int32_t)>("processFeatures", &processFeatures); - def &, const OrientedRect &, const ParametrizationType &, int32_t)>( + def &, const OrientedRect &, const ParametrizationType &, int32_t)>( "processFeatures", &processFeatures); - def &, const OrientedRect &, const ParametrizationType &, int32_t)>( + def &, const OrientedRect &, const ParametrizationType &, int32_t)>( "processFeatures", &processFeatures); } diff --git a/lanelet2_python/test/test_lanelet2_map_learning.py b/lanelet2_python/test/test_lanelet2_map_learning.py index 334a4080..a2eecf03 100644 --- a/lanelet2_python/test/test_lanelet2_map_learning.py +++ b/lanelet2_python/test/test_lanelet2_map_learning.py @@ -24,7 +24,7 @@ def test_map_data_interface(self): mDataIf = MapDataInterface(mymap) mDataIf.setCurrPosAndExtractSubmap(pos, 0) lData = mDataIf.laneData() - fmat = lData.compoundRoadBorders[0].pointMatrix(True) + fmat = lanelet2.map_learning.getPointsMatrices(lData.compoundRoadBorders, True) print(fmat) From 8cfe875103133a8272a8ed5de215d2e66c246d01 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Wed, 10 Jan 2024 23:09:57 +0100 Subject: [PATCH 40/64] first finished version with python bindings; add transformation to vehicle coordinates --- .../include/lanelet2_map_learning/MapData.h | 36 +++- .../lanelet2_map_learning/MapFeatures.h | 8 +- .../include/lanelet2_map_learning/Types.h | 9 +- .../include/lanelet2_map_learning/Utils.h | 2 + lanelet2_map_learning/src/MapData.cpp | 19 +++ lanelet2_map_learning/src/MapFeatures.cpp | 7 +- lanelet2_map_learning/src/Utils.cpp | 13 ++ lanelet2_map_learning/test/test_map_data.cpp | 12 +- .../test/test_map_data_interface.cpp | 14 +- .../test/test_map_features.cpp | 34 ++-- .../lanelet2_python/internal/converter.h | 14 ++ lanelet2_python/python_api/map_learning.cpp | 156 ++++++++++-------- .../test/test_lanelet2_map_learning.py | 7 +- 13 files changed, 225 insertions(+), 106 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index 4f3481b8..30a4ac03 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -5,6 +5,9 @@ #include #include +#include +#include +#include #include #include "lanelet2_map_learning/Forward.h" @@ -36,7 +39,30 @@ using Edges = std::map; // key = id from class LaneData { public: - LaneData() noexcept {} + struct TensorFeatureData { + public: + const std::vector& roadBorders() { return roadBorders_; } + const std::vector& laneDividers() { return laneDividers_; } + const std::vector& laneDividerTypes() { return laneDividerTypes_; } + const std::vector& compoundRoadBorders() { return compoundRoadBorders_; } + const std::vector& compoundLaneDividers() { return compoundLaneDividers_; } + const std::vector& compoundLaneDividerTypes() { return compoundLaneDividerTypes_; } + const std::vector& compoundCenterlines() { return compoundCenterlines_; } + const std::string& uuid() { return uuid_; } + friend class LaneData; + + private: + std::vector roadBorders_; + std::vector laneDividers_; + std::vector laneDividerTypes_; + std::vector compoundRoadBorders_; + std::vector compoundLaneDividers_; + std::vector compoundLaneDividerTypes_; + std::vector compoundCenterlines_; + std::string uuid_; + }; + + LaneData() noexcept : uuid_{boost::lexical_cast(boost::uuids::random_generator()())} {} static LaneData build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); bool processAll(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints); @@ -55,6 +81,10 @@ class LaneData { const LaneletFeatures& laneletFeatures() { return laneletFeatures_; } const Edges& edges() { return edges_; } + const std::string& uuid() { return uuid_; } + + /// The computed data will be buffered, if the underlying features change you need to set ignoreBuffer appropriately + TensorFeatureData getTensorFeatureData(bool pointsIn2d, bool ignoreBuffer); template friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::LaneData& feat, @@ -82,10 +112,14 @@ class LaneData { CompoundLaneLineStringFeatureList compoundCenterlines_; LaneletFeatures laneletFeatures_; + std::map> associatedCpdRoadBorderIndices_; // related to the "unfiltered" compound features! std::map> associatedCpdLaneDividerIndices_; // related to the "unfiltered" compound features! std::map> associatedCpdCenterlineIndices_; // related to the "unfiltered" compound features! Edges edges_; // edge list for centerlines + std::string uuid_; // sample id + + Optional tfData_; }; /// TODO: finish TE support diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index 176ab5c6..5d44dc17 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -77,6 +77,7 @@ class LaneLineStringFeature : public LineStringFeature { const BasicLineString3d& cutFeature() const { return cutFeature_; } const BasicLineString3d& cutAndResampledFeature() const { return cutAndResampledFeature_; } + const BasicLineString3d& cutResampledAndTransformedFeature() const { return cutResampledAndTransformedFeature_; } LineStringType type() const { return type_; } int typeInt() const { return static_cast(type_); } const std::vector& laneletIDs() const { return laneletIDs_; } @@ -89,6 +90,7 @@ class LaneLineStringFeature : public LineStringFeature { protected: BasicLineString3d cutFeature_; BasicLineString3d cutAndResampledFeature_; + BasicLineString3d cutResampledAndTransformedFeature_; LineStringType type_; std::vector laneletIDs_; }; @@ -213,16 +215,16 @@ MatrixXd getFeatureVectorMatrix(const std::vector>& mapFeatur } template -std::vector getPointsMatrices(const std::map>& mapFeatures, bool pointsIn2d) { +std::vector getPointMatrices(const std::map>& mapFeatures, bool pointsIn2d) { std::vector> featList; for (const auto& pair : mapFeatures) { featList.push_back(pair.second); } - return getPointsMatrices(featList, pointsIn2d); + return getPointMatrices(featList, pointsIn2d); } template -std::vector getPointsMatrices(const std::vector>& mapFeatures, bool pointsIn2d) { +std::vector getPointMatrices(const std::vector>& mapFeatures, bool pointsIn2d) { if (mapFeatures.empty()) { throw std::runtime_error("Empty mapFeatures vector supplied!"); } diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h index a95640f0..0c95cb11 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Types.h @@ -24,11 +24,18 @@ enum class LineStringType { }; // Mixed == DashedSolid or SolidDashed enum class TEType { TrafficLight, TrafficSign, Unknown }; -// wrapper for boost::python reasons struct OrientedRect { + BasicPoint2d center; + double yaw; boost::geometry::model::polygon bg_poly; boost::geometry::model::polygon::ring_type& bounds() { return bg_poly.outer(); } const boost::geometry::model::polygon::ring_type& bounds_const() { return bg_poly.outer(); } + + friend OrientedRect getRotatedRect(const BasicPoint2d& center, double extentLongitudinal, double extentLateral, + double yaw); + + private: + OrientedRect() noexcept {} }; } // namespace map_learning diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h index 31a7f750..f4a17097 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h @@ -66,6 +66,8 @@ BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t BasicLineString3d cutLineString(const OrientedRect& bbox, const BasicLineString3d& polyline); +BasicLineString3d transformLineString(const OrientedRect& bbox, const BasicLineString3d& polyline); + void saveLaneData(const std::string& filename, const std::vector& lDataVec, bool binary = false); std::vector loadLaneData(const std::string& filename, bool binary = false); diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 63049075..5097231b 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -297,5 +297,24 @@ bool LaneData::processAll(const OrientedRect& bbox, const ParametrizationType& p } } +LaneData::TensorFeatureData LaneData::getTensorFeatureData(bool pointsIn2d, bool ignoreBuffer) { + if (!tfData_.has_value() || ignoreBuffer) { + tfData_ = LaneData::TensorFeatureData(); + tfData_->uuid_ = uuid_; + tfData_->roadBorders_ = getPointMatrices(roadBorders_, pointsIn2d); + tfData_->laneDividers_ = getPointMatrices(laneDividers_, pointsIn2d); + tfData_->compoundRoadBorders_ = getPointMatrices(compoundRoadBorders_, pointsIn2d); + tfData_->compoundLaneDividers_ = getPointMatrices(compoundLaneDividers_, pointsIn2d); + tfData_->compoundCenterlines_ = getPointMatrices(compoundCenterlines_, pointsIn2d); + for (const auto& ft : laneDividers_) { + tfData_->laneDividerTypes_.push_back(ft.second->typeInt()); + } + for (const auto& ft : compoundLaneDividers_) { + tfData_->compoundLaneDividerTypes_.push_back(ft->typeInt()); + } + } + return tfData_.value(); +} + } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index 5496689b..00d0e42f 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -11,6 +11,7 @@ namespace map_learning { struct LStringProcessResult { BasicLineString3d cutFeature_; BasicLineString3d cutAndResampledFeature_; + BasicLineString3d cutResampledAndTransformedFeature_; bool wasCut_{false}; bool valid_{true}; }; @@ -28,6 +29,7 @@ LStringProcessResult processLineStringImpl(const BasicLineString3d& lstring, con return result; } result.cutAndResampledFeature_ = resampleLineString(result.cutFeature_, nPoints); + result.cutResampledAndTransformedFeature_ = transformLineString(bbox, result.cutAndResampledFeature_); double lengthOriginal = boost::geometry::length(lstring, boost::geometry::strategy::distance::pythagoras()); double lengthProcessed = boost::geometry::length(result.cutAndResampledFeature_, @@ -44,6 +46,7 @@ bool LaneLineStringFeature::process(const OrientedRect& bbox, const Parametrizat if (result.valid_) { cutFeature_ = result.cutFeature_; cutAndResampledFeature_ = result.cutAndResampledFeature_; + cutResampledAndTransformedFeature_ = result.cutResampledAndTransformedFeature_; } else { valid_ = result.valid_; } @@ -53,7 +56,7 @@ bool LaneLineStringFeature::process(const OrientedRect& bbox, const Parametrizat VectorXd LaneLineStringFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { const BasicLineString3d& selectedFeature = - (cutAndResampledFeature_.size() > 0) ? cutAndResampledFeature_ : rawFeature_; + (cutResampledAndTransformedFeature_.size() > 0) ? cutResampledAndTransformedFeature_ : rawFeature_; VectorXd vec = pointsIn2d ? VectorXd(2 * selectedFeature.size() + 1) : VectorXd(3 * selectedFeature.size() + 1); // n points with 2/3 dims + type if (pointsIn2d == true) { @@ -96,7 +99,7 @@ VectorXd TEFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const MatrixXd LaneLineStringFeature::pointMatrix(bool pointsIn2d) const { const BasicLineString3d& selectedFeature = - (cutAndResampledFeature_.size() > 0) ? cutAndResampledFeature_ : rawFeature_; + (cutResampledAndTransformedFeature_.size() > 0) ? cutResampledAndTransformedFeature_ : rawFeature_; MatrixXd mat = pointsIn2d ? MatrixXd(selectedFeature.size(), 2) : MatrixXd(selectedFeature.size(), 3); // n points with 2/3 dims + type if (pointsIn2d == true) { diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index bc2ddac1..00f468de 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -32,6 +32,9 @@ OrientedRect getRotatedRect(const BasicPoint2d& center, double extentLongitudina boost::geometry::transform(axisAlignedRect.bg_poly, trans1Rect.bg_poly, trans1); boost::geometry::transform(trans1Rect.bg_poly, rotatedRect.bg_poly, rotate); boost::geometry::transform(rotatedRect.bg_poly, trans2Rect.bg_poly, trans2); + + trans2Rect.center = center; + trans2Rect.yaw = yaw; return trans2Rect; } @@ -93,6 +96,16 @@ BasicLineString3d cutLineString(const OrientedRect& bbox, const BasicLineString3 return cut3d; } +BasicLineString3d transformLineString(const OrientedRect& bbox, const BasicLineString3d& polyline) { + boost::geometry::strategy::transform::translate_transformer trans1(-bbox.center.x(), -bbox.center.y()); + boost::geometry::strategy::transform::rotate_transformer rotate(-bbox.yaw); + BasicLineString3d transPolyline; + BasicLineString3d rotatedPolyline; + boost::geometry::transform(polyline, transPolyline, trans1); + boost::geometry::transform(transPolyline, rotatedPolyline, rotate); + return rotatedPolyline; +} + void saveLaneData(const std::string& filename, const std::vector& lDataVec, bool binary) { if (binary) { std::ofstream fs(filename, std::ofstream::binary); diff --git a/lanelet2_map_learning/test/test_map_data.cpp b/lanelet2_map_learning/test/test_map_data.cpp index 0ba16de1..f6e409e7 100644 --- a/lanelet2_map_learning/test/test_map_data.cpp +++ b/lanelet2_map_learning/test/test_map_data.cpp @@ -21,14 +21,14 @@ TEST_F(MapLearningTest, LaneData) { // NOLINT LaneData laneData = LaneData::build(laneletSubmap, laneletMapGraph); - std::vector rawCompoundRoadBorders = getPointsMatrices(laneData.compoundRoadBorders(), true); - std::vector rawCompoundLaneDividers = getPointsMatrices(laneData.compoundLaneDividers(), true); - std::vector rawCompoundCenterlines = getPointsMatrices(laneData.compoundCenterlines(), true); + std::vector rawCompoundRoadBorders = getPointMatrices(laneData.compoundRoadBorders(), true); + std::vector rawCompoundLaneDividers = getPointMatrices(laneData.compoundLaneDividers(), true); + std::vector rawCompoundCenterlines = getPointMatrices(laneData.compoundCenterlines(), true); bool valid = laneData.processAll(bbox, ParametrizationType::LineString, 20); - std::vector compoundRoadBorders = getPointsMatrices(laneData.compoundRoadBorders(), true); - std::vector compoundLaneDividers = getPointsMatrices(laneData.compoundLaneDividers(), true); - std::vector compoundCenterlines = getPointsMatrices(laneData.compoundCenterlines(), true); + std::vector compoundRoadBorders = getPointMatrices(laneData.compoundRoadBorders(), true); + std::vector compoundLaneDividers = getPointMatrices(laneData.compoundLaneDividers(), true); + std::vector compoundCenterlines = getPointMatrices(laneData.compoundCenterlines(), true); EXPECT_TRUE(laneData.laneletFeatures().find(2007) != laneData.laneletFeatures().end()); EXPECT_EQ(laneData.laneletFeatures().find(2007)->second->leftBoundary()->mapID(), 1012); diff --git a/lanelet2_map_learning/test/test_map_data_interface.cpp b/lanelet2_map_learning/test/test_map_data_interface.cpp index 2380e902..6b6973c9 100644 --- a/lanelet2_map_learning/test/test_map_data_interface.cpp +++ b/lanelet2_map_learning/test/test_map_data_interface.cpp @@ -1,8 +1,8 @@ #include - -// #include // #include +#include + #include "lanelet2_map_learning/MapData.h" #include "lanelet2_map_learning/MapDataInterface.h" #include "lanelet2_map_learning/MapFeatures.h" @@ -41,11 +41,11 @@ TEST_F(MapLearningTest, MapDataInterface) { // NOLINT for (size_t i = 0; i < lDataVec.size(); i++) { std::vector compoundRoadBorders = - getPointsMatrices(getValidElements(lDataVec[i].compoundRoadBorders()), true); + getPointMatrices(getValidElements(lDataVec[i].compoundRoadBorders()), true); std::vector compoundLaneDividers = - getPointsMatrices(getValidElements(lDataVec[i].compoundLaneDividers()), true); + getPointMatrices(getValidElements(lDataVec[i].compoundLaneDividers()), true); std::vector compoundCenterlines = - getPointsMatrices(getValidElements(lDataVec[i].compoundCenterlines()), true); + getPointMatrices(getValidElements(lDataVec[i].compoundCenterlines()), true); switch (i) { case 0: { @@ -69,8 +69,8 @@ TEST_F(MapLearningTest, MapDataInterface) { // NOLINT // matplot::hold(matplot::on); // matplot::cla(); - // matplot::xlim({-2, 16}); - // matplot::ylim({-13, 2}); + // matplot::xlim({-15, 15}); + // matplot::ylim({-15, 15}); // matplot::gcf()->size(1000, 1000); // for (const auto& mat : compoundRoadBorders) { // std::vector x; diff --git a/lanelet2_map_learning/test/test_map_features.cpp b/lanelet2_map_learning/test/test_map_features.cpp index d2e0e349..081d74e5 100644 --- a/lanelet2_map_learning/test/test_map_features.cpp +++ b/lanelet2_map_learning/test/test_map_features.cpp @@ -20,17 +20,18 @@ TEST_F(MapLearningTest, LaneLineStringFeature) { // NOLINT Eigen::VectorXd vec = feat.computeFeatureVector(false, true); EXPECT_EQ(vec.size(), 9); - EXPECT_NEAR(vec[0], 0, 10e-5); - EXPECT_NEAR(vec[2], 5, 10e-5); - EXPECT_NEAR(vec[6], 15, 10e-5); + EXPECT_NEAR(vec[0], 5, 10e-5); + EXPECT_NEAR(vec[1], -5, 10e-5); + EXPECT_NEAR(vec[6], 5, 10e-5); + EXPECT_NEAR(vec[7], 10, 10e-5); EXPECT_EQ(vec[8], 2); Eigen::MatrixXd pointMat = feat.pointMatrix(false); EXPECT_EQ(pointMat.rows(), 4); EXPECT_EQ(pointMat.cols(), 3); - EXPECT_NEAR(pointMat(0, 0), 0, 10e-5); - EXPECT_NEAR(pointMat(2, 0), 10, 10e-5); - EXPECT_NEAR(pointMat(3, 0), 15, 10e-5); + EXPECT_NEAR(pointMat(0, 1), -5, 10e-5); + EXPECT_NEAR(pointMat(2, 1), 5, 10e-5); + EXPECT_NEAR(pointMat(3, 1), 10, 10e-5); } TEST_F(MapLearningTest, LaneletFeature) { // NOLINT @@ -59,9 +60,9 @@ TEST_F(MapLearningTest, LaneletFeature) { // NOLINT Eigen::VectorXd vec = llFeat.computeFeatureVector(false, true); EXPECT_EQ(vec.size(), 18); - EXPECT_NEAR(vec[8], 0, 10e-5); - EXPECT_NEAR(vec[10], 5, 10e-5); - EXPECT_NEAR(vec[14], 15, 10e-5); + EXPECT_NEAR(vec[9], -5, 10e-5); + EXPECT_NEAR(vec[10], 1, 10e-5); + EXPECT_NEAR(vec[15], 10, 10e-5); EXPECT_EQ(vec[17], 1); } @@ -99,15 +100,18 @@ TEST_F(MapLearningTest, CompoundLaneLineStringFeature) { // NOLINT Eigen::VectorXd vec = cpdFeat.computeFeatureVector(false, true); EXPECT_EQ(vec.size(), 11); - EXPECT_NEAR(vec[0], -5, 10e-5); - EXPECT_NEAR(vec[2], 0, 10e-5); - EXPECT_NEAR(vec[6], 10, 10e-5); + EXPECT_NEAR(vec[1], -10, 10e-5); + EXPECT_NEAR(vec[2], 5, 10e-5); + EXPECT_NEAR(vec[3], -5, 10e-5); + EXPECT_NEAR(vec[6], 5, 10e-5); + EXPECT_NEAR(vec[7], 5, 10e-5); EXPECT_EQ(vec[10], 2); Eigen::MatrixXd pointMat = cpdFeat.pointMatrix(false); EXPECT_EQ(pointMat.rows(), 5); EXPECT_EQ(pointMat.cols(), 3); - EXPECT_NEAR(pointMat(0, 0), -5, 10e-5); - EXPECT_NEAR(pointMat(2, 0), 5, 10e-5); - EXPECT_NEAR(pointMat(3, 0), 10, 10e-5); + EXPECT_NEAR(pointMat(0, 1), -10, 10e-5); + EXPECT_NEAR(pointMat(2, 1), 0, 10e-5); + EXPECT_NEAR(pointMat(3, 1), 5, 10e-5); + EXPECT_NEAR(pointMat(4, 0), 5, 10e-5); } \ No newline at end of file diff --git a/lanelet2_python/include/lanelet2_python/internal/converter.h b/lanelet2_python/include/lanelet2_python/internal/converter.h index 2d0238fa..bc76d5a7 100644 --- a/lanelet2_python/include/lanelet2_python/internal/converter.h +++ b/lanelet2_python/include/lanelet2_python/internal/converter.h @@ -157,6 +157,17 @@ struct VectorToList { } }; +template +struct MapToDict { + static PyObject* convert(const T& v) { + py::dict d; + for (auto& e : v) { + d[e.first] = e.second; + } + return py::incref(d.ptr()); + } +}; + template struct OptionalToObject { static PyObject* convert(const lanelet::Optional& v) { @@ -218,6 +229,9 @@ struct PyPair { template using VectorToListConverter = py::to_python_converter>; +template +using MapToDictConverter = py::to_python_converter>; + template using OptionalConverter = py::to_python_converter, OptionalToObject>; diff --git a/lanelet2_python/python_api/map_learning.cpp b/lanelet2_python/python_api/map_learning.cpp index d8fb0f8c..e957128c 100644 --- a/lanelet2_python/python_api/map_learning.cpp +++ b/lanelet2_python/python_api/map_learning.cpp @@ -122,6 +122,32 @@ class LaneletFeatureWrap : public LaneletFeature, public wrapper } }; +template +struct DictToMapConverter { + DictToMapConverter() { converter::registry::push_back(&convertible, &construct, type_id()); } + static void *convertible(PyObject *obj) { + if (!PyDict_CheckExact(obj)) { // NOLINT + return nullptr; + } + return obj; + } + static void construct(PyObject *obj, converter::rvalue_from_python_stage1_data *data) { + dict d(borrowed(obj)); + list keys = d.keys(); + list values = d.values(); + T map; + for (auto i = 0u; i < len(keys); ++i) { + typename T::key_type key = extract(keys[i]); + typename T::mapped_type value = extract(values[i]); + map.insert(std::make_pair(key, value)); + } + using StorageType = converter::rvalue_from_python_storage; + void *storage = reinterpret_cast(data)->storage.bytes; // NOLINT + new (storage) T(map); + data->convertible = storage; + } +}; + BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT Py_Initialize(); @@ -145,7 +171,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .value("Centerline", LineStringType::Centerline) .value("Unknown", LineStringType::Unknown); - class_("OrientedRect", "Oriented rectangle for feature crop area", init<>()) + class_("OrientedRect", "Oriented rectangle for feature crop area", no_init) .add_property("bounds", make_function(&OrientedRect::bounds_const, return_value_policy())); def("getRotatedRect", &getRotatedRect); @@ -181,6 +207,9 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT make_function(&LaneLineStringFeature::cutFeature, return_value_policy())) .add_property("cutAndResampledFeature", make_function(&LaneLineStringFeature::cutAndResampledFeature, return_value_policy())) + .add_property("cutResampledAndTransformedFeature", + make_function(&LaneLineStringFeature::cutResampledAndTransformedFeature, + return_value_policy())) .add_property("type", &LaneLineStringFeature::type) .add_property("typeInt", &LaneLineStringFeature::typeInt) .add_property("laneletIDs", @@ -224,31 +253,56 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT class_("Edge", "Struct of a lane graph edge", init()) .def(init<>()) - .add_property("el1", &Edge::el1_) - .add_property("el2", &Edge::el2_); + .def_readwrite("el1", &Edge::el1_) + .def_readwrite("el2", &Edge::el2_); - class_("LaneData", "Class for holding, accessing and processing of lane data") - .def(init<>()) - .def("build", &LaneData::build) - .staticmethod("build") - .def("processAll", &LaneData::processAll) - .add_property("roadBorders", make_function(&LaneData::roadBorders, return_value_policy())) - .add_property("laneDividers", make_function(&LaneData::laneDividers, return_value_policy())) - .add_property("compoundRoadBorders", - make_function(&LaneData::compoundRoadBorders, return_value_policy())) - .add_property("compoundLaneDividers", - make_function(&LaneData::compoundLaneDividers, return_value_policy())) - .add_property("compoundCenterlines", - make_function(&LaneData::compoundCenterlines, return_value_policy())) - .add_property("associatedCpdRoadBorderIndices", make_function(&LaneData::associatedCpdRoadBorderIndices, - return_value_policy())) - .add_property("associatedCpdLaneDividerIndices", make_function(&LaneData::associatedCpdLaneDividerIndices, - return_value_policy())) - .add_property("associatedCpdCenterlineIndices", make_function(&LaneData::associatedCpdCenterlineIndices, - return_value_policy())) - .add_property("laneletFeatures", - make_function(&LaneData::laneletFeatures, return_value_policy())) - .add_property("edges", make_function(&LaneData::edges, return_value_policy())); + { + scope inLaneData = + class_("LaneData", "Class for holding, accessing and processing of lane data") + .def(init<>()) + .def("build", &LaneData::build) + .staticmethod("build") + .def("processAll", &LaneData::processAll) + .add_property("roadBorders", + make_function(&LaneData::roadBorders, return_value_policy())) + .add_property("laneDividers", + make_function(&LaneData::laneDividers, return_value_policy())) + .add_property("compoundRoadBorders", + make_function(&LaneData::compoundRoadBorders, return_value_policy())) + .add_property("compoundLaneDividers", + make_function(&LaneData::compoundLaneDividers, return_value_policy())) + .add_property("compoundCenterlines", + make_function(&LaneData::compoundCenterlines, return_value_policy())) + .add_property("associatedCpdRoadBorderIndices", make_function(&LaneData::associatedCpdRoadBorderIndices, + return_value_policy())) + .add_property("associatedCpdLaneDividerIndices", make_function(&LaneData::associatedCpdLaneDividerIndices, + return_value_policy())) + .add_property("associatedCpdCenterlineIndices", make_function(&LaneData::associatedCpdCenterlineIndices, + return_value_policy())) + .add_property("laneletFeatures", + make_function(&LaneData::laneletFeatures, return_value_policy())) + .add_property("edges", make_function(&LaneData::edges, return_value_policy())) + .add_property("uuid", make_function(&LaneData::uuid, return_value_policy())) + .def("getTensorFeatureData", &LaneData::getTensorFeatureData); + + class_("TensorFeatureData", "TensorFeatureData class for LaneData", init<>()) + .add_property("roadBorders", make_function(&LaneData::TensorFeatureData::roadBorders, + return_value_policy())) + .add_property("laneDividers", make_function(&LaneData::TensorFeatureData::laneDividers, + return_value_policy())) + .add_property("laneDividerTypes", make_function(&LaneData::TensorFeatureData::laneDividerTypes, + return_value_policy())) + .add_property("compoundRoadBorders", make_function(&LaneData::TensorFeatureData::compoundRoadBorders, + return_value_policy())) + .add_property("compoundLaneDividers", make_function(&LaneData::TensorFeatureData::compoundLaneDividers, + return_value_policy())) + .add_property("compoundLaneDividerTypes", make_function(&LaneData::TensorFeatureData::compoundLaneDividerTypes, + return_value_policy())) + .add_property("compoundCenterlines", make_function(&LaneData::TensorFeatureData::compoundCenterlines, + return_value_policy())) + .add_property("uuid", + make_function(&LaneData::TensorFeatureData::uuid, return_value_policy())); + } { scope inMapDataInterface = @@ -281,55 +335,21 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT converters::VectorToListConverter(); converters::VectorToListConverter< boost::geometry::model::ring>(); + converters::VectorToListConverter>(); + converters::VectorToListConverter>(); converters::IterableConverter() .fromPython>() .fromPython() .fromPython() .fromPython(); - class_("LaneLineStringFeatures").def(map_indexing_suite()); - class_("LaneletFeatures").def(map_indexing_suite()); - converters::VectorToListConverter>(); + converters::MapToDictConverter(); + converters::MapToDictConverter(); + DictToMapConverter(); + DictToMapConverter(); class_>("BoolList").def(vector_indexing_suite>()); - // class_("LaneLineStringFeatureList") - // .def(vector_indexing_suite()); - // class_("CompoundLaneLineStringFeatureList") - // .def(vector_indexing_suite()); - implicitly_convertible(); - - // overloaded template function instantiations - def (*)(const std::map &, bool)>( - "getPointsMatrices", &getPointsMatrices); - def (*)(const std::vector &, bool)>( - "getPointsMatrices", &getPointsMatrices); - def (*)(const std::map &, bool)>( - "getPointsMatrices", &getPointsMatrices); def (*)(const std::vector &, bool)>( - "getPointsMatrices", &getPointsMatrices); - - def &, bool, bool)>( - "getFeatureVectorMatrix", &getFeatureVectorMatrix); - def &, bool, bool)>( - "getFeatureVectorMatrix", &getFeatureVectorMatrix); - def &, bool, bool)>( - "getFeatureVectorMatrix", &getFeatureVectorMatrix); - def &, bool, bool)>( - "getFeatureVectorMatrix", &getFeatureVectorMatrix); - def &, bool, bool)>("getFeatureVectorMatrix", - &getFeatureVectorMatrix); - def &, bool, bool)>("getFeatureVectorMatrix", - &getFeatureVectorMatrix); - - def &, const OrientedRect &, const ParametrizationType &, int32_t)>( - "processFeatures", &processFeatures); - def &, const OrientedRect &, const ParametrizationType &, int32_t)>( - "processFeatures", &processFeatures); - def &, const OrientedRect &, const ParametrizationType &, - int32_t)>("processFeatures", &processFeatures); - def &, const OrientedRect &, const ParametrizationType &, - int32_t)>("processFeatures", &processFeatures); - def &, const OrientedRect &, const ParametrizationType &, int32_t)>( - "processFeatures", &processFeatures); - def &, const OrientedRect &, const ParametrizationType &, int32_t)>( - "processFeatures", &processFeatures); + "getPointsMatrices", &getPointMatrices); + + implicitly_convertible(); } diff --git a/lanelet2_python/test/test_lanelet2_map_learning.py b/lanelet2_python/test/test_lanelet2_map_learning.py index a2eecf03..5413de28 100644 --- a/lanelet2_python/test/test_lanelet2_map_learning.py +++ b/lanelet2_python/test/test_lanelet2_map_learning.py @@ -1,7 +1,7 @@ import unittest import lanelet2 # if we fail here, there is something wrong with lanelet2 registration from lanelet2.core import getId, Point3d, BasicPoint2d, LineString3d, Lanelet, LaneletMap -from lanelet2.map_learning import MapDataInterface, getFeatureVectorMatrix +from lanelet2.map_learning import MapDataInterface def get_sample_lanelet_map(): mymap = LaneletMap() @@ -24,8 +24,9 @@ def test_map_data_interface(self): mDataIf = MapDataInterface(mymap) mDataIf.setCurrPosAndExtractSubmap(pos, 0) lData = mDataIf.laneData() - fmat = lanelet2.map_learning.getPointsMatrices(lData.compoundRoadBorders, True) - print(fmat) + tfData = lData.getTensorFeatureData(True, False) + self.assertEqual(len(tfData.compoundCenterlines), 2) + self.assertEqual(tfData.compoundLaneDividerTypes[-1], 1) if __name__ == '__main__': From dab156d126a245c3702c60467471a1dcc5a88b99 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Fri, 12 Jan 2024 19:59:35 +0100 Subject: [PATCH 41/64] bugfixes --- .../lanelet2_map_learning/MapFeatures.h | 15 +++----- .../include/lanelet2_map_learning/Utils.h | 16 ++++---- lanelet2_map_learning/src/MapData.cpp | 2 + .../src/MapDataInterface.cpp | 2 +- lanelet2_map_learning/src/MapFeatures.cpp | 7 +++- lanelet2_map_learning/src/Utils.cpp | 37 +++++++++++++------ lanelet2_map_learning/test/test_utils.cpp | 7 ++-- 7 files changed, 51 insertions(+), 35 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index 5d44dc17..06da4c4f 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -182,17 +182,14 @@ using LaneletFeatures = std::map; template MatrixXd getFeatureVectorMatrix(const std::map>& mapFeatures, bool onlyPoints, bool pointsIn2d) { std::vector> featList; - for (const auto& pair : mapFeatures) { - featList.push_back(pair.second); - } + // for (const auto& pair : mapFeatures) { + // featList.push_back(pair.second); + // } return getFeatureVectorMatrix(featList, onlyPoints, pointsIn2d); } template MatrixXd getFeatureVectorMatrix(const std::vector>& mapFeatures, bool onlyPoints, bool pointsIn2d) { - if (mapFeatures.empty()) { - throw std::runtime_error("Empty mapFeatures vector supplied!"); - } std::vector featureVectors; for (const auto& feat : mapFeatures) { if (!feat->valid()) { @@ -225,13 +222,11 @@ std::vector getPointMatrices(const std::map>& m template std::vector getPointMatrices(const std::vector>& mapFeatures, bool pointsIn2d) { - if (mapFeatures.empty()) { - throw std::runtime_error("Empty mapFeatures vector supplied!"); - } std::vector pointMatrices; for (const auto& feat : mapFeatures) { if (!feat->valid()) { - throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); + continue; + // throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); } pointMatrices.push_back(feat->pointMatrix(pointsIn2d)); } diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h index f4a17097..817b36aa 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h @@ -19,20 +19,20 @@ LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPo double extentLongitudinal, double extentLateral); inline bool isRoadBorder(const ConstLineString3d& lstring) { - Attribute type = lstring.attribute(AttributeName::Type); + Attribute type = lstring.attributeOr(AttributeName::Type, ""); return type == AttributeValueString::RoadBorder || type == AttributeValueString::Curbstone || type == AttributeValueString::Fence; } inline LineStringType bdTypeToEnum(ConstLineString3d lString) { - Attribute type = lString.attribute(AttributeName::Type); + Attribute type = lString.attributeOr(AttributeName::Type, ""); if (type == AttributeValueString::RoadBorder || type == AttributeValueString::Curbstone || type == AttributeValueString::Fence) { return LineStringType::RoadBorder; } else if (type == AttributeValueString::Virtual) { return LineStringType::Virtual; } - std::string subtype = lString.attribute(AttributeName::Subtype).value(); + Attribute subtype = lString.attributeOr(AttributeName::Subtype, ""); if (subtype == AttributeValueString::Dashed) return LineStringType::Dashed; else if (subtype == AttributeValueString::Solid) @@ -44,27 +44,27 @@ inline LineStringType bdTypeToEnum(ConstLineString3d lString) { else if (subtype == AttributeValueString::DashedSolid) return LineStringType::Mixed; else { - throw std::runtime_error("Unexpected Line String Subtype!"); + // throw std::runtime_error("Unexpected Line String Subtype!"); return LineStringType::Unknown; } } inline TEType teTypeToEnum(const ConstLineString3d& te) { - std::string type = te.attribute(AttributeName::Type).value(); - std::string subtype = te.attribute(AttributeName::Subtype).value(); + Attribute type = te.attributeOr(AttributeName::Type, ""); + Attribute subtype = te.attributeOr(AttributeName::Subtype, ""); if (type == AttributeValueString::TrafficLight) { return TEType::TrafficLight; } else if (type == AttributeValueString::TrafficSign) { return TEType::TrafficSign; } else { - throw std::runtime_error("Unexpected Traffic Element Type!"); + // throw std::runtime_error("Unexpected Traffic Element Type!"); return TEType::Unknown; } } BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t nPoints); -BasicLineString3d cutLineString(const OrientedRect& bbox, const BasicLineString3d& polyline); +std::vector cutLineString(const OrientedRect& bbox, const BasicLineString3d& polyline); BasicLineString3d transformLineString(const OrientedRect& bbox, const BasicLineString3d& polyline); diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 5097231b..7b590d09 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -95,12 +95,14 @@ void LaneData::initLaneletFeatures(LaneletSubmapConstPtr& localSubmap, // no successors void getPaths(lanelet::routing::RoutingGraphConstPtr localSubmapGraph, std::vector& paths, ConstLanelet start, ConstLanelets initPath = ConstLanelets()) { + std::cerr << "Starting new path from " << start.id() << std::endl; initPath.push_back(start); ConstLanelets successorLLs = localSubmapGraph->following(start, false); while (!successorLLs.empty()) { for (size_t i = 1; i != successorLLs.size(); i++) { getPaths(localSubmapGraph, paths, successorLLs[i], initPath); } + std::cerr << "Adding ll to path: " << successorLLs.front().id() << std::endl; initPath.push_back(successorLLs.front()); successorLLs = localSubmapGraph->following(successorLLs.front(), false); } diff --git a/lanelet2_map_learning/src/MapDataInterface.cpp b/lanelet2_map_learning/src/MapDataInterface.cpp index 2cd3784b..35003cb2 100644 --- a/lanelet2_map_learning/src/MapDataInterface.cpp +++ b/lanelet2_map_learning/src/MapDataInterface.cpp @@ -19,7 +19,7 @@ void MapDataInterface::setCurrPosAndExtractSubmap(const BasicPoint2d& pt, double currPos_ = pt; currYaw_ = yaw; localSubmap_ = extractSubmap(laneletMap_, *currPos_, config_.submapExtentLongitudinal, config_.submapExtentLateral); - localSubmapGraph_ = lanelet::routing::RoutingGraph::build(*laneletMap_, *trafficRules_); + localSubmapGraph_ = lanelet::routing::RoutingGraph::build(*localSubmap_, *trafficRules_); } MapDataInterface::MapDataInterface(LaneletMapConstPtr laneletMap) diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index 00d0e42f..755029ad 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -22,7 +22,12 @@ LStringProcessResult processLineStringImpl(const BasicLineString3d& lstring, con if (paramType != ParametrizationType::LineString) { throw std::runtime_error("Only polyline parametrization is implemented so far!"); } - result.cutFeature_ = cutLineString(bbox, lstring); + std::vector cutLines = cutLineString(bbox, lstring); + result.cutFeature_ = std::accumulate(cutLines.begin(), cutLines.end(), BasicLineString3d(), + [](BasicLineString3d a, BasicLineString3d b) { + a.insert(a.end(), b.begin(), b.end()); + return a; + }); if (result.cutFeature_.empty()) { result.wasCut_ = true; result.valid_ = false; diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index 00f468de..7aae2c36 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -65,7 +65,7 @@ BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t return bdInterp; } -BasicLineString3d cutLineString(const OrientedRect& bbox, const BasicLineString3d& polyline) { +std::vector cutLineString(const OrientedRect& bbox, const BasicLineString3d& polyline) { BasicLineString2d polyline2d; for (const auto& pt : polyline) { polyline2d.push_back(BasicPoint2d(pt.x(), pt.y())); @@ -73,25 +73,38 @@ BasicLineString3d cutLineString(const OrientedRect& bbox, const BasicLineString3 std::deque cut2d; boost::geometry::intersection(bbox.bg_poly, polyline2d, cut2d); - BasicLineString3d cut3d; + std::vector cut3d; if (cut2d.empty()) { return cut3d; } else if (cut2d.size() > 1) { - throw std::runtime_error("More than one cut line!"); + for (const auto& ls : cut2d) { + std::cerr << "Cut line:" << std::endl; + for (const auto& pt : ls) { + std::cerr << pt << std::endl; + } + std::cerr << "--------------" << std::endl; + } + + std::cerr << "More than one cut line!" << std::endl; + // throw std::runtime_error("More than one cut line!"); } // restore z value from closest point on the original linestring and remove double points of boost intersection - for (const auto& pt2d : cut2d[0]) { - double lastDist = std::numeric_limits::max(); - double bestZ; - for (const auto& pt : polyline) { - double currDist = (pt2d - BasicPoint2d(pt.x(), pt.y())).norm(); - if (currDist < lastDist) { - lastDist = currDist; - bestZ = pt.z(); + for (const auto& el : cut2d) { + BasicLineString3d ls; + for (const auto& pt2d : el) { + double lastDist = std::numeric_limits::max(); + double bestZ; + for (const auto& pt : polyline) { + double currDist = (pt2d - BasicPoint2d(pt.x(), pt.y())).norm(); + if (currDist < lastDist) { + lastDist = currDist; + bestZ = pt.z(); + } } + ls.push_back(BasicPoint3d(pt2d.x(), pt2d.y(), bestZ)); } - cut3d.push_back(BasicPoint3d(pt2d.x(), pt2d.y(), bestZ)); + cut3d.push_back(ls); } return cut3d; } diff --git a/lanelet2_map_learning/test/test_utils.cpp b/lanelet2_map_learning/test/test_utils.cpp index 5fb38e93..90732942 100644 --- a/lanelet2_map_learning/test/test_utils.cpp +++ b/lanelet2_map_learning/test/test_utils.cpp @@ -44,8 +44,9 @@ TEST(UtilsTest, ResampleLineString) { // NOLINT TEST_F(MapLearningTest, CutLineString) { // NOLINT BasicLineString3d polyline{BasicPoint3d{0, 0, 0}, BasicPoint3d{30, 0, 0}, BasicPoint3d{40, 0, 0}, BasicPoint3d{50, 0, 0}}; - BasicLineString3d polylineCut = cutLineString(bbox, polyline); + std::vector polylineCut = cutLineString(bbox, polyline); - EXPECT_EQ(polylineCut.size(), 2); - EXPECT_NEAR(polylineCut[1].x(), 15, 10e-5); + EXPECT_EQ(polylineCut.size(), 1); + EXPECT_EQ(polylineCut[0].size(), 2); + EXPECT_NEAR(polylineCut[0][1].x(), 15, 10e-5); } \ No newline at end of file From 506a891d5886a6c2780169f58891d011c3de3947 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Mon, 15 Jan 2024 21:25:22 +0100 Subject: [PATCH 42/64] more bugfixes, now tested working on real map --- .../include/lanelet2_map_learning/MapData.h | 45 ++++++---- .../lanelet2_map_learning/MapFeatures.h | 23 ++--- .../include/lanelet2_map_learning/Serialize.h | 1 + lanelet2_map_learning/src/MapData.cpp | 90 +++++++++++-------- lanelet2_map_learning/src/MapFeatures.cpp | 59 ++++++------ lanelet2_map_learning/src/Utils.cpp | 13 +-- .../test/test_map_features.cpp | 18 ++-- lanelet2_python/python_api/map_learning.cpp | 17 ++-- 8 files changed, 147 insertions(+), 119 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index 30a4ac03..0b6965d4 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -28,15 +28,33 @@ struct Edge { namespace internal { struct CompoundElsList { - CompoundElsList(const Id& start, LineStringType type) : ids{start}, type{type} {} - CompoundElsList(const std::vector& ids, LineStringType type) : ids{ids}, type{type} {} + CompoundElsList(const Id& start, bool startInverted, LineStringType type) + : ids{start}, inverted{startInverted}, type{type} {} + CompoundElsList(const std::vector& ids, const std::vector& inverted, LineStringType type) + : ids{ids}, inverted{inverted}, type{type} {} std::vector ids; + std::vector inverted; LineStringType type; }; } // namespace internal using Edges = std::map; // key = id from +template +std::vector> getValidElements(const std::vector>& vec) { + std::vector> res; + std::copy_if(vec.begin(), vec.end(), std::back_inserter(res), [](std::shared_ptr el) { return el->valid(); }); + return res; +} + +template +std::map> getValidElements(const std::map>& map) { + std::map> res; + std::copy_if(map.begin(), map.end(), std::inserter(res, res.end()), + [](const auto& pair) { return pair.second->valid(); }); + return res; +} + class LaneData { public: struct TensorFeatureData { @@ -73,6 +91,13 @@ class LaneData { const CompoundLaneLineStringFeatureList& compoundLaneDividers() { return compoundLaneDividers_; } const CompoundLaneLineStringFeatureList& compoundCenterlines() { return compoundCenterlines_; } + LaneLineStringFeatures validRoadBorders() { return getValidElements(roadBorders_); } + LaneLineStringFeatures validLaneDividers() { return getValidElements(laneDividers_); } + + CompoundLaneLineStringFeatureList validCompoundRoadBorders() { return getValidElements(compoundRoadBorders_); } + CompoundLaneLineStringFeatureList validCompoundLaneDividers() { return getValidElements(compoundLaneDividers_); } + CompoundLaneLineStringFeatureList validCompoundCenterlines() { return getValidElements(compoundCenterlines_); } + const std::map>& associatedCpdRoadBorderIndices() { return associatedCpdRoadBorderIndices_; } const std::map>& associatedCpdLaneDividerIndices() { return associatedCpdLaneDividerIndices_; @@ -99,7 +124,7 @@ class LaneData { void updateAssociatedCpdFeatureIndices(); LineStringType getLineStringTypeFromId(Id id); - LaneLineStringFeaturePtr getLineStringFeatFromId(Id id); + LaneLineStringFeaturePtr getLineStringFeatFromId(Id id, bool inverted); std::vector computeCompoundLeftBorders(const ConstLanelets& path); std::vector computeCompoundRightBorders(const ConstLanelets& path); CompoundLaneLineStringFeaturePtr computeCompoundCenterline(const ConstLanelets& path); @@ -136,19 +161,5 @@ class TEData { /// xLane and xTE stacked, with xLane being first }; -template -std::vector> getValidElements(const std::vector>& vec) { - std::vector> res; - std::copy_if(vec.begin(), vec.end(), std::back_inserter(res), [](std::shared_ptr el) { return el->valid(); }); - return res; -} - -template -std::map> getValidElements(const std::map>& map) { - std::map> res; - std::copy_if(map.begin(), map.end(), std::back_inserter(res), [](const auto& pair) { return pair.second->valid(); }); - return res; -} - } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index 06da4c4f..39c33177 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -58,7 +58,6 @@ class LineStringFeature : public MapFeature { protected: BasicLineString3d rawFeature_; - LineStringFeature() {} LineStringFeature(const BasicLineString3d& feature, Id mapID) : MapFeature(mapID), rawFeature_{feature} {} }; @@ -66,8 +65,9 @@ class LineStringFeature : public MapFeature { class LaneLineStringFeature : public LineStringFeature { public: LaneLineStringFeature() {} - LaneLineStringFeature(const BasicLineString3d& feature, Id mapID, LineStringType type, Id laneletID) - : LineStringFeature(feature, mapID), type_{type}, laneletIDs_{laneletID} {} + LaneLineStringFeature(const BasicLineString3d& feature, Id mapID, LineStringType type, + const std::vector& laneletID, bool inverted) + : LineStringFeature(feature, mapID), type_{type}, laneletIDs_{laneletID}, inverted_{inverted} {} virtual ~LaneLineStringFeature() noexcept = default; virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; @@ -79,8 +79,9 @@ class LaneLineStringFeature : public LineStringFeature { const BasicLineString3d& cutAndResampledFeature() const { return cutAndResampledFeature_; } const BasicLineString3d& cutResampledAndTransformedFeature() const { return cutResampledAndTransformedFeature_; } LineStringType type() const { return type_; } + bool inverted() const { return inverted_; } int typeInt() const { return static_cast(type_); } - const std::vector& laneletIDs() const { return laneletIDs_; } + const Ids& laneletIDs() const { return laneletIDs_; } void addLaneletID(Id id) { laneletIDs_.push_back(id); } template @@ -92,7 +93,8 @@ class LaneLineStringFeature : public LineStringFeature { BasicLineString3d cutAndResampledFeature_; BasicLineString3d cutResampledAndTransformedFeature_; LineStringType type_; - std::vector laneletIDs_; + bool inverted_{false}; // = inverted compared to element with that Id in lineStringLayer + Ids laneletIDs_; }; using LaneLineStringFeaturePtr = std::shared_ptr; @@ -182,9 +184,9 @@ using LaneletFeatures = std::map; template MatrixXd getFeatureVectorMatrix(const std::map>& mapFeatures, bool onlyPoints, bool pointsIn2d) { std::vector> featList; - // for (const auto& pair : mapFeatures) { - // featList.push_back(pair.second); - // } + for (const auto& pair : mapFeatures) { + featList.push_back(pair.second); + } return getFeatureVectorMatrix(featList, onlyPoints, pointsIn2d); } @@ -225,8 +227,7 @@ std::vector getPointMatrices(const std::vector>& ma std::vector pointMatrices; for (const auto& feat : mapFeatures) { if (!feat->valid()) { - continue; - // throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); + throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); } pointMatrices.push_back(feat->pointMatrix(pointsIn2d)); } @@ -257,5 +258,7 @@ bool processFeatures(std::vector>& featVec, const OrientedRec return allValid; } +MatrixXd toPointMatrix(const BasicLineString3d& lString, bool pointsIn2d); + } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h b/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h index 19590167..6ca815f2 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h @@ -70,6 +70,7 @@ void serialize(Archive& ar, lanelet::map_learning::LaneLineStringFeature& feat, ar& BOOST_SERIALIZATION_NVP(feat.cutFeature_); ar& BOOST_SERIALIZATION_NVP(feat.cutAndResampledFeature_); ar& BOOST_SERIALIZATION_NVP(feat.type_); + ar& BOOST_SERIALIZATION_NVP(feat.inverted_); ar& BOOST_SERIALIZATION_NVP(feat.laneletIDs_); } diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 7b590d09..e9b30f7c 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -20,23 +20,24 @@ LaneData LaneData::build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::R void LaneData::initLeftBoundaries(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { for (const auto& ll : localSubmap->laneletLayer) { + Id boundID = ll.leftBound3d().id(); + BasicLineString3d bound = + !ll.leftBound3d().inverted() ? ll.leftBound3d().basicLineString() : ll.leftBound3d().invert().basicLineString(); if (isRoadBorder(ll.leftBound3d())) { - LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(ll.leftBound3d().id()); + LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(boundID); if (itRoadBd != roadBorders_.end()) { itRoadBd->second->addLaneletID(ll.id()); } else { - roadBorders_.insert({ll.leftBound3d().id(), std::make_shared( - ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), - bdTypeToEnum(ll.leftBound3d()), ll.id())}); + roadBorders_.insert({boundID, std::make_shared( + bound, boundID, bdTypeToEnum(ll.leftBound3d()), Ids{ll.id()}, false)}); } } else { - LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(ll.leftBound3d().id()); + LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(boundID); if (itLaneBd != laneDividers_.end()) { itLaneBd->second->addLaneletID(ll.id()); } else { - laneDividers_.insert({ll.leftBound3d().id(), std::make_shared( - ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), - bdTypeToEnum(ll.leftBound3d()), ll.id())}); + laneDividers_.insert({boundID, std::make_shared( + bound, boundID, bdTypeToEnum(ll.leftBound3d()), Ids{ll.id()}, false)}); } } @@ -52,23 +53,24 @@ void LaneData::initLeftBoundaries(LaneletSubmapConstPtr& localSubmap, void LaneData::initRightBoundaries(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { for (const auto& ll : localSubmap->laneletLayer) { + Id boundID = ll.rightBound3d().id(); + BasicLineString3d bound = !ll.rightBound3d().inverted() ? ll.rightBound3d().basicLineString() + : ll.rightBound3d().invert().basicLineString(); if (isRoadBorder(ll.rightBound3d())) { - LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(ll.rightBound3d().id()); + LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(boundID); if (itRoadBd != roadBorders_.end()) { itRoadBd->second->addLaneletID(ll.id()); } else { - roadBorders_.insert({ll.rightBound3d().id(), std::make_shared( - ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), - bdTypeToEnum(ll.rightBound3d()), ll.id())}); + roadBorders_.insert({boundID, std::make_shared( + bound, boundID, bdTypeToEnum(ll.rightBound3d()), Ids{ll.id()}, false)}); } } else { - LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(ll.rightBound3d().id()); + LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(boundID); if (itLaneBd != laneDividers_.end()) { itLaneBd->second->addLaneletID(ll.id()); } else { - laneDividers_.insert({ll.rightBound3d().id(), std::make_shared( - ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), - bdTypeToEnum(ll.rightBound3d()), ll.id())}); + laneDividers_.insert({boundID, std::make_shared( + bound, boundID, bdTypeToEnum(ll.rightBound3d()), Ids{ll.id()}, false)}); } } Optional rightLL = localSubmapGraph->right(ll); @@ -82,10 +84,10 @@ void LaneData::initRightBoundaries(LaneletSubmapConstPtr& localSubmap, void LaneData::initLaneletFeatures(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { for (const auto& ll : localSubmap->laneletLayer) { - LaneLineStringFeaturePtr leftBoundary = getLineStringFeatFromId(ll.leftBound().id()); - LaneLineStringFeaturePtr rightBoundary = getLineStringFeatFromId(ll.rightBound().id()); + LaneLineStringFeaturePtr leftBoundary = getLineStringFeatFromId(ll.leftBound().id(), ll.leftBound().inverted()); + LaneLineStringFeaturePtr rightBoundary = getLineStringFeatFromId(ll.rightBound().id(), ll.leftBound().inverted()); LaneLineStringFeaturePtr centerline = std::make_shared( - ll.centerline3d().basicLineString(), ll.centerline3d().id(), LineStringType::Centerline, ll.id()); + ll.centerline3d().basicLineString(), ll.centerline3d().id(), LineStringType::Centerline, Ids{ll.id()}, false); laneletFeatures_.insert( {ll.id(), std::make_shared(leftBoundary, rightBoundary, centerline, ll.id())}); } @@ -95,14 +97,12 @@ void LaneData::initLaneletFeatures(LaneletSubmapConstPtr& localSubmap, // no successors void getPaths(lanelet::routing::RoutingGraphConstPtr localSubmapGraph, std::vector& paths, ConstLanelet start, ConstLanelets initPath = ConstLanelets()) { - std::cerr << "Starting new path from " << start.id() << std::endl; initPath.push_back(start); ConstLanelets successorLLs = localSubmapGraph->following(start, false); while (!successorLLs.empty()) { for (size_t i = 1; i != successorLLs.size(); i++) { getPaths(localSubmapGraph, paths, successorLLs[i], initPath); } - std::cerr << "Adding ll to path: " << successorLLs.front().id() << std::endl; initPath.push_back(successorLLs.front()); successorLLs = localSubmapGraph->following(successorLLs.front(), false); } @@ -124,13 +124,19 @@ LineStringType LaneData::getLineStringTypeFromId(Id id) { return bdType; } -LaneLineStringFeaturePtr LaneData::getLineStringFeatFromId(Id id) { +LaneLineStringFeaturePtr makeInverted(const LaneLineStringFeaturePtr& feat) { + return std::make_shared( + BasicLineString3d(feat->rawFeature().rbegin(), feat->rawFeature().rend()), feat->mapID(), feat->type(), + feat->laneletIDs(), !feat->inverted()); +} + +LaneLineStringFeaturePtr LaneData::getLineStringFeatFromId(Id id, bool inverted) { LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(id); LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(id); if (itLaneBd != laneDividers_.end()) { - return itLaneBd->second; + return (inverted == itLaneBd->second->inverted()) ? itLaneBd->second : makeInverted(itLaneBd->second); } else if (itRoadBd != roadBorders_.end()) { - return itRoadBd->second; + return (inverted == itRoadBd->second->inverted()) ? itRoadBd->second : makeInverted(itRoadBd->second); } else { throw std::runtime_error("Lanelet boundary " + std::to_string(id) + " is neither a road border nor a lane divider!"); @@ -142,14 +148,15 @@ std::vector LaneData::computeCompoundLeftBorders(const ConstLan ConstLanelet start = path.front(); LineStringType currType = getLineStringTypeFromId(start.leftBound3d().id()); - compoundBorders.push_back(CompoundElsList{start.leftBound3d().id(), currType}); + compoundBorders.push_back(CompoundElsList{start.leftBound3d().id(), start.leftBound3d().inverted(), currType}); for (size_t i = 1; i != path.size(); i++) { LineStringType newType = getLineStringTypeFromId(path[i].leftBound3d().id()); if (currType == newType) { compoundBorders.back().ids.push_back(path[i].leftBound3d().id()); + compoundBorders.back().inverted.push_back(path[i].leftBound3d().inverted()); } else { - compoundBorders.push_back(CompoundElsList{path[i].leftBound3d().id(), newType}); + compoundBorders.push_back(CompoundElsList{path[i].leftBound3d().id(), path[i].leftBound3d().inverted(), newType}); currType = newType; } } @@ -161,14 +168,16 @@ std::vector LaneData::computeCompoundRightBorders(const ConstLa ConstLanelet start = path.front(); LineStringType currType = getLineStringTypeFromId(start.rightBound3d().id()); - compoundBorders.push_back(CompoundElsList{start.rightBound3d().id(), currType}); + compoundBorders.push_back(CompoundElsList{start.rightBound3d().id(), start.rightBound3d().inverted(), currType}); for (size_t i = 1; i != path.size(); i++) { LineStringType newType = getLineStringTypeFromId(path[i].rightBound3d().id()); if (currType == newType) { compoundBorders.back().ids.push_back(path[i].rightBound3d().id()); + compoundBorders.back().inverted.push_back(path[i].rightBound3d().inverted()); } else { - compoundBorders.push_back(CompoundElsList{path[i].rightBound3d().id(), newType}); + compoundBorders.push_back( + CompoundElsList{path[i].rightBound3d().id(), path[i].rightBound3d().inverted(), newType}); currType = newType; } } @@ -178,8 +187,8 @@ std::vector LaneData::computeCompoundRightBorders(const ConstLa CompoundLaneLineStringFeaturePtr LaneData::computeCompoundCenterline(const ConstLanelets& path) { LaneLineStringFeatureList compoundCenterlines; for (const auto& ll : path) { - compoundCenterlines.push_back(std::make_shared(ll.centerline3d().basicLineString(), ll.id(), - LineStringType::Centerline, ll.id())); + compoundCenterlines.push_back(std::make_shared( + ll.centerline3d().basicLineString(), ll.id(), LineStringType::Centerline, Ids{ll.id()}, false)); } return std::make_shared(compoundCenterlines, LineStringType::Centerline); } @@ -243,8 +252,11 @@ void LaneData::initCompoundFeatures(LaneletSubmapConstPtr& localSubmap, } for (const auto& compFeat : compoundedBordersAndDividers) { LaneLineStringFeatureList toBeCompounded; - for (const auto& el : compFeat.ids) { - LaneLineStringFeaturePtr cmpdFeat = getLineStringFeatFromId(el); + if (compFeat.ids.size() != compFeat.inverted.size()) { + throw std::runtime_error("Unequal sizes of ids and inverted!"); + } + for (size_t i = 0; i < compFeat.ids.size(); i++) { + LaneLineStringFeaturePtr cmpdFeat = getLineStringFeatFromId(compFeat.ids[i], compFeat.inverted[i]); toBeCompounded.push_back(cmpdFeat); } LineStringType cmpdType = toBeCompounded.front()->type(); @@ -303,15 +315,15 @@ LaneData::TensorFeatureData LaneData::getTensorFeatureData(bool pointsIn2d, bool if (!tfData_.has_value() || ignoreBuffer) { tfData_ = LaneData::TensorFeatureData(); tfData_->uuid_ = uuid_; - tfData_->roadBorders_ = getPointMatrices(roadBorders_, pointsIn2d); - tfData_->laneDividers_ = getPointMatrices(laneDividers_, pointsIn2d); - tfData_->compoundRoadBorders_ = getPointMatrices(compoundRoadBorders_, pointsIn2d); - tfData_->compoundLaneDividers_ = getPointMatrices(compoundLaneDividers_, pointsIn2d); - tfData_->compoundCenterlines_ = getPointMatrices(compoundCenterlines_, pointsIn2d); - for (const auto& ft : laneDividers_) { + tfData_->roadBorders_ = getPointMatrices(validRoadBorders(), pointsIn2d); + tfData_->laneDividers_ = getPointMatrices(validLaneDividers(), pointsIn2d); + tfData_->compoundRoadBorders_ = getPointMatrices(validCompoundRoadBorders(), pointsIn2d); + tfData_->compoundLaneDividers_ = getPointMatrices(validCompoundLaneDividers(), pointsIn2d); + tfData_->compoundCenterlines_ = getPointMatrices(validCompoundCenterlines(), pointsIn2d); + for (const auto& ft : validLaneDividers()) { tfData_->laneDividerTypes_.push_back(ft.second->typeInt()); } - for (const auto& ft : compoundLaneDividers_) { + for (const auto& ft : validCompoundLaneDividers()) { tfData_->compoundLaneDividerTypes_.push_back(ft->typeInt()); } } diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index 755029ad..4e93f53e 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -60,8 +60,7 @@ bool LaneLineStringFeature::process(const OrientedRect& bbox, const Parametrizat } VectorXd LaneLineStringFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { - const BasicLineString3d& selectedFeature = - (cutResampledAndTransformedFeature_.size() > 0) ? cutResampledAndTransformedFeature_ : rawFeature_; + const BasicLineString3d& selectedFeature = cutResampledAndTransformedFeature_; VectorXd vec = pointsIn2d ? VectorXd(2 * selectedFeature.size() + 1) : VectorXd(3 * selectedFeature.size() + 1); // n points with 2/3 dims + type if (pointsIn2d == true) { @@ -103,36 +102,10 @@ VectorXd TEFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const } MatrixXd LaneLineStringFeature::pointMatrix(bool pointsIn2d) const { - const BasicLineString3d& selectedFeature = - (cutResampledAndTransformedFeature_.size() > 0) ? cutResampledAndTransformedFeature_ : rawFeature_; - MatrixXd mat = pointsIn2d ? MatrixXd(selectedFeature.size(), 2) - : MatrixXd(selectedFeature.size(), 3); // n points with 2/3 dims + type - if (pointsIn2d == true) { - for (size_t i = 0; i < selectedFeature.size(); i++) { - mat.row(i) = selectedFeature[i](Eigen::seq(0, 1)); - } - } else { - for (size_t i = 0; i < selectedFeature.size(); i++) { - mat.row(i) = selectedFeature[i](Eigen::seq(0, 2)); - } - } - return mat; + return toPointMatrix(cutResampledAndTransformedFeature_, pointsIn2d); } -MatrixXd TEFeature::pointMatrix(bool pointsIn2d) const { - MatrixXd mat = - pointsIn2d ? MatrixXd(rawFeature_.size(), 2) : MatrixXd(rawFeature_.size(), 3); // n points with 2/3 dims + type - if (pointsIn2d == true) { - for (size_t i = 0; i < rawFeature_.size(); i++) { - mat.row(i) = rawFeature_[i](Eigen::seq(0, 1)); - } - } else { - for (size_t i = 0; i < rawFeature_.size(); i++) { - mat.row(i) = rawFeature_[i](Eigen::seq(0, 2)); - } - } - return mat; -} +MatrixXd TEFeature::pointMatrix(bool pointsIn2d) const { return toPointMatrix(rawFeature_, pointsIn2d); } LaneletFeature::LaneletFeature(LaneLineStringFeaturePtr leftBoundary, LaneLineStringFeaturePtr rightBoundary, LaneLineStringFeaturePtr centerline, Id mapID) @@ -140,11 +113,13 @@ LaneletFeature::LaneletFeature(LaneLineStringFeaturePtr leftBoundary, LaneLineSt LaneletFeature::LaneletFeature(const ConstLanelet& ll) : leftBoundary_{std::make_shared(ll.leftBound3d().basicLineString(), ll.leftBound3d().id(), - bdTypeToEnum(ll.leftBound3d()), ll.id())}, - rightBoundary_{std::make_shared( - ll.rightBound3d().basicLineString(), ll.rightBound3d().id(), bdTypeToEnum(ll.rightBound3d()), ll.id())}, + bdTypeToEnum(ll.leftBound3d()), Ids{ll.id()}, + ll.leftBound3d().inverted())}, + rightBoundary_{std::make_shared(ll.rightBound3d().basicLineString(), + ll.rightBound3d().id(), bdTypeToEnum(ll.rightBound3d()), + Ids{ll.id()}, ll.rightBound3d().inverted())}, centerline_{std::make_shared(ll.centerline3d().basicLineString(), ll.centerline3d().id(), - LineStringType::Centerline, ll.id())} {} + LineStringType::Centerline, Ids{ll.id()}, false)} {} bool LaneletFeature::process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { leftBoundary_->process(bbox, paramType, nPoints); @@ -204,6 +179,7 @@ CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStrin if (features[i]->rawFeature().empty()) { throw std::runtime_error("Feature with empty rawFeature() supplied!"); } + // checking if the linestring is correctly if (i == features.size() - 1) { rawFeature_.insert(rawFeature_.end(), features[i]->rawFeature().begin(), features[i]->rawFeature().end()); } else { @@ -243,5 +219,20 @@ bool CompoundLaneLineStringFeature::process(const OrientedRect& bbox, const Para return valid; } +MatrixXd toPointMatrix(const BasicLineString3d& lString, bool pointsIn2d) { + MatrixXd mat = + pointsIn2d ? MatrixXd(lString.size(), 2) : MatrixXd(lString.size(), 3); // n points with 2/3 dims + type + if (pointsIn2d == true) { + for (size_t i = 0; i < lString.size(); i++) { + mat.row(i) = lString[i](Eigen::seq(0, 1)); + } + } else { + for (size_t i = 0; i < lString.size(); i++) { + mat.row(i) = lString[i](Eigen::seq(0, 2)); + } + } + return mat; +} + } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index 7aae2c36..110e536c 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -77,19 +77,22 @@ std::vector cutLineString(const OrientedRect& bbox, const Bas if (cut2d.empty()) { return cut3d; } else if (cut2d.size() > 1) { + std::cerr << "More than one cut line!" << std::endl; + std::cerr << "Raw line:" << std::endl; + for (const auto& pt : polyline) { + std::cerr << "[" << pt.x() << "," << pt.y() << "]" << std::endl; + } for (const auto& ls : cut2d) { std::cerr << "Cut line:" << std::endl; for (const auto& pt : ls) { - std::cerr << pt << std::endl; + std::cerr << "[" << pt.x() << "," << pt.y() << "]" << std::endl; } std::cerr << "--------------" << std::endl; } - - std::cerr << "More than one cut line!" << std::endl; - // throw std::runtime_error("More than one cut line!"); + throw std::runtime_error("More than one cut line!"); } - // restore z value from closest point on the original linestring and remove double points of boost intersection + // restore z value from closest point on the original linestring for (const auto& el : cut2d) { BasicLineString3d ls; for (const auto& pt2d : el) { diff --git a/lanelet2_map_learning/test/test_map_features.cpp b/lanelet2_map_learning/test/test_map_features.cpp index 081d74e5..f58f5acf 100644 --- a/lanelet2_map_learning/test/test_map_features.cpp +++ b/lanelet2_map_learning/test/test_map_features.cpp @@ -10,7 +10,7 @@ using namespace lanelet::map_learning::tests; TEST_F(MapLearningTest, LaneLineStringFeature) { // NOLINT BasicLineString3d polyline{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}, BasicPoint3d{20, 0, 0}}; - LaneLineStringFeature feat(polyline, Id(123), LineStringType::Solid, Id(1234)); + LaneLineStringFeature feat(polyline, Id(123), LineStringType::Solid, Ids(1234), false); feat.process(bbox, ParametrizationType::LineString, 4); EXPECT_EQ(feat.cutAndResampledFeature().size(), 4); @@ -38,15 +38,15 @@ TEST_F(MapLearningTest, LaneletFeature) { // NOLINT BasicLineString3d leftBd{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}, BasicPoint3d{20, 0, 0}}; LaneLineStringFeaturePtr leftBdFeat = - std::make_shared(leftBd, Id(123), LineStringType::Solid, Id(1234)); + std::make_shared(leftBd, Id(123), LineStringType::Solid, Ids(1234), false); BasicLineString3d rightBd{BasicPoint3d{0, 4, 0}, BasicPoint3d{5, 4, 0}, BasicPoint3d{10, 4, 0}, BasicPoint3d{20, 4, 0}}; LaneLineStringFeaturePtr rightBdFeat = - std::make_shared(rightBd, Id(124), LineStringType::Dashed, Id(1234)); + std::make_shared(rightBd, Id(124), LineStringType::Dashed, Ids(1234), false); BasicLineString3d centerline{BasicPoint3d{0, 2, 0}, BasicPoint3d{5, 2, 0}, BasicPoint3d{10, 2, 0}, BasicPoint3d{20, 2, 0}}; LaneLineStringFeaturePtr centerlineFeat = - std::make_shared(centerline, Id(125), LineStringType::Centerline, Id(1234)); + std::make_shared(centerline, Id(125), LineStringType::Centerline, Ids(1234), false); LaneletFeature llFeat(leftBdFeat, rightBdFeat, centerlineFeat, Id(1234)); llFeat.setReprType(LaneletRepresentationType::Boundaries); @@ -69,19 +69,19 @@ TEST_F(MapLearningTest, LaneletFeature) { // NOLINT TEST_F(MapLearningTest, CompoundLaneLineStringFeature) { // NOLINT BasicLineString3d p1{BasicPoint3d{-10, 0, 0}, BasicPoint3d{-5, 0, 0}}; LaneLineStringFeaturePtr feat1 = - std::make_shared(p1, Id(123), LineStringType::Solid, Id(1234)); + std::make_shared(p1, Id(123), LineStringType::Solid, Ids(1234), false); BasicLineString3d p2{BasicPoint3d{-5, 0, 0}, BasicPoint3d{0, 0, 0}}; LaneLineStringFeaturePtr feat2 = - std::make_shared(p2, Id(123), LineStringType::Solid, Id(1234)); + std::make_shared(p2, Id(123), LineStringType::Solid, Ids(1234), false); BasicLineString3d p3{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}}; LaneLineStringFeaturePtr feat3 = - std::make_shared(p3, Id(123), LineStringType::Solid, Id(1234)); + std::make_shared(p3, Id(123), LineStringType::Solid, Ids(1234), false); BasicLineString3d p4{BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}}; LaneLineStringFeaturePtr feat4 = - std::make_shared(p4, Id(124), LineStringType::Solid, Id(1234)); + std::make_shared(p4, Id(124), LineStringType::Solid, Ids(1234), false); BasicLineString3d p5{BasicPoint3d{10, 0, 0}, BasicPoint3d{20, 0, 0}}; LaneLineStringFeaturePtr feat5 = - std::make_shared(p5, Id(125), LineStringType::Solid, Id(1234)); + std::make_shared(p5, Id(125), LineStringType::Solid, Ids(1234), false); CompoundLaneLineStringFeature cpdFeat(LaneLineStringFeatureList{feat1, feat2, feat3, feat4, feat5}, LineStringType::Solid); diff --git a/lanelet2_python/python_api/map_learning.cpp b/lanelet2_python/python_api/map_learning.cpp index e957128c..22e71448 100644 --- a/lanelet2_python/python_api/map_learning.cpp +++ b/lanelet2_python/python_api/map_learning.cpp @@ -42,8 +42,9 @@ class LaneLineStringFeatureWrap : public LaneLineStringFeature, public wrapper &laneletID, bool inverted) + : LaneLineStringFeature(feature, mapID, type, laneletID, inverted) {} VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { if (override f = this->get_override("computeFeatureVector")) return f(onlyPoints, pointsIn2d); @@ -201,7 +202,8 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .def("pointMatrix", pure_virtual(&LineStringFeature::pointMatrix)); class_, LaneLineStringFeaturePtr, boost::noncopyable>( - "LaneLineStringFeature", "Lane line string feature class", init()) + "LaneLineStringFeature", "Lane line string feature class", + init()) .def(init<>()) .add_property("cutFeature", make_function(&LaneLineStringFeature::cutFeature, return_value_policy())) @@ -211,6 +213,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT make_function(&LaneLineStringFeature::cutResampledAndTransformedFeature, return_value_policy())) .add_property("type", &LaneLineStringFeature::type) + .add_property("inverted", &LaneLineStringFeature::inverted) .add_property("typeInt", &LaneLineStringFeature::typeInt) .add_property("laneletIDs", make_function(&LaneLineStringFeature::laneletIDs, return_value_policy())) @@ -273,6 +276,11 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT make_function(&LaneData::compoundLaneDividers, return_value_policy())) .add_property("compoundCenterlines", make_function(&LaneData::compoundCenterlines, return_value_policy())) + .add_property("validRoadBorders", &LaneData::validRoadBorders) + .add_property("validLaneDividers", &LaneData::validLaneDividers) + .add_property("validCompoundRoadBorders", &LaneData::validCompoundRoadBorders) + .add_property("validCompoundLaneDividers", &LaneData::validCompoundLaneDividers) + .add_property("validCompoundCenterlines", &LaneData::validCompoundCenterlines) .add_property("associatedCpdRoadBorderIndices", make_function(&LaneData::associatedCpdRoadBorderIndices, return_value_policy())) .add_property("associatedCpdLaneDividerIndices", make_function(&LaneData::associatedCpdLaneDividerIndices, @@ -348,8 +356,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT DictToMapConverter(); class_>("BoolList").def(vector_indexing_suite>()); - def (*)(const std::vector &, bool)>( - "getPointsMatrices", &getPointMatrices); + def("toPointMatrix", &toPointMatrix); implicitly_convertible(); } From 63cd77d11422fc36dc5e2135ca15eea49a86f161 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Tue, 16 Jan 2024 20:25:50 +0100 Subject: [PATCH 43/64] fix python serialization, properly handle splitted cut lines --- .../include/lanelet2_map_learning/MapData.h | 4 +- .../lanelet2_map_learning/MapDataInterface.h | 14 +- .../lanelet2_map_learning/MapFeatures.h | 45 ++--- .../include/lanelet2_map_learning/Serialize.h | 5 +- .../include/lanelet2_map_learning/Utils.h | 4 +- lanelet2_map_learning/src/MapData.cpp | 32 ++-- .../src/MapDataInterface.cpp | 26 ++- lanelet2_map_learning/src/MapFeatures.cpp | 158 +++++++++++------- lanelet2_map_learning/src/Utils.cpp | 8 +- lanelet2_map_learning/test/test_map_data.cpp | 50 +++--- .../test/test_map_data_interface.cpp | 50 +++--- .../test/test_map_features.cpp | 109 ++++++------ lanelet2_python/python_api/map_learning.cpp | 92 +++++----- 13 files changed, 328 insertions(+), 269 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index 0b6965d4..1da92713 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -19,6 +19,8 @@ namespace lanelet { namespace map_learning { +using LaneDataPtr = std::shared_ptr; + struct Edge { Edge() = default; Edge(Id el1, Id el2) : el1_{el1}, el2_{el2} {} @@ -81,7 +83,7 @@ class LaneData { }; LaneData() noexcept : uuid_{boost::lexical_cast(boost::uuids::random_generator()())} {} - static LaneData build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); + static LaneDataPtr build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); bool processAll(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints); const LaneLineStringFeatures& roadBorders() { return roadBorders_; } diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h index 18b3bad4..d09feeac 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h @@ -36,17 +36,15 @@ class MapDataInterface { const Configuration& config() { return config_; } void setCurrPosAndExtractSubmap(const BasicPoint2d& pt, double yaw); - LaneData laneData(); - TEData teData(); + LaneDataPtr laneData(bool processAll); + TEData teData(bool processAll); - std::vector laneDataBatch(const BasicPoints2d& pts, const std::vector& yaws, - bool processAll = true); - std::vector laneTEDataBatch(const BasicPoints2d& pts, const std::vector& yaws, - bool processAll = true); + std::vector laneDataBatch(std::vector pts, std::vector yaws); + std::vector laneTEDataBatch(std::vector pts, std::vector yaws); private: - LaneData getLaneData(LaneletSubmapConstPtr localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph, - const BasicPoint2d& pos, double yaw, bool processAll = true); + LaneDataPtr getLaneData(LaneletSubmapConstPtr localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph, + const BasicPoint2d& pos, double yaw, bool processAll = true); TEData getLaneTEData(LaneletSubmapConstPtr localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph, const BasicPoint2d& pos, double yaw, bool processAll = true); diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h index 39c33177..eed0838b 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h @@ -16,6 +16,8 @@ namespace map_learning { using VectorXd = Eigen::Matrix; using MatrixXd = Eigen::Matrix; +using BasicLineStrings3d = std::vector; + class MapFeature { public: const Id mapID() const { return mapID_.get_value_or(InvalId); } @@ -23,7 +25,7 @@ class MapFeature { bool valid() const { return valid_; } bool wasCut() const { return wasCut_; } - virtual VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const = 0; + virtual std::vector computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const = 0; virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) = 0; template @@ -46,8 +48,8 @@ class LineStringFeature : public MapFeature { public: const BasicLineString3d& rawFeature() const { return rawFeature_; } - virtual VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const = 0; - virtual MatrixXd pointMatrix(bool pointsIn2d) const = 0; + virtual std::vector computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const = 0; + virtual std::vector pointMatrices(bool pointsIn2d) const = 0; virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) = 0; template @@ -71,13 +73,14 @@ class LaneLineStringFeature : public LineStringFeature { virtual ~LaneLineStringFeature() noexcept = default; virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; - virtual VectorXd computeFeatureVector(bool onlyPoints, - bool pointsIn2d) const override; // uses processedFeature_ when available - virtual MatrixXd pointMatrix(bool pointsIn2d) const override; // uses processedFeature_ when available - - const BasicLineString3d& cutFeature() const { return cutFeature_; } - const BasicLineString3d& cutAndResampledFeature() const { return cutAndResampledFeature_; } - const BasicLineString3d& cutResampledAndTransformedFeature() const { return cutResampledAndTransformedFeature_; } + virtual std::vector computeFeatureVectors( + bool onlyPoints, + bool pointsIn2d) const override; // uses processedFeature_ when available + virtual std::vector pointMatrices(bool pointsIn2d) const override; // uses processedFeature_ when available + + const BasicLineStrings3d& cutFeature() const { return cutFeatures_; } + const BasicLineStrings3d& cutAndResampledFeature() const { return cutAndResampledFeatures_; } + const BasicLineStrings3d& cutResampledAndTransformedFeature() const { return cutResampledAndTransformedFeatures_; } LineStringType type() const { return type_; } bool inverted() const { return inverted_; } int typeInt() const { return static_cast(type_); } @@ -89,9 +92,9 @@ class LaneLineStringFeature : public LineStringFeature { const unsigned int /*version*/); protected: - BasicLineString3d cutFeature_; - BasicLineString3d cutAndResampledFeature_; - BasicLineString3d cutResampledAndTransformedFeature_; + BasicLineStrings3d cutFeatures_; + BasicLineStrings3d cutAndResampledFeatures_; + BasicLineStrings3d cutResampledAndTransformedFeatures_; LineStringType type_; bool inverted_{false}; // = inverted compared to element with that Id in lineStringLayer Ids laneletIDs_; @@ -110,9 +113,9 @@ class TEFeature : public LineStringFeature { bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) override; // not implemented yet - VectorXd computeFeatureVector(bool onlyPoints, - bool pointsIn2d) const override; // currently uses raw feature only - virtual MatrixXd pointMatrix(bool pointsIn2d) const override; + std::vector computeFeatureVectors(bool onlyPoints, + bool pointsIn2d) const override; // currently uses raw feature only + virtual std::vector pointMatrices(bool pointsIn2d) const override; const TEType& teType() { return teType_; } @@ -129,7 +132,7 @@ class LaneletFeature : public MapFeature { virtual ~LaneletFeature() noexcept = default; bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; - VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const override; + std::vector computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const override; void setReprType(LaneletRepresentationType reprType) { reprType_ = reprType; } @@ -197,7 +200,8 @@ MatrixXd getFeatureVectorMatrix(const std::vector>& mapFeatur if (!feat->valid()) { throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); } - featureVectors.push_back(feat->computeFeatureVector(onlyPoints, pointsIn2d)); + std::vector individualVecs = feat->computeFeatureVectors(onlyPoints, pointsIn2d); + featureVectors.insert(featureVectors.end(), individualVecs.begin(), individualVecs.end()); } if (std::adjacent_find(featureVectors.begin(), featureVectors.end(), [](const VectorXd& v1, const VectorXd& v2) { return v1.size() != v2.size(); }) == @@ -229,7 +233,8 @@ std::vector getPointMatrices(const std::vector>& ma if (!feat->valid()) { throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); } - pointMatrices.push_back(feat->pointMatrix(pointsIn2d)); + std::vector individualMats = feat->pointMatrices(pointsIn2d); + pointMatrices.insert(pointMatrices.end(), individualMats.begin(), individualMats.end()); } return pointMatrices; } @@ -259,6 +264,6 @@ bool processFeatures(std::vector>& featVec, const OrientedRec } MatrixXd toPointMatrix(const BasicLineString3d& lString, bool pointsIn2d); - +VectorXd toFeatureVector(const BasicLineString3d& line, int typeInt, bool onlyPoints, bool pointsIn2d); } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h b/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h index 6ca815f2..23df6767 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h @@ -67,8 +67,9 @@ void serialize(Archive& ar, lanelet::map_learning::LineStringFeature& feat, cons template void serialize(Archive& ar, lanelet::map_learning::LaneLineStringFeature& feat, const unsigned int /*version*/) { ar& make_nvp("LineStringFeature", boost::serialization::base_object(feat)); - ar& BOOST_SERIALIZATION_NVP(feat.cutFeature_); - ar& BOOST_SERIALIZATION_NVP(feat.cutAndResampledFeature_); + ar& BOOST_SERIALIZATION_NVP(feat.cutFeatures_); + ar& BOOST_SERIALIZATION_NVP(feat.cutAndResampledFeatures_); + ar& BOOST_SERIALIZATION_NVP(feat.cutResampledAndTransformedFeatures_); ar& BOOST_SERIALIZATION_NVP(feat.type_); ar& BOOST_SERIALIZATION_NVP(feat.inverted_); ar& BOOST_SERIALIZATION_NVP(feat.laneletIDs_); diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h index 817b36aa..517ba175 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h @@ -68,9 +68,9 @@ std::vector cutLineString(const OrientedRect& bbox, const Bas BasicLineString3d transformLineString(const OrientedRect& bbox, const BasicLineString3d& polyline); -void saveLaneData(const std::string& filename, const std::vector& lDataVec, bool binary = false); +void saveLaneData(const std::string& filename, const std::vector& lDataVec, bool binary); -std::vector loadLaneData(const std::string& filename, bool binary = false); +std::vector loadLaneData(const std::string& filename, bool binary); } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index e9b30f7c..a0521f90 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -7,13 +7,14 @@ namespace map_learning { using namespace internal; -LaneData LaneData::build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { - LaneData data; - data.initLeftBoundaries(localSubmap, localSubmapGraph); - data.initRightBoundaries(localSubmap, localSubmapGraph); - data.initLaneletFeatures(localSubmap, localSubmapGraph); - data.initCompoundFeatures(localSubmap, localSubmapGraph); - data.updateAssociatedCpdFeatureIndices(); +LaneDataPtr LaneData::build(LaneletSubmapConstPtr& localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { + LaneDataPtr data = std::make_shared(); + data->initLeftBoundaries(localSubmap, localSubmapGraph); + data->initRightBoundaries(localSubmap, localSubmapGraph); + data->initLaneletFeatures(localSubmap, localSubmapGraph); + data->initCompoundFeatures(localSubmap, localSubmapGraph); + data->updateAssociatedCpdFeatureIndices(); return data; } @@ -22,7 +23,7 @@ void LaneData::initLeftBoundaries(LaneletSubmapConstPtr& localSubmap, for (const auto& ll : localSubmap->laneletLayer) { Id boundID = ll.leftBound3d().id(); BasicLineString3d bound = - !ll.leftBound3d().inverted() ? ll.leftBound3d().basicLineString() : ll.leftBound3d().invert().basicLineString(); + ll.leftBound3d().inverted() ? ll.leftBound3d().invert().basicLineString() : ll.leftBound3d().basicLineString(); if (isRoadBorder(ll.leftBound3d())) { LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(boundID); if (itRoadBd != roadBorders_.end()) { @@ -54,8 +55,8 @@ void LaneData::initRightBoundaries(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { for (const auto& ll : localSubmap->laneletLayer) { Id boundID = ll.rightBound3d().id(); - BasicLineString3d bound = !ll.rightBound3d().inverted() ? ll.rightBound3d().basicLineString() - : ll.rightBound3d().invert().basicLineString(); + BasicLineString3d bound = ll.rightBound3d().inverted() ? ll.rightBound3d().invert().basicLineString() + : ll.rightBound3d().basicLineString(); if (isRoadBorder(ll.rightBound3d())) { LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(boundID); if (itRoadBd != roadBorders_.end()) { @@ -86,8 +87,9 @@ void LaneData::initLaneletFeatures(LaneletSubmapConstPtr& localSubmap, for (const auto& ll : localSubmap->laneletLayer) { LaneLineStringFeaturePtr leftBoundary = getLineStringFeatFromId(ll.leftBound().id(), ll.leftBound().inverted()); LaneLineStringFeaturePtr rightBoundary = getLineStringFeatFromId(ll.rightBound().id(), ll.leftBound().inverted()); - LaneLineStringFeaturePtr centerline = std::make_shared( - ll.centerline3d().basicLineString(), ll.centerline3d().id(), LineStringType::Centerline, Ids{ll.id()}, false); + LaneLineStringFeaturePtr centerline = + std::make_shared(ll.centerline3d().basicLineString(), ll.centerline3d().id(), + LineStringType::Centerline, Ids{ll.id()}, ll.centerline3d().inverted()); laneletFeatures_.insert( {ll.id(), std::make_shared(leftBoundary, rightBoundary, centerline, ll.id())}); } @@ -187,8 +189,9 @@ std::vector LaneData::computeCompoundRightBorders(const ConstLa CompoundLaneLineStringFeaturePtr LaneData::computeCompoundCenterline(const ConstLanelets& path) { LaneLineStringFeatureList compoundCenterlines; for (const auto& ll : path) { - compoundCenterlines.push_back(std::make_shared( - ll.centerline3d().basicLineString(), ll.id(), LineStringType::Centerline, Ids{ll.id()}, false)); + compoundCenterlines.push_back(std::make_shared(ll.centerline3d().basicLineString(), ll.id(), + LineStringType::Centerline, Ids{ll.id()}, + ll.centerline3d().inverted())); } return std::make_shared(compoundCenterlines, LineStringType::Centerline); } @@ -311,6 +314,7 @@ bool LaneData::processAll(const OrientedRect& bbox, const ParametrizationType& p } } +/// TODO: INCLUDE REASSOCIATION FROM VECTOR ELEMENT TO FEATURE LaneData::TensorFeatureData LaneData::getTensorFeatureData(bool pointsIn2d, bool ignoreBuffer) { if (!tfData_.has_value() || ignoreBuffer) { tfData_ = LaneData::TensorFeatureData(); diff --git a/lanelet2_map_learning/src/MapDataInterface.cpp b/lanelet2_map_learning/src/MapDataInterface.cpp index 35003cb2..a1197a32 100644 --- a/lanelet2_map_learning/src/MapDataInterface.cpp +++ b/lanelet2_map_learning/src/MapDataInterface.cpp @@ -32,35 +32,33 @@ MapDataInterface::MapDataInterface(LaneletMapConstPtr laneletMap, Configuration config_{config}, trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} -LaneData MapDataInterface::getLaneData(LaneletSubmapConstPtr localSubmap, - lanelet::routing::RoutingGraphConstPtr localSubmapGraph, const BasicPoint2d& pos, - double yaw, bool processAll) { - LaneData laneData = LaneData::build(localSubmap, localSubmapGraph); +LaneDataPtr MapDataInterface::getLaneData(LaneletSubmapConstPtr localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph, + const BasicPoint2d& pos, double yaw, bool processAll) { + LaneDataPtr laneData = LaneData::build(localSubmap, localSubmapGraph); OrientedRect bbox = getRotatedRect(pos, config_.submapExtentLongitudinal, config_.submapExtentLateral, yaw); if (processAll) { - laneData.processAll(bbox, config_.paramType, config_.nPoints); + laneData->processAll(bbox, config_.paramType, config_.nPoints); } return laneData; } -std::vector MapDataInterface::laneDataBatch(const BasicPoints2d& pts, const std::vector& yaws, - bool processAll) { +std::vector MapDataInterface::laneDataBatch(std::vector pts, std::vector yaws) { if (pts.size() != yaws.size()) { throw std::runtime_error("Unequal sizes of pts and yaws!"); } - std::vector lDataVec; + std::vector lDataVec; for (size_t i = 0; i < pts.size(); i++) { LaneletSubmapConstPtr localSubmap = extractSubmap(laneletMap_, pts[i], config_.submapExtentLongitudinal, config_.submapExtentLateral); routing::RoutingGraphConstPtr localSubmapGraph = lanelet::routing::RoutingGraph::build(*localSubmap, *trafficRules_); - lDataVec.push_back(getLaneData(localSubmap, localSubmapGraph, pts[i], yaws[i], processAll)); + lDataVec.push_back(getLaneData(localSubmap, localSubmapGraph, pts[i], yaws[i], true)); } return lDataVec; } -std::vector MapDataInterface::laneTEDataBatch(const BasicPoints2d& pts, const std::vector& yaws, - bool processAll) { +std::vector MapDataInterface::laneTEDataBatch(std::vector pts, std::vector yaws) { throw std::runtime_error("Not implemented yet!"); } @@ -75,7 +73,7 @@ TEData MapDataInterface::getLaneTEData(LaneletSubmapConstPtr localSubmap, throw std::runtime_error("Not implemented yet!"); } -LaneData MapDataInterface::laneData() { +LaneDataPtr MapDataInterface::laneData(bool processAll) { if (!currPos_) { throw InvalidObjectStateError( "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); @@ -84,10 +82,10 @@ LaneData MapDataInterface::laneData() { throw InvalidObjectStateError( "Your current yaw angle is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); } - return getLaneData(localSubmap_, localSubmapGraph_, *currPos_, *currYaw_); + return getLaneData(localSubmap_, localSubmapGraph_, *currPos_, *currYaw_, processAll); } -TEData MapDataInterface::teData() { +TEData MapDataInterface::teData(bool processAll) { if (!currPos_) { throw InvalidObjectStateError( "Your current position is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index 4e93f53e..a351d9b3 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -9,9 +9,9 @@ namespace lanelet { namespace map_learning { struct LStringProcessResult { - BasicLineString3d cutFeature_; - BasicLineString3d cutAndResampledFeature_; - BasicLineString3d cutResampledAndTransformedFeature_; + BasicLineStrings3d cutFeatures; + BasicLineStrings3d cutAndResampledFeatures; + BasicLineStrings3d cutResampledAndTransformedFeatures; bool wasCut_{false}; bool valid_{true}; }; @@ -23,22 +23,25 @@ LStringProcessResult processLineStringImpl(const BasicLineString3d& lstring, con throw std::runtime_error("Only polyline parametrization is implemented so far!"); } std::vector cutLines = cutLineString(bbox, lstring); - result.cutFeature_ = std::accumulate(cutLines.begin(), cutLines.end(), BasicLineString3d(), - [](BasicLineString3d a, BasicLineString3d b) { - a.insert(a.end(), b.begin(), b.end()); - return a; - }); - if (result.cutFeature_.empty()) { + if (cutLines.empty()) { result.wasCut_ = true; result.valid_ = false; return result; } - result.cutAndResampledFeature_ = resampleLineString(result.cutFeature_, nPoints); - result.cutResampledAndTransformedFeature_ = transformLineString(bbox, result.cutAndResampledFeature_); + + for (const auto& line : cutLines) { + result.cutFeatures.push_back(line); + BasicLineString3d lineResampled = resampleLineString(line, nPoints); + result.cutAndResampledFeatures.push_back(lineResampled); + result.cutResampledAndTransformedFeatures.push_back(transformLineString(bbox, lineResampled)); + } double lengthOriginal = boost::geometry::length(lstring, boost::geometry::strategy::distance::pythagoras()); - double lengthProcessed = boost::geometry::length(result.cutAndResampledFeature_, - boost::geometry::strategy::distance::pythagoras()); + double lengthProcessed = 0; + for (const auto& line : result.cutResampledAndTransformedFeatures) { + lengthProcessed = + lengthProcessed + boost::geometry::length(line, boost::geometry::strategy::distance::pythagoras()); + } if (lengthOriginal - lengthProcessed > 1e-2) { result.wasCut_ = true; @@ -49,9 +52,9 @@ LStringProcessResult processLineStringImpl(const BasicLineString3d& lstring, con bool LaneLineStringFeature::process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { LStringProcessResult result = processLineStringImpl(rawFeature_, bbox, paramType, nPoints); if (result.valid_) { - cutFeature_ = result.cutFeature_; - cutAndResampledFeature_ = result.cutAndResampledFeature_; - cutResampledAndTransformedFeature_ = result.cutResampledAndTransformedFeature_; + cutFeatures_ = result.cutFeatures; + cutAndResampledFeatures_ = result.cutAndResampledFeatures; + cutResampledAndTransformedFeatures_ = result.cutResampledAndTransformedFeatures; } else { valid_ = result.valid_; } @@ -59,29 +62,15 @@ bool LaneLineStringFeature::process(const OrientedRect& bbox, const Parametrizat return result.valid_; } -VectorXd LaneLineStringFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { - const BasicLineString3d& selectedFeature = cutResampledAndTransformedFeature_; - VectorXd vec = pointsIn2d ? VectorXd(2 * selectedFeature.size() + 1) - : VectorXd(3 * selectedFeature.size() + 1); // n points with 2/3 dims + type - if (pointsIn2d == true) { - for (size_t i = 0; i < selectedFeature.size(); i++) { - vec(Eigen::seq(2 * i, 2 * i + 1)) = selectedFeature[i](Eigen::seq(0, 1)); - } - } else { - for (size_t i = 0; i < selectedFeature.size(); i++) { - vec(Eigen::seq(3 * i, 3 * i + 2)) = selectedFeature[i](Eigen::seq(0, 2)); - } - } - - vec[vec.size() - 1] = typeInt(); - if (onlyPoints) { - return vec(Eigen::seq(0, vec.size() - 2)); - } else { - return vec; +std::vector LaneLineStringFeature::computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { + std::vector featVecs; + for (const auto& split : cutResampledAndTransformedFeatures_) { + featVecs.push_back(toFeatureVector(split, typeInt(), onlyPoints, pointsIn2d)); } + return featVecs; } -VectorXd TEFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { +std::vector TEFeature::computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { VectorXd vec = pointsIn2d ? VectorXd(2 * rawFeature_.size() + 1) : VectorXd(3 * rawFeature_.size() + 1); // n points with 2/3 dims + type if (pointsIn2d == true) { @@ -95,17 +84,23 @@ VectorXd TEFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const } vec[vec.size() - 1] = static_cast(teType_); if (onlyPoints) { - return vec(Eigen::seq(0, vec.size() - 2)); + return std::vector{vec(Eigen::seq(0, vec.size() - 2))}; } else { - return vec; + return std::vector{vec}; } } -MatrixXd LaneLineStringFeature::pointMatrix(bool pointsIn2d) const { - return toPointMatrix(cutResampledAndTransformedFeature_, pointsIn2d); +std::vector LaneLineStringFeature::pointMatrices(bool pointsIn2d) const { + std::vector pointMatrices; + for (const auto& split : cutResampledAndTransformedFeatures_) { + pointMatrices.push_back(toPointMatrix(split, pointsIn2d)); + } + return pointMatrices; } -MatrixXd TEFeature::pointMatrix(bool pointsIn2d) const { return toPointMatrix(rawFeature_, pointsIn2d); } +std::vector TEFeature::pointMatrices(bool pointsIn2d) const { + return std::vector{toPointMatrix(rawFeature_, pointsIn2d)}; +} LaneletFeature::LaneletFeature(LaneLineStringFeaturePtr leftBoundary, LaneLineStringFeaturePtr rightBoundary, LaneLineStringFeaturePtr centerline, Id mapID) @@ -135,35 +130,54 @@ bool LaneletFeature::process(const OrientedRect& bbox, const ParametrizationType return valid_; } -VectorXd LaneletFeature::computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { +VectorXd stackVector(const std::vector& vec) { + size_t flattenedLength = 0; + for (const auto& el : vec) { + flattenedLength = flattenedLength + el.size(); + } + VectorXd stacked(flattenedLength); + size_t currIndex = 0; + for (const auto& el : vec) { + stacked(Eigen::seq(currIndex, currIndex + el.size())) = el; + } + return stacked; +} + +std::vector LaneletFeature::computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { if (reprType_ == LaneletRepresentationType::Centerline) { - VectorXd vecCenterlinePts = centerline_->computeFeatureVector(true, pointsIn2d); - VectorXd vec(vecCenterlinePts.size() + 2); // pts vec + left and right type - vec(Eigen::seq(0, vecCenterlinePts.size() - 1)) = vecCenterlinePts; - vec[vec.size() - 2] = leftBoundary_->typeInt(); - vec[vec.size() - 1] = rightBoundary_->typeInt(); + std::vector vecCenterlinePts = centerline_->computeFeatureVectors(true, pointsIn2d); if (onlyPoints) { - return vec(Eigen::seq(0, vec.size() - 2)); - } else { - return vec; + return vecCenterlinePts; + } + std::vector featureVecs(vecCenterlinePts.size()); + for (size_t i = 0; i < vecCenterlinePts.size(); i++) { + VectorXd vec(vecCenterlinePts[i].size() + 2); // pts vec + left and right type + vec(Eigen::seq(0, vecCenterlinePts.size() - 1)) = vecCenterlinePts[i]; + vec[vec.size() - 2] = leftBoundary_->typeInt(); + vec[vec.size() - 1] = rightBoundary_->typeInt(); + featureVecs[i] = vec; } + return featureVecs; } else if (reprType_ == LaneletRepresentationType::Boundaries) { - VectorXd vecLeftBdPts = leftBoundary_->computeFeatureVector(true, pointsIn2d); - VectorXd vecRightBdPts = rightBoundary_->computeFeatureVector(true, pointsIn2d); + std::vector vecLeftBdPts = leftBoundary_->computeFeatureVectors(true, pointsIn2d); + VectorXd leftBdPts = stackVector(vecLeftBdPts); + std::vector vecRightBdPts = rightBoundary_->computeFeatureVectors(true, pointsIn2d); + VectorXd rightBdPts = stackVector(vecRightBdPts); - VectorXd vec(vecLeftBdPts.size() + vecRightBdPts.size() + 2); // pts vec + left and right type - vec(Eigen::seq(0, vecLeftBdPts.size() - 1)) = vecLeftBdPts; - vec(Eigen::seq(vecLeftBdPts.size(), vecLeftBdPts.size() + vecRightBdPts.size() - 1)) = vecRightBdPts; + VectorXd vec(leftBdPts.size() + rightBdPts.size() + 2); // pts vec + left and right type + vec(Eigen::seq(0, leftBdPts.size() - 1)) = leftBdPts; + vec(Eigen::seq(leftBdPts.size(), leftBdPts.size() + rightBdPts.size() - 1)) = rightBdPts; vec[vec.size() - 2] = leftBoundary_->typeInt(); vec[vec.size() - 1] = rightBoundary_->typeInt(); + if (onlyPoints) { - return vec(Eigen::seq(0, vec.size() - 2)); + return std::vector{vec(Eigen::seq(0, vec.size() - 2))}; } else { - return vec; + return std::vector{vec}; } } else { throw std::runtime_error("Unknown LaneletRepresentationType!"); - return VectorXd(); + return std::vector(); } } @@ -202,8 +216,12 @@ bool CompoundLaneLineStringFeature::process(const OrientedRect& bbox, const Para LaneLineStringFeature::process(bbox, paramType, nPoints); for (size_t i = 0; i < individualFeatures_.size(); i++) { individualFeatures_[i]->process(bbox, paramType, nPoints); - double processedLength = boost::geometry::length(individualFeatures_[i]->cutFeature(), - boost::geometry::strategy::distance::pythagoras()); + + double processedLength = 0; + for (const auto& line : individualFeatures_[i]->cutFeature()) { + processedLength = + processedLength + boost::geometry::length(line, boost::geometry::strategy::distance::pythagoras()); + } if (processedLength > validLengthThresh_) { processedFeaturesValid_[i] = true; @@ -234,5 +252,25 @@ MatrixXd toPointMatrix(const BasicLineString3d& lString, bool pointsIn2d) { return mat; } +VectorXd toFeatureVector(const BasicLineString3d& line, int typeInt, bool onlyPoints, bool pointsIn2d) { + VectorXd vec = + pointsIn2d ? VectorXd(2 * line.size() + 1) : VectorXd(3 * line.size() + 1); // n points with 2/3 dims + type + if (pointsIn2d == true) { + for (size_t i = 0; i < line.size(); i++) { + vec(Eigen::seq(2 * i, 2 * i + 1)) = line[i](Eigen::seq(0, 1)); + } + } else { + for (size_t i = 0; i < line.size(); i++) { + vec(Eigen::seq(3 * i, 3 * i + 2)) = line[i](Eigen::seq(0, 2)); + } + } + vec[vec.size() - 1] = typeInt; + if (onlyPoints) { + return vec(Eigen::seq(0, vec.size() - 2)); + } else { + return vec; + } +} + } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index 110e536c..9f782a11 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -89,7 +89,7 @@ std::vector cutLineString(const OrientedRect& bbox, const Bas } std::cerr << "--------------" << std::endl; } - throw std::runtime_error("More than one cut line!"); + // throw std::runtime_error("More than one cut line!"); } // restore z value from closest point on the original linestring @@ -122,7 +122,7 @@ BasicLineString3d transformLineString(const OrientedRect& bbox, const BasicLineS return rotatedPolyline; } -void saveLaneData(const std::string& filename, const std::vector& lDataVec, bool binary) { +void saveLaneData(const std::string& filename, const std::vector& lDataVec, bool binary) { if (binary) { std::ofstream fs(filename, std::ofstream::binary); if (!fs.good()) { @@ -140,11 +140,11 @@ void saveLaneData(const std::string& filename, const std::vector& lDat } } -std::vector loadLaneData(const std::string& filename, bool binary) { +std::vector loadLaneData(const std::string& filename, bool binary) { if (!fs::exists(fs::path(filename))) { throw std::runtime_error("Could not find file under " + filename); } - std::vector lDataVec; + std::vector lDataVec; if (binary) { std::ifstream fs(filename, std::ifstream::binary); if (!fs.good()) { diff --git a/lanelet2_map_learning/test/test_map_data.cpp b/lanelet2_map_learning/test/test_map_data.cpp index f6e409e7..4d8f1636 100644 --- a/lanelet2_map_learning/test/test_map_data.cpp +++ b/lanelet2_map_learning/test/test_map_data.cpp @@ -19,41 +19,41 @@ TEST_F(MapLearningTest, LaneData) { // NOLINT lls.insert(lls.end(), laneletMap->laneletLayer.begin(), laneletMap->laneletLayer.end()); LaneletSubmapConstPtr laneletSubmap = utils::createConstSubmap(lls, {}); - LaneData laneData = LaneData::build(laneletSubmap, laneletMapGraph); + LaneDataPtr laneData = LaneData::build(laneletSubmap, laneletMapGraph); - std::vector rawCompoundRoadBorders = getPointMatrices(laneData.compoundRoadBorders(), true); - std::vector rawCompoundLaneDividers = getPointMatrices(laneData.compoundLaneDividers(), true); - std::vector rawCompoundCenterlines = getPointMatrices(laneData.compoundCenterlines(), true); + std::vector rawCompoundRoadBorders = getPointMatrices(laneData->compoundRoadBorders(), true); + std::vector rawCompoundLaneDividers = getPointMatrices(laneData->compoundLaneDividers(), true); + std::vector rawCompoundCenterlines = getPointMatrices(laneData->compoundCenterlines(), true); - bool valid = laneData.processAll(bbox, ParametrizationType::LineString, 20); - std::vector compoundRoadBorders = getPointMatrices(laneData.compoundRoadBorders(), true); - std::vector compoundLaneDividers = getPointMatrices(laneData.compoundLaneDividers(), true); - std::vector compoundCenterlines = getPointMatrices(laneData.compoundCenterlines(), true); + bool valid = laneData->processAll(bbox, ParametrizationType::LineString, 20); + std::vector compoundRoadBorders = getPointMatrices(laneData->compoundRoadBorders(), true); + std::vector compoundLaneDividers = getPointMatrices(laneData->compoundLaneDividers(), true); + std::vector compoundCenterlines = getPointMatrices(laneData->compoundCenterlines(), true); - EXPECT_TRUE(laneData.laneletFeatures().find(2007) != laneData.laneletFeatures().end()); - EXPECT_EQ(laneData.laneletFeatures().find(2007)->second->leftBoundary()->mapID(), 1012); - EXPECT_TRUE(laneData.roadBorders().find(1001) != laneData.roadBorders().end()); + EXPECT_TRUE(laneData->laneletFeatures().find(2007) != laneData->laneletFeatures().end()); + EXPECT_EQ(laneData->laneletFeatures().find(2007)->second->leftBoundary()->mapID(), 1012); + EXPECT_TRUE(laneData->roadBorders().find(1001) != laneData->roadBorders().end()); EXPECT_EQ(compoundRoadBorders.size(), 3); EXPECT_EQ(compoundLaneDividers.size(), 8); EXPECT_EQ(compoundCenterlines.size(), 4); - EXPECT_EQ(laneData.associatedCpdRoadBorderIndices().at(2001).size(), 1); - size_t assoBorderIdx = laneData.associatedCpdRoadBorderIndices().at(2001).front(); - EXPECT_EQ(laneData.compoundRoadBorders()[assoBorderIdx]->features().back()->mapID(), 1000); - EXPECT_EQ(laneData.compoundRoadBorders()[assoBorderIdx]->features().back()->laneletIDs().front(), 2002); + EXPECT_EQ(laneData->associatedCpdRoadBorderIndices().at(2001).size(), 1); + size_t assoBorderIdx = laneData->associatedCpdRoadBorderIndices().at(2001).front(); + EXPECT_EQ(laneData->compoundRoadBorders()[assoBorderIdx]->features().back()->mapID(), 1000); + EXPECT_EQ(laneData->compoundRoadBorders()[assoBorderIdx]->features().back()->laneletIDs().front(), 2002); - EXPECT_EQ(laneData.associatedCpdLaneDividerIndices().at(2004).size(), 2); - EXPECT_EQ(laneData.associatedCpdLaneDividerIndices().at(2009).size(), 1); - size_t assoDividerIdx = laneData.associatedCpdLaneDividerIndices().at(2009).front(); - EXPECT_EQ(laneData.compoundLaneDividers()[assoDividerIdx]->features().front()->mapID(), 1015); - EXPECT_EQ(laneData.compoundLaneDividers()[assoDividerIdx]->features().front()->laneletIDs().size(), 2); + EXPECT_EQ(laneData->associatedCpdLaneDividerIndices().at(2004).size(), 2); + EXPECT_EQ(laneData->associatedCpdLaneDividerIndices().at(2009).size(), 1); + size_t assoDividerIdx = laneData->associatedCpdLaneDividerIndices().at(2009).front(); + EXPECT_EQ(laneData->compoundLaneDividers()[assoDividerIdx]->features().front()->mapID(), 1015); + EXPECT_EQ(laneData->compoundLaneDividers()[assoDividerIdx]->features().front()->laneletIDs().size(), 2); - EXPECT_EQ(laneData.associatedCpdCenterlineIndices().at(2003).size(), 2); - EXPECT_EQ(laneData.associatedCpdCenterlineIndices().at(2000).size(), 1); - size_t assoCenterlineIdx = laneData.associatedCpdCenterlineIndices().at(2000).front(); - EXPECT_EQ(laneData.compoundCenterlines()[assoCenterlineIdx]->features().front()->mapID(), 2000); - EXPECT_EQ(laneData.compoundCenterlines()[assoCenterlineIdx]->features().front()->laneletIDs().front(), 2000); + EXPECT_EQ(laneData->associatedCpdCenterlineIndices().at(2003).size(), 2); + EXPECT_EQ(laneData->associatedCpdCenterlineIndices().at(2000).size(), 1); + size_t assoCenterlineIdx = laneData->associatedCpdCenterlineIndices().at(2000).front(); + EXPECT_EQ(laneData->compoundCenterlines()[assoCenterlineIdx]->features().front()->mapID(), 2000); + EXPECT_EQ(laneData->compoundCenterlines()[assoCenterlineIdx]->features().front()->laneletIDs().front(), 2000); // matplot::hold(matplot::on); // matplot::xlim({-2, 16}); diff --git a/lanelet2_map_learning/test/test_map_data_interface.cpp b/lanelet2_map_learning/test/test_map_data_interface.cpp index 6b6973c9..e22476a5 100644 --- a/lanelet2_map_learning/test/test_map_data_interface.cpp +++ b/lanelet2_map_learning/test/test_map_data_interface.cpp @@ -31,38 +31,38 @@ TEST_F(MapLearningTest, MapDataInterface) { // NOLINT config.nPoints = 20; MapDataInterface parser(laneletMap, config); - BasicPoints2d pts{BasicPoint2d(0, -3), BasicPoint2d(3, -3), BasicPoint2d(5, -3), - BasicPoint2d(6, -5), BasicPoint2d(6, -8), BasicPoint2d(6, -11)}; + std::vector pts{BasicPoint2d(0, -3), BasicPoint2d(3, -3), BasicPoint2d(5, -3), + BasicPoint2d(6, -5), BasicPoint2d(6, -8), BasicPoint2d(6, -11)}; std::vector yaws{0, 0, M_PI / 4, M_PI / 3, M_PI / 2, M_PI / 2}; // auto start = std::chrono::steady_clock::now(); - std::vector lDataVec = parser.laneDataBatch(pts, yaws); + std::vector lDataVec = parser.laneDataBatch(pts, yaws); // std::cerr << "Elapsed(micros)=" << since(start).count() << std::endl; for (size_t i = 0; i < lDataVec.size(); i++) { std::vector compoundRoadBorders = - getPointMatrices(getValidElements(lDataVec[i].compoundRoadBorders()), true); + getPointMatrices(getValidElements(lDataVec[i]->compoundRoadBorders()), true); std::vector compoundLaneDividers = - getPointMatrices(getValidElements(lDataVec[i].compoundLaneDividers()), true); + getPointMatrices(getValidElements(lDataVec[i]->compoundLaneDividers()), true); std::vector compoundCenterlines = - getPointMatrices(getValidElements(lDataVec[i].compoundCenterlines()), true); + getPointMatrices(getValidElements(lDataVec[i]->compoundCenterlines()), true); switch (i) { case 0: { - LaneletFeatures::const_iterator itLL0 = lDataVec[i].laneletFeatures().find(2000); - EXPECT_TRUE(itLL0 != lDataVec[i].laneletFeatures().end()); - LaneletFeatures::const_iterator itLL1 = lDataVec[i].laneletFeatures().find(2002); - EXPECT_TRUE(itLL1 == lDataVec[i].laneletFeatures().end()); + LaneletFeatures::const_iterator itLL0 = lDataVec[i]->laneletFeatures().find(2000); + EXPECT_TRUE(itLL0 != lDataVec[i]->laneletFeatures().end()); + LaneletFeatures::const_iterator itLL1 = lDataVec[i]->laneletFeatures().find(2002); + EXPECT_TRUE(itLL1 == lDataVec[i]->laneletFeatures().end()); break; } case 1: { - LaneletFeatures::const_iterator itLL = lDataVec[i].laneletFeatures().find(2011); - EXPECT_TRUE(itLL != lDataVec[i].laneletFeatures().end()); + LaneletFeatures::const_iterator itLL = lDataVec[i]->laneletFeatures().find(2011); + EXPECT_TRUE(itLL != lDataVec[i]->laneletFeatures().end()); break; } case 5: { - LaneletFeatures::const_iterator itLL = lDataVec[i].laneletFeatures().find(2004); - EXPECT_TRUE(itLL == lDataVec[i].laneletFeatures().end()); + LaneletFeatures::const_iterator itLL = lDataVec[i]->laneletFeatures().find(2004); + EXPECT_TRUE(itLL == lDataVec[i]->laneletFeatures().end()); break; } } @@ -114,23 +114,23 @@ TEST_F(MapLearningTest, MapDataSaveLoad) { config.nPoints = 20; MapDataInterface parser(laneletMap, config); - BasicPoints2d pts{BasicPoint2d(0, -3), BasicPoint2d(3, -3), BasicPoint2d(5, -3), - BasicPoint2d(6, -5), BasicPoint2d(6, -8), BasicPoint2d(6, -11)}; + std::vector pts{BasicPoint2d(0, -3), BasicPoint2d(3, -3), BasicPoint2d(5, -3), + BasicPoint2d(6, -5), BasicPoint2d(6, -8), BasicPoint2d(6, -11)}; std::vector yaws{0, 0, M_PI / 4, M_PI / 3, M_PI / 2, M_PI / 2}; - std::vector lDataVec = parser.laneDataBatch(pts, yaws); + std::vector lDataVec = parser.laneDataBatch(pts, yaws); - saveLaneData("/tmp/lane_data_save_test.xml", lDataVec); - std::vector lDataLoaded = loadLaneData("/tmp/lane_data_save_test.xml"); + saveLaneData("/tmp/lane_data_save_test.xml", lDataVec, false); + std::vector lDataLoaded = loadLaneData("/tmp/lane_data_save_test.xml", false); EXPECT_EQ(lDataVec.size(), lDataLoaded.size()); - EXPECT_EQ(lDataVec.front().compoundCenterlines().size(), lDataLoaded.front().compoundCenterlines().size()); - EXPECT_EQ(lDataVec.front().compoundRoadBorders().size(), lDataLoaded.front().compoundRoadBorders().size()); - EXPECT_EQ(lDataVec.back().compoundLaneDividers().size(), lDataLoaded.back().compoundLaneDividers().size()); + EXPECT_EQ(lDataVec.front()->compoundCenterlines().size(), lDataLoaded.front()->compoundCenterlines().size()); + EXPECT_EQ(lDataVec.front()->compoundRoadBorders().size(), lDataLoaded.front()->compoundRoadBorders().size()); + EXPECT_EQ(lDataVec.back()->compoundLaneDividers().size(), lDataLoaded.back()->compoundLaneDividers().size()); saveLaneData("/tmp/lane_data_save_test.bin", lDataVec, true); lDataLoaded = loadLaneData("/tmp/lane_data_save_test.bin", true); EXPECT_EQ(lDataVec.size(), lDataLoaded.size()); - EXPECT_EQ(lDataVec.front().compoundCenterlines().size(), lDataLoaded.front().compoundCenterlines().size()); - EXPECT_EQ(lDataVec.front().compoundRoadBorders().size(), lDataLoaded.front().compoundRoadBorders().size()); - EXPECT_EQ(lDataVec.back().compoundLaneDividers().size(), lDataLoaded.back().compoundLaneDividers().size()); + EXPECT_EQ(lDataVec.front()->compoundCenterlines().size(), lDataLoaded.front()->compoundCenterlines().size()); + EXPECT_EQ(lDataVec.front()->compoundRoadBorders().size(), lDataLoaded.front()->compoundRoadBorders().size()); + EXPECT_EQ(lDataVec.back()->compoundLaneDividers().size(), lDataLoaded.back()->compoundLaneDividers().size()); } \ No newline at end of file diff --git a/lanelet2_map_learning/test/test_map_features.cpp b/lanelet2_map_learning/test/test_map_features.cpp index f58f5acf..a894ed75 100644 --- a/lanelet2_map_learning/test/test_map_features.cpp +++ b/lanelet2_map_learning/test/test_map_features.cpp @@ -13,25 +13,27 @@ TEST_F(MapLearningTest, LaneLineStringFeature) { // NOLINT LaneLineStringFeature feat(polyline, Id(123), LineStringType::Solid, Ids(1234), false); feat.process(bbox, ParametrizationType::LineString, 4); - EXPECT_EQ(feat.cutAndResampledFeature().size(), 4); - EXPECT_NEAR(feat.cutAndResampledFeature()[0].x(), 0, 10e-5); - EXPECT_NEAR(feat.cutAndResampledFeature()[1].x(), 5, 10e-5); - EXPECT_NEAR(feat.cutAndResampledFeature()[3].x(), 15, 10e-5); - - Eigen::VectorXd vec = feat.computeFeatureVector(false, true); - EXPECT_EQ(vec.size(), 9); - EXPECT_NEAR(vec[0], 5, 10e-5); - EXPECT_NEAR(vec[1], -5, 10e-5); - EXPECT_NEAR(vec[6], 5, 10e-5); - EXPECT_NEAR(vec[7], 10, 10e-5); - EXPECT_EQ(vec[8], 2); - - Eigen::MatrixXd pointMat = feat.pointMatrix(false); - EXPECT_EQ(pointMat.rows(), 4); - EXPECT_EQ(pointMat.cols(), 3); - EXPECT_NEAR(pointMat(0, 1), -5, 10e-5); - EXPECT_NEAR(pointMat(2, 1), 5, 10e-5); - EXPECT_NEAR(pointMat(3, 1), 10, 10e-5); + EXPECT_EQ(feat.cutAndResampledFeature().size(), 1); + EXPECT_EQ(feat.cutAndResampledFeature()[0].size(), 4); + EXPECT_NEAR(feat.cutAndResampledFeature()[0][0].x(), 0, 10e-5); + EXPECT_NEAR(feat.cutAndResampledFeature()[0][1].x(), 5, 10e-5); + EXPECT_NEAR(feat.cutAndResampledFeature()[0][3].x(), 15, 10e-5); + + std::vector vec = feat.computeFeatureVectors(false, true); + EXPECT_EQ(vec.size(), 1); + EXPECT_EQ(vec[0].size(), 9); + EXPECT_NEAR(vec[0][0], 5, 10e-5); + EXPECT_NEAR(vec[0][1], -5, 10e-5); + EXPECT_NEAR(vec[0][6], 5, 10e-5); + EXPECT_NEAR(vec[0][7], 10, 10e-5); + EXPECT_EQ(vec[0][8], 2); + + std::vector pointMat = feat.pointMatrices(false); + EXPECT_EQ(pointMat[0].rows(), 4); + EXPECT_EQ(pointMat[0].cols(), 3); + EXPECT_NEAR(pointMat[0](0, 1), -5, 10e-5); + EXPECT_NEAR(pointMat[0](2, 1), 5, 10e-5); + EXPECT_NEAR(pointMat[0](3, 1), 10, 10e-5); } TEST_F(MapLearningTest, LaneletFeature) { // NOLINT @@ -52,18 +54,18 @@ TEST_F(MapLearningTest, LaneletFeature) { // NOLINT llFeat.setReprType(LaneletRepresentationType::Boundaries); llFeat.process(bbox, ParametrizationType::LineString, 4); - EXPECT_EQ(llFeat.centerline()->cutAndResampledFeature().size(), 4); - EXPECT_NEAR(llFeat.centerline()->cutAndResampledFeature()[0].x(), 0, 10e-5); - EXPECT_NEAR(llFeat.centerline()->cutAndResampledFeature()[1].x(), 5, 10e-5); - EXPECT_NEAR(llFeat.centerline()->cutAndResampledFeature()[3].x(), 15, 10e-5); - - Eigen::VectorXd vec = llFeat.computeFeatureVector(false, true); - - EXPECT_EQ(vec.size(), 18); - EXPECT_NEAR(vec[9], -5, 10e-5); - EXPECT_NEAR(vec[10], 1, 10e-5); - EXPECT_NEAR(vec[15], 10, 10e-5); - EXPECT_EQ(vec[17], 1); + EXPECT_EQ(llFeat.centerline()->cutAndResampledFeature()[0].size(), 4); + EXPECT_NEAR(llFeat.centerline()->cutAndResampledFeature()[0][0].x(), 0, 10e-5); + EXPECT_NEAR(llFeat.centerline()->cutAndResampledFeature()[0][1].x(), 5, 10e-5); + EXPECT_NEAR(llFeat.centerline()->cutAndResampledFeature()[0][3].x(), 15, 10e-5); + + std::vector vec = llFeat.computeFeatureVectors(false, true); + EXPECT_EQ(vec.size(), 1); + EXPECT_EQ(vec[0].size(), 18); + EXPECT_NEAR(vec[0][9], -5, 10e-5); + EXPECT_NEAR(vec[0][10], 1, 10e-5); + EXPECT_NEAR(vec[0][15], 10, 10e-5); + EXPECT_EQ(vec[0][17], 1); } TEST_F(MapLearningTest, CompoundLaneLineStringFeature) { // NOLINT @@ -87,31 +89,34 @@ TEST_F(MapLearningTest, CompoundLaneLineStringFeature) { // NOLINT LineStringType::Solid); cpdFeat.process(bbox, ParametrizationType::LineString, 5); - EXPECT_EQ(cpdFeat.cutAndResampledFeature().size(), 5); - EXPECT_NEAR(cpdFeat.cutAndResampledFeature()[0].x(), -5, 10e-5); - EXPECT_NEAR(cpdFeat.cutAndResampledFeature()[1].x(), 0, 10e-5); - EXPECT_NEAR(cpdFeat.cutAndResampledFeature()[3].x(), 10, 10e-5); + EXPECT_EQ(cpdFeat.cutAndResampledFeature().size(), 1); + EXPECT_EQ(cpdFeat.cutAndResampledFeature()[0].size(), 5); + EXPECT_NEAR(cpdFeat.cutAndResampledFeature()[0][0].x(), -5, 10e-5); + EXPECT_NEAR(cpdFeat.cutAndResampledFeature()[0][1].x(), 0, 10e-5); + EXPECT_NEAR(cpdFeat.cutAndResampledFeature()[0][3].x(), 10, 10e-5); EXPECT_NEAR(cpdFeat.pathLengthsProcessed().front(), 0, 10e-5); EXPECT_EQ(cpdFeat.processedFeaturesValid().front(), false); double cpdFeatLength = - boost::geometry::length(cpdFeat.cutFeature(), boost::geometry::strategy::distance::pythagoras()); + boost::geometry::length(cpdFeat.cutFeature()[0], boost::geometry::strategy::distance::pythagoras()); EXPECT_NEAR(cpdFeatLength, cpdFeat.pathLengthsProcessed().back(), 10e-5); - Eigen::VectorXd vec = cpdFeat.computeFeatureVector(false, true); - EXPECT_EQ(vec.size(), 11); - EXPECT_NEAR(vec[1], -10, 10e-5); - EXPECT_NEAR(vec[2], 5, 10e-5); - EXPECT_NEAR(vec[3], -5, 10e-5); - EXPECT_NEAR(vec[6], 5, 10e-5); - EXPECT_NEAR(vec[7], 5, 10e-5); - EXPECT_EQ(vec[10], 2); - - Eigen::MatrixXd pointMat = cpdFeat.pointMatrix(false); - EXPECT_EQ(pointMat.rows(), 5); - EXPECT_EQ(pointMat.cols(), 3); - EXPECT_NEAR(pointMat(0, 1), -10, 10e-5); - EXPECT_NEAR(pointMat(2, 1), 0, 10e-5); - EXPECT_NEAR(pointMat(3, 1), 5, 10e-5); - EXPECT_NEAR(pointMat(4, 0), 5, 10e-5); + std::vector vec = cpdFeat.computeFeatureVectors(false, true); + EXPECT_EQ(vec.size(), 1); + EXPECT_EQ(vec[0].size(), 11); + EXPECT_NEAR(vec[0][1], -10, 10e-5); + EXPECT_NEAR(vec[0][2], 5, 10e-5); + EXPECT_NEAR(vec[0][3], -5, 10e-5); + EXPECT_NEAR(vec[0][6], 5, 10e-5); + EXPECT_NEAR(vec[0][7], 5, 10e-5); + EXPECT_EQ(vec[0][10], 2); + + std::vector pointMat = cpdFeat.pointMatrices(false); + EXPECT_EQ(pointMat.size(), 1); + EXPECT_EQ(pointMat[0].rows(), 5); + EXPECT_EQ(pointMat[0].cols(), 3); + EXPECT_NEAR(pointMat[0](0, 1), -10, 10e-5); + EXPECT_NEAR(pointMat[0](2, 1), 0, 10e-5); + EXPECT_NEAR(pointMat[0](3, 1), 5, 10e-5); + EXPECT_NEAR(pointMat[0](4, 0), 5, 10e-5); } \ No newline at end of file diff --git a/lanelet2_python/python_api/map_learning.cpp b/lanelet2_python/python_api/map_learning.cpp index 22e71448..fba2bf24 100644 --- a/lanelet2_python/python_api/map_learning.cpp +++ b/lanelet2_python/python_api/map_learning.cpp @@ -19,8 +19,8 @@ using namespace lanelet::map_learning; class MapFeatureWrap : public MapFeature, public wrapper { public: - VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { - return this->get_override("computeFeatureVector")(onlyPoints, pointsIn2d); + std::vector computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { + return this->get_override("computeFeatureVectors")(onlyPoints, pointsIn2d); } bool process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { return this->get_override("process")(bbox, paramType, nPoints); @@ -29,13 +29,13 @@ class MapFeatureWrap : public MapFeature, public wrapper { class LineStringFeatureWrap : public LineStringFeature, public wrapper { public: - VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { - return this->get_override("computeFeatureVector")(onlyPoints, pointsIn2d); + std::vector computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { + return this->get_override("computeFeatureVectors")(onlyPoints, pointsIn2d); } bool process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { return this->get_override("process")(bbox, paramType, nPoints); } - MatrixXd pointMatrix(bool pointsIn2d) const { return this->get_override("pointMatrix")(pointsIn2d); } + std::vector pointMatrices(bool pointsIn2d) const { return this->get_override("pointMatrices")(pointsIn2d); } }; class LaneLineStringFeatureWrap : public LaneLineStringFeature, public wrapper { @@ -46,12 +46,12 @@ class LaneLineStringFeatureWrap : public LaneLineStringFeature, public wrapper &laneletID, bool inverted) : LaneLineStringFeature(feature, mapID, type, laneletID, inverted) {} - VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { - if (override f = this->get_override("computeFeatureVector")) return f(onlyPoints, pointsIn2d); - return LaneLineStringFeature::computeFeatureVector(onlyPoints, pointsIn2d); + std::vector computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { + if (override f = this->get_override("computeFeatureVectors")) return f(onlyPoints, pointsIn2d); + return LaneLineStringFeature::computeFeatureVectors(onlyPoints, pointsIn2d); } - VectorXd default_computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { - return this->LaneLineStringFeature::computeFeatureVector(onlyPoints, pointsIn2d); + std::vector default_computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { + return this->LaneLineStringFeature::computeFeatureVectors(onlyPoints, pointsIn2d); } bool process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { if (override f = this->get_override("process")) return f(bbox, paramType, nPoints); @@ -60,11 +60,13 @@ class LaneLineStringFeatureWrap : public LaneLineStringFeature, public wrapperLaneLineStringFeature::process(bbox, paramType, nPoints); } - MatrixXd pointMatrix(bool pointsIn2d) const { - if (override f = this->get_override("pointMatrix")) return f(pointsIn2d); - return LaneLineStringFeature::pointMatrix(pointsIn2d); + std::vector pointMatrices(bool pointsIn2d) const { + if (override f = this->get_override("pointMatrices")) return f(pointsIn2d); + return LaneLineStringFeature::pointMatrices(pointsIn2d); + } + std::vector default_pointMatrices(bool pointsIn2d) const { + return this->LaneLineStringFeature::pointMatrices(pointsIn2d); } - MatrixXd default_pointMatrix(bool pointsIn2d) const { return this->LaneLineStringFeature::pointMatrix(pointsIn2d); } }; class CompoundLaneLineStringFeatureWrap : public CompoundLaneLineStringFeature, @@ -75,12 +77,12 @@ class CompoundLaneLineStringFeatureWrap : public CompoundLaneLineStringFeature, CompoundLaneLineStringFeatureWrap(const LaneLineStringFeatureList &features, LineStringType compoundType) : CompoundLaneLineStringFeature(features, compoundType) {} - VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { - if (override f = this->get_override("computeFeatureVector")) return f(onlyPoints, pointsIn2d); - return CompoundLaneLineStringFeature::computeFeatureVector(onlyPoints, pointsIn2d); + std::vector computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { + if (override f = this->get_override("computeFeatureVectors")) return f(onlyPoints, pointsIn2d); + return CompoundLaneLineStringFeature::computeFeatureVectors(onlyPoints, pointsIn2d); } - VectorXd default_computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { - return this->CompoundLaneLineStringFeature::computeFeatureVector(onlyPoints, pointsIn2d); + std::vector default_computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { + return this->CompoundLaneLineStringFeature::computeFeatureVectors(onlyPoints, pointsIn2d); } bool process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { if (override f = this->get_override("process")) return f(bbox, paramType, nPoints); @@ -89,12 +91,12 @@ class CompoundLaneLineStringFeatureWrap : public CompoundLaneLineStringFeature, bool default_process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { return this->CompoundLaneLineStringFeature::process(bbox, paramType, nPoints); } - MatrixXd pointMatrix(bool pointsIn2d) const { - if (override f = this->get_override("pointMatrix")) return f(pointsIn2d); - return CompoundLaneLineStringFeature::pointMatrix(pointsIn2d); + std::vector pointMatrices(bool pointsIn2d) const { + if (override f = this->get_override("pointMatrices")) return f(pointsIn2d); + return CompoundLaneLineStringFeature::pointMatrices(pointsIn2d); } - MatrixXd default_pointMatrix(bool pointsIn2d) const { - return this->CompoundLaneLineStringFeature::pointMatrix(pointsIn2d); + std::vector default_pointMatrices(bool pointsIn2d) const { + return this->CompoundLaneLineStringFeature::pointMatrices(pointsIn2d); } }; @@ -107,12 +109,12 @@ class LaneletFeatureWrap : public LaneletFeature, public wrapper : LaneletFeature(leftBoundary, rightBoundary, centerline, mapID) {} LaneletFeatureWrap(const ConstLanelet &ll) : LaneletFeature(ll) {} - VectorXd computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { - if (override f = this->get_override("computeFeatureVector")) return f(onlyPoints, pointsIn2d); - return LaneletFeature::computeFeatureVector(onlyPoints, pointsIn2d); + std::vector computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { + if (override f = this->get_override("computeFeatureVectors")) return f(onlyPoints, pointsIn2d); + return LaneletFeature::computeFeatureVectors(onlyPoints, pointsIn2d); } - VectorXd default_computeFeatureVector(bool onlyPoints, bool pointsIn2d) const { - return this->LaneletFeature::computeFeatureVector(onlyPoints, pointsIn2d); + std::vector default_computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { + return this->LaneletFeature::computeFeatureVectors(onlyPoints, pointsIn2d); } bool process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { if (override f = this->get_override("process")) return f(bbox, paramType, nPoints); @@ -190,16 +192,16 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .add_property("mapID", &MapFeature::mapID) .add_property("initialized", &MapFeature::initialized) .add_property("valid", &MapFeature::valid) - .def("computeFeatureVector", pure_virtual(&MapFeature::computeFeatureVector)) + .def("computeFeatureVectors", pure_virtual(&MapFeature::computeFeatureVectors)) .def("process", pure_virtual(&MapFeature::process)); class_, boost::noncopyable>("LineStringFeature", "Abstract line string feature class", no_init) .add_property("rawFeature", make_function(&LineStringFeature::rawFeature, return_value_policy())) - .def("computeFeatureVector", pure_virtual(&LineStringFeature::computeFeatureVector)) + .def("computeFeatureVectors", pure_virtual(&LineStringFeature::computeFeatureVectors)) .def("process", pure_virtual(&LineStringFeature::process)) - .def("pointMatrix", pure_virtual(&LineStringFeature::pointMatrix)); + .def("pointMatrices", pure_virtual(&LineStringFeature::pointMatrices)); class_, LaneLineStringFeaturePtr, boost::noncopyable>( "LaneLineStringFeature", "Lane line string feature class", @@ -218,10 +220,10 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .add_property("laneletIDs", make_function(&LaneLineStringFeature::laneletIDs, return_value_policy())) .add_property("addLaneletID", &LaneLineStringFeature::addLaneletID) - .def("computeFeatureVector", &LaneLineStringFeature::computeFeatureVector, - &LaneLineStringFeatureWrap::default_computeFeatureVector) + .def("computeFeatureVectors", &LaneLineStringFeature::computeFeatureVectors, + &LaneLineStringFeatureWrap::default_computeFeatureVectors) .def("process", &LaneLineStringFeature::process, &LaneLineStringFeatureWrap::default_process) - .def("pointMatrix", &LaneLineStringFeature::pointMatrix, &LaneLineStringFeatureWrap::default_pointMatrix); + .def("pointMatrices", &LaneLineStringFeature::pointMatrices, &LaneLineStringFeatureWrap::default_pointMatrices); class_, LaneletFeaturePtr, boost::noncopyable>( "LaneletFeature", "Lanelet feature class that contains lower level LaneLineStringFeatures", @@ -232,8 +234,8 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .add_property("rightBoundary", make_function(&LaneletFeature::rightBoundary)) .add_property("centerline", make_function(&LaneletFeature::centerline)) .def("setReprType", &LaneletFeature::setReprType) - .def("computeFeatureVector", &LaneletFeature::computeFeatureVector, - &LaneletFeatureWrap::default_computeFeatureVector) + .def("computeFeatureVectors", &LaneletFeature::computeFeatureVectors, + &LaneletFeatureWrap::default_computeFeatureVectors) .def("process", &LaneletFeature::process, &LaneletFeatureWrap::default_process); class_, CompoundLaneLineStringFeaturePtr, @@ -248,11 +250,11 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT return_value_policy())) .add_property("processedFeaturesValid", make_function(&CompoundLaneLineStringFeature::processedFeaturesValid, return_value_policy())) - .def("computeFeatureVector", &CompoundLaneLineStringFeature::computeFeatureVector, - &CompoundLaneLineStringFeatureWrap::default_computeFeatureVector) + .def("computeFeatureVectors", &CompoundLaneLineStringFeature::computeFeatureVectors, + &CompoundLaneLineStringFeatureWrap::default_computeFeatureVectors) .def("process", &CompoundLaneLineStringFeature::process, &CompoundLaneLineStringFeatureWrap::default_process) - .def("pointMatrix", &CompoundLaneLineStringFeature::pointMatrix, - &CompoundLaneLineStringFeatureWrap::default_pointMatrix); + .def("pointMatrices", &CompoundLaneLineStringFeature::pointMatrices, + &CompoundLaneLineStringFeatureWrap::default_pointMatrices); class_("Edge", "Struct of a lane graph edge", init()) .def(init<>()) @@ -261,7 +263,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT { scope inLaneData = - class_("LaneData", "Class for holding, accessing and processing of lane data") + class_("LaneData", "Class for holding, accessing and processing of lane data") .def(init<>()) .def("build", &LaneData::build) .staticmethod("build") @@ -339,15 +341,21 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT converters::convertVector(true); converters::VectorToListConverter>(); + converters::VectorToListConverter(); converters::VectorToListConverter(); converters::VectorToListConverter(); converters::VectorToListConverter< boost::geometry::model::ring>(); converters::VectorToListConverter>(); converters::VectorToListConverter>(); + converters::VectorToListConverter>(); converters::IterableConverter() .fromPython>() .fromPython() + .fromPython() + .fromPython>() + .fromPython>() + .fromPython>() .fromPython() .fromPython(); converters::MapToDictConverter(); From bf6d8c0706e2b192cd50efcd8ad5221c5df36885 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Wed, 17 Jan 2024 10:59:17 +0100 Subject: [PATCH 44/64] add following edges --- .../include/lanelet2_map_learning/MapData.h | 15 ++++++++++----- lanelet2_map_learning/src/MapData.cpp | 12 ++++++++---- 2 files changed, 18 insertions(+), 9 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index 1da92713..8cedfb35 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -124,6 +124,8 @@ class LaneData { void initCompoundFeatures(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); void updateAssociatedCpdFeatureIndices(); + void getPaths(lanelet::routing::RoutingGraphConstPtr localSubmapGraph, std::vector& paths, + ConstLanelet start, ConstLanelets initPath = ConstLanelets()); LineStringType getLineStringTypeFromId(Id id); LaneLineStringFeaturePtr getLineStringFeatFromId(Id id, bool inverted); @@ -140,11 +142,14 @@ class LaneData { LaneletFeatures laneletFeatures_; - std::map> associatedCpdRoadBorderIndices_; // related to the "unfiltered" compound features! - std::map> associatedCpdLaneDividerIndices_; // related to the "unfiltered" compound features! - std::map> associatedCpdCenterlineIndices_; // related to the "unfiltered" compound features! - Edges edges_; // edge list for centerlines - std::string uuid_; // sample id + std::map> + associatedCpdRoadBorderIndices_; // relates lanelet id to the "unfiltered" compound features! + std::map> + associatedCpdLaneDividerIndices_; // relates lanelet id to the "unfiltered" compound features! + std::map> + associatedCpdCenterlineIndices_; // relates lanelet id to the "unfiltered" compound features! + Edges edges_; // edge list for centerlines + std::string uuid_; // sample id Optional tfData_; }; diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index a0521f90..c273a361 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -97,16 +97,20 @@ void LaneData::initLaneletFeatures(LaneletSubmapConstPtr& localSubmap, // Idea: algorithm for paths that starts with LLs with no previous, splits on junctions and terminates on LLs with // no successors -void getPaths(lanelet::routing::RoutingGraphConstPtr localSubmapGraph, std::vector& paths, - ConstLanelet start, ConstLanelets initPath = ConstLanelets()) { +void LaneData::getPaths(lanelet::routing::RoutingGraphConstPtr localSubmapGraph, std::vector& paths, + ConstLanelet start, ConstLanelets initPath) { initPath.push_back(start); - ConstLanelets successorLLs = localSubmapGraph->following(start, false); + ConstLanelet current = start; + ConstLanelets successorLLs = localSubmapGraph->following(current, false); while (!successorLLs.empty()) { for (size_t i = 1; i != successorLLs.size(); i++) { + edges_.insert({current.id(), Edge(current.id(), successorLLs[i].id())}); getPaths(localSubmapGraph, paths, successorLLs[i], initPath); } initPath.push_back(successorLLs.front()); - successorLLs = localSubmapGraph->following(successorLLs.front(), false); + edges_.insert({current.id(), Edge(current.id(), successorLLs.front().id())}); + current = successorLLs.front(); + successorLLs = localSubmapGraph->following(current, false); } paths.push_back(initPath); } From d444210113e76646c721d1b98a547240668db4d1 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 18 Jan 2024 19:44:24 +0100 Subject: [PATCH 45/64] add tracing pointMatrix -> cpdFeat --- .../include/lanelet2_map_learning/MapData.h | 35 +++++++--- lanelet2_map_learning/src/MapData.cpp | 68 ++++++++++++++++++- lanelet2_map_learning/test/test_map_data.cpp | 32 ++++----- lanelet2_python/python_api/map_learning.cpp | 14 ++-- 4 files changed, 113 insertions(+), 36 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index 8cedfb35..3a2031bf 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -69,6 +69,12 @@ class LaneData { const std::vector& compoundLaneDividerTypes() { return compoundLaneDividerTypes_; } const std::vector& compoundCenterlines() { return compoundCenterlines_; } const std::string& uuid() { return uuid_; } + CompoundLaneLineStringFeaturePtr pointMatrixCpdRoadBorder( + size_t index); // get feature associated to point matrix index + CompoundLaneLineStringFeaturePtr pointMatrixCpdLaneDivider( + size_t index); // get feature associated to point matrix index + CompoundLaneLineStringFeaturePtr pointMatrixCpdCenterline( + size_t index); // get feature associated to point matrix index friend class LaneData; private: @@ -80,6 +86,12 @@ class LaneData { std::vector compoundLaneDividerTypes_; std::vector compoundCenterlines_; std::string uuid_; + std::map + pointMatrixCpdRoadBorder_; // relates point matrix index to compound features + std::map + pointMatrixCpdLaneDivider_; // relates point matrix index to compound features, tfData_ must be initialized + std::map + pointMatrixCpdCenterline_; // relates point matrix index to compound features, tfData_ must be initialized }; LaneData() noexcept : uuid_{boost::lexical_cast(boost::uuids::random_generator()())} {} @@ -100,17 +112,19 @@ class LaneData { CompoundLaneLineStringFeatureList validCompoundLaneDividers() { return getValidElements(compoundLaneDividers_); } CompoundLaneLineStringFeatureList validCompoundCenterlines() { return getValidElements(compoundCenterlines_); } - const std::map>& associatedCpdRoadBorderIndices() { return associatedCpdRoadBorderIndices_; } - const std::map>& associatedCpdLaneDividerIndices() { - return associatedCpdLaneDividerIndices_; - } - const std::map>& associatedCpdCenterlineIndices() { return associatedCpdCenterlineIndices_; } + CompoundLaneLineStringFeatureList associatedCpdRoadBorders( + Id mapId); // get features associated to map element with id + CompoundLaneLineStringFeatureList associatedCpdLaneDividers( + Id mapId); // get features associated to map element with id + CompoundLaneLineStringFeatureList associatedCpdCenterlines( + Id mapId); // get features associated to map element with id const LaneletFeatures& laneletFeatures() { return laneletFeatures_; } const Edges& edges() { return edges_; } const std::string& uuid() { return uuid_; } /// The computed data will be buffered, if the underlying features change you need to set ignoreBuffer appropriately + /// The data will also only be useful if you called process on the features beforehand (e.g. with processAll()) TensorFeatureData getTensorFeatureData(bool pointsIn2d, bool ignoreBuffer); template @@ -143,13 +157,14 @@ class LaneData { LaneletFeatures laneletFeatures_; std::map> - associatedCpdRoadBorderIndices_; // relates lanelet id to the "unfiltered" compound features! + associatedCpdRoadBorderIndices_; // relates map element id to the "unfiltered" compound features! std::map> - associatedCpdLaneDividerIndices_; // relates lanelet id to the "unfiltered" compound features! + associatedCpdLaneDividerIndices_; // relates map element id to the "unfiltered" compound features! std::map> - associatedCpdCenterlineIndices_; // relates lanelet id to the "unfiltered" compound features! - Edges edges_; // edge list for centerlines - std::string uuid_; // sample id + associatedCpdCenterlineIndices_; // relates map element id to the "unfiltered" compound features! + + Edges edges_; // edge list for centerlines + std::string uuid_; // sample id Optional tfData_; }; diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index c273a361..cf7af71e 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -282,6 +282,7 @@ void LaneData::updateAssociatedCpdFeatureIndices() { for (const auto& id : indFeat->laneletIDs()) { associatedCpdRoadBorderIndices_[id].push_back(i); } + associatedCpdRoadBorderIndices_[indFeat->mapID()].push_back(i); } } for (size_t i = 0; i < compoundLaneDividers_.size(); i++) { @@ -290,6 +291,7 @@ void LaneData::updateAssociatedCpdFeatureIndices() { for (const auto& id : indFeat->laneletIDs()) { associatedCpdLaneDividerIndices_[id].push_back(i); } + associatedCpdLaneDividerIndices_[indFeat->mapID()].push_back(i); } } for (size_t i = 0; i < compoundCenterlines_.size(); i++) { @@ -318,7 +320,6 @@ bool LaneData::processAll(const OrientedRect& bbox, const ParametrizationType& p } } -/// TODO: INCLUDE REASSOCIATION FROM VECTOR ELEMENT TO FEATURE LaneData::TensorFeatureData LaneData::getTensorFeatureData(bool pointsIn2d, bool ignoreBuffer) { if (!tfData_.has_value() || ignoreBuffer) { tfData_ = LaneData::TensorFeatureData(); @@ -334,9 +335,74 @@ LaneData::TensorFeatureData LaneData::getTensorFeatureData(bool pointsIn2d, bool for (const auto& ft : validCompoundLaneDividers()) { tfData_->compoundLaneDividerTypes_.push_back(ft->typeInt()); } + size_t pointMatrixIdxRb = 0; + for (const auto& cpdFeat : compoundRoadBorders_) { + for (const auto& lString : cpdFeat->cutResampledAndTransformedFeature()) { + tfData_->pointMatrixCpdRoadBorder_[pointMatrixIdxRb] = cpdFeat; + pointMatrixIdxRb++; + } + } + size_t pointMatrixIdxLd = 0; + for (const auto& cpdFeat : compoundLaneDividers_) { + for (const auto& lString : cpdFeat->cutResampledAndTransformedFeature()) { + tfData_->pointMatrixCpdLaneDivider_[pointMatrixIdxLd] = cpdFeat; + pointMatrixIdxLd++; + } + } + size_t pointMatrixIdxCl = 0; + for (const auto& cpdFeat : compoundCenterlines_) { + for (const auto& lString : cpdFeat->cutResampledAndTransformedFeature()) { + tfData_->pointMatrixCpdCenterline_[pointMatrixIdxCl] = cpdFeat; + pointMatrixIdxCl++; + } + } } return tfData_.value(); } +CompoundLaneLineStringFeatureList associatedCpdFeats(Id mapId, const CompoundLaneLineStringFeatureList& featList, + const std::map>& assoIndices) { + CompoundLaneLineStringFeatureList assoFeats; + for (const auto& idx : assoIndices.at(mapId)) { + assoFeats.push_back(featList[idx]); + } + return assoFeats; +} + +CompoundLaneLineStringFeatureList LaneData::associatedCpdRoadBorders(Id mapId) { + return associatedCpdFeats(mapId, compoundRoadBorders_, associatedCpdRoadBorderIndices_); +} + +CompoundLaneLineStringFeatureList LaneData::associatedCpdLaneDividers(Id mapId) { + return associatedCpdFeats(mapId, compoundLaneDividers_, associatedCpdLaneDividerIndices_); +} + +CompoundLaneLineStringFeatureList LaneData::associatedCpdCenterlines(Id mapId) { + return associatedCpdFeats(mapId, compoundCenterlines_, associatedCpdCenterlineIndices_); +} + +CompoundLaneLineStringFeaturePtr pointMatrixCpdFeat( + size_t index, const std::map& assoFeats) { + CompoundLaneLineStringFeaturePtr feat; + try { + feat = assoFeats.at(index); + } catch (const std::out_of_range& e) { + throw std::out_of_range("A point matrix with index " + std::to_string(index) + " does not exist!"); + } + return feat; +} + +CompoundLaneLineStringFeaturePtr LaneData::TensorFeatureData::pointMatrixCpdRoadBorder(size_t index) { + return pointMatrixCpdFeat(index, pointMatrixCpdRoadBorder_); +} + +CompoundLaneLineStringFeaturePtr LaneData::TensorFeatureData::pointMatrixCpdLaneDivider(size_t index) { + return pointMatrixCpdFeat(index, pointMatrixCpdLaneDivider_); +} + +CompoundLaneLineStringFeaturePtr LaneData::TensorFeatureData::pointMatrixCpdCenterline(size_t index) { + return pointMatrixCpdFeat(index, pointMatrixCpdCenterline_); +} + } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/test/test_map_data.cpp b/lanelet2_map_learning/test/test_map_data.cpp index 4d8f1636..d68ce945 100644 --- a/lanelet2_map_learning/test/test_map_data.cpp +++ b/lanelet2_map_learning/test/test_map_data.cpp @@ -21,10 +21,6 @@ TEST_F(MapLearningTest, LaneData) { // NOLINT LaneDataPtr laneData = LaneData::build(laneletSubmap, laneletMapGraph); - std::vector rawCompoundRoadBorders = getPointMatrices(laneData->compoundRoadBorders(), true); - std::vector rawCompoundLaneDividers = getPointMatrices(laneData->compoundLaneDividers(), true); - std::vector rawCompoundCenterlines = getPointMatrices(laneData->compoundCenterlines(), true); - bool valid = laneData->processAll(bbox, ParametrizationType::LineString, 20); std::vector compoundRoadBorders = getPointMatrices(laneData->compoundRoadBorders(), true); std::vector compoundLaneDividers = getPointMatrices(laneData->compoundLaneDividers(), true); @@ -38,22 +34,22 @@ TEST_F(MapLearningTest, LaneData) { // NOLINT EXPECT_EQ(compoundLaneDividers.size(), 8); EXPECT_EQ(compoundCenterlines.size(), 4); - EXPECT_EQ(laneData->associatedCpdRoadBorderIndices().at(2001).size(), 1); - size_t assoBorderIdx = laneData->associatedCpdRoadBorderIndices().at(2001).front(); - EXPECT_EQ(laneData->compoundRoadBorders()[assoBorderIdx]->features().back()->mapID(), 1000); - EXPECT_EQ(laneData->compoundRoadBorders()[assoBorderIdx]->features().back()->laneletIDs().front(), 2002); + EXPECT_EQ(laneData->associatedCpdRoadBorders(2001).size(), 1); + CompoundLaneLineStringFeaturePtr assoBorder = laneData->associatedCpdRoadBorders(2001).front(); + EXPECT_EQ(assoBorder->features().back()->mapID(), 1000); + EXPECT_EQ(assoBorder->features().back()->laneletIDs().front(), 2002); - EXPECT_EQ(laneData->associatedCpdLaneDividerIndices().at(2004).size(), 2); - EXPECT_EQ(laneData->associatedCpdLaneDividerIndices().at(2009).size(), 1); - size_t assoDividerIdx = laneData->associatedCpdLaneDividerIndices().at(2009).front(); - EXPECT_EQ(laneData->compoundLaneDividers()[assoDividerIdx]->features().front()->mapID(), 1015); - EXPECT_EQ(laneData->compoundLaneDividers()[assoDividerIdx]->features().front()->laneletIDs().size(), 2); + EXPECT_EQ(laneData->associatedCpdLaneDividers(2004).size(), 2); + EXPECT_EQ(laneData->associatedCpdLaneDividers(2009).size(), 1); + CompoundLaneLineStringFeaturePtr assoDivider = laneData->associatedCpdLaneDividers(2009).front(); + EXPECT_EQ(assoDivider->features().front()->mapID(), 1015); + EXPECT_EQ(assoDivider->features().front()->laneletIDs().size(), 2); - EXPECT_EQ(laneData->associatedCpdCenterlineIndices().at(2003).size(), 2); - EXPECT_EQ(laneData->associatedCpdCenterlineIndices().at(2000).size(), 1); - size_t assoCenterlineIdx = laneData->associatedCpdCenterlineIndices().at(2000).front(); - EXPECT_EQ(laneData->compoundCenterlines()[assoCenterlineIdx]->features().front()->mapID(), 2000); - EXPECT_EQ(laneData->compoundCenterlines()[assoCenterlineIdx]->features().front()->laneletIDs().front(), 2000); + EXPECT_EQ(laneData->associatedCpdCenterlines(2003).size(), 2); + EXPECT_EQ(laneData->associatedCpdCenterlines(2000).size(), 1); + CompoundLaneLineStringFeaturePtr assoCenterline = laneData->associatedCpdCenterlines(2000).front(); + EXPECT_EQ(assoCenterline->features().front()->mapID(), 2000); + EXPECT_EQ(assoCenterline->features().front()->laneletIDs().front(), 2000); // matplot::hold(matplot::on); // matplot::xlim({-2, 16}); diff --git a/lanelet2_python/python_api/map_learning.cpp b/lanelet2_python/python_api/map_learning.cpp index fba2bf24..257df423 100644 --- a/lanelet2_python/python_api/map_learning.cpp +++ b/lanelet2_python/python_api/map_learning.cpp @@ -283,12 +283,9 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .add_property("validCompoundRoadBorders", &LaneData::validCompoundRoadBorders) .add_property("validCompoundLaneDividers", &LaneData::validCompoundLaneDividers) .add_property("validCompoundCenterlines", &LaneData::validCompoundCenterlines) - .add_property("associatedCpdRoadBorderIndices", make_function(&LaneData::associatedCpdRoadBorderIndices, - return_value_policy())) - .add_property("associatedCpdLaneDividerIndices", make_function(&LaneData::associatedCpdLaneDividerIndices, - return_value_policy())) - .add_property("associatedCpdCenterlineIndices", make_function(&LaneData::associatedCpdCenterlineIndices, - return_value_policy())) + .def("associatedCpdRoadBorders", &LaneData::associatedCpdRoadBorders) + .def("associatedCpdLaneDividers", &LaneData::associatedCpdLaneDividers) + .def("associatedCpdCenterlines", &LaneData::associatedCpdCenterlines) .add_property("laneletFeatures", make_function(&LaneData::laneletFeatures, return_value_policy())) .add_property("edges", make_function(&LaneData::edges, return_value_policy())) @@ -311,7 +308,10 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .add_property("compoundCenterlines", make_function(&LaneData::TensorFeatureData::compoundCenterlines, return_value_policy())) .add_property("uuid", - make_function(&LaneData::TensorFeatureData::uuid, return_value_policy())); + make_function(&LaneData::TensorFeatureData::uuid, return_value_policy())) + .def("pointMatrixCpdRoadBorder", &LaneData::TensorFeatureData::pointMatrixCpdRoadBorder) + .def("pointMatrixCpdLaneDivider", &LaneData::TensorFeatureData::pointMatrixCpdLaneDivider) + .def("pointMatrixCpdCenterline", &LaneData::TensorFeatureData::pointMatrixCpdCenterline); } { From d9578d1d0f9c8af755163d44832c55d966a2b7d7 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 18 Jan 2024 20:36:49 +0100 Subject: [PATCH 46/64] remove debug prints --- lanelet2_map_learning/src/Utils.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index 9f782a11..3c874ffd 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -77,18 +77,18 @@ std::vector cutLineString(const OrientedRect& bbox, const Bas if (cut2d.empty()) { return cut3d; } else if (cut2d.size() > 1) { - std::cerr << "More than one cut line!" << std::endl; - std::cerr << "Raw line:" << std::endl; - for (const auto& pt : polyline) { - std::cerr << "[" << pt.x() << "," << pt.y() << "]" << std::endl; - } - for (const auto& ls : cut2d) { - std::cerr << "Cut line:" << std::endl; - for (const auto& pt : ls) { - std::cerr << "[" << pt.x() << "," << pt.y() << "]" << std::endl; - } - std::cerr << "--------------" << std::endl; - } + // std::cerr << "More than one cut line!" << std::endl; + // std::cerr << "Raw line:" << std::endl; + // for (const auto& pt : polyline) { + // std::cerr << "[" << pt.x() << "," << pt.y() << "]" << std::endl; + // } + // for (const auto& ls : cut2d) { + // std::cerr << "Cut line:" << std::endl; + // for (const auto& pt : ls) { + // std::cerr << "[" << pt.x() << "," << pt.y() << "]" << std::endl; + // } + // std::cerr << "--------------" << std::endl; + // } // throw std::runtime_error("More than one cut line!"); } From 65aac9e5625c7b379c5e8f4d32b81278ac40fa8e Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Fri, 19 Jan 2024 19:47:01 +0100 Subject: [PATCH 47/64] fix path loop edge case, support multi file save and load --- .../include/lanelet2_map_learning/Utils.h | 12 ++++- lanelet2_map_learning/src/MapData.cpp | 15 ++++++ lanelet2_map_learning/src/Utils.cpp | 54 +++++++++++++++++++ lanelet2_python/python_api/map_learning.cpp | 4 ++ 4 files changed, 83 insertions(+), 2 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h index 517ba175..5644e772 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h @@ -68,9 +68,17 @@ std::vector cutLineString(const OrientedRect& bbox, const Bas BasicLineString3d transformLineString(const OrientedRect& bbox, const BasicLineString3d& polyline); -void saveLaneData(const std::string& filename, const std::vector& lDataVec, bool binary); +void saveLaneData(const std::string& filename, const std::vector& lDataVec, + bool binary); // saves all in one file -std::vector loadLaneData(const std::string& filename, bool binary); +std::vector loadLaneData(const std::string& filename, bool binary); // loads entire vector from one file + +void saveLaneDataMultiFile(const std::string& path, const std::vector& filenames, + const std::vector& lDataVec, + bool binary); // saves in one file per LaneData + +std::vector loadLaneDataMultiFile(const std::string& path, const std::vector& filenames, + bool binary); // loads from one file per LaneData } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index cf7af71e..d67e6a60 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -95,6 +95,15 @@ void LaneData::initLaneletFeatures(LaneletSubmapConstPtr& localSubmap, } } +bool isLaneletInPath(const ConstLanelets& path, const ConstLanelet& ll) { + for (const auto& el : path) { + if (ll.id() == el.id()) { + return true; + } + } + return false; +} + // Idea: algorithm for paths that starts with LLs with no previous, splits on junctions and terminates on LLs with // no successors void LaneData::getPaths(lanelet::routing::RoutingGraphConstPtr localSubmapGraph, std::vector& paths, @@ -104,9 +113,15 @@ void LaneData::getPaths(lanelet::routing::RoutingGraphConstPtr localSubmapGraph, ConstLanelets successorLLs = localSubmapGraph->following(current, false); while (!successorLLs.empty()) { for (size_t i = 1; i != successorLLs.size(); i++) { + if (isLaneletInPath(initPath, successorLLs[i])) { + continue; + } edges_.insert({current.id(), Edge(current.id(), successorLLs[i].id())}); getPaths(localSubmapGraph, paths, successorLLs[i], initPath); } + if (isLaneletInPath(initPath, successorLLs.front())) { + continue; + } initPath.push_back(successorLLs.front()); edges_.insert({current.id(), Edge(current.id(), successorLLs.front().id())}); current = successorLLs.front(); diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index 3c874ffd..ceca3ce8 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -164,5 +164,59 @@ std::vector loadLaneData(const std::string& filename, bool binary) return lDataVec; } +void saveLaneDataMultiFile(const std::string& path, const std::vector& filenames, + const std::vector& lDataVec, bool binary) { + if (filenames.size() != lDataVec.size()) { + throw std::runtime_error("Unequal number of file names and LaneData objects!"); + } + for (size_t i = 0; i < filenames.size(); i++) { + const auto& filename = filenames[i]; + const auto& lData = lDataVec[i]; + if (binary) { + std::ofstream fs(path + filename, std::ofstream::binary); + if (!fs.good()) { + throw std::runtime_error("Failed to open archive " + filename); + } + boost::archive::binary_oarchive oa(fs); + oa << lDataVec; + } else { + std::ofstream fs(path + filename); + if (!fs.good()) { + throw std::runtime_error("Failed to open archive " + filename); + } + boost::archive::xml_oarchive oa(fs, boost::archive::no_header); + oa << BOOST_SERIALIZATION_NVP(lData); + } + } +} + +std::vector loadLaneDataMultiFile(const std::string& path, const std::vector& filenames, + bool binary) { + std::vector lDataVec; + for (size_t i = 0; i < filenames.size(); i++) { + const auto& filename = filenames[i]; + LaneDataPtr lData; + if (!fs::exists(fs::path(path + filename))) { + throw std::runtime_error("Could not find file under " + filename); + } + if (binary) { + std::ifstream fs(path + filename, std::ifstream::binary); + if (!fs.good()) { + throw std::runtime_error("Failed to open archive " + filename); + } + boost::archive::binary_iarchive ia(fs); + ia >> lData; + } else { + std::ifstream fs(path + filename); + if (!fs.good()) { + throw std::runtime_error("Failed to open archive " + filename); + } + boost::archive::xml_iarchive ia(fs, boost::archive::no_header); + ia >> BOOST_SERIALIZATION_NVP(lData); + } + lDataVec.push_back(lData); + } + return lDataVec; +} } // namespace map_learning } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_python/python_api/map_learning.cpp b/lanelet2_python/python_api/map_learning.cpp index 257df423..e0ad4b6a 100644 --- a/lanelet2_python/python_api/map_learning.cpp +++ b/lanelet2_python/python_api/map_learning.cpp @@ -186,6 +186,8 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT def("cutLineString", &cutLineString); def("saveLaneData", &saveLaneData); def("loadLaneData", &loadLaneData); + def("saveLaneDataMultiFile", &saveLaneDataMultiFile); + def("loadLaneDataMultiFile", &loadLaneDataMultiFile); class_("MapFeature", "Abstract base map feature class", no_init) .add_property("wasCut", &MapFeature::wasCut) @@ -348,6 +350,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT boost::geometry::model::ring>(); converters::VectorToListConverter>(); converters::VectorToListConverter>(); + converters::VectorToListConverter>(); converters::VectorToListConverter>(); converters::IterableConverter() .fromPython>() @@ -355,6 +358,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .fromPython() .fromPython>() .fromPython>() + .fromPython>() .fromPython>() .fromPython() .fromPython(); From df5454783ffb6afb1f22d8f1d6dd5fdcf43d78b2 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Mon, 22 Jan 2024 19:02:28 +0100 Subject: [PATCH 48/64] bugfix double cpdFeats --- lanelet2_map_learning/src/MapData.cpp | 5 ----- lanelet2_python/python_api/map_learning.cpp | 1 - 2 files changed, 6 deletions(-) diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index d67e6a60..88d4f026 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -242,11 +242,6 @@ void insertAndCheckNewCompoundFeatures(std::vector& compFeats, for (const Id& el : compEl.ids) { elInsertIdx[el] = firstOccIt->second; } - } else if (compFeats[firstOccIt->second].ids.size() != compEl.ids.size()) { - compFeats.push_back(compEl); - for (const Id& el : compEl.ids) { - elInsertIdx[el] = compFeats.size() - 1; - } } } } diff --git a/lanelet2_python/python_api/map_learning.cpp b/lanelet2_python/python_api/map_learning.cpp index e0ad4b6a..0d577267 100644 --- a/lanelet2_python/python_api/map_learning.cpp +++ b/lanelet2_python/python_api/map_learning.cpp @@ -350,7 +350,6 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT boost::geometry::model::ring>(); converters::VectorToListConverter>(); converters::VectorToListConverter>(); - converters::VectorToListConverter>(); converters::VectorToListConverter>(); converters::IterableConverter() .fromPython>() From 8763e4dc61785d3b1989918a0ab0498130775908 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Mon, 22 Jan 2024 19:32:33 +0100 Subject: [PATCH 49/64] bugfix invalid feats --- lanelet2_map_learning/src/MapFeatures.cpp | 22 +++++++++++++--------- lanelet2_map_learning/src/Utils.cpp | 9 ++++++++- 2 files changed, 21 insertions(+), 10 deletions(-) diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_map_learning/src/MapFeatures.cpp index a351d9b3..f6f8abaa 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_map_learning/src/MapFeatures.cpp @@ -29,23 +29,27 @@ LStringProcessResult processLineStringImpl(const BasicLineString3d& lstring, con return result; } - for (const auto& line : cutLines) { - result.cutFeatures.push_back(line); - BasicLineString3d lineResampled = resampleLineString(line, nPoints); - result.cutAndResampledFeatures.push_back(lineResampled); - result.cutResampledAndTransformedFeatures.push_back(transformLineString(bbox, lineResampled)); - } - double lengthOriginal = boost::geometry::length(lstring, boost::geometry::strategy::distance::pythagoras()); double lengthProcessed = 0; - for (const auto& line : result.cutResampledAndTransformedFeatures) { + for (const auto& line : cutLines) { lengthProcessed = lengthProcessed + boost::geometry::length(line, boost::geometry::strategy::distance::pythagoras()); } - + if (lengthProcessed < 1e-1) { + result.wasCut_ = true; + result.valid_ = false; + return result; + } if (lengthOriginal - lengthProcessed > 1e-2) { result.wasCut_ = true; } + + for (const auto& line : cutLines) { + result.cutFeatures.push_back(line); + BasicLineString3d lineResampled = resampleLineString(line, nPoints); + result.cutAndResampledFeatures.push_back(lineResampled); + result.cutResampledAndTransformedFeatures.push_back(transformLineString(bbox, lineResampled)); + } return result; } diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index ceca3ce8..015d6b8f 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -50,6 +50,9 @@ LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPo /// TODO: FURTHER INVESTIGATE THE WEIRD BEHAVIOR OF BOOST LINE_INTERPOLATE BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t nPoints) { + if (polyline.size() < 1) { + throw std::runtime_error("A polyline requires at least 2 points!"); + } double length = boost::geometry::length(polyline, boost::geometry::strategy::distance::pythagoras()); double dist = length / static_cast(nPoints - 1); // to get all points of line boost::geometry::model::multi_point bdInterp; @@ -60,7 +63,11 @@ BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t } else if (bdInterp.size() == nPoints && (bdInterp.back() - polyline.back()).norm() > 10e-5) { bdInterp.back() = polyline.back(); } else if (bdInterp.size() != nPoints) { - throw std::runtime_error("LineString resampling failed to produce the desired number of points!"); + std::stringstream ss; + for (const auto& pt : bdInterp) { + ss << pt; + } + throw std::runtime_error("LineString resampling failed to produce the desired number of points: " + ss.str()); } return bdInterp; } From 6e8af4737017d5942108655d18013ccd6f2daed0 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Tue, 23 Jan 2024 16:06:59 +0100 Subject: [PATCH 50/64] bugfix transform rotation --- lanelet2_map_learning/src/Utils.cpp | 8 +++-- .../test/test_map_features.cpp | 36 +++++++++---------- lanelet2_map_learning/test/test_utils.cpp | 16 ++++----- 3 files changed, 32 insertions(+), 28 deletions(-) diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_map_learning/src/Utils.cpp index 015d6b8f..28b8e8c0 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_map_learning/src/Utils.cpp @@ -21,9 +21,10 @@ OrientedRect getRotatedRect(const BasicPoint2d& center, double extentLongitudina BasicPoint2d{center.x() - extentLongitudinal, center.y() - extentLateral}}; OrientedRect axisAlignedRect; boost::geometry::assign_points(axisAlignedRect.bg_poly, pts); + boost::geometry::correct(axisAlignedRect.bg_poly); boost::geometry::strategy::transform::translate_transformer trans1(-center.x(), -center.y()); - boost::geometry::strategy::transform::rotate_transformer rotate(yaw); + boost::geometry::strategy::transform::rotate_transformer rotate(-yaw); boost::geometry::strategy::transform::translate_transformer trans2(center.x(), center.y()); OrientedRect trans1Rect; @@ -54,6 +55,9 @@ BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t throw std::runtime_error("A polyline requires at least 2 points!"); } double length = boost::geometry::length(polyline, boost::geometry::strategy::distance::pythagoras()); + if (length < 1e-1) { + return BasicLineString3d(); + } double dist = length / static_cast(nPoints - 1); // to get all points of line boost::geometry::model::multi_point bdInterp; boost::geometry::line_interpolate(polyline, dist, bdInterp); @@ -121,7 +125,7 @@ std::vector cutLineString(const OrientedRect& bbox, const Bas BasicLineString3d transformLineString(const OrientedRect& bbox, const BasicLineString3d& polyline) { boost::geometry::strategy::transform::translate_transformer trans1(-bbox.center.x(), -bbox.center.y()); - boost::geometry::strategy::transform::rotate_transformer rotate(-bbox.yaw); + boost::geometry::strategy::transform::rotate_transformer rotate(bbox.yaw); BasicLineString3d transPolyline; BasicLineString3d rotatedPolyline; boost::geometry::transform(polyline, transPolyline, trans1); diff --git a/lanelet2_map_learning/test/test_map_features.cpp b/lanelet2_map_learning/test/test_map_features.cpp index a894ed75..44e83e32 100644 --- a/lanelet2_map_learning/test/test_map_features.cpp +++ b/lanelet2_map_learning/test/test_map_features.cpp @@ -22,18 +22,18 @@ TEST_F(MapLearningTest, LaneLineStringFeature) { // NOLINT std::vector vec = feat.computeFeatureVectors(false, true); EXPECT_EQ(vec.size(), 1); EXPECT_EQ(vec[0].size(), 9); - EXPECT_NEAR(vec[0][0], 5, 10e-5); - EXPECT_NEAR(vec[0][1], -5, 10e-5); - EXPECT_NEAR(vec[0][6], 5, 10e-5); - EXPECT_NEAR(vec[0][7], 10, 10e-5); + EXPECT_NEAR(vec[0][0], -5, 10e-5); + EXPECT_NEAR(vec[0][1], 5, 10e-5); + EXPECT_NEAR(vec[0][6], -5, 10e-5); + EXPECT_NEAR(vec[0][7], -10, 10e-5); EXPECT_EQ(vec[0][8], 2); std::vector pointMat = feat.pointMatrices(false); EXPECT_EQ(pointMat[0].rows(), 4); EXPECT_EQ(pointMat[0].cols(), 3); - EXPECT_NEAR(pointMat[0](0, 1), -5, 10e-5); - EXPECT_NEAR(pointMat[0](2, 1), 5, 10e-5); - EXPECT_NEAR(pointMat[0](3, 1), 10, 10e-5); + EXPECT_NEAR(pointMat[0](0, 1), 5, 10e-5); + EXPECT_NEAR(pointMat[0](2, 1), -5, 10e-5); + EXPECT_NEAR(pointMat[0](3, 1), -10, 10e-5); } TEST_F(MapLearningTest, LaneletFeature) { // NOLINT @@ -62,9 +62,9 @@ TEST_F(MapLearningTest, LaneletFeature) { // NOLINT std::vector vec = llFeat.computeFeatureVectors(false, true); EXPECT_EQ(vec.size(), 1); EXPECT_EQ(vec[0].size(), 18); - EXPECT_NEAR(vec[0][9], -5, 10e-5); - EXPECT_NEAR(vec[0][10], 1, 10e-5); - EXPECT_NEAR(vec[0][15], 10, 10e-5); + EXPECT_NEAR(vec[0][9], 5, 10e-5); + EXPECT_NEAR(vec[0][10], -1, 10e-5); + EXPECT_NEAR(vec[0][15], -10, 10e-5); EXPECT_EQ(vec[0][17], 1); } @@ -104,19 +104,19 @@ TEST_F(MapLearningTest, CompoundLaneLineStringFeature) { // NOLINT std::vector vec = cpdFeat.computeFeatureVectors(false, true); EXPECT_EQ(vec.size(), 1); EXPECT_EQ(vec[0].size(), 11); - EXPECT_NEAR(vec[0][1], -10, 10e-5); - EXPECT_NEAR(vec[0][2], 5, 10e-5); - EXPECT_NEAR(vec[0][3], -5, 10e-5); - EXPECT_NEAR(vec[0][6], 5, 10e-5); - EXPECT_NEAR(vec[0][7], 5, 10e-5); + EXPECT_NEAR(vec[0][1], 10, 10e-5); + EXPECT_NEAR(vec[0][2], -5, 10e-5); + EXPECT_NEAR(vec[0][3], 5, 10e-5); + EXPECT_NEAR(vec[0][6], -5, 10e-5); + EXPECT_NEAR(vec[0][7], -5, 10e-5); EXPECT_EQ(vec[0][10], 2); std::vector pointMat = cpdFeat.pointMatrices(false); EXPECT_EQ(pointMat.size(), 1); EXPECT_EQ(pointMat[0].rows(), 5); EXPECT_EQ(pointMat[0].cols(), 3); - EXPECT_NEAR(pointMat[0](0, 1), -10, 10e-5); + EXPECT_NEAR(pointMat[0](0, 1), 10, 10e-5); EXPECT_NEAR(pointMat[0](2, 1), 0, 10e-5); - EXPECT_NEAR(pointMat[0](3, 1), 5, 10e-5); - EXPECT_NEAR(pointMat[0](4, 0), 5, 10e-5); + EXPECT_NEAR(pointMat[0](3, 1), -5, 10e-5); + EXPECT_NEAR(pointMat[0](4, 0), -5, 10e-5); } \ No newline at end of file diff --git a/lanelet2_map_learning/test/test_utils.cpp b/lanelet2_map_learning/test/test_utils.cpp index 90732942..1ea92987 100644 --- a/lanelet2_map_learning/test/test_utils.cpp +++ b/lanelet2_map_learning/test/test_utils.cpp @@ -8,14 +8,14 @@ using namespace lanelet::map_learning; using namespace lanelet::map_learning::tests; TEST_F(MapLearningTest, GetRotatedRect) { // NOLINT - EXPECT_DOUBLE_EQ(bbox.bounds_const()[0].x(), -5); - EXPECT_DOUBLE_EQ(bbox.bounds_const()[0].y(), 20); - EXPECT_DOUBLE_EQ(bbox.bounds_const()[1].x(), 15); - EXPECT_DOUBLE_EQ(bbox.bounds_const()[1].y(), 20); - EXPECT_DOUBLE_EQ(bbox.bounds_const()[2].x(), 15); - EXPECT_DOUBLE_EQ(bbox.bounds_const()[2].y(), -10); - EXPECT_DOUBLE_EQ(bbox.bounds_const()[3].x(), -5); - EXPECT_DOUBLE_EQ(bbox.bounds_const()[3].y(), -10); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[0].x(), 15); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[0].y(), -10); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[1].x(), -5); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[1].y(), -10); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[2].x(), -5); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[2].y(), 20); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[3].x(), 15); + EXPECT_DOUBLE_EQ(bbox.bounds_const()[3].y(), 20); } TEST_F(MapLearningTest, ExtractSubmap) { // NOLINT From 8beb2cafba6b0871b23611852b541c120fc41da6 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 25 Jan 2024 20:34:43 +0100 Subject: [PATCH 51/64] better edge support --- .../include/lanelet2_map_learning/MapData.h | 5 +++-- lanelet2_map_learning/src/MapData.cpp | 8 ++++---- lanelet2_python/python_api/map_learning.cpp | 7 +++++-- 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h index 3a2031bf..e36bbade 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h @@ -23,9 +23,10 @@ using LaneDataPtr = std::shared_ptr; struct Edge { Edge() = default; - Edge(Id el1, Id el2) : el1_{el1}, el2_{el2} {} + Edge(Id el1, Id el2, bool isLaneChange) : el1_{el1}, el2_{el2}, isLaneChange_{isLaneChange} {} Id el1_; Id el2_; + bool isLaneChange_; }; namespace internal { @@ -40,7 +41,7 @@ struct CompoundElsList { }; } // namespace internal -using Edges = std::map; // key = id from +using Edges = std::map>; // key = id from template std::vector> getValidElements(const std::vector>& vec) { diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_map_learning/src/MapData.cpp index 88d4f026..50282b74 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_map_learning/src/MapData.cpp @@ -46,7 +46,7 @@ void LaneData::initLeftBoundaries(LaneletSubmapConstPtr& localSubmap, Optional adjLeftLL = localSubmapGraph->adjacentLeft(ll); if (leftLL) { - edges_.insert({ll.id(), Edge(ll.id(), leftLL->id())}); + edges_[ll.id()].push_back(Edge(ll.id(), leftLL->id(), true)); } } } @@ -77,7 +77,7 @@ void LaneData::initRightBoundaries(LaneletSubmapConstPtr& localSubmap, Optional rightLL = localSubmapGraph->right(ll); Optional adjRightLL = localSubmapGraph->adjacentRight(ll); if (rightLL) { - edges_.insert({ll.id(), Edge(ll.id(), rightLL->id())}); + edges_[ll.id()].push_back(Edge(ll.id(), rightLL->id(), true)); } } } @@ -116,14 +116,14 @@ void LaneData::getPaths(lanelet::routing::RoutingGraphConstPtr localSubmapGraph, if (isLaneletInPath(initPath, successorLLs[i])) { continue; } - edges_.insert({current.id(), Edge(current.id(), successorLLs[i].id())}); + edges_[current.id()].push_back(Edge(current.id(), successorLLs[i].id(), false)); getPaths(localSubmapGraph, paths, successorLLs[i], initPath); } if (isLaneletInPath(initPath, successorLLs.front())) { continue; } initPath.push_back(successorLLs.front()); - edges_.insert({current.id(), Edge(current.id(), successorLLs.front().id())}); + edges_[current.id()].push_back(Edge(current.id(), successorLLs.front().id(), false)); current = successorLLs.front(); successorLLs = localSubmapGraph->following(current, false); } diff --git a/lanelet2_python/python_api/map_learning.cpp b/lanelet2_python/python_api/map_learning.cpp index 0d577267..9ee7e25e 100644 --- a/lanelet2_python/python_api/map_learning.cpp +++ b/lanelet2_python/python_api/map_learning.cpp @@ -258,10 +258,11 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .def("pointMatrices", &CompoundLaneLineStringFeature::pointMatrices, &CompoundLaneLineStringFeatureWrap::default_pointMatrices); - class_("Edge", "Struct of a lane graph edge", init()) + class_("Edge", "Struct of a lane graph edge", init()) .def(init<>()) .def_readwrite("el1", &Edge::el1_) - .def_readwrite("el2", &Edge::el2_); + .def_readwrite("el2", &Edge::el2_) + .def_readwrite("isLaneChange", &Edge::isLaneChange_); { scope inLaneData = @@ -351,6 +352,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT converters::VectorToListConverter>(); converters::VectorToListConverter>(); converters::VectorToListConverter>(); + converters::VectorToListConverter>(); converters::IterableConverter() .fromPython>() .fromPython() @@ -363,6 +365,7 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .fromPython(); converters::MapToDictConverter(); converters::MapToDictConverter(); + converters::MapToDictConverter(); DictToMapConverter(); DictToMapConverter(); class_>("BoolList").def(vector_indexing_suite>()); From f914eb540d841341a5dc95c132db15a77891cb3f Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Mon, 29 Jan 2024 15:10:47 +0100 Subject: [PATCH 52/64] rename module --- lanelet2_map_learning/README.md | 3 - .../.vscode/c_cpp_properties.json | 2 +- .../.vscode/settings.json | 0 .../CHANGELOG.rst | 2 +- .../CMakeLists.txt | 2 +- lanelet2_ml_converter/README.md | 3 + .../include/lanelet2_ml_converter}/Forward.h | 16 +++--- .../include/lanelet2_ml_converter}/MapData.h | 12 ++-- .../lanelet2_ml_converter}/MapDataInterface.h | 10 ++-- .../lanelet2_ml_converter}/MapFeatures.h | 18 +++--- .../lanelet2_ml_converter}/Serialize.h | 56 +++++++++---------- .../include/lanelet2_ml_converter}/Types.h | 6 +- .../include/lanelet2_ml_converter}/Utils.h | 4 +- .../package.xml | 4 +- .../src/.gitignore | 0 .../src/MapData.cpp | 8 +-- .../src/MapDataInterface.cpp | 10 ++-- .../src/MapFeatures.cpp | 8 +-- .../src/Utils.cpp | 6 +- .../test/LaneletTestMap.xml | 0 .../test/test_map.h | 8 +-- .../test/test_map_data.cpp | 8 +-- .../test/test_map_data_interface.cpp | 10 ++-- .../test/test_map_features.cpp | 6 +- .../test/test_utils.cpp | 6 +- lanelet2_python/package.xml | 2 +- .../{map_learning.cpp => ml_converter.cpp} | 10 ++-- .../test/test_lanelet2_map_learning.py | 2 +- 28 files changed, 111 insertions(+), 111 deletions(-) delete mode 100644 lanelet2_map_learning/README.md rename {lanelet2_map_learning => lanelet2_ml_converter}/.vscode/c_cpp_properties.json (98%) rename {lanelet2_map_learning => lanelet2_ml_converter}/.vscode/settings.json (100%) rename {lanelet2_map_learning => lanelet2_ml_converter}/CHANGELOG.rst (63%) rename {lanelet2_map_learning => lanelet2_ml_converter}/CMakeLists.txt (98%) create mode 100644 lanelet2_ml_converter/README.md rename {lanelet2_map_learning/include/lanelet2_map_learning => lanelet2_ml_converter/include/lanelet2_ml_converter}/Forward.h (79%) rename {lanelet2_map_learning/include/lanelet2_map_learning => lanelet2_ml_converter/include/lanelet2_ml_converter}/MapData.h (97%) rename {lanelet2_map_learning/include/lanelet2_map_learning => lanelet2_ml_converter/include/lanelet2_ml_converter}/MapDataInterface.h (92%) rename {lanelet2_map_learning/include/lanelet2_map_learning => lanelet2_ml_converter/include/lanelet2_ml_converter}/MapFeatures.h (96%) rename {lanelet2_map_learning/include/lanelet2_map_learning => lanelet2_ml_converter/include/lanelet2_ml_converter}/Serialize.h (74%) rename {lanelet2_map_learning/include/lanelet2_map_learning => lanelet2_ml_converter/include/lanelet2_ml_converter}/Types.h (91%) rename {lanelet2_map_learning/include/lanelet2_map_learning => lanelet2_ml_converter/include/lanelet2_ml_converter}/Utils.h (98%) rename {lanelet2_map_learning => lanelet2_ml_converter}/package.xml (87%) rename {lanelet2_map_learning => lanelet2_ml_converter}/src/.gitignore (100%) rename {lanelet2_map_learning => lanelet2_ml_converter}/src/MapData.cpp (99%) rename {lanelet2_map_learning => lanelet2_ml_converter}/src/MapDataInterface.cpp (95%) rename {lanelet2_map_learning => lanelet2_ml_converter}/src/MapFeatures.cpp (98%) rename {lanelet2_map_learning => lanelet2_ml_converter}/src/Utils.cpp (98%) rename {lanelet2_map_learning => lanelet2_ml_converter}/test/LaneletTestMap.xml (100%) rename {lanelet2_map_learning => lanelet2_ml_converter}/test/test_map.h (98%) rename {lanelet2_map_learning => lanelet2_ml_converter}/test/test_map_data.cpp (96%) rename {lanelet2_map_learning => lanelet2_ml_converter}/test/test_map_data_interface.cpp (96%) rename {lanelet2_map_learning => lanelet2_ml_converter}/test/test_map_features.cpp (97%) rename {lanelet2_map_learning => lanelet2_ml_converter}/test/test_utils.cpp (94%) rename lanelet2_python/python_api/{map_learning.cpp => ml_converter.cpp} (98%) diff --git a/lanelet2_map_learning/README.md b/lanelet2_map_learning/README.md deleted file mode 100644 index 8b84889c..00000000 --- a/lanelet2_map_learning/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# Lanelet2 Map Learning - -The map learning module for lanelet2. \ No newline at end of file diff --git a/lanelet2_map_learning/.vscode/c_cpp_properties.json b/lanelet2_ml_converter/.vscode/c_cpp_properties.json similarity index 98% rename from lanelet2_map_learning/.vscode/c_cpp_properties.json rename to lanelet2_ml_converter/.vscode/c_cpp_properties.json index d33cce5a..3ba59e05 100644 --- a/lanelet2_map_learning/.vscode/c_cpp_properties.json +++ b/lanelet2_ml_converter/.vscode/c_cpp_properties.json @@ -13,7 +13,7 @@ "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_core/include/**", "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_examples/include/**", "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_io/include/**", - "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_map_learning/include/**", + "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_ml_converter/include/**", "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_maps/include/**", "/home/immel/workspaces/av2_link_pred_ws/src/lanelet2_maps_mrt/include/**", "/home/immel/workspaces/av2_link_pred_ws/src/Lanelet2/lanelet2_matching/include/**", diff --git a/lanelet2_map_learning/.vscode/settings.json b/lanelet2_ml_converter/.vscode/settings.json similarity index 100% rename from lanelet2_map_learning/.vscode/settings.json rename to lanelet2_ml_converter/.vscode/settings.json diff --git a/lanelet2_map_learning/CHANGELOG.rst b/lanelet2_ml_converter/CHANGELOG.rst similarity index 63% rename from lanelet2_map_learning/CHANGELOG.rst rename to lanelet2_ml_converter/CHANGELOG.rst index c8962696..b45eb850 100644 --- a/lanelet2_map_learning/CHANGELOG.rst +++ b/lanelet2_ml_converter/CHANGELOG.rst @@ -1,3 +1,3 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package lanelet2_map_learning +Changelog for package lanelet2_ml_converter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ \ No newline at end of file diff --git a/lanelet2_map_learning/CMakeLists.txt b/lanelet2_ml_converter/CMakeLists.txt similarity index 98% rename from lanelet2_map_learning/CMakeLists.txt rename to lanelet2_ml_converter/CMakeLists.txt index 06562477..882ae797 100644 --- a/lanelet2_map_learning/CMakeLists.txt +++ b/lanelet2_ml_converter/CMakeLists.txt @@ -1,7 +1,7 @@ set(MRT_PKG_VERSION 4.0.0) # Modify only if you know what you are doing! cmake_minimum_required(VERSION 3.5.1) -project(lanelet2_map_learning) +project(lanelet2_ml_converter) ################### ## Find packages ## diff --git a/lanelet2_ml_converter/README.md b/lanelet2_ml_converter/README.md new file mode 100644 index 00000000..7a956019 --- /dev/null +++ b/lanelet2_ml_converter/README.md @@ -0,0 +1,3 @@ +# Lanelet2 ML Converter + +Converter module to convert Lanelet2 maps into local label instances for machine learning tasks. \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h b/lanelet2_ml_converter/include/lanelet2_ml_converter/Forward.h similarity index 79% rename from lanelet2_map_learning/include/lanelet2_map_learning/Forward.h rename to lanelet2_ml_converter/include/lanelet2_ml_converter/Forward.h index 00c1a100..c4464472 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Forward.h +++ b/lanelet2_ml_converter/include/lanelet2_ml_converter/Forward.h @@ -4,7 +4,7 @@ #include namespace lanelet { -namespace map_learning { +namespace ml_converter { /// @brief the distinction between left and right disappears here, /// generalized to passable and not passable (adjacent) @@ -40,29 +40,29 @@ class CompoundLaneLineStringFeature; class LaneletFeature; class LaneData; -} // namespace map_learning +} // namespace ml_converter } // namespace lanelet namespace boost { namespace serialization { template -void serialize(Archive& ar, lanelet::map_learning::MapFeature& feat, const unsigned int /*version*/); +void serialize(Archive& ar, lanelet::ml_converter::MapFeature& feat, const unsigned int /*version*/); template -void serialize(Archive& ar, lanelet::map_learning::LineStringFeature& feat, const unsigned int /*version*/); +void serialize(Archive& ar, lanelet::ml_converter::LineStringFeature& feat, const unsigned int /*version*/); template -void serialize(Archive& ar, lanelet::map_learning::LaneLineStringFeature& feat, const unsigned int /*version*/); +void serialize(Archive& ar, lanelet::ml_converter::LaneLineStringFeature& feat, const unsigned int /*version*/); template -void serialize(Archive& ar, lanelet::map_learning::CompoundLaneLineStringFeature& feat, const unsigned int /*version*/); +void serialize(Archive& ar, lanelet::ml_converter::CompoundLaneLineStringFeature& feat, const unsigned int /*version*/); template -void serialize(Archive& ar, lanelet::map_learning::LaneletFeature& feat, const unsigned int /*version*/); +void serialize(Archive& ar, lanelet::ml_converter::LaneletFeature& feat, const unsigned int /*version*/); template -void serialize(Archive& ar, lanelet::map_learning::LaneData& feat, const unsigned int /*version*/); +void serialize(Archive& ar, lanelet::ml_converter::LaneData& feat, const unsigned int /*version*/); } // namespace serialization } // namespace boost \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h b/lanelet2_ml_converter/include/lanelet2_ml_converter/MapData.h similarity index 97% rename from lanelet2_map_learning/include/lanelet2_map_learning/MapData.h rename to lanelet2_ml_converter/include/lanelet2_ml_converter/MapData.h index e36bbade..ac8112e1 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapData.h +++ b/lanelet2_ml_converter/include/lanelet2_ml_converter/MapData.h @@ -10,14 +10,14 @@ #include #include -#include "lanelet2_map_learning/Forward.h" -#include "lanelet2_map_learning/MapFeatures.h" -#include "lanelet2_map_learning/Types.h" +#include "lanelet2_ml_converter/Forward.h" +#include "lanelet2_ml_converter/MapFeatures.h" +#include "lanelet2_ml_converter/Types.h" #include "lanelet2_routing/RoutingGraph.h" #include "lanelet2_routing/internal/Graph.h" namespace lanelet { -namespace map_learning { +namespace ml_converter { using LaneDataPtr = std::shared_ptr; @@ -129,7 +129,7 @@ class LaneData { TensorFeatureData getTensorFeatureData(bool pointsIn2d, bool ignoreBuffer); template - friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::LaneData& feat, + friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::LaneData& feat, const unsigned int /*version*/); private: @@ -184,5 +184,5 @@ class TEData { /// xLane and xTE stacked, with xLane being first }; -} // namespace map_learning +} // namespace ml_converter } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h b/lanelet2_ml_converter/include/lanelet2_ml_converter/MapDataInterface.h similarity index 92% rename from lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h rename to lanelet2_ml_converter/include/lanelet2_ml_converter/MapDataInterface.h index d09feeac..102a188e 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapDataInterface.h +++ b/lanelet2_ml_converter/include/lanelet2_ml_converter/MapDataInterface.h @@ -3,15 +3,15 @@ #include #include -#include "lanelet2_map_learning/Forward.h" -#include "lanelet2_map_learning/MapData.h" -#include "lanelet2_map_learning/Types.h" +#include "lanelet2_ml_converter/Forward.h" +#include "lanelet2_ml_converter/MapData.h" +#include "lanelet2_ml_converter/Types.h" #include "lanelet2_routing/RoutingGraph.h" namespace lanelet { class LaneletLayer; -namespace map_learning { +namespace ml_converter { class MapDataInterface { public: @@ -59,5 +59,5 @@ class MapDataInterface { traffic_rules::TrafficRulesPtr trafficRules_; }; -} // namespace map_learning +} // namespace ml_converter } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h b/lanelet2_ml_converter/include/lanelet2_ml_converter/MapFeatures.h similarity index 96% rename from lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h rename to lanelet2_ml_converter/include/lanelet2_ml_converter/MapFeatures.h index eed0838b..18d717fa 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/MapFeatures.h +++ b/lanelet2_ml_converter/include/lanelet2_ml_converter/MapFeatures.h @@ -7,11 +7,11 @@ #include #include -#include "lanelet2_map_learning/Forward.h" -#include "lanelet2_map_learning/Types.h" +#include "lanelet2_ml_converter/Forward.h" +#include "lanelet2_ml_converter/Types.h" namespace lanelet { -namespace map_learning { +namespace ml_converter { using VectorXd = Eigen::Matrix; using MatrixXd = Eigen::Matrix; @@ -29,7 +29,7 @@ class MapFeature { virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) = 0; template - friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::MapFeature& feat, + friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::MapFeature& feat, const unsigned int /*version*/); virtual ~MapFeature() noexcept = default; @@ -53,7 +53,7 @@ class LineStringFeature : public MapFeature { virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) = 0; template - friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::LineStringFeature& feat, + friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::LineStringFeature& feat, const unsigned int /*version*/); virtual ~LineStringFeature() noexcept = default; @@ -88,7 +88,7 @@ class LaneLineStringFeature : public LineStringFeature { void addLaneletID(Id id) { laneletIDs_.push_back(id); } template - friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::LaneLineStringFeature& feat, + friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::LaneLineStringFeature& feat, const unsigned int /*version*/); protected: @@ -141,7 +141,7 @@ class LaneletFeature : public MapFeature { LaneLineStringFeaturePtr centerline() const { return centerline_; } template - friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::LaneletFeature& feat, + friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::LaneletFeature& feat, const unsigned int /*version*/); private: @@ -166,7 +166,7 @@ class CompoundLaneLineStringFeature : public LaneLineStringFeature { const std::vector& processedFeaturesValid() const { return processedFeaturesValid_; } template - friend void boost::serialization::serialize(Archive& ar, lanelet::map_learning::CompoundLaneLineStringFeature& feat, + friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::CompoundLaneLineStringFeature& feat, const unsigned int /*version*/); private: @@ -265,5 +265,5 @@ bool processFeatures(std::vector>& featVec, const OrientedRec MatrixXd toPointMatrix(const BasicLineString3d& lString, bool pointsIn2d); VectorXd toFeatureVector(const BasicLineString3d& line, int typeInt, bool onlyPoints, bool pointsIn2d); -} // namespace map_learning +} // namespace ml_converter } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h b/lanelet2_ml_converter/include/lanelet2_ml_converter/Serialize.h similarity index 74% rename from lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h rename to lanelet2_ml_converter/include/lanelet2_ml_converter/Serialize.h index 23df6767..37bfdafc 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Serialize.h +++ b/lanelet2_ml_converter/include/lanelet2_ml_converter/Serialize.h @@ -36,22 +36,22 @@ inline void serialize(Archive& ar, Eigen::Matrix -void serialize(Archive& ar, lanelet::map_learning::LaneletRepresentationType& type, const unsigned int /*version*/) { +void serialize(Archive& ar, lanelet::ml_converter::LaneletRepresentationType& type, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_NVP(type); } template -void serialize(Archive& ar, lanelet::map_learning::ParametrizationType& type, const unsigned int /*version*/) { +void serialize(Archive& ar, lanelet::ml_converter::ParametrizationType& type, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_NVP(type); } template -void serialize(Archive& ar, lanelet::map_learning::LineStringType& type, const unsigned int /*version*/) { +void serialize(Archive& ar, lanelet::ml_converter::LineStringType& type, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_NVP(type); } template -void serialize(Archive& ar, lanelet::map_learning::MapFeature& feat, const unsigned int /*version*/) { +void serialize(Archive& ar, lanelet::ml_converter::MapFeature& feat, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_NVP(feat.initialized_); ar& BOOST_SERIALIZATION_NVP(feat.wasCut_); ar& BOOST_SERIALIZATION_NVP(feat.valid_); @@ -59,14 +59,14 @@ void serialize(Archive& ar, lanelet::map_learning::MapFeature& feat, const unsig } template -void serialize(Archive& ar, lanelet::map_learning::LineStringFeature& feat, const unsigned int /*version*/) { - ar& make_nvp("MapFeature", boost::serialization::base_object(feat)); +void serialize(Archive& ar, lanelet::ml_converter::LineStringFeature& feat, const unsigned int /*version*/) { + ar& make_nvp("MapFeature", boost::serialization::base_object(feat)); ar& BOOST_SERIALIZATION_NVP(feat.rawFeature_); } template -void serialize(Archive& ar, lanelet::map_learning::LaneLineStringFeature& feat, const unsigned int /*version*/) { - ar& make_nvp("LineStringFeature", boost::serialization::base_object(feat)); +void serialize(Archive& ar, lanelet::ml_converter::LaneLineStringFeature& feat, const unsigned int /*version*/) { + ar& make_nvp("LineStringFeature", boost::serialization::base_object(feat)); ar& BOOST_SERIALIZATION_NVP(feat.cutFeatures_); ar& BOOST_SERIALIZATION_NVP(feat.cutAndResampledFeatures_); ar& BOOST_SERIALIZATION_NVP(feat.cutResampledAndTransformedFeatures_); @@ -76,10 +76,10 @@ void serialize(Archive& ar, lanelet::map_learning::LaneLineStringFeature& feat, } template -void serialize(Archive& ar, lanelet::map_learning::CompoundLaneLineStringFeature& feat, +void serialize(Archive& ar, lanelet::ml_converter::CompoundLaneLineStringFeature& feat, const unsigned int /*version*/) { ar& make_nvp("LaneLineStringFeature", - boost::serialization::base_object(feat)); + boost::serialization::base_object(feat)); ar& BOOST_SERIALIZATION_NVP(feat.individualFeatures_); ar& BOOST_SERIALIZATION_NVP(feat.pathLengthsRaw_); ar& BOOST_SERIALIZATION_NVP(feat.pathLengthsProcessed_); @@ -88,8 +88,8 @@ void serialize(Archive& ar, lanelet::map_learning::CompoundLaneLineStringFeature } template -void serialize(Archive& ar, lanelet::map_learning::LaneletFeature& feat, const unsigned int /*version*/) { - ar& make_nvp("MapFeature", boost::serialization::base_object(feat)); +void serialize(Archive& ar, lanelet::ml_converter::LaneletFeature& feat, const unsigned int /*version*/) { + ar& make_nvp("MapFeature", boost::serialization::base_object(feat)); ar& BOOST_SERIALIZATION_NVP(feat.leftBoundary_); ar& BOOST_SERIALIZATION_NVP(feat.rightBoundary_); ar& BOOST_SERIALIZATION_NVP(feat.centerline_); @@ -97,13 +97,13 @@ void serialize(Archive& ar, lanelet::map_learning::LaneletFeature& feat, const u } template -void serialize(Archive& ar, lanelet::map_learning::Edge& edge, const unsigned int /*version*/) { +void serialize(Archive& ar, lanelet::ml_converter::Edge& edge, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_NVP(edge.el1_); ar& BOOST_SERIALIZATION_NVP(edge.el2_); } template -void serialize(Archive& ar, lanelet::map_learning::LaneData& lData, const unsigned int /*version*/) { +void serialize(Archive& ar, lanelet::ml_converter::LaneData& lData, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_NVP(lData.roadBorders_); ar& BOOST_SERIALIZATION_NVP(lData.laneDividers_); ar& BOOST_SERIALIZATION_NVP(lData.compoundRoadBorders_); @@ -120,23 +120,23 @@ void serialize(Archive& ar, lanelet::map_learning::LaneData& lData, const unsign } // namespace boost // prevent unneccessary boost serialization xml tags -// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LaneData, boost::serialization::object_serializable); -// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::Edge, boost::serialization::object_serializable); -// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LaneletFeature, boost::serialization::object_serializable); -// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LaneLineStringFeature, boost::serialization::object_serializable); -// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::CompoundLaneLineStringFeature, +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::LaneData, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::Edge, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::LaneletFeature, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::LaneLineStringFeature, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::CompoundLaneLineStringFeature, // boost::serialization::object_serializable); -// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LineStringFeature, boost::serialization::object_serializable); -// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::MapFeature, boost::serialization::object_serializable); -// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LineStringType, boost::serialization::object_serializable); -// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::ParametrizationType, boost::serialization::object_serializable); -// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LaneletRepresentationType, +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::LineStringFeature, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::MapFeature, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::LineStringType, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::ParametrizationType, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::LaneletRepresentationType, // boost::serialization::object_serializable); // -// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LaneLineStringFeatures, boost::serialization::object_serializable); -// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LaneLineStringFeatureList, +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::LaneLineStringFeatures, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::LaneLineStringFeatureList, // boost::serialization::object_serializable); -// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::CompoundLaneLineStringFeatureList, +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::CompoundLaneLineStringFeatureList, // boost::serialization::object_serializable); -// BOOST_CLASS_IMPLEMENTATION(lanelet::map_learning::LaneletFeatures, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::LaneletFeatures, boost::serialization::object_serializable); // BOOST_CLASS_IMPLEMENTATION(lanelet::BasicLineString3d, boost::serialization::object_serializable); diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h b/lanelet2_ml_converter/include/lanelet2_ml_converter/Types.h similarity index 91% rename from lanelet2_map_learning/include/lanelet2_map_learning/Types.h rename to lanelet2_ml_converter/include/lanelet2_ml_converter/Types.h index 0c95cb11..ed91db15 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Types.h +++ b/lanelet2_ml_converter/include/lanelet2_ml_converter/Types.h @@ -6,10 +6,10 @@ #include #include -#include "lanelet2_map_learning/Forward.h" +#include "lanelet2_ml_converter/Forward.h" namespace lanelet { -namespace map_learning { +namespace ml_converter { enum class LaneletRepresentationType { Centerline, Boundaries }; enum class ParametrizationType { Bezier, BezierEndpointFixed, LineString }; @@ -38,5 +38,5 @@ struct OrientedRect { OrientedRect() noexcept {} }; -} // namespace map_learning +} // namespace ml_converter } // namespace lanelet diff --git a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h b/lanelet2_ml_converter/include/lanelet2_ml_converter/Utils.h similarity index 98% rename from lanelet2_map_learning/include/lanelet2_map_learning/Utils.h rename to lanelet2_ml_converter/include/lanelet2_ml_converter/Utils.h index 5644e772..c66943aa 100644 --- a/lanelet2_map_learning/include/lanelet2_map_learning/Utils.h +++ b/lanelet2_ml_converter/include/lanelet2_ml_converter/Utils.h @@ -11,7 +11,7 @@ #include "Serialize.h" #include "Types.h" namespace lanelet { -namespace map_learning { +namespace ml_converter { OrientedRect getRotatedRect(const BasicPoint2d& center, double extentLongitudinal, double extentLateral, double yaw); @@ -80,5 +80,5 @@ void saveLaneDataMultiFile(const std::string& path, const std::vector loadLaneDataMultiFile(const std::string& path, const std::vector& filenames, bool binary); // loads from one file per LaneData -} // namespace map_learning +} // namespace ml_converter } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/package.xml b/lanelet2_ml_converter/package.xml similarity index 87% rename from lanelet2_map_learning/package.xml rename to lanelet2_ml_converter/package.xml index df18b481..fbddc3f6 100644 --- a/lanelet2_map_learning/package.xml +++ b/lanelet2_ml_converter/package.xml @@ -1,9 +1,9 @@ - lanelet2_map_learning + lanelet2_ml_converter 1.2.1 - Map learning module for lanelet2 + Converter module to convert Lanelet2 maps into local label instances for machine learning tasks BSD Fabian Immel diff --git a/lanelet2_map_learning/src/.gitignore b/lanelet2_ml_converter/src/.gitignore similarity index 100% rename from lanelet2_map_learning/src/.gitignore rename to lanelet2_ml_converter/src/.gitignore diff --git a/lanelet2_map_learning/src/MapData.cpp b/lanelet2_ml_converter/src/MapData.cpp similarity index 99% rename from lanelet2_map_learning/src/MapData.cpp rename to lanelet2_ml_converter/src/MapData.cpp index 50282b74..76d554b6 100644 --- a/lanelet2_map_learning/src/MapData.cpp +++ b/lanelet2_ml_converter/src/MapData.cpp @@ -1,9 +1,9 @@ -#include "lanelet2_map_learning/MapData.h" +#include "lanelet2_ml_converter/MapData.h" -#include "lanelet2_map_learning/Utils.h" +#include "lanelet2_ml_converter/Utils.h" namespace lanelet { -namespace map_learning { +namespace ml_converter { using namespace internal; @@ -414,5 +414,5 @@ CompoundLaneLineStringFeaturePtr LaneData::TensorFeatureData::pointMatrixCpdCent return pointMatrixCpdFeat(index, pointMatrixCpdCenterline_); } -} // namespace map_learning +} // namespace ml_converter } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapDataInterface.cpp b/lanelet2_ml_converter/src/MapDataInterface.cpp similarity index 95% rename from lanelet2_map_learning/src/MapDataInterface.cpp rename to lanelet2_ml_converter/src/MapDataInterface.cpp index a1197a32..7bc8f621 100644 --- a/lanelet2_map_learning/src/MapDataInterface.cpp +++ b/lanelet2_ml_converter/src/MapDataInterface.cpp @@ -1,4 +1,4 @@ -#include "lanelet2_map_learning/MapDataInterface.h" +#include "lanelet2_ml_converter/MapDataInterface.h" #include #include @@ -9,11 +9,11 @@ #include #include -#include "lanelet2_map_learning/MapData.h" -#include "lanelet2_map_learning/Utils.h" +#include "lanelet2_ml_converter/MapData.h" +#include "lanelet2_ml_converter/Utils.h" namespace lanelet { -namespace map_learning { +namespace ml_converter { void MapDataInterface::setCurrPosAndExtractSubmap(const BasicPoint2d& pt, double yaw) { currPos_ = pt; @@ -94,5 +94,5 @@ TEData MapDataInterface::teData(bool processAll) { return TEData(); } -} // namespace map_learning +} // namespace ml_converter } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/MapFeatures.cpp b/lanelet2_ml_converter/src/MapFeatures.cpp similarity index 98% rename from lanelet2_map_learning/src/MapFeatures.cpp rename to lanelet2_ml_converter/src/MapFeatures.cpp index f6f8abaa..4bdc488c 100644 --- a/lanelet2_map_learning/src/MapFeatures.cpp +++ b/lanelet2_ml_converter/src/MapFeatures.cpp @@ -1,12 +1,12 @@ -#include "lanelet2_map_learning/MapFeatures.h" +#include "lanelet2_ml_converter/MapFeatures.h" #include #include -#include "lanelet2_map_learning/Utils.h" +#include "lanelet2_ml_converter/Utils.h" namespace lanelet { -namespace map_learning { +namespace ml_converter { struct LStringProcessResult { BasicLineStrings3d cutFeatures; @@ -276,5 +276,5 @@ VectorXd toFeatureVector(const BasicLineString3d& line, int typeInt, bool onlyPo } } -} // namespace map_learning +} // namespace ml_converter } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/src/Utils.cpp b/lanelet2_ml_converter/src/Utils.cpp similarity index 98% rename from lanelet2_map_learning/src/Utils.cpp rename to lanelet2_ml_converter/src/Utils.cpp index 28b8e8c0..92d02ea4 100644 --- a/lanelet2_map_learning/src/Utils.cpp +++ b/lanelet2_ml_converter/src/Utils.cpp @@ -1,4 +1,4 @@ -#include "lanelet2_map_learning/Utils.h" +#include "lanelet2_ml_converter/Utils.h" #include #include @@ -11,7 +11,7 @@ namespace fs = boost::filesystem; namespace lanelet { -namespace map_learning { +namespace ml_converter { OrientedRect getRotatedRect(const BasicPoint2d& center, double extentLongitudinal, double extentLateral, double yaw) { BasicPoints2d pts{BasicPoint2d{center.x() - extentLongitudinal, center.y() - extentLateral}, @@ -229,5 +229,5 @@ std::vector loadLaneDataMultiFile(const std::string& path, const st } return lDataVec; } -} // namespace map_learning +} // namespace ml_converter } // namespace lanelet \ No newline at end of file diff --git a/lanelet2_map_learning/test/LaneletTestMap.xml b/lanelet2_ml_converter/test/LaneletTestMap.xml similarity index 100% rename from lanelet2_map_learning/test/LaneletTestMap.xml rename to lanelet2_ml_converter/test/LaneletTestMap.xml diff --git a/lanelet2_map_learning/test/test_map.h b/lanelet2_ml_converter/test/test_map.h similarity index 98% rename from lanelet2_map_learning/test/test_map.h rename to lanelet2_ml_converter/test/test_map.h index b39de3b5..e7f6bebd 100644 --- a/lanelet2_map_learning/test/test_map.h +++ b/lanelet2_ml_converter/test/test_map.h @@ -7,13 +7,13 @@ #include #include -#include "lanelet2_map_learning/Forward.h" -#include "lanelet2_map_learning/Utils.h" +#include "lanelet2_ml_converter/Forward.h" +#include "lanelet2_ml_converter/Utils.h" /// The coordinates and relations for this test can be found in "LaneletTestMap.xml" which can be viewed in /// https://www.draw.io namespace lanelet { -namespace map_learning { +namespace ml_converter { namespace tests { class MapTestData { @@ -186,5 +186,5 @@ class MapLearningTest : public ::testing::Test { }; } // namespace tests -} // namespace map_learning +} // namespace ml_converter } // namespace lanelet diff --git a/lanelet2_map_learning/test/test_map_data.cpp b/lanelet2_ml_converter/test/test_map_data.cpp similarity index 96% rename from lanelet2_map_learning/test/test_map_data.cpp rename to lanelet2_ml_converter/test/test_map_data.cpp index d68ce945..b3a77a58 100644 --- a/lanelet2_map_learning/test/test_map_data.cpp +++ b/lanelet2_ml_converter/test/test_map_data.cpp @@ -2,14 +2,14 @@ #include // #include -#include "lanelet2_map_learning/MapData.h" -#include "lanelet2_map_learning/MapFeatures.h" +#include "lanelet2_ml_converter/MapData.h" +#include "lanelet2_ml_converter/MapFeatures.h" #include "lanelet2_routing/RoutingGraph.h" #include "test_map.h" using namespace lanelet; -using namespace lanelet::map_learning; -using namespace lanelet::map_learning::tests; +using namespace lanelet::ml_converter; +using namespace lanelet::ml_converter::tests; TEST_F(MapLearningTest, LaneData) { // NOLINT traffic_rules::TrafficRulesPtr trafficRules{ diff --git a/lanelet2_map_learning/test/test_map_data_interface.cpp b/lanelet2_ml_converter/test/test_map_data_interface.cpp similarity index 96% rename from lanelet2_map_learning/test/test_map_data_interface.cpp rename to lanelet2_ml_converter/test/test_map_data_interface.cpp index e22476a5..24634e5f 100644 --- a/lanelet2_map_learning/test/test_map_data_interface.cpp +++ b/lanelet2_ml_converter/test/test_map_data_interface.cpp @@ -3,16 +3,16 @@ #include -#include "lanelet2_map_learning/MapData.h" -#include "lanelet2_map_learning/MapDataInterface.h" -#include "lanelet2_map_learning/MapFeatures.h" +#include "lanelet2_ml_converter/MapData.h" +#include "lanelet2_ml_converter/MapDataInterface.h" +#include "lanelet2_ml_converter/MapFeatures.h" #include "lanelet2_routing/RoutingGraph.h" #include "lanelet2_traffic_rules/TrafficRulesFactory.h" #include "test_map.h" using namespace lanelet; -using namespace lanelet::map_learning; -using namespace lanelet::map_learning::tests; +using namespace lanelet::ml_converter; +using namespace lanelet::ml_converter::tests; // template diff --git a/lanelet2_map_learning/test/test_map_features.cpp b/lanelet2_ml_converter/test/test_map_features.cpp similarity index 97% rename from lanelet2_map_learning/test/test_map_features.cpp rename to lanelet2_ml_converter/test/test_map_features.cpp index 44e83e32..380a62c9 100644 --- a/lanelet2_map_learning/test/test_map_features.cpp +++ b/lanelet2_ml_converter/test/test_map_features.cpp @@ -1,11 +1,11 @@ #include -#include "lanelet2_map_learning/MapFeatures.h" +#include "lanelet2_ml_converter/MapFeatures.h" #include "test_map.h" using namespace lanelet; -using namespace lanelet::map_learning; -using namespace lanelet::map_learning::tests; +using namespace lanelet::ml_converter; +using namespace lanelet::ml_converter::tests; TEST_F(MapLearningTest, LaneLineStringFeature) { // NOLINT BasicLineString3d polyline{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}, diff --git a/lanelet2_map_learning/test/test_utils.cpp b/lanelet2_ml_converter/test/test_utils.cpp similarity index 94% rename from lanelet2_map_learning/test/test_utils.cpp rename to lanelet2_ml_converter/test/test_utils.cpp index 1ea92987..df41d094 100644 --- a/lanelet2_map_learning/test/test_utils.cpp +++ b/lanelet2_ml_converter/test/test_utils.cpp @@ -1,11 +1,11 @@ #include -#include "lanelet2_map_learning/Utils.h" +#include "lanelet2_ml_converter/Utils.h" #include "test_map.h" using namespace lanelet; -using namespace lanelet::map_learning; -using namespace lanelet::map_learning::tests; +using namespace lanelet::ml_converter; +using namespace lanelet::ml_converter::tests; TEST_F(MapLearningTest, GetRotatedRect) { // NOLINT EXPECT_DOUBLE_EQ(bbox.bounds_const()[0].x(), 15); diff --git a/lanelet2_python/package.xml b/lanelet2_python/package.xml index 2b4f4f7c..bdea1906 100644 --- a/lanelet2_python/package.xml +++ b/lanelet2_python/package.xml @@ -18,7 +18,7 @@ lanelet2_core lanelet2_io lanelet2_routing - lanelet2_map_learning + lanelet2_ml_converter lanelet2_traffic_rules lanelet2_projection lanelet2_matching diff --git a/lanelet2_python/python_api/map_learning.cpp b/lanelet2_python/python_api/ml_converter.cpp similarity index 98% rename from lanelet2_python/python_api/map_learning.cpp rename to lanelet2_python/python_api/ml_converter.cpp index 9ee7e25e..3f5208d7 100644 --- a/lanelet2_python/python_api/map_learning.cpp +++ b/lanelet2_python/python_api/ml_converter.cpp @@ -3,16 +3,16 @@ #include #include -#include "lanelet2_map_learning/MapData.h" -#include "lanelet2_map_learning/MapDataInterface.h" -#include "lanelet2_map_learning/MapFeatures.h" -#include "lanelet2_map_learning/Utils.h" +#include "lanelet2_ml_converter/MapData.h" +#include "lanelet2_ml_converter/MapDataInterface.h" +#include "lanelet2_ml_converter/MapFeatures.h" +#include "lanelet2_ml_converter/Utils.h" #include "lanelet2_python/internal/converter.h" #include "lanelet2_python/internal/eigen_converter.h" using namespace boost::python; using namespace lanelet; -using namespace lanelet::map_learning; +using namespace lanelet::ml_converter; /// TODO: ADD VIRTUAL FUNCTIONS, CONSTRUCTORS, EIGEN TO NUMPY /// TODO: DEBUG EIGEN SEGFAULTS FROM PYTHON diff --git a/lanelet2_python/test/test_lanelet2_map_learning.py b/lanelet2_python/test/test_lanelet2_map_learning.py index 5413de28..4fa25253 100644 --- a/lanelet2_python/test/test_lanelet2_map_learning.py +++ b/lanelet2_python/test/test_lanelet2_map_learning.py @@ -1,7 +1,7 @@ import unittest import lanelet2 # if we fail here, there is something wrong with lanelet2 registration from lanelet2.core import getId, Point3d, BasicPoint2d, LineString3d, Lanelet, LaneletMap -from lanelet2.map_learning import MapDataInterface +from lanelet2.ml_converter import MapDataInterface def get_sample_lanelet_map(): mymap = LaneletMap() From d255929e572b21b67fddee72da75c4f3e92555f9 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Mon, 29 Jan 2024 16:58:38 +0100 Subject: [PATCH 53/64] remove seq to support older eigen --- conanfile.py | 1 + lanelet2_ml_converter/src/MapFeatures.cpp | 26 +++++++++++------------ 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/conanfile.py b/conanfile.py index 0160043b..937c91d1 100644 --- a/conanfile.py +++ b/conanfile.py @@ -85,6 +85,7 @@ class Lanelet2Conan(ConanFile): 'lanelet2_core', 'lanelet2_io', 'lanelet2_matching', + 'lanelet2_ml_converter', 'lanelet2_projection', 'lanelet2_traffic_rules', 'lanelet2_routing', diff --git a/lanelet2_ml_converter/src/MapFeatures.cpp b/lanelet2_ml_converter/src/MapFeatures.cpp index 4bdc488c..38f26d23 100644 --- a/lanelet2_ml_converter/src/MapFeatures.cpp +++ b/lanelet2_ml_converter/src/MapFeatures.cpp @@ -79,16 +79,16 @@ std::vector TEFeature::computeFeatureVectors(bool onlyPoints, bool poi : VectorXd(3 * rawFeature_.size() + 1); // n points with 2/3 dims + type if (pointsIn2d == true) { for (size_t i = 0; i < rawFeature_.size(); i++) { - vec(Eigen::seq(2 * i, 2 * i + 1)) = rawFeature_[i](Eigen::seq(0, 1)); + vec.segment(2 * i, 2) = rawFeature_[i].segment(0, 2); } } else { for (size_t i = 0; i < rawFeature_.size(); i++) { - vec(Eigen::seq(3 * i, 3 * i + 2)) = rawFeature_[i](Eigen::seq(0, 2)); + vec.segment(3 * i, 3) = rawFeature_[i].segment(0, 3); } } vec[vec.size() - 1] = static_cast(teType_); if (onlyPoints) { - return std::vector{vec(Eigen::seq(0, vec.size() - 2))}; + return std::vector{vec.segment(0, vec.size() - 1)}; } else { return std::vector{vec}; } @@ -142,7 +142,7 @@ VectorXd stackVector(const std::vector& vec) { VectorXd stacked(flattenedLength); size_t currIndex = 0; for (const auto& el : vec) { - stacked(Eigen::seq(currIndex, currIndex + el.size())) = el; + stacked.segment(currIndex, el.size()) = el; } return stacked; } @@ -156,7 +156,7 @@ std::vector LaneletFeature::computeFeatureVectors(bool onlyPoints, boo std::vector featureVecs(vecCenterlinePts.size()); for (size_t i = 0; i < vecCenterlinePts.size(); i++) { VectorXd vec(vecCenterlinePts[i].size() + 2); // pts vec + left and right type - vec(Eigen::seq(0, vecCenterlinePts.size() - 1)) = vecCenterlinePts[i]; + vec.segment(0, vecCenterlinePts.size()) = vecCenterlinePts[i]; vec[vec.size() - 2] = leftBoundary_->typeInt(); vec[vec.size() - 1] = rightBoundary_->typeInt(); featureVecs[i] = vec; @@ -169,13 +169,13 @@ std::vector LaneletFeature::computeFeatureVectors(bool onlyPoints, boo VectorXd rightBdPts = stackVector(vecRightBdPts); VectorXd vec(leftBdPts.size() + rightBdPts.size() + 2); // pts vec + left and right type - vec(Eigen::seq(0, leftBdPts.size() - 1)) = leftBdPts; - vec(Eigen::seq(leftBdPts.size(), leftBdPts.size() + rightBdPts.size() - 1)) = rightBdPts; + vec.segment(0, leftBdPts.size()) = leftBdPts; + vec.segment(leftBdPts.size(), rightBdPts.size()) = rightBdPts; vec[vec.size() - 2] = leftBoundary_->typeInt(); vec[vec.size() - 1] = rightBoundary_->typeInt(); if (onlyPoints) { - return std::vector{vec(Eigen::seq(0, vec.size() - 2))}; + return std::vector{vec.segment(0, vec.size() - 2)}; } else { return std::vector{vec}; } @@ -246,11 +246,11 @@ MatrixXd toPointMatrix(const BasicLineString3d& lString, bool pointsIn2d) { pointsIn2d ? MatrixXd(lString.size(), 2) : MatrixXd(lString.size(), 3); // n points with 2/3 dims + type if (pointsIn2d == true) { for (size_t i = 0; i < lString.size(); i++) { - mat.row(i) = lString[i](Eigen::seq(0, 1)); + mat.row(i) = lString[i].segment(0, 2); } } else { for (size_t i = 0; i < lString.size(); i++) { - mat.row(i) = lString[i](Eigen::seq(0, 2)); + mat.row(i) = lString[i].segment(0, 3); } } return mat; @@ -261,16 +261,16 @@ VectorXd toFeatureVector(const BasicLineString3d& line, int typeInt, bool onlyPo pointsIn2d ? VectorXd(2 * line.size() + 1) : VectorXd(3 * line.size() + 1); // n points with 2/3 dims + type if (pointsIn2d == true) { for (size_t i = 0; i < line.size(); i++) { - vec(Eigen::seq(2 * i, 2 * i + 1)) = line[i](Eigen::seq(0, 1)); + vec.segment(2 * i, 2) = line[i].segment(0, 2); } } else { for (size_t i = 0; i < line.size(); i++) { - vec(Eigen::seq(3 * i, 3 * i + 2)) = line[i](Eigen::seq(0, 2)); + vec.segment(3 * i, 3) = line[i].segment(0, 3); } } vec[vec.size() - 1] = typeInt; if (onlyPoints) { - return vec(Eigen::seq(0, vec.size() - 2)); + return vec.segment(0, vec.size() - 1); } else { return vec; } From b0d70a6c7c79ceeafda0eda08f5588d6bbbe4ea2 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Mon, 29 Jan 2024 17:26:49 +0100 Subject: [PATCH 54/64] add missing virtual placeholder --- lanelet2_ml_converter/src/MapFeatures.cpp | 4 ++++ lanelet2_ml_converter/test/test_map.h | 4 ++-- lanelet2_ml_converter/test/test_map_data.cpp | 2 +- lanelet2_ml_converter/test/test_map_data_interface.cpp | 4 ++-- lanelet2_ml_converter/test/test_map_features.cpp | 6 +++--- lanelet2_ml_converter/test/test_utils.cpp | 6 +++--- lanelet2_python/python_api/ml_converter.cpp | 3 --- 7 files changed, 15 insertions(+), 14 deletions(-) diff --git a/lanelet2_ml_converter/src/MapFeatures.cpp b/lanelet2_ml_converter/src/MapFeatures.cpp index 38f26d23..f5626ae0 100644 --- a/lanelet2_ml_converter/src/MapFeatures.cpp +++ b/lanelet2_ml_converter/src/MapFeatures.cpp @@ -106,6 +106,10 @@ std::vector TEFeature::pointMatrices(bool pointsIn2d) const { return std::vector{toPointMatrix(rawFeature_, pointsIn2d)}; } +bool TEFeature::process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) { + throw std::runtime_error("Not implemented yet!"); +} + LaneletFeature::LaneletFeature(LaneLineStringFeaturePtr leftBoundary, LaneLineStringFeaturePtr rightBoundary, LaneLineStringFeaturePtr centerline, Id mapID) : MapFeature(mapID), leftBoundary_{leftBoundary}, rightBoundary_{rightBoundary}, centerline_{centerline} {} diff --git a/lanelet2_ml_converter/test/test_map.h b/lanelet2_ml_converter/test/test_map.h index e7f6bebd..065ef97d 100644 --- a/lanelet2_ml_converter/test/test_map.h +++ b/lanelet2_ml_converter/test/test_map.h @@ -171,7 +171,7 @@ namespace { // NOLINT static MapTestData testData; // NOLINT } // namespace -class MapLearningTest : public ::testing::Test { +class MLConverterTest : public ::testing::Test { public: const std::unordered_map& lanelets{testData.lanelets}; const std::unordered_map& points{testData.points}; @@ -182,7 +182,7 @@ class MapLearningTest : public ::testing::Test { const double extentLateralBbox{10}; const double yawBbox{M_PI / 2.0}; OrientedRect bbox; - MapLearningTest() : bbox{getRotatedRect(centerBbox, extentLongitudinalBbox, extentLateralBbox, yawBbox)} {} + MLConverterTest() : bbox{getRotatedRect(centerBbox, extentLongitudinalBbox, extentLateralBbox, yawBbox)} {} }; } // namespace tests diff --git a/lanelet2_ml_converter/test/test_map_data.cpp b/lanelet2_ml_converter/test/test_map_data.cpp index b3a77a58..c87abb5a 100644 --- a/lanelet2_ml_converter/test/test_map_data.cpp +++ b/lanelet2_ml_converter/test/test_map_data.cpp @@ -11,7 +11,7 @@ using namespace lanelet; using namespace lanelet::ml_converter; using namespace lanelet::ml_converter::tests; -TEST_F(MapLearningTest, LaneData) { // NOLINT +TEST_F(MLConverterTest, LaneData) { // NOLINT traffic_rules::TrafficRulesPtr trafficRules{ traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)}; routing::RoutingGraphConstPtr laneletMapGraph = routing::RoutingGraph::build(*laneletMap, *trafficRules); diff --git a/lanelet2_ml_converter/test/test_map_data_interface.cpp b/lanelet2_ml_converter/test/test_map_data_interface.cpp index 24634e5f..c7afbe9d 100644 --- a/lanelet2_ml_converter/test/test_map_data_interface.cpp +++ b/lanelet2_ml_converter/test/test_map_data_interface.cpp @@ -20,7 +20,7 @@ using namespace lanelet::ml_converter::tests; // return std::chrono::duration_cast(clock_t::now() - start); // } -TEST_F(MapLearningTest, MapDataInterface) { // NOLINT +TEST_F(MLConverterTest, MapDataInterface) { // NOLINT traffic_rules::TrafficRulesPtr trafficRules{ traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)}; MapDataInterface::Configuration config{}; @@ -103,7 +103,7 @@ TEST_F(MapLearningTest, MapDataInterface) { // NOLINT } } -TEST_F(MapLearningTest, MapDataSaveLoad) { +TEST_F(MLConverterTest, MapDataSaveLoad) { traffic_rules::TrafficRulesPtr trafficRules{ traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)}; MapDataInterface::Configuration config{}; diff --git a/lanelet2_ml_converter/test/test_map_features.cpp b/lanelet2_ml_converter/test/test_map_features.cpp index 380a62c9..982e73b1 100644 --- a/lanelet2_ml_converter/test/test_map_features.cpp +++ b/lanelet2_ml_converter/test/test_map_features.cpp @@ -7,7 +7,7 @@ using namespace lanelet; using namespace lanelet::ml_converter; using namespace lanelet::ml_converter::tests; -TEST_F(MapLearningTest, LaneLineStringFeature) { // NOLINT +TEST_F(MLConverterTest, LaneLineStringFeature) { // NOLINT BasicLineString3d polyline{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}, BasicPoint3d{20, 0, 0}}; LaneLineStringFeature feat(polyline, Id(123), LineStringType::Solid, Ids(1234), false); @@ -36,7 +36,7 @@ TEST_F(MapLearningTest, LaneLineStringFeature) { // NOLINT EXPECT_NEAR(pointMat[0](3, 1), -10, 10e-5); } -TEST_F(MapLearningTest, LaneletFeature) { // NOLINT +TEST_F(MLConverterTest, LaneletFeature) { // NOLINT BasicLineString3d leftBd{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}, BasicPoint3d{20, 0, 0}}; LaneLineStringFeaturePtr leftBdFeat = @@ -68,7 +68,7 @@ TEST_F(MapLearningTest, LaneletFeature) { // NOLINT EXPECT_EQ(vec[0][17], 1); } -TEST_F(MapLearningTest, CompoundLaneLineStringFeature) { // NOLINT +TEST_F(MLConverterTest, CompoundLaneLineStringFeature) { // NOLINT BasicLineString3d p1{BasicPoint3d{-10, 0, 0}, BasicPoint3d{-5, 0, 0}}; LaneLineStringFeaturePtr feat1 = std::make_shared(p1, Id(123), LineStringType::Solid, Ids(1234), false); diff --git a/lanelet2_ml_converter/test/test_utils.cpp b/lanelet2_ml_converter/test/test_utils.cpp index df41d094..3fffdb17 100644 --- a/lanelet2_ml_converter/test/test_utils.cpp +++ b/lanelet2_ml_converter/test/test_utils.cpp @@ -7,7 +7,7 @@ using namespace lanelet; using namespace lanelet::ml_converter; using namespace lanelet::ml_converter::tests; -TEST_F(MapLearningTest, GetRotatedRect) { // NOLINT +TEST_F(MLConverterTest, GetRotatedRect) { // NOLINT EXPECT_DOUBLE_EQ(bbox.bounds_const()[0].x(), 15); EXPECT_DOUBLE_EQ(bbox.bounds_const()[0].y(), -10); EXPECT_DOUBLE_EQ(bbox.bounds_const()[1].x(), -5); @@ -18,7 +18,7 @@ TEST_F(MapLearningTest, GetRotatedRect) { // NOLINT EXPECT_DOUBLE_EQ(bbox.bounds_const()[3].y(), 20); } -TEST_F(MapLearningTest, ExtractSubmap) { // NOLINT +TEST_F(MLConverterTest, ExtractSubmap) { // NOLINT LaneletSubmapConstPtr submap = extractSubmap(laneletMap, centerBbox, extentLongitudinalBbox, extentLateralBbox); EXPECT_TRUE(submap->laneletLayer.exists(2000)); EXPECT_TRUE(submap->laneletLayer.exists(2001)); @@ -41,7 +41,7 @@ TEST(UtilsTest, ResampleLineString) { // NOLINT EXPECT_NEAR(polylineResampled[10].x(), 15, 10e-5); } -TEST_F(MapLearningTest, CutLineString) { // NOLINT +TEST_F(MLConverterTest, CutLineString) { // NOLINT BasicLineString3d polyline{BasicPoint3d{0, 0, 0}, BasicPoint3d{30, 0, 0}, BasicPoint3d{40, 0, 0}, BasicPoint3d{50, 0, 0}}; std::vector polylineCut = cutLineString(bbox, polyline); diff --git a/lanelet2_python/python_api/ml_converter.cpp b/lanelet2_python/python_api/ml_converter.cpp index 3f5208d7..6493fdad 100644 --- a/lanelet2_python/python_api/ml_converter.cpp +++ b/lanelet2_python/python_api/ml_converter.cpp @@ -14,9 +14,6 @@ using namespace boost::python; using namespace lanelet; using namespace lanelet::ml_converter; -/// TODO: ADD VIRTUAL FUNCTIONS, CONSTRUCTORS, EIGEN TO NUMPY -/// TODO: DEBUG EIGEN SEGFAULTS FROM PYTHON - class MapFeatureWrap : public MapFeature, public wrapper { public: std::vector computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { From f271a5ddc1314bd038c81df52537fa69c14b16cf Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Mon, 29 Jan 2024 17:37:40 +0100 Subject: [PATCH 55/64] add module to lint --- .github/lint.bash | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/lint.bash b/.github/lint.bash index c75c3adc..6030dc53 100755 --- a/.github/lint.bash +++ b/.github/lint.bash @@ -25,7 +25,7 @@ export AMENT_PREFIX_PATH=${LANELET2_ROOT} CMAKE_ROOT=${LANELET2_ROOT}/CMakeLists.txt echo "cmake_minimum_required(VERSION 3.12)" > ${CMAKE_ROOT} echo "project(lanelet2)" >> ${CMAKE_ROOT} -export LANELET2_PACKAGES_TOPOLOGICAL="lanelet2_core lanelet2_io lanelet2_projection lanelet2_traffic_rules lanelet2_routing lanelet2_maps lanelet2_validation lanelet2_matching lanelet2_python lanelet2_examples" +export LANELET2_PACKAGES_TOPOLOGICAL="lanelet2_core lanelet2_io lanelet2_projection lanelet2_traffic_rules lanelet2_routing lanelet2_maps lanelet2_validation lanelet2_matching lanelet2_ml_converter lanelet2_python lanelet2_examples" for pkg in $LANELET2_PACKAGES_TOPOLOGICAL; do echo "add_subdirectory($pkg)" >> ${CMAKE_ROOT}; echo "set(${pkg}_LIBRARIES $pkg)" >> ${CMAKE_ROOT}; From df0d71de1f6f013aebb6d5814b9a379f2613b87e Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Mon, 29 Jan 2024 18:20:22 +0100 Subject: [PATCH 56/64] add missing commands to conanfile --- conanfile.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/conanfile.py b/conanfile.py index 937c91d1..5fee95c0 100644 --- a/conanfile.py +++ b/conanfile.py @@ -38,6 +38,7 @@ add_subdirectory(lanelet2_python) add_subdirectory(lanelet2_maps) add_subdirectory(lanelet2_matching) +add_subdirectory(lanelet2_ml_converter) # declare dependencies target_link_libraries(lanelet2_io PUBLIC lanelet2_core) target_link_libraries(lanelet2_projection PUBLIC lanelet2_core) @@ -45,6 +46,7 @@ target_link_libraries(lanelet2_routing PUBLIC lanelet2_core lanelet2_traffic_rules) target_link_libraries(lanelet2_matching PUBLIC lanelet2_core lanelet2_traffic_rules lanelet2_io lanelet2_projection lanelet2_maps) target_link_libraries(lanelet2_validation PUBLIC lanelet2_core lanelet2_io lanelet2_routing lanelet2_traffic_rules lanelet2_projection) +target_link_libraries(lanelet2_ml_converter PUBLIC lanelet2_core lanelet2_routing lanelet2_traffic_rules) target_link_libraries(lanelet2_examples_compiler_flags INTERFACE lanelet2_core lanelet2_io lanelet2_routing lanelet2_traffic_rules lanelet2_projection lanelet2_matching) target_link_libraries(lanelet2_python_compiler_flags INTERFACE lanelet2_core lanelet2_io lanelet2_routing lanelet2_traffic_rules lanelet2_projection lanelet2_matching) """ @@ -85,11 +87,11 @@ class Lanelet2Conan(ConanFile): 'lanelet2_core', 'lanelet2_io', 'lanelet2_matching', - 'lanelet2_ml_converter', 'lanelet2_projection', 'lanelet2_traffic_rules', 'lanelet2_routing', - 'lanelet2_validation' + 'lanelet2_validation', + 'lanelet2_ml_converter' ] def _configure_cmake(self): From 059cca149f937ace79dd3dcff9064493921e0cbb Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Mon, 29 Jan 2024 19:00:45 +0100 Subject: [PATCH 57/64] add missing include --- lanelet2_ml_converter/src/Utils.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/lanelet2_ml_converter/src/Utils.cpp b/lanelet2_ml_converter/src/Utils.cpp index 92d02ea4..4e124ee8 100644 --- a/lanelet2_ml_converter/src/Utils.cpp +++ b/lanelet2_ml_converter/src/Utils.cpp @@ -8,6 +8,7 @@ #include #include #include +#include namespace fs = boost::filesystem; namespace lanelet { From e0b63fccd49e4f3d40171f8f1ccc13e03dbb8777 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Tue, 30 Jan 2024 13:50:17 +0100 Subject: [PATCH 58/64] add missing conan include --- conanfile.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conanfile.py b/conanfile.py index 5fee95c0..d2503186 100644 --- a/conanfile.py +++ b/conanfile.py @@ -27,7 +27,7 @@ # declare dependencies include_directories(lanelet2_core/include lanelet2_io/include lanelet2_projection/include lanelet2_traffic_rules/include - lanelet2_routing/include lanelet2_validation/include) + lanelet2_routing/include lanelet2_validation/include lanelet2_ml_converter/include) add_subdirectory(lanelet2_core) add_subdirectory(lanelet2_io) add_subdirectory(lanelet2_projection) From 2b050fb74d7bcf67cf717ad00fb6bde276359699 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Tue, 30 Jan 2024 18:15:09 +0100 Subject: [PATCH 59/64] fix conan build --- conanfile.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/conanfile.py b/conanfile.py index d2503186..0613a6ad 100644 --- a/conanfile.py +++ b/conanfile.py @@ -47,8 +47,8 @@ target_link_libraries(lanelet2_matching PUBLIC lanelet2_core lanelet2_traffic_rules lanelet2_io lanelet2_projection lanelet2_maps) target_link_libraries(lanelet2_validation PUBLIC lanelet2_core lanelet2_io lanelet2_routing lanelet2_traffic_rules lanelet2_projection) target_link_libraries(lanelet2_ml_converter PUBLIC lanelet2_core lanelet2_routing lanelet2_traffic_rules) -target_link_libraries(lanelet2_examples_compiler_flags INTERFACE lanelet2_core lanelet2_io lanelet2_routing lanelet2_traffic_rules lanelet2_projection lanelet2_matching) -target_link_libraries(lanelet2_python_compiler_flags INTERFACE lanelet2_core lanelet2_io lanelet2_routing lanelet2_traffic_rules lanelet2_projection lanelet2_matching) +target_link_libraries(lanelet2_examples_compiler_flags INTERFACE lanelet2_core lanelet2_io lanelet2_routing lanelet2_traffic_rules lanelet2_projection lanelet2_matching lanelet2_ml_converter) +target_link_libraries(lanelet2_python_compiler_flags INTERFACE lanelet2_core lanelet2_io lanelet2_routing lanelet2_traffic_rules lanelet2_projection lanelet2_matching lanelet2_ml_converter) """ def read_version(): From e773b16a84b1b3d7f9d647461c7e7107d9371300 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Tue, 30 Jan 2024 19:07:51 +0100 Subject: [PATCH 60/64] fix python test and dockerfile --- Dockerfile | 82 ++++++++++--------- ...rning.py => test_lanelet2_ml_converter.py} | 2 +- 2 files changed, 44 insertions(+), 40 deletions(-) rename lanelet2_python/test/{test_lanelet2_map_learning.py => test_lanelet2_ml_converter.py} (97%) diff --git a/Dockerfile b/Dockerfile index 5fa3cddf..af8ecad0 100644 --- a/Dockerfile +++ b/Dockerfile @@ -16,27 +16,27 @@ SHELL ["/bin/bash", "-c"] # basics RUN set -ex; \ if [ "${ROS_DISTRO}" = "melodic" ] || [ "${ROS_DISTRO}" = "kinetic" ]; \ - then export PY_VERSION=python; \ - else export PY_VERSION=python3; \ + then export PY_VERSION=python; \ + else export PY_VERSION=python3; \ fi; \ if [ "$DEV" -ne "0" ]; then \ - export DEV_PACKAGES="clang-format-11 clang-tidy-11 clang-11 i${PY_VERSION} nano lcov"; \ + export DEV_PACKAGES="clang-format-11 clang-tidy-11 clang-11 i${PY_VERSION} nano lcov"; \ fi; \ apt-get update && \ DEBIAN_FRONTEND=noninteractive apt-get install -y \ - bash-completion \ - build-essential \ - curl \ - git \ - cmake \ - keyboard-configuration \ - locales \ - lsb-core \ - lib${PY_VERSION}-dev \ - software-properties-common \ - sudo \ - wget \ - ${DEV_PACKAGES} && \ + bash-completion \ + build-essential \ + curl \ + git \ + cmake \ + keyboard-configuration \ + locales \ + lsb-core \ + lib${PY_VERSION}-dev \ + software-properties-common \ + sudo \ + wget \ + ${DEV_PACKAGES} && \ locale-gen en_US.UTF-8 && \ apt-get clean && \ rm -rf /var/lib/apt/lists/* @@ -57,30 +57,30 @@ RUN set -ex; \ # dependencies for lanelet2 RUN if [ "${ROS_DISTRO}" = "melodic" ] || [ "${ROS_DISTRO}" = "kinetic" ]; \ - then export PY_VERSION=python; \ - else export PY_VERSION=python3; \ + then export PY_VERSION=python; \ + else export PY_VERSION=python3; \ fi; \ apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \ - libgtest-dev \ - libboost-all-dev \ - libeigen3-dev \ - libgeographic-dev \ - libpugixml-dev \ - libboost-python-dev \ - ${PY_VERSION}-rospkg \ - ros-$ROS_DISTRO-ros-environment && \ + libgtest-dev \ + libboost-all-dev \ + libeigen3-dev \ + libgeographic-dev \ + libpugixml-dev \ + libboost-python-dev \ + ${PY_VERSION}-rospkg \ + ros-$ROS_DISTRO-ros-environment && \ apt-get clean && \ rm -rf /var/lib/apt/lists/* # ros version specific dependencies RUN set -ex; \ if [ "${ROS_DISTRO}" = "melodic" ] || [ "${ROS_DISTRO}" = "kinetic" ]; \ - then export PY_VERSION=python; \ - else export PY_VERSION=python3; \ + then export PY_VERSION=python; \ + else export PY_VERSION=python3; \ fi; \ if [ "$ROS" = "ros" ]; \ - then export ROS_DEPS="ros-$ROS_DISTRO-catkin ros-$ROS_DISTRO-rosbash ${PY_VERSION}-catkin-tools"; \ - else export ROS_DEPS="ros-$ROS_DISTRO-ament-cmake python3-colcon-ros ros-$ROS_DISTRO-ros2cli"; \ + then export ROS_DEPS="ros-$ROS_DISTRO-catkin ros-$ROS_DISTRO-rosbash ${PY_VERSION}-catkin-tools"; \ + else export ROS_DEPS="ros-$ROS_DISTRO-ament-cmake python3-colcon-ros ros-$ROS_DISTRO-ros2cli"; \ fi; \ apt-get update && \ DEBIAN_FRONTEND=noninteractive apt-get install -y $ROS_DEPS && \ @@ -111,8 +111,8 @@ RUN set -ex; \ cd /home/developer/workspace && \ mkdir -p /home/developer/workspace/src && \ if [ "$ROS" = "ros" ]; then \ - source /home/developer/.bashrc && \ - catkin init; \ + source /home/developer/.bashrc && \ + catkin init; \ fi; \ git clone https://github.com/KIT-MRT/mrt_cmake_modules.git /home/developer/workspace/src/mrt_cmake_modules @@ -125,27 +125,31 @@ COPY --chown=developer:developer . /home/developer/workspace/src/lanelet2 # update dependencies RUN git -C /home/developer/workspace/src/mrt_cmake_modules pull +# install python dependencies (numpy) +RUN sudo apt install pip +RUN pip install numpy + # third stage: build FROM lanelet2_src AS lanelet2 # build RUN set -ex; \ if [ "$DEV" -ne "0" ]; then \ - export CMAKE_ARGS="-DCMAKE_BUILD_TYPE=Debug -DMRT_SANITIZER=checks -DMRT_ENABLE_COVERAGE=1"; \ + export CMAKE_ARGS="-DCMAKE_BUILD_TYPE=Debug -DMRT_SANITIZER=checks -DMRT_ENABLE_COVERAGE=1"; \ else \ - export CMAKE_ARGS="-DCMAKE_BUILD_TYPE=Release"; \ + export CMAKE_ARGS="-DCMAKE_BUILD_TYPE=Release"; \ fi; \ if [ "$ROS" = "ros" ]; then \ - export CONFIGURE="catkin config --cmake-args $CMAKE_ARGS"; \ - export BUILD_CMD="catkin build --no-status"; \ + export CONFIGURE="catkin config --cmake-args $CMAKE_ARGS"; \ + export BUILD_CMD="catkin build --no-status"; \ else \ - export CONFIGURE=true; \ - export BUILD_CMD="colcon build --symlink-install --cmake-args $CMAKE_ARGS"; \ + export CONFIGURE=true; \ + export BUILD_CMD="colcon build --symlink-install --cmake-args $CMAKE_ARGS"; \ fi; \ source /opt/ros/$ROS_DISTRO/setup.bash && \ env && \ $CONFIGURE && \ $BUILD_CMD && \ if [ "$DEV" -eq "0" ]; then \ - rm -rf build logs; \ + rm -rf build logs; \ fi \ No newline at end of file diff --git a/lanelet2_python/test/test_lanelet2_map_learning.py b/lanelet2_python/test/test_lanelet2_ml_converter.py similarity index 97% rename from lanelet2_python/test/test_lanelet2_map_learning.py rename to lanelet2_python/test/test_lanelet2_ml_converter.py index 4fa25253..e9aac30f 100644 --- a/lanelet2_python/test/test_lanelet2_map_learning.py +++ b/lanelet2_python/test/test_lanelet2_ml_converter.py @@ -23,7 +23,7 @@ def test_map_data_interface(self): pos = BasicPoint2d(1, 1) mDataIf = MapDataInterface(mymap) mDataIf.setCurrPosAndExtractSubmap(pos, 0) - lData = mDataIf.laneData() + lData = mDataIf.laneData(True) tfData = lData.getTensorFeatureData(True, False) self.assertEqual(len(tfData.compoundCenterlines), 2) self.assertEqual(tfData.compoundLaneDividerTypes[-1], 1) From 54d7712d0ffa92a30a42320c87c9c0b4235c8e5e Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Tue, 30 Jan 2024 19:36:38 +0100 Subject: [PATCH 61/64] add numpy to dockerfile --- Dockerfile | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Dockerfile b/Dockerfile index af8ecad0..83e2ab5c 100644 --- a/Dockerfile +++ b/Dockerfile @@ -67,11 +67,15 @@ RUN if [ "${ROS_DISTRO}" = "melodic" ] || [ "${ROS_DISTRO}" = "kinetic" ]; \ libgeographic-dev \ libpugixml-dev \ libboost-python-dev \ + pip \ ${PY_VERSION}-rospkg \ ros-$ROS_DISTRO-ros-environment && \ apt-get clean && \ rm -rf /var/lib/apt/lists/* +# install python dependencies (numpy) +RUN pip install numpy + # ros version specific dependencies RUN set -ex; \ if [ "${ROS_DISTRO}" = "melodic" ] || [ "${ROS_DISTRO}" = "kinetic" ]; \ @@ -125,10 +129,6 @@ COPY --chown=developer:developer . /home/developer/workspace/src/lanelet2 # update dependencies RUN git -C /home/developer/workspace/src/mrt_cmake_modules pull -# install python dependencies (numpy) -RUN sudo apt install pip -RUN pip install numpy - # third stage: build FROM lanelet2_src AS lanelet2 From ed33a0ebf4df40e008b9bbc29020b2039f87fb83 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 1 Feb 2024 19:39:56 +0100 Subject: [PATCH 62/64] add doc and readme --- README.md | 6 ++ lanelet2_ml_converter/README.md | 60 +++++++++++++++++- .../doc/summary_flowchart.png | Bin 0 -> 158452 bytes .../test/test_map_data_interface.cpp | 4 +- 4 files changed, 66 insertions(+), 4 deletions(-) create mode 100644 lanelet2_ml_converter/doc/summary_flowchart.png diff --git a/README.md b/README.md index a7049829..4229f3db 100644 --- a/README.md +++ b/README.md @@ -13,6 +13,10 @@ https://build.ros2.org/job/Hdev__lanelet2__ubuntu_jammy_amd64/lastBuild/) [![PyPI Downloads](https://img.shields.io/pypi/dm/lanelet2.svg?label=PyPI%20downloads)]( https://pypi.org/project/lanelet2/) +## New module :rocket: : `lanelet2_ml_converter` + +Convert Lanelet2 maps directly to instance labels for map perception and other map-based learning tasks! `lanelet2_ml_converter` provides local instance labels of various abstraction layers and representations accessible as numpy arrays from python. Check out the `lanelet2_ml_converter` module for more information and usage examples. + ## Overview Lanelet2 is a C++ library for handling map data in the context of automated driving. It is designed to utilize high-definition map data in order to efficiently handle the challenges posed to a vehicle in complex traffic scenarios. Flexibility and extensibility are some of the core principles to handle the upcoming challenges of future maps. @@ -30,6 +34,7 @@ Features: - **Boost Geometry** support for all thinkable kinds of geometry calculations on map primitives - Released under the [**BSD 3-Clause license**](LICENSE) - Support for **ROS1, ROS2, Docker and Conan** (see instructions below) +- Converter module to convert Lanelet2 maps into **local instance labels for machine learning tasks**. ![](lanelet2_core/doc/images/lanelet2_example_image.png) @@ -181,6 +186,7 @@ Examples and common use cases in both C++ and Python can be found [here](lanelet * **lanelet2_routing** implements the routing graph for routing or reachable set or queries as well as collision checking * **lanelet2_maps** provides example maps and functionality to visualize and modify them easily in JOSM * **lanelet2_matching** provides functions to determine in which lanelet an object is/could be currently located +* **lanelet2_ml_converter** converts Lanelet2 maps into local instance labels for machine learning tasks * **lanelet2_python** implements the python interface for lanelet2 * **lanelet2_validation** provides checks to ensure a valid lanelet2 map * **lanelet2_examples** contains tutorials for working with Lanelet2 in C++ and Python diff --git a/lanelet2_ml_converter/README.md b/lanelet2_ml_converter/README.md index 7a956019..f78ca7e5 100644 --- a/lanelet2_ml_converter/README.md +++ b/lanelet2_ml_converter/README.md @@ -1,3 +1,61 @@ # Lanelet2 ML Converter -Converter module to convert Lanelet2 maps into local label instances for machine learning tasks. \ No newline at end of file +Converter module to convert Lanelet2 maps into local instance labels for machine learning tasks. + +![](doc/summary_flowchart.png) + +## Usage Examples + +### Python + +```python +import lanelet2 +from lanelet2.core import BasicPoint2d +from lanelet2.ml_converter import MapDataInterface + +pos = BasicPoint2d(10, 10) # set your local reference frame origin +yaw = 0 # set your local reference frame yaw angle (heading) +mDataIf = MapDataInterface(ll2_map) # get the MapDataInterface object and pass the ll2 map +mDataIf.setCurrPosAndExtractSubmap(pos, yaw) # extract the local submap +lData = mDataIf.laneData(True) # get the LaneData local instance labels +tfData = lData.getTensorFeatureData(True, False) # get the local instance labels as numpy arrays +``` + +### C++ +```c++ +#include "lanelet2_ml_converter/MapData.h" +#include "lanelet2_ml_converter/MapDataInterface.h" +#include "lanelet2_ml_converter/MapFeatures.h" + +using TensorData = lanelet::ml_converter::LaneData::TensorFeatureData; + +lanelet::BasicPoint2d pos(10, 10); // set your local reference frame origin +double yaw = 0; // set your local reference frame yaw angle (heading) +MapDataInterface mDataIf(laneletMap); // get the MapDataInterface object and pass the ll2 map +mDataIf.setCurrPosAndExtractSubmap(pos, yaw); // extract the local submap +lanelet::ml_converter::LaneData lData = mDataIf.laneData(True); // get the LaneData local instance labels +TensorData tData = lData.getTensorFeatureData(True, False); // get the local instance labels as Eigen mats +``` + +## Components + +### `MapDataInterface` + +Main interface class that is used to generate `LaneData` objects. + +### `LaneData` + +`LaneData` objects hold all local instance labels of a local reference frame pose. + +### `TensorFeatureData` + +Subclass of `LaneData` that holds all local instance label in a convenient numpy array / Eigen matrix form. + +### `LaneLineStringFeature` / `CompoundLaneLineStringFeature` / `LaneletFeature` / ... + +Each object of these classes holds a single instance label. You can access various information such as the map element ID and the raw or partially processed LineStrings. + +## Important to know + +- **Road border tagging**: To correctly recognize road borders, they must be tagged as such! It is not possible to automatically recognize untagged road borders, they will be processed as lane dividers. +- **Python numpy arrays**: To make it possible to convert the underlying Eigen matrices to numpy arrays, they are copied for each access (such as calling `getTensorFeatureData`). This means that if you change a numpy array returned from a function like this, the underlying Eigen matrix or object will not be modified! \ No newline at end of file diff --git a/lanelet2_ml_converter/doc/summary_flowchart.png b/lanelet2_ml_converter/doc/summary_flowchart.png new file mode 100644 index 0000000000000000000000000000000000000000..cc160dfa089208b9806ce9c4d8708bd9917b97e6 GIT binary patch literal 158452 zcmW(+2RxPU7r!XlY_FhTZ6tZO%viF|J<|1V8z4u=K<-=}=;{!F#6Ahx-#OC^ri*H=$2w&=%xoK-Rn>xFtjz;iRWGlrl&2rv4p#cpWy5Ei{TUe;=3EsAq*NX!;7f!# zDS7j^kega)|T_nun;!@Aje@uDuc$3A2BJX&hpU{8~dqMC5sSILu>?oT#)-Ph%Qk zWnt;sbmD2v)yNtB=p@AYW4$^%J8^$(Y%C*#K_u?mx1k2_>*pMG2PgC4$1Wlx3V639 zFzbucC)cN+ju`9JSGLv~Y_ARF7`ktkEq2AcpRRw9pv@Y6aNv;8^SN!aYokE6auB6) z`&d(5eO4}^M?+cJ=IV4g{DE+}^Y)a>UrubjM;Mi3$H@7y{8}Ow4UzqBp6$A0O1_nW z%-+?^X6NKQfBqbwL3f%I)nyh9bhKOh-v@F^ceo01XF6 zK}rfWtir0p97(v-^!O?+jT&)6e0=%)X#+L2Qpfdwz2es%&7bd{tpCdVF7&!HYT$)% zD4q!kI?XN3^Y`qBbvuZhHYYb523I-deeH>8@9cD&`GCP*yCWfi`Jcm5nQYDZIh}nd>1UcC6Q+Cd zT&As(xS|XDr-+#l9;CoL{1Dk!s2HC-%07C16!W8w-LaWdUS596^LWyAf9djUy#Qe| z7szxm-xe01;1lGW2)I3lF!B@iTk)>C)5ugq9IgORlj@eb;QS$mC$!aSLc&| zZqjM5l@xwJ8y6Y)isW;KWp?(Q`_r*X+j;sJJ>P9x2$`Qhe14utLA?|%~tU*Mee zS&KCqR{Y&aAx-(u49ob8GLDi4&NBTqhWA|KQnX*g#jjju8b4HyaWq`)>DM^zqUmn- z9T_iaK6YGFtDXO*X;5Xq{6Zu!S$pHn+subDOQq}8*7-iC`^&iGeAtNMk`nRDgO^_K zN(46|Mo;}I-7tqo{O%BH9>2e@M(wAj)61(sLPpdlW@bzG4Lv^G-7%1jttp$^&C~E0H`K+y7ENwT)pMuo z*ht7^>19;RufHO%Hg3QeYe%0mAqmQ4tcHPM+|u5sM=c8 zcqGD3tcZiv0rUCR&_sLD&tW?b6pzCtwHsgUoUaz1<_K0_DvfeUm`yAE+FXn=jDAgu z`0g@yXaAyz$4?7gQ(IfmVMQ6SGFfvRM8qPnoRzvW?WXOYLoS2$pyR$Lo(S?CAxk!A zYj+PKe0=JSI6Su5*sm;aZc6&5FFwBZKFwtIG#l*^>?PNr^?wq6kDy#8 zt>IMDZ$ckn>5IxRIjvhciaB}4JNT?m)OPe!PB$q9R6V#l7*M)%=MKWAi>+eEGr;+CeoBBcCEsho0?MZSwknMY3O-Kq^V~-hYV|ch8y4APvhfsv@uG^_`*RbX&Pet z3yq}0^neP*?e{1;s82XaDEO@>(xihK6yqP;FDb}HcyUau;@!F53wszG6!hPJ|K-=8 zy-~RrzK~8%PQG2W{2iiQ@w=R zh0`|iE89O}Ym%uNIi5q=>ZWaBc?AXD2mP||8!nv)H4EFuLnc|oP8SH}E#2INva^_O z&nRKOL?9mbmW}DUggp5mL;F%`!F|m$OQMy};-_8384=aN#eOfE4>hK=v~-b@tm#6M z^HfyM9-$yVmK-om~d66VE^zUE%R#$pG+PsbOa}rTE2RZ_->8`K$Sat%L zo*k_XVb%1%2^y;p&~U;Yu>LqN!Duu-+`p#V=-t)TguAHphq% z)|Ip97xtsYuhWBJFlmS@lM*9x9e6MOrmZWN!BlarnV{)A+oQ7Zfv@uzr{ zE;Aq2*49`GYV9sgcA55eNl++V@is{HEh3o7d0M@Ik7Q>*3U2vZp1?VjXo&M%*-Td?WVCa1}&PcvDBEu zdiILO#;LA*e+ZTI3XPgtL&@L1dBe@aqr#k(m8Df>_tTi<^z_v8?4T)CT}_Sp_ZO}u z2cbHH_ibSh?$D$$Sx?t{Le)X^0U`mjcpBWQqNAf@VDLRACMGKCpof3DQZL`Q`Sa-J zk*BDyD!gK#p%MM{Yk#Vwe_C2v+1p{Aynia`GI2WSM}6DJrkI73)>HyEM)LAOg3`X! zrJLb`buK>Jb8+V1wM#Afb>G)H?zY^=eIRteB$FCDuLGq4IK#Y+z!XrG($dF(Ik6EV z(F)0u<5P1As`7OP9Qo=J&j7-pG!=$sXD9H#*SPl+cocE)VrH^e_OD;etgKQUm@!O` zJcI9Z3>+J@!9x`=x=ATgP-~P*m_vqES6y>iM zvo`FulCQ3=E`N950#3Kwn|!@q&@jhHd>*<({QY}GgsOo-z0K_J_57MJykfXxBd}pt z^^Q;1%8HH8EJQ9sO;@+ZZn4vdlL=G8+S>a1>e5v9L7)^mHc6x4^==#0rI&ZIn5U5A z+N=D!hgDfN4rxql)nZ;3C&F8m^Zl|^bw^`w8_u_cEm|^c{sV}o$CQ8_ke2?$>{hC3 zT6D=j?a^%dCv-&7v?YkB?SXURDcjGCj47vaD~sQ4=p|-7on2i#R7o*0#~qBxGn2n( zcW&R;!(-7jfBo7M3gbysjG-4PLWHvSXmi|^To6b4xUy0}tw>Sm@?e1ZCn+f@(9Gy-*A24@v+?VuwgpN%0@3FUc5kd z$MN(`UTxKETaPDeso)!8Os!pCZC@jd0Fm9l-3wdr3dYNXw@QZe#!%6B@w0yiy)eH} z8ZqBJ51cRVoE`x*><%Hr;LwmQ5}2{Qy}cFRI8U#L{e*3sF`9p~Eo523Zyj~@Aq=a9-C#uh3~1n zPK-D1zo{7S72d8j;%pb3uCh0Gaw=~zfz>isV6`?enKKO+ZPoo#_wYi(RPjl6HVz4B zy(zbKw)#x5TTE}R)(x*weHfEHA}X}K&zEg$rL`uj9a)eW&9egQxh36-+u1q!ziXi6 z;Kn`c{Vr;HJ6}4O*cJ)o!R%r`;yHU25DXjqg~df0Tp(gGN6quF&|)52IR*9AMoqrg zm)qVz1%^s32W&hOZ}#xrzg+E;0N~wvjzj9~F<1zY5){Z&G+dFsR7s2lVZikVVjh(l zmQW@s%Ef_^vo~DW>G@`K+Vo4`M?qvaLnM9r^vSE?x*1pe-FX0P|13T54PGxcjhw^c zF7^ZgTiO@x+U7~0LW}^(Ft1kr{_H9nHW;sSKi1kh1?JQ6%ZLIdwah+6UwI-a8lYZTm+@Z&QqkTt*x!9 zDggr1w+}v6*Se}juY212T69^2ATWf43l*WqKCG^(|LGa)zI4>LZ?k(|C$s*7%d<8v z1)-4oRG~7UCM}Az5Y&Q#RUquJh*>!?d~@8cE>1Oah>mVy?~+i)#K-@KIG%FnC@x9% zI+GhdtoJ;pjoh55;z3Yn{0+f=@Ytdc7WReED&56Cz~XY^jY8}Ri2A#+u>r9Xn|`&^ zjC=ehPzd!Ni$9xIp$}d}WaLXt&HGr`sL0cDt8u6R&+_jnQn zgoMKdhP{Hxu)kg|6W-V7(^|}|(Pu}SHIC~V9>*sJ*M|k(vt@9#g<7n|tm$8_q43Yp zcSWX3{`a^|xRf^&|P!{a$gZK&j5oPJv}W85+Nh zb^M!c-)c?vJR;>I;W5MhSqcmA;fasEnB`wO#0o4_>n}AmHQkE&f=J zQ+YV#GMU?zhKrqHB3_7JI(Q*Ie-pR3L-rXzP|9Wx;-M`(f)x4Va$jIW{q^fb5R;(5 zhtnp%*n2_BtH^Uy&Y&V)<>chd%oq^Z4}=D{kL{75T^MtQh& zVCjcrLpQc-_mQLK$)O=-85uJ7q!W6MHY=u`)&}n&3|tq9kf&gKKy42L9HDHHL<)ML zUZ3`Q3oN&^v~+ZI_%pm%6)N-w15F2k^)_hqhj2^l%L_r-1-3L5$hI~N!% zKYq~Z4`X3rd7sQ;_&3km2nZ|#%}*4tHF0%4g*tpT05osg`+A-&>v(#@C-6Q%;yr16 zs;Jo56W|gT=hN5aI(dovK|w)my5%!ol@|T9_ZqDD5{B3HtLzp5jZL)aXkKlOm1WIV z>*e3ZB2L*>h+b4?I)KZ*2^f1EL|k0la-x0M(3>74ata6rgtox)xtm{=wM;14Cu$!` zQMYa=VRaSN`5e0C*qKbcyz0Y;-QC^5<~5>Uhqvu_aWtrDYHpW_KaXbZ=v3O zx%*b(#5+Npp8#<-tZ`y9?e1p$?C@8Pu}Y719We@q(Fwn`7G8DJZ* zzunzkL?O^1KzTWW_eFnD8o?l`9d2D8E;x3pv#v;}_>=fBC|DKjh87d;dXLka@R<5I zY~aqu#%3ihKqJ4}{`KJvM1tS*%G`XtTFhpq5tyIF9mQZbAj!@~D!mVyk8RB_>UFcA9EjOpJMPFYwhrfBv+# z$`lOeWM@xrV<0NVL!&iYwSc#cb^)sc>2e5Bf{F-k@qNQVF@h0jwc&KWF5m8ZeK_-wO_1TcrO37lPN599$bQZ*12##r!d;K@z>TaAOx+7a~ zhq%Khtt;B*n_6@ZGdck?B?z#uCFk~yP(*7!?PQ=s@Nso?kj4ZG132vXwF#oc=P5BA_eFu z#Cm1c&LjO{*t;wKDHpVX3NVhq7PNRgCNBwIm4siCF^R_1@6p#W-=d?%K(j$o+&Er);JR?P&dtsB-L?nIBGKz?^~00KRR>xGFW6`L z(Jq9g+iCuToFV61HSZVNzkYr8XVoDnt(~M;a^HV^3yUP#4+9TsB3u8S>_QE6+eY#sFG5;7?`5lKgA zKedMQA~O;%B0BmA1Y|nzUH#=WCQw&=yu2;t)0(Do5l{@iBs>ZB=^%-G4P3IvJlWF; z&Jx^di12>rhU>}a1Es%z|Bi}^DlRVG+p|M7Y6;QZ6TT5C7?dtg?zLb*EN+zaZuO-$ z!|D#_=?<@bgWbXO#S4^TCC1w7Bo3x$XU`v!0e>x#LZV&zzWxDFFt42^jQa*IG)IZ- zy5+27pY)Wn~~u78e)Q|9^u!j*gDR!MBsJ4I!zs)pOqQk<+|t4Iyox zhvG>uM-VRpfC!eDWtk30aYk@xoHj;c4BR@_5A(H4WbU;9)kw5$CB^Yao5gRq?-;tt zt5^fuhJj0ojj&%v0{zqMmyb_MQe@7{&7~0YIJrsWFJE@%f;nyo3WG4`G8O_t6f}_U zv)!)`WpA9*#C=F`NX1@@xlpT+nB}D<%9)2_1n6M=WnnCHip0+k~&|F@{ z)Y`7gzXHPs<7V`3>|5trX8jg}W+gQIw-8V9WI{-Qg6u4vZ2R8El2X|K=Bn>JIRuaf zVFeI0vijQ_PDcOb%XtSOaxyZgioz1loSgV*6T`L=q{H}LwST#|xByryhf|N+G(n|5 z2-)C$Oyt-*T4MfE{`S*na&jGD`5=);<#V` zmyKrVbxQwz6{7xUj6u){yUM*74#Rw=|Aew{x&c7Ke!a|`dnK6k0(;~;mnr(s$22s? z;v_Rr^M`UY?UDT^&_J+@^0e8WOHA{ycF61f;H0m!vI_NR3 z*81x!cRi~lA;;>2zt7!|HUihT$KN-!w7PHN{{A*zFHoYn(y3Mas^hDJR~#}6R2-sl zZ?Q{TVJ#s2nE={~B-mJ$5nuQA_WlkGXoBx;92`Lh#^PzqeM6}blArhL`045CLB~a@ z{-Lo!LX7l1Jm{0ewMBwqP+xO!Z~)S4AF{tq-S&ne8uBtS`l|+weD&n5->P1>A-g%H zt>4gFFkS5YwvztkwRQwuOc%XeM2{Jv^ezyfkWU_QV*cIT3L*NYA(KEBvt+=X$8b(0n0`6dol(pILr=Dvj?oRpbvW+Z@6_&f zvh=k6fv5~fs{|6_c}SoXpA}I4*r*Ya`0qx}Hj*L=N8i27e4&1?MQ;myrtH86Xg;K zg=ql7uz$Wo@Ot|C%AYZYT|bCFP$nHHiB&F!8MG(N&}9#19JVInDm;1x?l74B+e23le!Vp7Tbh$!zQ? zpP%2wg~#Jz%EK&fdvt}s3^q@A4f1E=2g;3{UrzQGtH&uA*45DXJTB-fF0lOV+c%52 zYPdAP<&dJo_2Gma=83rc>a|A`o?V@tU$l*CXX?okPL-+LH@NmQ0pf`sYK^Iaw=%w0 z`9Rp|PfH6*BPji5(?+xyeJ4`Wcfg=dzSW(~1&?qkNCiAL>yEWY3S!)pul#j4OH1ak zT_|gODvF@u>Y9uCYSX2c7=osOZbu3($7g&!IdSoCE&^f>cgibL^pJuyf7TDTCaMT^ zqNu0l)@KuilaRn9(wHAp;1qyq2=fSU(tZ#7>2sd*M?7s$Y~Gq+OId&I=?kHSYQKwATievaqo>x-Ju(`x#z z6li7JiyLD;&uF~j{dA$Rq^YT?lhYAEO~2_A=DW90K}#T%wX`fM#(RE^KrAlxB!JNb zoEX}x6gU`RrSOHRsZCJ85`vQ=Jw?H*OM`x4mraVEJ^+la?%OgQtpEN~=<{}Tgfq_p z0udPz0byl&reND{bzZ`uME|~n?nXcAIgP#s**60f&+!i`W#5-l=sz}n#JC2N!tMI< zc(+?cQ_VIFgaWwzImnsk-CbRPNH6cbp8fsl_3PJQ#6FY_TS$CARvL(>s-?vw_ZI|` zgMM#&`xD8U49oFytAWRCqN1X_yhM!JDp9azfV%-{BX#c1CG~?J`1#5jpyIuTO>wgB2=Ae;TZ z7;#pDc?V1FxG`c2%q3MB^JftBUo_R!ut_*m^7FT#m{!mDVnO9PH{u)t7P~cBQxdI! zB758;@jN07ND(qg7?N=Hw>7q^x(z1UFsKlXAQ+K4d$%u`SNayeP7`gp;T08m z-`e!H+6Ga`#ulo22A!jd>b3T^oGo+AkRo7_nMzdQZ65{1@OAnK&bgoM#}e?lFD99N zpm>Vc%_KrMQQ?Z+$XNdL-M63-9XbZgtWEW$yQYhkeRC4rRIjSZdRp@+ z`?Kqqx#Dm`L-9g3UB_ih*D?j61bHggA~PteWPKgArVc3ejwB0VS`7FKF(r?JHm zQr5Vnv^>1;PJu-lDEiTGz@XvN^ibtyrvK)tm`SVRgPhH>rIMqw%sbwJo1afvL$Y3n zfSFBbU~R_rrc5lHeKw&?ixm(-r^HMVG6j?dCSoO)rw&l%WAuB+YiEB?k7r9W#}E6; z>%2FT^Q_6;n=h}&*1o$xXfeR3OjfDzwAe_&*@>tNdn^9(AmQ>`^b`g{$GAD=OQW(Bl$rZ+oBfYA12;-(q6)go=)P9gNVGhZw*Ytvx@x z6~>;rioO|QP!NMhE76T(zu{1tbVcUJ@PdbHx_|41@B?fbj^~PsEhJ?lk!@s~MA*|4 zDp$IQe5W^=AE~9f6JkF)o$*9;P;?tP323-N*UX5v93_dT=ZT+W%)E;NaS2+{eTvP| z5p#dj#eLHsIo<{mJD2sWRhtKyDIZ_Pg==UQnFg$YQ;&#jd(avv@Wj1ICG*mm#F=d% z%_HRCR7tjN$$I*Qw`)+|n$3tyA~W%~sHj*j+M)*e2sIch2oV^jW+3o_Y7~?ftMb8@ z8mLchE~^7!Ggoue_^+g~RRpt>z34j;9Pp|3k;3nR>_(~bun`D3ynOi*OriG`<20XX z;_s&Pe5dNtHkqZ)BP*@$sBf(rHoQY4mi!*%CzP5IZ{=J! zj3!UlmM4w@Jw~-#vzoTlaoly8(z=L&W;zbotvn#Zwn#)DF0Sh*mOg5d@c{CR{Xr-2-j_Q zbMS>Xp-{I74x$jX4}7kok=`xFKw8w@v89U{v1a)GZK@J zmN(D0g@fTGLq`xOMGGdA2S|*&%`Qiy=Ca|Gp(E{eQ(xK!i{FhmYgM$?pY10MmzPw* zwo0Zp_s;J`wh|z%ysa!OT)?rt@Va&!)<2&0r$Q!+UoS)|)h|ZtVmG@$?Q1w)BF|kN z0=u{)lF_u@KO!OmSZ8~&2f(x^d||T3ULaEt$crOD8mQz~z?`8D{wgfo0TI3Z_h)e6 zxIwMUE|}p(oqxclJ_6D1Zm6Epjbm{=Z2_TCu-f_gIq0;X$|5htiZ zKf)K4&_pybHM_>SS3?!cXqv7qu#C>{wQa?m8dZujL-Rzyrj^wHl0hC-LayXnv~BB& zVV^K79ty(7#I{yIun_mWyQ%IIT4r5+!3Yk;LhE#ap}1H3*N`k5<{;ld6CnO-QGvg~ zbfO58qK!pM&&t9)H#cJvgfn-2E*hf+EgG%Vd63j?JVkQ&h#AnS!HS~s-@d7R@3rLnTNMcS;BZtHnj z8PyIl$_Z2B3Lm|#? z<^Acg7}BH4$!k3G8>~W0g#k)xzPZkBD^t?c80@m#lJ9aPH)xiR{GXM}k zL;zdOREU#cP69_wU7d)yvj$#-nISm23Ip)a;dbK$z_%8nLQ@LB>l_#eII$enD{1f% zVB2}Z7}QO#wAOVWSa&@DDA2w&;BMnEesY(8BE@n{9Gt_Oo*A_0KnkBjIEa}133yvj zS8mnxA5stj9=7Lx4}W~dCy7+BhHkM;Vv}-9MK2n03c%0~oJz)z9~aBw*B!mR$#=n6F{m?J00nuQNOoWM}8@nWkB|q!euTe0&Ah7tY#!82;z)3v-9^e7( zl;`sDZbut}a5&s+Gy7CU$np#}>-ts43p+b8PG>%1p213$kY3Rx=QA{gDt? zyTtn|fz!X6KudzaqZS%=XFS-7>|->Mx+nRRrscx2DgXA+>>K|DyucB8z9#fv{|50b zAHI5&qcFKPUY+X|_vBWBkz~=9h0+KOKD`>R8Z{@MX|AN*h1Q{|VktvMgoDJ{9qw;} zF#eSBqm3xwRK!NdE9;ErhyN|wizSv5eLsGxc#+3#ap~Md(`u|-tw4|%ys!1`CaBVK zP<=qWxV(#$Nd0okV)|}lq^SKZ3N=CCyNOCJfoec0a`Uyj#S5F_-4*Pq0*(3 zIa487Y;x719=eBuf`WB!hi;JWFa%LFHw?BUFd-csvZWv7+^=qwT_o%cD3_bwiq*d7 z!0F#8!;TZhj+>OjCzcar$CR(3a%8Agm@`%(4Oe)Pr@g67IM*gpOxXLk_bjZhJJmgbs~S|DfNT?j7bcohF=CsTCn zx+(xcg5cI0IE=q?yKW1jboU%uXAWe3S)-IU_=wcBI+c)`FX3*iL1~xfG8Rtzb5()x z$cgQD-pi+Dy=bRspOp}5l~#RL%Z?%iDwb08V-7s3^6Kh6ot;7qE)>?kfdT%Bc+J$- zQ4Fhx5pc(zT+Bm&IQ_R&25>#7w4)z!#cnhfbjM7|Uj{W1B_}6`n{$r4n3$LV=`bdV z&oCYn@Sym&_@93>A%nsfIypQ)Am^*q^&kM?1`{Y6a&fyO&=nG)l)#K0d%*W@1P~x5 zIk{KRu*B>x0`x~H8Sj5NLu2pC36(^4LEz-)?!X9z$j%cPnN~vvV6@aHrdZ`avo=z+_&ASQn&ODf30dtTX0+5wRyLE~3s1iI z8gQO?^zlK;lUogIxRS9!ga2v3VC7Q_X1Nkg=fw|wv&*(4c^ez=N_=2I4W)6@$#l;_ z;u!=qxO>s8+NcOdmi4Hnsrrn__jCnwO+0D5Wmc6QL` zGuW4<8NYFXp;`rAy#$s_ceX-G?blvRk#L2%`8LbIacjaAP!X*B3#h?2!;ddt?gMDU z24+H^a{`)o;i^*x=Y)Y$po~ghbwq&q)e92{pf;ht1Gq!W-DB@YBY{Wfez<&9cIPy{ z3Ht)UlnS$v)$Coxs6m)>w_WdJD^Xr;WSDzrpLEb_(LEgtUwYez$olY@A^jumTUg6I@46U91eSx}5VQT# z=e9rZ_I5lO{xzBzCQn}W!>Qw3cq-XYG}+dS&?>6W)X-geljFyytFNcwHEz?3UqwUZ zAI|L?&JcCLJ9R^R>gPU+D`n2tFlSp;giG&Lc~A5eEspt zqpm%U>!j=L`Iq71?1z;FoegIeZL;eL|71e~6bn-&cXo7s;mk*+eO)g`#^`If#69uL z!oQ6k!jsVUZ^5c~e507@ZMIp-Xkk7Vjnu&7QCcji5VHRJd=1Kkzl!dZiz5ru6D4qpM!}1Q!ELsjd_>|gbC<} z;Ue5OL#L-dDjhwMRsqywkX69kL7 zE}O;|A1){Drq>0y(D1w{QOkeynO3!E=zE23BL>n(`*x0`!-eu?^H76<=W%vT#c6pJ zU5s?0%?t|L6HLK|nTz2tcdIwYzdHUc_z)uyAO3rSAky+*DZEoIT^4Mu<1|1NoPScg zu^-8;XJjEa)^Fuat$1{sTPL{A(a z*Ib`j58Kb6lXM#er9F#~y5l$kg75@I;`v=pX_Z&n=&0P^rdi$WEhklTj9xe56!Dys zF=MbRr?Ts0+V+&jmDjCNuk7qsKA~F+%f8rt5opBcdQ_s~K#a)OQ7-Itecpc2)v%~fD{uQ8- zDT;&lmM1niMC)V+K6U=%@^o_-OauzY+I zw27!d$Ayw2cI*?sPUXeB&#hfdyU2OUyDLq0iKaC@x1%TSG0lf?5eG`eHsxx(4fv$G zI4JBsak`MgXg^uy)e_G7H?d*_4Jl|cL6P>+{!0B+QQrEZ9!6kH#_3iAi}26oowRC3q}H4xycAimQ4MFeQEp7r4R*jLDQ8Gb97N2 zKKtA^Sx?#8efmX9qRTeBy}WqGcSL!J)aDj&DP3Ipha9IDF@q4avunBW>bHTKJ$j^Tt0X@ZdU>HXmFkEm&xcM3e zET27lb^;@^AG8{5=UZJxzC$J|Gc7pP_}?Wz0+b3N<9+nct4-HrZ#Z9n9iGN{;P(3t zV`==YfwO-wF9Y_pU$e3P@TQYHw8BF9r%u3xGVR$1FY)CI!It&IM?tEZn#=G;P#Mr* zf#YZH-uzKP`DK%dEu=LJdRf1D6C{&<_wHTFaPyO$S!ft=|3=3xYR8&N&CT5a_DaTS z)CfY?38a^^DA^=4|D;3Y_~ulDFt+zy7qnC(rjF>S?QRYn;`7JJnuo}O?*%+{ni3rT z3(9fwn6n||-G%MMFRId*Kjr@>rqi|t5*dl22VvRA67~l2eGzCDG$^c;jJz z7ygXgR_Dm_!Q_L4!HlzKp31mEYEsKH{j~B-XvF`T(|KgXN4A?-Wi_7R2C7M=WKCM% zmsY{M6~(A5t$y$JdW=1iKKK>kJK>MQRQlHzk$>s>#64L3@X$9FawA2H+h$~PTi;0Z z&CSmrH+;bpYyh4LqVEmZ_Lt|!jEFvHKz(D;q$~hVbVbdm5&pRAgw=ZdfQomA~vmHlW_c~t(K1<@D?gc=0M=t zZ{s_5jm~E}C7Hl8Jo?w@{S6Dv!rPSVRPGC#sF*NxGz??p+eMA0#4%LG2njW`^xHk=+gYVEB4 zM%+Y8@D(Mv3^So^3FUS+r?k1-^q>0Wg(Q2E)<9HzTDM!6c+`ANtv;8v>&wT_rwoS* zgYZmJiC@{DykJ&FXHAo(dvsTUOhC;2VkEbS;A`|hwj_NacMynA*;c+#FnvJ%aOuo& zMEx(cynrtcM`!XSwQr}|2h95pwKD*mwn)c(Lh4x?ExcPfL!E*#uStIbNe@DIm^gIQ zk6|l*CY`)i<9t}xwH~9Jb-O1CV>oV__pdC5h!CxnlooD!CsptR82kpqVES4@0XWZ$&pkw+8w5OKdqt2C78LV5MNX2?fTwlAmXyRQYUD~e3*Vx#F(#Sq0xZE&! zQG7!#>9>{GV@3GvH~OMdVF7!m1kcAi-%HB0GmJ^$6-=|#@fX+YFtI{C&Pu5!ZRE{8{4a1ByKV} zv&Tu}!4OkLr8AN0lw3r`CoxCGKbh@Q$Mp{DE?*Qm8fbjwZMOEcZZBQ~M8&u*%qgW3N>CH@yznW=A)rx4d>6Mxqbck8BD=;zpph<8%(@+ zLnq%;B#O9r9G!&ahbg^xaRY1)Hgc^vnar!t({!$?_Ex?ALM$Q2Mi8jQN}SnD8unRm ze%V=|p(gX3Mb|)EqXL^m>-M_W9IY~weE<$ZORvU7a^UlwQ9@*gIAd>|f2{EVT}Thb zI7{kqo3Pkb_1yZJO__ROn&*EAg=v;$5B-5^VU3Ah?N#$gAqBFpd?prqeYV>3Q*2(& z>do0c#D#%;7uMGT(=450D7?xm%(WH0b)xg`XS@whZd=`S{V)%~v8Y*$*M@L2s=gmH+ve-3Uzn74wTN5aOEje2ij-K=qyc`k0&=3U*( z>SS~)@5`RI*BVkj2$YzGvAJ=(f#QUzM{ZpOr!{WXVuC9jlmt7Whv20#$v-A5)7>P_X(>2$uXGm?UpRF?pG+`Q!pcISHj0ltTAPt{_IqiU zt&pTGvftUFS+9LyriCj^&nQHJ>PFoZVIx^VZXmifv@i7CoY0DCQAOeoO^s(kge1Wa z!#CbluBSc27j8YEMn9^e*SSsP4EQThCG!|$*ePSBp%n>cZxo)X_nVOLrkOhmtp7~q zaNKv;f9oapB`V8=L`Fv^o=)yXCgE-V1!t{ic-!FCKT=LGV!?_Qt(ujic*u=JTa|og zj#KEl{*}N&ne1`K=GzU7r){^w-)1_EDN;COj7!%vg)Z}nIj`^O0-0>-u#LoGXaHJ3 z`JJ*l^{qlez2({A^L)eU$|?HlKAF!*TQkngo?pIXDi7y*+8X#S%IvXvhhmBU{bqWw zA?wZ^+aE$2_D{5AHtxAs)k69Mst&dCU?)^5u6xN#&ntdTkL~}{^sjR>L&o8Y68vlK zLoH|#PElhb*jPv)%KxABhLl0T-1KMbFD7f3W+v2e~!!of{7lKvn>znm5? z&CgzE`}7bMu|hF?8u&p;p)*{WLT=uzbVtoBYyh? zi|*N->($+4e{_+kWh9mb3bHD&ZtOmd*$90Vz%Oor`^gbeMS-=JRVs1hz^78>dY0=g zF=uh1nSQZ5p7$gJ$y9h|s1VBjj`A;pjkJeLx5QblcwX;DZRgZ)FRdOaH~1Tnoc*>O z&FoOau|Po!8NtGhzeVeJpzKROTVH>y-?!`gg_66GI`P)Wrz-dQk-=5ozD+4ZaC)qm zQ@+IGI5LAaNcu;0hI>~s+cGf=LVHc*sQ4c7941m;`@9bXE>&!C{F9u7V{N7q7Igm_(*pwuby%UNPORFQmHqdQ4gu!3gHh|tR7 zNJ72tH0Ab^2@>5>dov*R_EhdTAvKC9CzEBLe%;pE%o<)FnlJTAd_Ggq&>QJt9$Hm; zIf>kFFrtY58k?Wi4}tNblC`Um`kh~%FOl#7t48BfA=hJjymignh_3+#L*#!YccWdB zR`a%8Z4S(5Y}p9NmeCRPY@dmcA1uA7)xIPmP`$_#S1FiiR*G*4&}%F_TDMQX?NKiU_pa!b+!INCL~X zhxqw^sXJYtiltNcXMF6!@dEG@3Gb?5%d?dQKhqzKwG;{7)`Bcm4nJGKhZn4ij7Dl?a3|FPdO)4%~2%(rW~^|wlXQ;I6C-iGUNLf zFWdm3CXL-oOWqwh^1&->pogkci9AnXU|2b&n3@zG7r{OoOaX1i( z39pP+1WldQ9_&{?OOkZ>GA)np-%w4bIT%o{c`;Ss*kGV%vl~dt2v2F@KFln^j1I

8q^nx++Zv`js#h${v^7JVp8IlM{kgZFub_S#fo&}yZJs`MlKkyj zkoSCWz1kqgLQ#b0xLYjH^ZWS8L@%%Sxb%6&w`ukQT`$+=uHH`JVEcT|PXuJ4GSVug z+E&Fh{^`aaF!>#7W(PN38cv<<)dsoV3eYb#jgKrJi*{}FQIXIu&f(T`ogOY`bK75`zHy{OTxJrUOdH>HFxqQ*|E8?Ng6oXS)rQUmGKAM`6S$6nT$J5YTS z>zw)gN$1s5;(!!;&nJIxEoar0)~CjTbWu=HMy3{A%emFO3fstR4t zwY=KjDoT#F$x=xa+Celi&JTMxH4nU&67*+(vi%Kd(JIbh%634Z+z@^?@7>9m%uEW`4Mv>dE&!+A1he$yvgRy2j5~_2T0w!ZTn(0v1h91_@F9N zQ=))E7HNt`d0u?cb)UZ>%ZK2vWO%;2gXQGkn+@CU#I-;{{nHm(h#&oWbkRXN?Q82Z z(`erHSufmVVADVmnRlZF=w#n{{qS@`Uc$}*%oG;CbUAOMsO)@L& zkO!-vVwsNY5cWrTt40=l*`n*ATYggh?!cNAH##A1M(RmZOT3_=UjGlH5*uTj1`#vz6 z=%INPN&bCeV{#)ZQC#D?a#AJE&=+TB6iAJ;Emn-JWmfc2AXu7@*jZRj1+j)=D^8ChbrHGI;?qo?YvQ zb6$ZTjYeW~FM;e|Z!zPd*^&1|{K);s;+kYWeWFr4qL_)#%LhxRjyR3KEB2zAkCv+`%k83zlr=CMa>}6cl?2i)O3p7&0YQS z5ldN++T*%>t8f3zwtN^QI$m?me|$i7QMm3OEUf1-);@r3ZAARL5I^IIl7g^`d373d zO_V-=L1h)KG77d2T0oD=C1s8|uF9tkVhll!F{>BX2_IE745Zx_dq)=7h6^ambb(n8CLQK{{v$B z0Y#vCr*b3qDoy4FqNgc9M(Ps}wV!ezH_{`%pntqF-}1q-UW!CDn`ra*{-&YSnoRf9 znw=jZk+&Y5s2S>)S_i-HV%W~uyxJt14JstYS$m53l0}EbEkvkbxM|Jr7Ls4hQoUYK zkV7CLFmQ*+b9_3-@UXTeGKzPF^tqm9{$m9G9U|wh5ew&ZR19R?e+s(`$+$*p?B0cI znfrT4J^HZ$m08g-(>IxD53k0N0tn7n8lRb{#Ayw$nwE;ij4l391#-*#o5K!uHv+aM7N4n7zJ!{E*O&K`Jz>_3;Nxi~n(Sm2pkBU3`)w9SDe&boT%WL1J`E zjgszeDd{eyQ$j(yky1KDq`N`7r6q*-dft8DBfs7Et~lpkC%+EK9!!n+q#nl7&-DBi zwMz%4$g8*h`}-370D`&wdDyK_NJ68nV!_H#&ohs#np!vzz&AjNLe5Yet9%x8sH*dZJy<{klDvioR1x6vm+mSk&&26u>!>oA_=VdshP9iLDQSc%j>Z5 zqyEFv%cb8Nf*&H)>Gm}I6Pbk^R0=*M42pSMc2B$;I!-hY#O~8|$$MMowXaJ2$|HK8 zkJHyc<8w)d4GMRkxq4ADMuTxstFdik$%wtrPiW5qrFajstQ)-*gqlFUsP?)5Yz*T3 zXp4qH=0d_0ih@t0hR9XUoxNRQL7P9X+#ckE1WMz2?|=UmKan}^yNKH#l-exn*DXjy zFgyxfvd?*u7GZY&4kL{D~v{ zCXZABY$VX>2H5N?y0LBPq1&n9EVpmnbF^`!dA9J4Wr8z5l+co|)qoN|qO??kgIR6u zR3+_7e&=j&S|@FqlUSl2n)+49i6vin;*$>%WZd3{(tr|!5sxB8tTd@CsmLxR|7Y(- z%zny^oAAe^%^#*HZY?=$)=foU8J7uPu{+G3+`Wynn_Alg_@BVT*SO_4i%#C0{T(5T zj?nr;i zk1$i>@&e`?t%sjCDPOj;Iir6cMS9^+<=aZYPP4tE^Rs78UM3t|TW{ZH`DNk7EufW& zBQ)V=7D^~+o4guWc>8M>7lWyjoR0$0>`+uxM*e;@{M~X#_wV_`uYX^L&oixu!i(?# zk9LfERF1DRgRt%LA#Iqq^x}ZlQUh+y=u8AHA|vrkZ6|%hB@|*NE@!-LwPvXQ1Y1UA zLe^~v&}iyNQUZF&>-+ofN5fLt3qzM1YKyKGD5D@-|unic)5BhfOc=@s%@>LJ9p*WPpj!CL-R75_v4G{ z8eKDDsWFMCkpS>VsK+KGJjMtC&Y3TNZ{FU#eD(Kw&{`Rc#4LnWQ=7rOQNI? z^H}0rAZtNQ#5j*W)*{u}NzPVQnCQw312Ema-mRT(bu8h{dgXJjW7q4ZE*FUa7+E( zoGh0eP@rt3w1IyP`mA0pTs~_6L{)DwGKqm_m~XVa;3BKmDVzb1JVT7P7~MkMvSU%; zYO%DL%jw-UXnbU>Vvkn$xNEw5o{X#BH8`GR*9rjv0op~LVg33aFJCjPgH(acq2Iwd z^8|eq0$rer0GDZi;R(O9Ts8fMu-jR-H}htk9JvW`xw5T>S$z$==2w6vSRQXhJ4^RO zqMbu#ypzT!Kl3%`IL20nw12GpDTKY>qjMgh8T@xKz{R*`eIpx{nAA+m=%xDoE+!?H zLiD~D_8$l=)KS9`L9f#$V(0#ES~XbYRl>0+u`|XwlEPl3p$K(UOJ8XH54l@pJbEih zxeXEyt}<|^;_h|UJTq(U^YFJBRD3AA%QdV|!%#fd^PV}q*_`qC&N=YGJW_@m#)nG> zgTf>!@MZbP@N6Z5pbx{hSMN7yoQS@kc+Aa&KH12pp+kdwG5_eQ{w3zr@S1c_lb|$R z>!qDz?;^k%Sv4$J0UYMc{5W-`?)2mK3$Yb=<#yhNM86fqW3kybhua;VPu+UR3I4h* z3WiA*VdVz*7rz^&gL>i7`Dl}Y&wL+q%sFz0KZ4`O4p1c7D=%-Ye66uD3mJ!S6zgUD zq$NJlHle$lz3D#%z3JEUNRs?e4CE@fmfARPZ>E&1-qo*vdL)m^xTQ>y&gU$784Aih z?HA?ufCUAx%1pZD6Z@Kp<;%Rnaa`!AK_tlo?*1BooSpVe6$VNz^#YoHk!T&b>U30R zx|+H^*w8rybL62b1T#qA7c}76q55=iG~-#;`!9n2^m@)svY2YQ(C3AM=a;DoJY4j^ zBW?APj^xoxoyyqhG~Fa~T7?d;FTgO0nx9R66sxf(5I|UYGd6LdFbnsx6rmh+cdm+Z z*&@mzpgUci#~ke5OcGI#EG%|HO zZhCp37f*g&HpRMEP{1tKn`SZSlXr|7l8E`e_Bw^vD1%-BH3{QhS#fyljl*l2bj&`VwEpp0(eh8z%1&Y0Bo>!Ya8URvs{fgm z4p%U;KrYNOys(5!?-O?t>Gq;AhSiX^cMx_hJ!+r{spTnR*qffv=FKcxIOAi=6anmt zdr<2QwGDwiQQAEPAcYodO6R=fpPH4pI%4g+*O<4Wk=bL$;TrDClj}wMsRmESr2tj+ z$8M<#03oqMq$2bdKZ?)QL1~kl>Q6S!@uWC#%J1@lQ%@I;PAiDOym>Y{^nIw^A<63r zclE$mcYU9r*1S=}*`W!Cw?F@A@Tzw1S<`UzV7)pwUN%eYcNq?{3(C=JT22y;S5R?e zuC^*5u9SXqlWRG$tIoIny3$VkQjV!#2-8vSE8hU4^_s$JEJY_D^3YxGT*PB)HWwz2 zNwf^OpxPD-p!Yh=ntky(jPuCd1)y50PJTXy>%stV*jRG!P9!IpVsH7pnkXp5NrW|b z=-JH;M00gvD(e0wxy;c|c$v;s4_>wUd4{LE=ndPbjOkLbK2+ay|5vjMR9=+5i&9Ac z8+vGSCcRG_+#4um__>ntbzLaCw^OI@cS2v$(c>U`MM4>YXHxPEkTHUqku==%BY>mPd5UAPGOSTWMFaUHz?JXUP|v(xGCl!qrL*S9sz3;^}ov zu->26=wqL^J_eJ!1%XtsV%TH|0OM6yvQKxyW!EgS z>xW%(Mt+EX((&U8!Zs1brNC#HxZxRC4}1alhbiH%?%g= zERRi9y*?YQT;Evpm0JIVlYhL7j6E9V-*bOaQ3mx;BmOc-m6QN*w=M;Ds^C&Zd>&V09L;Fq=Wv(EHO4^QT2SCY? zKw~p8uKM2tHXXQFZGPnEc3EZ9H_FfulcnxDLPNVHyKOU~kh&FwJTfI6uPH~KC%=To*i*O@CYvSqKce{+>537mm-%miN1W^~OBU5Y=PG{&wwkRL zh3%<)cKWKE)?DNtfqNt)dU=WM(?hbEkBfgdNLEtITHoZ@D{D8o>TVB$lNV@NirA4` zsY|^8zaHArFio&SuySg)BWcyUpoKME+!J2U0tw5o;8 zF@>eBrZ9oh!C(e&iAD(90R8(DpXX^l241RHns%jBplonWN+o ztQuKUB-_zv0^UoBS+HP&@^6K5q|tIM`Q*Q-Lj|Q3Aeu{R?jh;XJSxa#Z5la~3YpJD;zd;Wc(++s3%alX4fN$6POGeC>9e=?l6|l|62j z{uDQ_+*V9_ytkir;wXbC)93!Gz2=2WyY{c&)VdTgLf=rkcS+ zk;t$HOQ6i!BKy1H`Q@GA85h?#Kl`oOy3=8@_b*ogNIt}p`-!_(p`yK!dF0(21}Z+O z^5zryT_z(PKT+_sHqXAqsZs{OaP~kcj0U9%W>Bvdq>U?UCP0@Y$f?uk*o=(ZP=X6C zn29)~Mn@SJE9)T}tQb*=d8!|*9|Hy`5MNm*OdDHWadk#n(W&M01Os5i%9K15k3@Wy zc&KeU*7u)R#Z5+Q8ho{z+bf-F+*~e_t_P6`YLCj|2_->@8*?okN00h9iOQT zh}0=s!b9c|`u358=+Ws7fC}`ooh}g?rpU5J4J@Vt$9$wqI8Ce0|g@XV0*U!gjann2Ta&`kVdLyaTf(TgyR}&4X20&GGls0?( zcaq@C73(Z=)3CK$=RF~22NWis$Y(g=v7|(51ypq{)TgHG{S1S(OO9T`Ilw0IIUIul zZ!dQ?03d$P^eye}7nx&f!}JUi%Xl@tfxPVLk=|R2G&Y=d?BgAe{(%gpSFO8qpy+W( z#2fpDdGGn|OpDAIMy)b7Aedz&`Vf)x>aD}_!`8#oL2}B!!NQE*6l%j66gteUIJ6CR z4bFudKQTAl83|Cr8Jh`PSMFrNLcA@3cQ-}^3iY*c_`KA@&y&y zbo{E@h1GDZyoJ35;fDT6TzwgFoY-Mf!&O$KUZcDBztdyEs~mapzjzyJ*jj8*6`u z&z?zM6JU|9tem`S-+0qqhvDUNrAE|C*KcF6-oc<=fMOxsO|NKK;07-vq(J~RtyYeT zm4Sf)Js}ngjpUD$l76o<_uKmOi4nzDT#r-q(b-7M?!KNbfPVBp6ho;my^|#**Hi2t zXnO%K%J?~=8Zl>5{==*weyO&VWxk%RcAwhOmHo8Il2N(@emiM=t><)YH6VJA>XZHi zo1=?^F@-MJdT0+j93y=B)Z~w)bJX6M)-M8f(93*)bxBxX3Ec_pIPa8J;B)Uyun28y z4J~Y~2!Iv=RJuU=+5Pe4_$~lIibf4%dcA6aAzTMZ($9j*Nn)gDKsxDrN@Rt$@&o|C zn~$`FoM8?$bj61_d-KjxVWtzGJ(1auSk zB|YrOrrAsMYYR-IWQ?s@N-a z?TO11srh{~^vB7b-;^N?k;2S8o5&59$VXe*zAXl0O(zX~zvo=$tn?Pq4RHEoN(B;( zutKg&VPXUPv`juRZywJa#PVtl{=X=h^~X02k(Ox(04f-%NN@e@DX&|sWR@u>9kBy(@lvwZ zf41`?#pRm(2q=oxqQOD^&G=nU*0K{P(vAi;6q?? z<;>VLJ->9RqiHoVI`IIJ|CN4a@?R0CtFMjwF^y6J!Xz2i?LoUI%^%~q^R7Fi*~$~V z%*l=~V)G|sA`2@vud^r$P{-iyFB-@1@dYWe-PVz@nogXP9e4vNS+lpo97Kx^_cWCe zQO0W=EX&F^@uOQP_|`TUMFc`(uiFbOCvaJ%TpuD_*lc7U?xS`Re070dSt7OM&SjwA ztFmR{%gyLv9*^>Ni*$AQX=RJYbp^v3VGtH{1|&U^szV^1#_wUl8PW*WJ%h)^L2X9> zd{WJIS&Rh=Aqa$VUD~>`*K`b5dFBG)YwFN+8HJ~6aJJ817j2aiF<8Tn&pL+r(cSu0 z=q8uiqML%smkpvQC=19F~+FT?E#TzMHQXj+EXu-8x5~!U1GJ4U{BjAL+K= zJH)m8?O?00L9u=Ly1{WLJq4UmZu*7EGPDG`sY8)$LD-pj4AD`jPcUSVJ*(m3=#zgT z;$uV~qV5A9z873o-dvP=HMW=WPI0tau+k~=>Mod2P(!F!&`@Xz3~OTuZ^q&ihI75{ z!5UEa(BkWfBz%OT@@0aZvZB=X?5^jp47vo!1F%-oOb67{dTqb*zUUAwl9U?*S4!l_ z(EnI<>Xi_^EPzFNCoFD8ch}weA}+Xpr#>Dm>j`sl&ORdDbSc`9me#+CY&wX_Ti?7n;b+r;5)HyfkmSBO(| zRnJSG_@-5bzYftye^0gu!RH*9B69GtT!-uY^O@>0#4!w3)H#=<54OONj!lg*1g9a(p@Cbw>D$&9#!*%8*z z6OQGxUF2t8TeQE?zT6)Q-}Cay>=3GY;~Yl9OsBcAIGe+jpA+wWv+k~?smNbsIJH?? z)?&bbQ)^fspmjghpjc`6rhSp2`4^Y>edV>~kaqd81#3lSQN9CyudwojTAoxg77aaN z9CrvPU*~Rq#wj`(%+wFF2!WO3eD!05$yHeXeRhG{C$Dt}Hb-B-?sb4G;R4O)kR^Nx zA7Z>$@fMh9Eg7^Su&5mkf3Ox3MxH$%mx4mRLQ^9Nyx%T$Ypt=S9Ym87v|6{fjx5<+ z@`bTPNZb4?+oa@??G<7|tS-LKp3^lGt1A>6Yg#Lr$Aj;K$kZawG<%y+1q#v^$;D2J zpYy}js=r(*QT=_-=5<#oJu}IXrT*`%aL%m#k))cG(Es^|yjLAnwxy}iPv5_9A3pr} z8)3>Er?9@i@FrJ7AcAl&(V+Ncf$gQkW+Px6X3@wo;Kn?b6LKIy?zM&nOW~L`k+KuA zkq81*F`c+&*8bK*Qn^QeCmm~m9BY@DU&IahE$$YJz`?ge$STw%^)h3@G8KK0iu`YS zK6xlCZIBojW5`PSlX^@qu>JlrMn+CO586>`Z|)xwK@pjLQQ;-enX@aewYI-0NN^9b5y!!U<728AI!x81GT-Niuv~ewcZ7G~AGfh|es|Tlh-K>f5hZmLW zENkI$aU$=mneUCvPP~*wo^q2Y?+6L)e{K}cZsrC8 zfNi94qDC4*Z+`+6b8aNeh>JTU^XM9jf1u$JNg=_ZGFU zf(>ziePxuPwZN8-EUv|XvkpYve2_!a!xmfhmVh02ikRUxVv97aOx%9N^_4*+s7cn? z&I54XtGtP>RMvv^R&yn4m+8%adMT0u@q~g(pM#$+a^{WWKOd!U{8Bgy;DQw2n-+y- zP|+ju67{QlqAt(3c8xJ$c?F^lf6ElQtLF2EeNU3P zXmJWk6TC5ugx)i7{t5WLXf@XGt~`S2snVOJY9Hxt z>@PcaL$OM?G#Z4jBJC1vgg7nVHmNah$HMU-=3cUPrmDVsYWhE^75$!$eH?D?%m5)H zl2`QL{(-C>R2TKdz|UxAuSW0N~;YEReu%T%D%j$yu)Z_G+({AdlRRX0Gh< zIXBf^-G275A$DVZR_zRew;eOfR>!`+_zP(*q8CYU5MP$06QA|Rzp7u!``Ujv; z&F~^60kqAtF0|eE@qq`TP%Nh>NgW8y-aWdLkMggKp4K|PxyD}gCDOP3VwQ}5g@Fhx z4$lNwY{{)M(rb=$zMcKSwKYJbYvOVY=cB1|P^xL8E!3DEQ*)9gXwqduk?UxW88n-f%qyfux%6#oL!4Y6Jw2M4d$8?2+)v zkSVL8Q)C_8k3brtM!YX0E8|NuJ9Z4a8hYy7!pthaBMm_vP6txDQme`7TWS)2q z+(|5-8ro1{)T=dbO+CaFN}?Fovlr~Y%KaCU047X;Pj|kv99{~VBASG8$B(rMwg;#o z@q*dXPN}oj_t#wN^6EtOJtp(RAl`b*I)Wqv|6)#@I4WWQFCrz(<+4w3oGgupzv_3du|W$iia5?u%Ojg0pRRZ%~r;xuhXhhFrI5_*em?OLc%iECNumwdcgw;r$Lj$f&lJy?VLU7f7ySp ztcEG1)wwmEK-45Z$+(N3LR!sN4z;L_oz0ZjGm8lvhMN(_ddD_^BnWHZ7Om}#e`t9* zyoeUrkwK;7{KoZRKRJAFFPCdA){J2qeC-85ihK6m3`U}E~DVD}D06<{xg$-5>FyHCnIGviIEF^y;c(ILUF8L`m> zcAn#01=(IFbH!oHo>=4y@~!j_e5)p2{p)`E0mb?*m^Ag2JYKQmCgWP@Kp8tKpTVSj zz|B*PiA94yR`b>7@M>yhq40a4tJz~n2QBiTVCew2ud?8Ch%_mh1jurDOU&xO;I~j9 z17gITJda7ov>pXM;l!zxyX<(MpDb+L5eQPCC~6Cse>8)fgWIcPC&rHZfLOstcfvbRxcpbNj}Z;W+FZRv>)o!6Esb zqM%!nqgA81cU4o}B}SABMq`VZc~K#2-8=TN6O|TT37$-?z3*d|iWaUeD&#+R*N%=f zDW%f@Q>;#Y*TRc@r?MKj@qea=g*Wm(8z@zyO<3qS z-u!r4BJRnNS$gv|E5Cg78(o7Zm`^v*SqEJ@jz=J~;6+Acs`nsP zmAp1TewYMkqlwN`Fir*)I{vpp-XaoqGZ~^)>MylG&`nue$*M+EjIJv8YHc!GL*2If zU~T6w+#=4OwOKQAZ|vUYAKWlQc!r0_2!|5zCIN+P8N=(#gwhL*3`WpEk-^pmS1bkBs8#@@qz$n4@C3S@7=2dVPP3qKWSQWOz5YRCuF=w7*3sY(LFJb<>_;m zI?kOnqDBeXx*3gye~3OS+s3R9grJ}#pws5Xd|Ily!BmSe!Ed7J_fU9#O_u-7r?arM zJ$?5>Q5+Xa;NAilX7x?9Y)k!<)P$ChbCg11=Xj}e%Cq?BO4Tysp?WJ??XttLVtHxZ z^GwGXTzRSjSTC(EsZ~0)P9i-pH(9FkdZX3LF<`*66VIOSK5#clD0d&#RFv5oDAqDi z=KA^NG;*%uOwP=J$cwbp)EmDxFrX(^MVg5h0@}xf{v?nGmiqq>h~mi3rg-{v+gXsC zgE_@|CbV*Cy_GH8G&snD^(kg_3~Fy=Q+!I^g2&XWKtU!XL!{a;pU%;4)W0Iuxcf|_ zDkp=7`n@s~NTCVdHpP5Q4VAR;#t^z4q_W(|hJd}*f{!};3|NA0Uu&u>elI=K+V*<+ zsuy)Ws5Xh6ogje}L!@sifU0ck0s%d{82hF>Q!JFdx;RwXExPnZYp#p0j z*>1z`)X8T{a-p9@o7%A{Py0Fp2Q-(TEzZFlc2@WD_J!N%bM~=ZC8IeC#q_uY+CB0@ zOT#m56~6;{0|4gpNP70pcaR->@Ewzm9`fSRpk@nR8J6CI#NtDIIWp(heey=B?{>eo z;CZs&AU|kWD~YgOm7@vK#HRwOlz!J*#5_@2~a4anUp ze{iO4@OD6c+4fEX0$QY5#73#xy))ab%s4Dne3ec-fKCYCsKQ!V;rWS7L_np=j(3}} zmor8-D(q39Ize(co=JNw?g>-QF6Tgst+MnC>qo@X&idsEpPnmm46CnTS6C;; zd>67Us!_$o2q7Jg3b0jb&0oO#R3~#w+}?>d!R-~hQwr_JD-OcJ;ml5d!Bc$9nPbtQ z$1@0l6qiP$_L~{bfR4z(AbVi_z_WE==~tH1rK8NM99#x1($bplwXE?FruCX#MXANZ zSj*muT>N@#jH$DoX>RNj%f2h*a{Zcgz)6G*A1e}4{EJ@;M@N^?2a=y(dc6$d32f73 zY&|nopu^&|A);oVMIDf)RaxkErZ}PLrj1~GoU{gqhj5O{m%6WCr@xUeFYdYntdU*L+EWMyO)o7Q zz7SMgl=}C454fEiPg>s$?ZYULhkRdsF<(^vdwl&O3rn-(F6YJ7x9i0_H)5$yF#!4N zl~(Sj|9zT4NrNDg?zu4?ccb8!l>f!Iv!wxgZ>c&J`M{7)ye|g_IkV(OZ1p?GB%R~g zlW7m}{}p8S0&9LIJ*e9l&W?&y)*@?%f2n3(IJ})UdYQz+YL$20XqSJ)-@bgncJsc( z*k$1fPJdJ?JuNa2Zi)ERrrddo9z==bx3H|R7vSVK!}ci9ZF+bM6dhtg|2ZW|Hdth@ zB}q1LeKGCgn1& zAWLQrJE?!Ig1NBMSS|50h%5~c$LLF!>&=u?mTr=y4z`JnAsa){k$CEr7D+H?XifaK z&geo;xo;)1Kd8CeE&dJqFfMgZ>f(rJ*8{_OOS8%0dJv_NcjJ)#yr3wux%We2(&!1T z&OGy2CbR(639`7V@QJMQJk@_3c_-2Pt*pRfP$NQ26(Le9wEAsHz$?-n$tQ?iPirvt zS->Pi^G@DOFI-fL2mygw2LZc<)84zJpPCwdR$#h3D?=@9h0u%-jCBaK0!jB32NphhC=pZ=XA!?=tUD{ui@FAb}cc zj%dz*AbT6z%9oqpNCQ7VKP{~kYA9%4L72GTh(aI;i;f6!F^n!SCRDZ_hyXig%yuAf zuDF8NK=*k?T4d;vJxVCg^uW>pov&#lY}gxKi}r3Cvhc68JtPzLSO|tKE@+iaKHamN z!8EKudE?wDXtTbR85W?gRF z-Ap-Yb2UK)rP5@8e_m6Vy*ULcr)m2->!+l}&>Iy=?tHm;ct7JTt<2|;vhMHx#~yO{ zd;@C@)?Q#77RVQ;#E$99idU3?0cYQ?YZyr`V-c_Wofb5Z4p>BjhEjpfMd~I zq!v{V%;BVkA_U)8Pp^24?$I-7=Ar2Q1BRAIA~a{A+Yv8w*MFV-=O>lygb6EwR3W+# zkpn-K3-u@JodxYNDl>~-P_PR)R+>shA_@ULpE+@txTVv9^`D4DVx~1{8bV?lrJOVu z|LA`G2ihYwNLNPwjTfIo_9L{{Ez0u8XL;1f=DkfNRBCZFNoh%GYLx9*a)Qd6g8?bm z@V4J_Z1#(P<(Ezupahvg9*6f4AefNHJKHTvL3ykQXe}&F-$+IdpBuu ze*gYG3_s%go2p$n&-X&;Nf;^ox!Et%n)DMY($tY45$SCpDsp=I-QkH>%r6W$;YKLc ziRh*CHwaTGg$eGT6-6Bf#UUBxVJI=V_QMNg8%xQ8&46LmwhuHgiG+@CI~$HFh$$KL z8U+oyS*lZOWsdi@)_u&vqwJ&SR*r=#Wnac}u#C-@kk8%4orll^;060=o-M>+70quU zIUFh>AM&mA=@S%qU$r=Im(kE=xJjNtI9ai`{%|uj6gYxEki4ZL7T)l$IoTsBHacU{ zO*%SsJ4UDE`U}K1&YJT6=9zMngS2yFxNpOMjyfy(_K;VnpN z%C#C{`^Cz56tw zD!_om9K?q)iXW$pvQ1@1M_SjF2~T)oc=a)6I$f3B*YpTs8jFWo|06q|Ry`QVU@>&s zQbIKjg))ZFg$6I-X`(UoG%)A+1}PYv4Z0X6wi@WRo!_@?dTqsKVGij?`0~w4jIaGE zD-h&>N`7c(wnd=NXAc1~8`o1c*H^en1e~rcYQ{1~6U%5E9?2i}d;9z86!1;!nG9#} zy8pa=37){O-lk60iyqrQ(n6v`0f+_l{Tgv5aTHJ(fP9Jgf^C|=!tQmZT3kc4Hk_d< zqnM5)!hRT5{{uma$m5ot;4d>)ZYac8dG=D!#u*4hDXij)?1I@5$j_CP!@nLy#7~0h zQ^UMEf;08v2Lrzjp1IvC`2<$i^H zsjTos5})U@m-N4AC${#67|#i+nIoNlW>K)NXE23Kf$zm^YxQqtqA%Smeaz?uWzaHM zXpT#r>czjeo~h|;zo?WRi@`>0l#b-^cQ&7OtM#H3x`gnTaFO_m&ePpajGMeZS2tXu zng#z8YZ@(QKNO~=f!9s?UWl3$tF$HhSC?a>U1m52OOh5#G@vRzF-J2;3uJMKiBU;u zE2{_pXW#^PH{gYnfBErQNW(d|RZrt~}m{laV8CEJZUd3VhDBB6lTz=p|%4h0cN zf`O^Y*VR^_Qx$i_N?`xB^x^yb$@muoLs@4ArOXtv5QU?r@{>-(1(d>E-J~0o7k^Fo zYwwnJs&`6BJ;5u?^3Rf;ld;LODGd+1;2T9#H^!Fo*O=f-R#sx*Ba8yme}KPaavmiJ z%$Njm(ROwrXSe@q#Dz3=zz~Z9=Dz<=yErtNLJ1z|1YZ?(l4Ft=2#l(eP2LTyvII(lczsP@btVR~z_W;z z@e#G5F**p=XpuKJ#dXG`H@AJ$4>wV*S4b*6*Z4T-b~r~q*P}|lO4%asdf5xBqVCUS zD3WicegLNopoS(%9f?XJwQ@5$A>uGJKx^Sdi6`WbY%hSRS3a6Uj-lOa#@y-2$)Ulq z1JFN`n)b>=M5c>XAO?gjsn2ty=jYl$BthqV73doRw(ieZ|CQc!K5PvrWq!!TpKu*=@u1u_ZZY|A(ezOKWFir3m%_kF1?WJ&1 zVyzhu09i*DM7d3893>ip^?R)6k*0bEfSmC1HA?1u;hBnH50F3LI2XrlhF}z6jf2Nn zMI#XB$Hy2#jr*mTirH?C2)npq$GCm`U|b*4O!ZlD=$gXpT)xvWW_#fN;HcWxtSa`kXb%4E@L9;biC+XQz6(x%UMKk+lJa0Tp z>Ez@jpMZd5tOeeo8UW?N)Sg6o6Jep*Cy6rujq`c?;!Ff_9>sFg8#-6dh=OJ(3(R5Z zC=}xky8x=&YJrkc;o(BKC7?g;OhlESHuFAx6iIKwKN@PrG0Ix&nr{BretjOH$@pk^IZL9 zk401lgwf|X+ti}uxE#D~>b&X{Jt^0?mp!9Go=BcM7v=|AD`U|1(Y|`?hJjEybn(f1 zcU$&CT|()Z%cBI0pz(CWFap+?MA^qWJVPT38r+m*lr5FfSbiva9h|eW+wq8acLB`uyn;A~r-o zEJK6pkY3hxBNE9C;@y{Nm73&sI90xeDg&DzAo(?<&%2uYV)Kj@X@fnC7Cb~KVgJWA zC3uLmbO4yq)-pJSI)Pz}=ssSt-@5P_n=&R_1!M=^2k87NUN=m7Z2bg`+NLY&=?PA^ zHDnh?E32aY(^da7>w#7S=vIQMsY^YtkXHwOXL#3+xi9;F34FW2XCh5N}d!iF0&cSB-!C}@$x7Y}Ur?aVR4(qDE3Y-Y3) zNC{LTtw&%Ld0{`XW`quGBpH+4v@h@L(Ehx zkPqyy-3o?M_hx8`$Ly`P@H@kzA2;-FICuo&cCh7 z{hJyHBoHExkkI^WV#C?K7hrWOQdD>A^+l8S@w~@W2)(4x3wqxc{t_qS{%>;Kw!ZUn zqamc}=%KF4H5Q&nuH8?>Nx4JFjpY?k`LTywdO;i2RP`H&)kSs-N%VU@hz?V=-AJd` z-1!m5`}JSVdF$w3EOHq&=E{7~S)mAVc23(5MfSn$xNwI&J|MjXja7~c)4Qo^E7P^5 z=xS=`VGyDm(wV30N~HHopE=%T7?f4a(dl~l$ALaPyxM0UM+>M-NndR*TDZt-&d*mj z&?5`rOLwK{tb&LL8-=He_oWGo~MPIiM7O%!kKj;cp zv1N>8pKa+v4J)fcNzscwW1!$t*!gF7^icGhys%g4F4jxcvz*<`DYvTB#mlrAvpbnI^MTIuEpj8ri`7h1!;R;d7gsMA)d7 zTGx|S2XEKL=L_FV#mIM$@ec@2WE{c$;=a{1 zpW;CA#~yJI5G1R`+4L7uSZy7Lk$fW0#Hr9O14Ld7NGD^Cu2~gJx>z{$G@Oyjf6p9^ zoL*L_0EstGuzXG~Sh;%|B1U}~t(E@LBZU-<#KU=qiG*wBnYU%K0WlXH?^x3B!CB6|IL z^t&->IQw8aJtq;i$)=iaDo^K&pMu(D{G6P_mUa2Pcz7nU@F47TCdvx4{{lm-Tf*g& zOjB5LN4}Jn_L}l!Fh*;F11a2uo}NCBJ}mCT6_%!2dgAfVS1f0rt4SQ+al%J2BHA=F zerk8J5@3BQ$rP=~E(<67fAob-ElpAmPv3jWtZ69_J97O2$aYLWojjr@ivtIO1dQ9K zlND*Sc>23vv25wHfMZ6_PwT$QtS(-iXI2;~Ny!YPd4$ z5k9Ginj4tooMY(QFctquj2-sNmW$Dn^lBIkOZIFvTgbu$%$sF=Tw~w9!3rH4mJO%d zl)(Y(7OR5IWF)=<6>y&bn)bu(r}xZ*yx10`RTvO3s0U{PCONu5z96dE!NZ;}Y~X)Z zo3#Sc$vEfD3e_RNg*A%OJrMzdv$W%EzA;cL9*B&m%LKXhc~oZQceY=1OFo>-ktTaijV9brsx2-da)6 zBp)6sM9njVM27_ea;E-2cN0b+!vdP+C^@nT*6tKgx>h;qwGXsL5rPHNKj1a-S{XDUI!o>sxTs4d$cc}yZFvR%a~kqk$Pasin}He|RI5NX<~B3} zQcUV!Y91wLGwPr(8jJt2yLXveKRWREB3)e^uAtw034`F{&en@fU@xNADOiTp!-}Ay zr#PiK{hf|ElcTf^?XL;Wo&B#9?UGsy_6MlAnoomR0vjAgsbm>e*$vw3-|Z{5ooHo$Sf^&D}8^((u=F^6QkNFtED;KX!bD8Ud1Iffo#TFJ~w?M$+l!D4IHBEuYe0 z=21VUOWuuKwG5b(ASTRT z<7DX37WH_>5scFR$!&oRp`Pi1gMdC5i^BF`bFhxv&8=wK-`hKP@HKoMBjd+$%D1b# zqHX@7hTy3G)G3m-)^Gd%*nBUF(&T$)u#na(m=x)*l~(4%blw&)^pCg``^y2M?vssD zplAEubLFABK0L|Z@Ho}$Mvg!3qH`JWHp_vEsj@eJj{H_W@czniNj&&b7@Xo}Y27Bb zzGpntC4OOA6a@FR-giN{U=n1PN=y%=cBaBi|rkaX&6jnmpo<4o^@}))7&&fP0 zL~Q!p@|v@;B)xlrsk&Fa3|8>_^z_JZ?yRBkuR?=@o%^VG5A`Ctcnb}Rb~7l0JGSEAae&_peDkfMU&oxI6W7Fc1DR0&^dYHCul zNRz7YcZrm2bx1cp$a(e6 zl!&1BpV)l($CdLtU?kx&qFOIss`4?1`paYF7mU|8JN9-b4(7c4qi$#Fh^N1l7; z;Qu{;XRz-#7ePGMDEr8xK#oE?(_{U0Y%coH-<0zc4^&jP@EE8pv^WS2gk%|3iisTC zw^~2mH?byId7mJc7-~VHM$o-;0~TWJr_;AD!RRQ^Q$t31!^U>Sa=EmH#p8G>@Zl*J z$_KZb!(1Q|ibYt+8KbqiTVQ?{eJRSgsd`#=k1Wq#IhIwc$yh}j0fEiB zsfL_spoe!DK?!CsW!h8*3`YH2SgjM~N0cc%f8d(<~o^BVp6Iu8`X`#-Z(X#F5+?OE@ zQm>g;@^k5n(cuTr67b$0fb{YI0z^NJ@4zTA3qqcg+~}V*ks@i!4By3UT@}SO&1BAI zk(V;G+1>m}OS(~snPk6k3g}n6%VrZtQ*fC6EH?bDj{Rf_Z4M8TP5xPwYs;iidsrl; zBsw2gfy~|PirlyF3U^PU4#ryl_GGllvZxpx@~Z}@+x-3g|Hs%@g;o6q-EKOiyQRCk z8|h9Fkd%<_PC;6vJET)O1w^`}rMtTuzW0Bg@7$e>!{ZgW_x|lSX4b5kSpx=ge<4rx zV#3@mKlBFB3!W?&YM9%gREd;8?Cf}R)!%iw05mmIXwFmW-K1aGHQ^vh7hM2p1$syP zE0w)t^t-UsBUHdm+BZ>o_q1aWI+>bM7Ue9Bz65@`b_9AEpqCZ&1b=32ZAaH0Tl?~% z)+6Q+88Qm>6#aIRU&m6K-VRVae(UH0W*?rETN1Ptia%NnL>1x6bhlkB-(F5bGigyf5BpiN(?19W^@ z&UJx1{;9{UEHw*s;DKZ|U>(ZC44Cl}{U~LrSFm3H28Mr0K@aeGTrBL?2;cjpQaM4Z z%$lA?%?MZDWp+sJp2`$a0UT&Gk;fiIjO_>nE!lakbe52bnzv_NP}3YE@~JSstfaV6q^(NS%gv z$7G+2uBwX`2{b;)umZb`iWID*`_5`*zPs8B0=gPPsz-5qN zjI~mceqrguC+?y{Bbkmej}eS~IzcvS9DxXd;5FojW+(eC^k%WLMl+$>bk|_^U7-BK zfa(APN2KINJ(0cycVDnLaGX$vt>+b*lIT4C1;CP|>mm-|o1ZT3bVl!VI1^X~%Y zO>E&_J)6rx?IJfw9_Equ#_cQXe|44%GLNJkCwt%jT0QLSC#@?elqlC=N{(w{F8g<93B$omj%M5b?WIrXq()Swx0il za=!^e+a_s9GSrX7Q4iPZZ}IC`YirMJ+5cNXUHKzzoj@%^`OSp=(@#BNvN$iV#WBY* zzjU_VWM_v}pZ{7i!baKbZXG-#(2#NT$H`WOci{io3ult)#dsnH4g2D*u^)l@rzM9Yct* zo#_nKJ!Ptx=Bgjw!V?Am5$OL!{ghzlhc-$U@~>9UXJ@}p*q$JC&AH87pEtEzpKA$f z@kq863rhI~(M5#g`41??$N?Jc=-Of?6<9?+Fo8N0=wB7sKKqy28Uo1;$-4Hpe+YVO zmdOA7DF;PUAgBF~dUB+NpT0H0URgB}yS!(HbKnuvQKPc7jhsb(PajVPGcOo%2jjd_U8bKbJz;s}swhAe2K4 z_-{RyfQ5vt-coE=?B|uw0M)c#j|FR!b_Iuj++s|8oU;ZtFc*4L^6fT-|}OlJM>wV)b5l3i78utesjKqq)I=5BuM7D1j>ZoKqZwjYqtDT zx0_6jZvUyoxQVE{MSn5%I2xJNK;t-^e8tj#8M`}wVIB{s^4G=NfQ&(fq0qo6!*m#kDq52ch@P4XKcdta|FUv z)kE?{&>#ndhu@g4k#OL>vcU!l{65~&j6#JQkIb+P!Z z#pz;7$2dJffnWaOV`JP!Y~)q6FU3}n|C@&_Cc!2av&EH)4mxBNJ!zB^gPyNy$4+xl zMTJ-@0|FO1`h9SxvppkkVHlFStjVgZi4G~_;ZRY@_`CeXv8!pr8YQE@!h~G~SqJ|W z8e|kP`@`nU=Q!j_((mQz34siYjg^g`2|L`dXSiAFCr2I06q;J+71RtEX&upzHEi$9 zUmzcc-QPSJ+S-F(D>0*Rg)4s&d^46APNs{E@{f|>+|$z0#w0#SI6v5iKrx_Muqx({Zt{_=aw zkDGZbh}xd#XY%Nv>W89x&Z_~$sIO74UogFJtiW{UckeJ@yZig)1~*epSqBr0s&iT# zi&~Pr#&QK%+*ky%d$JiVR2`Aa?1`G7eY$V)e;H@h3szDqv+XUz@-3+hd+Q2hjiETc zPgUCfkkuAQH zxX}FvAx3s{XUDQ>QT?yf*P`a;e>M$7O7I@v+hX@(x5*w^&)e#2>p6c`bQ#4vj2ClS zBw(OKuL+3uBsg)O?qRR1qB?LdDLXQKAA&%dV&RBjh*OEKKL6wEV15Sc+|v@idAY%s z@`1GxA3K)7GbCg7v(&+tcjE*7>0&!;q{8DU?G%X6{Z4{eV{>!(%g~7$%nCWl$?ki9 z^BO-d;^E;bBv32l2l06-WWy_OdkG<8R6u>2P5r=Xf+x#77g$g{@hLSn8rQ2nQ>S1? zI!yNZ>&eu7_2u@{Fv&KfY{%Q-zu^WGcsa<(rDWEm9w>zoDx)?UOs6JJHoar`1rr{zSqOs5}WPtM9(me*B8E{ja5jN zavl-k@vzz%qx+q+NAg#{h*Q=_z&t!=AuzCMtSzAD#gFKU0aFp{0vqcBD;AeTDBGi5 ziy7C#X>4EMPl!@PrkW|s&&A_CYA1e@l0wq~GMzLAW@cCj^Iq>ISoOreK<6yt{lIXA?b+L?i@ItA6j@P|2LR=mSTAa3{IP}g%9w73={M!&m(IX6muw)^@e?*KjLa}n|y3V1A`nW7ZOW@AP_zR z_EqS@=`BmngBb7pRX4A>cFjwb?Ho=-_`1H4GRj00eB4ma>7SmzGvF?Nwwr09D|J-A z_ijO{Zt_;0rTBPjZvP~(Es8`YEqNsLDfIdN5&ek&Y-gVj1s+G3 z?8!4vFQS^;FpVd4*DJgSlbCq1NSWATt?EPLUI?{7Y`CG{2o1pRfhOS0+=HUyj9e{JpfMd2^x0%sKU$@j%4DP5>qt zF580)-5Tm|@wBjGuHrFjF))_dqR>LHV{Ffo8>tA|wmbha>R0DohjMLNgXIW8dy3$f-i@iB{Pp6YCYp{V46dGa>kcQLT&rB2 zUjwiU=HJOESd>+)bZH=%4P?s}bpK-asQfa=?QHwUMVQWI!wZuiBOlhLFqAGHCLY~r z-H5i^{JWjf%^!Dm79k(l;n}ZUa*m-e&_k3{uc)F)Z-Up&Uk3NXi>wkY2;itr#yj`V zZHFY_Az-Oubbf4fXNJgK#M&t>dj|A83O#s4U&LNqox>*eYX@D#WiVTvWNgs?oawy% zoTclsw400U)PT&|NIgPuvcLaTe?<)m4ib&u?HU)|pXUJOfz#~MO6&^%X69&csc!8m zIJ6zMz=Yc-dPctWP11pm_^5%4js8KoQ?S{i>2B%8i4y zsGz27k978tEXSu7x%RJAus{~ZP$=|~s;k^U?Fh_iZY-V=3pFL~0`gmQ2!tAf5NJ4- zB?6~f(D$i;H)J-DGGp0Z&g)InQr*L>lJFDVX#_K-0=L1NL($Djy%oE$K>=*A55UtTm_Y5K6_+ z%}Z4lM&Y*JrIHZcsKG>q+><|Le&X=qE#vIP8O+0lg@pqHdV^M1nV{BwgVy(|B)?(h z6Q_-v0be&u2lw-Eo$*WZLxyr~-p7}|Lmy~JAhn`_iXzgom)9=1Zs@RLXP>^yb+{T? zn5Jn=z#zjSj-T^J9e?X51G8$pWV~Pz759FaBMkT@Mqv~r3dHtc8FB=1^hkYKl+MVi z8>SeBs$I5B5zwEGC|oJb3Kp`yB7(7smqrpOn4AZ5^dt(yi5xG*zTM&2F_KARvpjsnzQEql*fdAK?}dj{%|KH~Z2H6p^*M+l`YrbitmfUHeWdZ#Ash8NsJmC_FdlP?{ z=aBc~hcpH(IvYA^^;Dpx!|Y02T!3%YS*E%fH{zF!q}-pvk&=#e3bWN?{%-8MYl(Q8 z2Hp*S>YZ@gHrG#}Ab3$>o39_uN2tS5Kj;_!g$u=q|7b8}EnD!-9W?S$7vsoMhi}y^ zR0zljuv$-j_@wsI2Spu9;0lkxuVZM4S%Pl2KJ%N>AhCPA&~%@Tp>rv6RC@*?s)Y-Q z7U7iM9cNXX%7Y?KlgPDt%fs*{FU8KQ1ZQlv@Ip{+8^sr|L|0AI@)bF4hksFnkaJsmQL;=t~+ z8uN1V)DQCR%$qCcHJ~&sN5+KYvtG||^QR5S#8QSYOISpDegCTx_vKyNvi)p&lJ}=x z@&PEX?0&&;&w`#4eoe<6y|JIh8vfSX%g9nz?E8V)bN${OTw<(c5rOr%OyM{UF^@foD|mxvPbp0F{i4OwSI= zmYvB(uhmn9gZl?S)Bs<{$YvQ8Z`Tv$&&RiaT;C6~F0_`7`DyBZoKbErPtk&8@735& zVyjZ}zT~38JSC|Zh9Xl@Pj*JEihvXu8q>8s7Ax{C&#R+`p-QIG_Hjy693t_=-|4@!^NyPA2!_ctiunngNWYcIMM^YuJ2om>jjt8d zARF|`4EzoJxl<(OR{1=1wyRpdCB*v$+GjtEDjEsp%^hA|UMeaoa>OB)8*XdB8nieB zVQlPg54yB}<10!k^mBR1$Vu>rhPY<0d`a-Qi(r&&Qez&%;i8eh;JVLaYYt`!YbE*E zw3$PRd&%s@`OQKed& z3e$!7ww`fAC76xQXEZc{sbM;UkybK{U($N&GaEJFo^GU;s}G=&AzHQDIR-YUN0w-$ zs1Sj0qo0N5(vk6BbY;9qvjKlxZJbX*X=2qEmJ}6jNk^0Qcs2!fgJ!&NA zHM9N7_asro08L2;*WRjZs*(|3xw069>&~uzXQc@Vto`*e_$Bfph?`IR1eFNgprE5w zRD}qYR~tLGy?ALH<#X(~^D)Yf5SMp|p5XJeY?;zvAu76Tnqv9X&OaANhk%wU1qfsb zwn-DNxY2Ndvw3|@!-wI*H5nsO&}H|d?FidjO?|3Q8j2chIL2KS${=bW85LUSu7;6) z`dCzecpDKVgYVXUGFw_w2C5-*A#3#VMVxvrs#fH_y$LOv?gQ6?0c9L;>htGuar-iXKwGuQhy32I0b1*64+rL$F2`Qp~y;@hl?Xs zMAk1uB{hYsJZmeU>GBHZK8u3$^ zo1(7vK=~0u%SM1WePKQgN`E-UZ|f2om(}}I;{A4mGXvuop9rfuBqGh+p&c7K$4x;L z-@17_Fd5aLa&~I?yQ8!eH{|TPrMgq_Ry(hwCjZX82YI4%LYpN&Y`-i~BM961b4zu9 z$1gFTPJJ`H(s%R|POZ&L&A`4(E=k76X@#3W;>`!Uh(Tmk#HmOE85_+e>Hjd)=DM>) zi1!q<_!|rsytura&&KF5%1KEHH$>m&FIDS=AZsq-VO+#aiK`5Epq)l~QR2xZhMwDa>hON2rMy7%ci)u!=efD@}O; zAs${?{V_2qk&7^iqK-Ks%l3R3qXt>Y5RS7iQN8#n*&iNkhpN4ePBqq>;Xfw5PH0bz zpPiq_-CCV^%(X?;Mj*hc&^zNC4=zki7nEw(UZ_=mVq=#1gneDoLk;A{^ncC}I-C`wieS6#Ou0yX!y%n<#OcR&J??XF>cx+L>Y28q+ag|RLwd)5*B$q+ zy=QHs<9d+M*R-5t+A+V%DImQa>>a#s^zFz6?2fA2)nT)!g^s0B9a{9kSan&H^r+f`mcVVdtHN0fBOpq%g4G@;r*LHX5LQrv{gU{7&^4P}xTG)JYvZug@H9MSx z`lqJhYMXB_6WRj50ZZ{t#v%~WHj|1(Cr`I5A2+A`37;^(Z8nPsPPdF75^pw|H-^(a zu<2Ov?p{hD_VZ~kF-^R$tLQK9RY|$G$ww0y%D-5|Dw!*jbeW>e*O5+h(HycoO5X47 z|DMtlmAx)sk>(dc6{q%b8X|v=Vmsk@fSZc~O9s%Yg?oJviCBo3n_{dEp(jjaS|0c7W(m+(;dhj5s{J3_RI+eZ< z_gb^L6nSFwyd3&%s2&ZI3y)U2QU88dDZ9=D|6_`vi-VRwp&CWJ#bE8Qdt-Ho_Q6M) zf_cmlDz>5gwmy+A4DcN>J)x2e)v>-SCg?w+4X&48em1d*k55^{LD%|lZ3%VB2!Ug? zvLn~dtna64HZ!?;Po&B2h7k4(2pU12MqX#$AX^kt@?|PCRT8mu z5LM9;%zg%Ob*sXk( zFG3;Z=9$fGcgnld-Qd^*Jy4yuWzA!{ViJ{2jJ|;yynHql&a0=rRyn zM5cTlY*chnNzk#FTRuDV4^x35Dd@~p-*V-rs#j%|(Qpc^6a-fhkxu+x6kY+Al*0$x z!#JNO(8Uo6vxH@lWzWBJfR3cMw?%HHTRTs-XU5rNYc41gM)lPHVexVmIxvXuUG0zQ zozz*`tP;TO;Q{7U zCK_}XI4k|SAq#l}{}}p6qL?o{5mnm4!3Z*pAg0!+r-{aSw;-VZ%w(M9((j5s^aS{rzA38Lmjs*v2h!4?~tF9FBb3HFO3Y?)NnQ` zo^!=Vg9-VXU?5PYDaYICn@Le<_rTlXZ?S?u9(=`K9)xP-QB!g2em0iC&dGlwz`1OuTMy=Uq<) zkFG@ayWZjC;r8FOAy7_3>)OsD(9%ClBi=_O&n3@U(uC5rD>u^M(b)*FU_hWb2fU5Q z_X!8x)t0s<%5St*^jjI#B>8{Zaef*-|_ zJgq7H*OUyLGK^S_iP2M5NR9c4Gnx8!^8%VydTQe}8*b)eL3zIMjOO8fg{Zi?u1qK6 zcd;|A77_%KFs9Zs)lVlgLZ3F2_joHkBOyN(Z~S;V(PHlc_XrQ(oyiCIS?7zeeJiicu2ihO2}jX}g%8p)m37KnB6@SqC?$_Bm| z{Z21XFAxexNf=F-qHvAeP^uJ`6qMsIXiUBALii#uZJ?!SdggFOTb~xXq)-@*{w+oF zhh)31!)FG}wIQZWHMkg^0{pA)B9pDJ zcQ`D?$}GyGP20AfZ@@6|c?%zKaB{}RB}iX!Kkx_%5kr=imLNDn0`|k{)wJ{wsDoS_ z-Nz)b3W!E18W zLFJ|8P@>W{NWeTL);dMILT}_dGm@808-1k|Re{$Zwg%9qjzpIlo_=fo;;6QxxkRr3 z@MuKQm=fFdfaO?s%rz_kvl!KJWF{8p7IQgZ2}4q8Bbe`Mx1D2I!ox2}G2Q3M%n>gp z9usX>`8=-d%c_Q|Iz7)RA+T_8P@7JCmLqKzYxIS|@2R>W`8*kGyv#CU5wcbtnVuIt zau+u*pIR^+hR3J9VGS8%G}k=kncWf#ZAad=k7Zy$^X%f5)qPrN7l0W4@+Guc_PDal zH;i?WEUHzm)oxv)Pkhu~A9}YIX{40`CNE5Db39d=)`e1e&9-Zl*4O`-wy8_*f4~Z! zm%}(hgbdVXe#`#o@ec+Sib$d-UB>uNOTNp|+e8hP>Ni~BGcSbHV=+#5PBOs)|5f_Z zv(F`qVn`xRqz1!==vt47G8#DQN(p#{s)f(|t!YtxG{VW=DE`4>sb-;5N+v}^NZ(ac?8OiO zfrd=Lf76AaQK0}yTTdn#My*(Z@-6!ttBjZZ(4Mvnqm#CwCXSRSJcPjE6FULqR)C(# zaH)0MUxXrY2qc{h;?5?oU}p2{yia*4^dqJ=RT#Eml?Tq!pghDV=Z8?Qdjzt+M)FDy z)iJB-Q%cj1-F=59IB?Mk^m~grkdG^@FCSUngU7-8)s)UH-B0ar73la0?6DHz6>R>`+{84i_bM z;lYZwi0j>iksro-sgbDF$w4z0|Cgz~DicR7cq6*xLj0oVDh}k6k3C3l#qU~oTuiVH z^z;xRf>|dw*cfhL)BVP$rm{mj^6NC5;#W8mDHU2$@G+C%$L+Y z7J|!^X&xVvx3FA>{r&wg;u*Z3?Zu$p5OGt4f?@Bv(Q+LRpI>P{BwHV!_OvCdcljr9b0TCbXZ^;`oZ+qF01<@t<@=`lmJZ$mfv!6bBlZM)3fdr*>o z)l_@FBk-$KPBR&U0K%O~JD!}PlU%gjP!hOk;gicz(_-Zkeq;hQ^6Gz8CYCfap`C`B zn3kM|2~IRf&P(2Q(uZ( zwj1&4;E&qC&uw;=R1DL)d4iB0Do{l3vH$8q^%o>+5@oEBFv9nmwVc0>A|Cw4Ji#1t zJ>avrDicVW!-XLsCs#$ZUT)bVIsN{$q*W%dVgU6ed{Jor_kIGYkvn zIIjtW^F)0T2~^N}k#lgHWY*ND48;J#q7QxbJ=Mc#9A_sq-lxfs#Ay*3+#q>cAcY16 z*}hr4`J|QTD)>Q5D{<6P$oI~XMc;t0B||(gSl*0L^(_)C1PFXfD;5fpK4{+)N4%pG z4@!sC1QS@Rcy1&61w^7>xi=HBljcHO!}14i#@$E^89PAUER{YLVgEnVYK4$!}cZ+<^<|5V6zL7@2sI0-VWu+v~1tW%1g<_=*VZ9?VqH=_evXFXAL9I=myC8`J zV)k|%6`wd6J!+xkg1q;Hxr1fb;9>=1xW>?&yO7$|OxV^C`*Fk-teu=VP}0QJi4;Xl z?h#`hIR~-@#~=D0=4b8R6ifWR3KlKpmj6LnHY0q7^-g&yBeh{iLyke!QwREI#^@1;` zKx?PrHHg$A=xcGm(tzrdFazD1bw&yZ+L{F@_~s`;OB*0rKdKcer0>jlVG88^CwOhBh2zh^JY{{HlPqL4oCzD0mU}q@cqM<=9o)uK5nhSrE(zc>VZU zA;G-CfQKk%)UxMme8kpRONM}phC0@#%v0nrejh_wCs`3Gk&zMB;7^pa<@gS(ycUTupCW)EA_=>uy%1J zWT^Y;N2nuC?%hTlNF=be`6}Q^oNgDn*-G=XKn6^|^1McO;BeEjhojxtK7Ls#e)B+B zmP~Y_rDmnWnZ4_Ceb8VroV*(F+M${wN!g=Fv7o~t$i(%|zoOqFQ$@@ww zOU*mA7H0Y8KO28Q*kQ+X_!$O@Qo%Gr_gz#VUb9xBRoeO3x?{cd)DE~C;4a3*#`bN? znd`q?j*I!+9L+a5tiB=U4b;@Frtw`Tw}Mt!$Zy#DeLGtrRk$DU?_#XLB5(@JOdQ|L z#KY|QQR%u3YWn&hc$#p+|6sZljZ`pobO#t=o%fgSXo$}BF1usBfBt;pPOq~XH`Q`1 z{P@hMwFJ>ijHW;|_Pd_>t|~fTZQQx)|EQd7p*0DoU=7@6enCN#-MsVF-b89D!PCne z)=BPFnuh)sY=_mKO^oyABl>U0GPQc}nO#WmM!O*l!63BOczV-NfYL-blHTLAgm3Jt zZeZ{*smNx{o$lQ7%!`H?i3$osrKO_@I865^3&F$qzdqmU zD{U3rh`A$yKst>a-+X_K;Td}>Q)mvo_;6gi2oD`P*!fCA4Lh$dA<-R9?muw(q{!Cs zXQ?<81DyNB$@|M)m@XJ};lPV*=3!^JRf|zc%XpGnY8~ew^AsBOWQ)kxnUC)hsO3Rd zSVCJ{TU?x~fk8U+a02zRzrtBd0`-S>&xzI5RXbjC5${Be$QgK>MIz1f92nNO=B8|& zPn;0pr?Vcka48&_zCVxvTzve^#tdWs`>6JRJVvEUZV2C=~GGYhpqH@jdUwPt<5zuBzg|<%9ICVMu*Jg$e{M z;|d8srZq5g+XaE{ASERjxxytu) zGdx8b4|(SLiIX6w2qnJQ__?mGj!9cQ$fyPO_H^}hdm9d-n8I3E#;tqUeTQFG0$g=d z&DH)Cf=kCNRz}Dqdz0(F2ITDO%CN(m$IW?tLHtLg0b1ii8gC@(f0;%Oj&DZ2V6K*> zd+)>bA%})5xJZc_@(GUC8g96=ob0oK*oiqgai~?cb5&tsVIwI9K9(`x9>#?97K!m# z3_^>it4kxx)@v7BoSnbCA%=%2r*jMV+%R{vnjMpq6+cZ@>HUNr7E#I)#MKiwf9zhfaKMXjrQbOrtL@ccb!VUIezVgiGBR>WYoC@D{C$oZ za+-cJ__lGx$7W_`IzYj zk)n5NFiQ@Co@Z-7AF)jDcy7%}>s-MkYIiK_aJG^dLV)_kW7Q8|tZ}-uqs%gK5q`T^ zN#G)J^7--h_GFp(`WGRWrHOUy!r)q>-$&`9;$n1CL7aVZ9-HZ(X8r#x^66_GY$}gq zBQsF&OJjP~H54j!4#Bbk!vQ5;KI4BeNY@-%)w1vbyQN0VNeCSFR1u#Wq0@Fv0|O#F zyqjrF)fh67R{!URBh#f6O@t<;Wy`g8`YD>D48=0=abh^kiqdkLT)lX&^G6n{Q z`}J`0WR~}hwsWiy0%6ay^6Fni#KiB7C=I>|9F(!>m=}(~1&NJi3S16Q#jQDWrzwFW zuHR%&x^S-TuWqE|z~$cS8AE&rW1$M-^BU!U+gp<;R!3%Z4IJLXxoUDg`%F-yTNQb-+*Yk$2S2V8&|uu~1;*_p z)M`E%3kys1_34t_|L!dl6AAf!^6&+oeHQyZ8$uR{d~oba_oh#ZhsNzo7rqT~5FYAwItD z6qROE*X0v9o#fcqQ{}qVDp`W-5+lC%1w?g=;`U1nUw%M85^g@8y{tC7a4h65?cJ2h=S91ZMQ@eKw zTVT;j)C<5{k&}~ycZ-qj=?fL)QZznodJ;T%Q_H0OwM1k;xEencwAiKggrm=KzvY9j zm!@y&%DNh3(ngs6W&0|(hqn&=eGD;QHn;U;yVs>ndNNT6ePnKYxxHPfF+$D*-EJ)f zyARoPk&Tfy%zS6BDLD4y`<)kFS~{6Hu12&;r4<9cvvkrjiN2@x{JY351xW}*InzI=K)l*NsaCN1(6Z`#5O zTiwHp#3{cBNj+}^xrFI*C!h78%nqv5cz6+&c}{IgYudfWqSU!HBUq5G|Kr5>yE{)d zHnybxWlI87c>Uy`xtWo+!5WdJ_cLJ+4r;reB>hg{+A}jSfTM9R*^-r;SX5XDB_0C* z1}yq3q~4&iy4nE*M0qN-zMWs2V_OXwwVsIICH$E9v`}wdki4z!B%~e=8e8Zai;zlP zy1s#ONj>c38Qco%))(StP1~Af-s!hFA*~8F$hb^)r7lh%)WQ!}bI}H!5Zzjq{F@`* zx;2*MMiNPoCj7UI>InajVzu#w|n>*GryO8u>w$n!reJ&eV=8wgOl}Pwu8|m}klY`CZWq~?r$mFrr@%3$& zEm-GAg99+W-^rk|v8KlHWT^#QYi*c1?In%>{_iLPvJ>rh1}?5TVBt3V<2oJJgXisN zLlJ}33=M@HR@woI^nKVbjv?aF2)X<3MLBA?eB>qmSdzeM&k{94*>HiEbn(V zH#bjDbK>J65V7a0sStEg`pA5W=@va#+@?3+SU8mjjvS8it!4_i6h%o6UHWQiO@kVH z&uragG+?d40ou@!NVbrCXJlj~EiG+iludz{PY(@w(W|o@{hKE>GkVWx&!gq@-||dr zgCeK%^E6HiWd4vfun007yu6LTK<$0d?yUXacT0v`y1J0i4PR)gir`{k1e%g&XNmf? zPJEIUEwL&6zaI(JBW<^Is|S!P=3_cE&%Y%`+t~H~e@3 z)nBq#$a3$Jzki=9QU>C%B=QM5l?GwS{4-hJ#9d{+GxG#lt-YIx#L?6EN4#dhooE)T zbR1T-y}Uf0rWY03OW?@FZvMT}N0>Jx^&X#xr;3(lVPKF~Q4w`JNSv|R=GV6B4D0&F zM}R6kE+;Gda<>_Gkh)67ZNJ11e%RSDAb-O?;Lnp(5}R1So{jfL;Wq(4Q`B!17Wp2{ z!{c`FI~^Sjjk*G8Iy;qImLQ$fRu5)kWF!*ve?k{~f zJL*~*HBd2rd)z89BaJ; zcnaX&76I5;_EhHFPmR;nVu*dJ%8#=y5Z#J9;;mvDA;~+i1DA1 zlpziMU)x%Mc05=un&=aE#@FxE#FY?RI@8>Z2dQFmfYsiLbC$0kmW1mrZLf*HeLu(J zZ6RVUi8xH6%ZFPMiLJ%gzkVaW?UWSQl5eQ+W~ni3fXuZ6mfii!qhO+t)I9OFGYSeq zhnquslKjadCk?TJPtOp-o1pU&rokegGr`!*G3dmC5_r0GLPiE+o|OR1Ox!DY1hWu?*umJ5g@VgEFn3D&r8kc zT|qDf`T1vlZEg-k30d*+>>!ZH7@Y+X17Mq`l?7szF~-ciy}g0q)fPCJ%`d{vSwaJ8 zBZYl{aN!^Tc@!lIlEgA{j^uvPGcZ{AFLD8zL#(W^f|^ql=5%FJT|CvUEx@(^`1r5p zBfs)+3Y%PQxqb4o=UP_~IEi4#S9?!3EkO7WXq2IOps@3Q(4u=W<+-*8LjUKUFK_Pt z0%xyfrDShMU0uFSd~)%I`!{P3QY$#iXX`z3F+?)ehPr&SmNQ*9z^*}={E5Ky{BdHg zr?K5!9*nWEv(x94+14gBMaWPH=^8OdEYx{dUB97TjH%$2hBbupcdtm;mYe7t@q!jF z+rOdhYyv=8&x6w9BlkzY+hw}Q{QuIumiHIi;35AEFEoC(ZCZ>Y6#{KJ#@_pdeKEwk z#i$f+$qSN1e~WZ}dfaUUY0m4~Gye8Qqy1j`P$!PtsBVW>#Z5&ob(3_(&Qx~6X^HVi z61zg;N8JX`kRLREP?9nnrd_AKtUM-_aB0qfo#0L`dFC>;_7U3YvW*(`6@nPNH=as($xy2 z40fb{#>wOjip;WLl9*4t@bLHd*U`Znzx3`fYH=X)*^&&rSlpbH*f!8;S)9qeXg-8g z?YFnLyB*E~YrkOn6Hp$xxw%j<@W8388MyM31U#L~85kIdi+2Gkjm@z6Y%@*_L^gm3 z0Q??5Ev@X}Z~j;z@2kD)&X@G8tk2v;i%kw~8FWzpa7`q=#4+7S}vJMHzK4NjYAA!UiA_SoFOg@L~W&81w5%A$4$FI{8j%obk zc(Ex*$OH6a#E=MZiir4a4<(hBmVz$_Y_%h3!htdY_X2#$7RR6(;o2 z32^_wZ^6OAfk**_e768w|7YVt>;hJ2y(c_8G*nH2i<7g_Zk{qSAK;=+|7S;~_}gc& zi{QaX{xiUSAm7jx^54sV9gmEP5)lzOZQ0GXUv3Thes941ANwXkM^_A}`v0z=RgRzg z`9?2bOS$d-oq-5R?(5xBiwj_zwDt7#KzQl&jDw3?sb9CVHTXSoQZ|O@4GBLxHFc=l z`q6w{o5yM6;xP<7Dm@De;A1$(FM(Y;SZb+b?EiqvQ^~xFwp1fh1#k;wZGGM8YEK=g zKcB6iojIm(xO#XPH`);z0T8nUvJ#L_6ZyTh7Q^uw6!iwj2{ekJzLFlanaNKQJG6r` z>m+5jE1Oi^m}Cx>D?0S*kXz0HW9Kym|I9%i@nWB{#XuR+~VS0V2cFZ4nhND<>Y|# zrh3CFTou^{{54<(z|TUtF5CCPP?GMcsXD+8KoVpG zmz0zwBw#%m2%mMq97#>Ky4h}f?19L=hxb1YY;#iz@%-$pYY_pBWbg1W$`w%YrUm(;RV=AFMh3*aulzSw~0qx3G0u z>7$mFIzC|IRMR+n0ktPLGCIn_&VE)}oSk5C4^|yq`hk~L_hUiFHE}~pNR9Or?a}Yu zoPer&_`8P(=&ndyrg(f^5hupDw(m}Lo=bo|1Tiu(0Rg|=Kla8Sg6>BV)wg3=LJWmq zivbVR%nj({w3HM%h7OR{S3rQAJ)U-IDzF*<8d+%qvISeH-c+LSbh1K|1CLqa7)2Ze2=_$9t>Jt)`EiEsDwfW#DfX3oMUy?6f7x3y2 z^4bo|Qs^R~N*`w{^zT}4PFKk@I*bI+_N52IHk)Hfg*G<9n~~t+hH+OIG(>8&n)V{& z6dcTygPsCe1#5l%c|eej!78<%0~@fivf_CL>`9M4z?XGK)pViY>t_pjs3dQLNN;Nm z^v?lffGaa$-vn3*7>KLj^3Cz0h5r9g_T7(Iw{O^Yc9F=QsVFPSN}{4DvbT(ELb5}Y zkjjjVWE7HFW|2{mm607vWkkscAtb%W=lQeEGR|&9A<>V|M6_ z?6@o751z4Nf5>4i%O{E9CW3(V}zDw*BLlw5Pk`>g1GPQE_;; zZOfhAzM%`12_i1Blv!ml;&!eqBE+6%oc^z`}@?66T5qi103}W?75~;UyhF*SRQF~u5J0H~DLG!Am zzC1mg!cx&4+_DTsYfHUa?ypPi%A>cu2FOwtQg9!mSd>Na{MA2>zl3J@jvYH989ZdU z5l<;eQ7UEFf+mUMxLWYkY*z;P(^UKri)s4dOT1Zr$RcN&OfS_c3u&mTD)KhF4+Bud z*VxZ|#SefI zKZqx(s;;iCso4}FS{!8VB9V*{qFo!%l6#My(%}yUs$ebJz(#@AcHJW-~E^C_dlFcK(#Zv&4rqGlN||UIl5}(D4ieH-24%0M zxYyh;$ARC2S%2oLFCpZSf{G5y4*6SJKC~#x(LB$uxjjpdA@nq_pDpq?{Y{(M56^0^ z{I;m|f7b6r8PuJ1I*amGd$Ob~!I2%25LG&J_3E*($Pp9!{9p6)G_!Qk2Z4(a$Z{EEl!cxdi{@xOpx?2>VJ`<0)5)|^QPD(YZ(03qXlkUbe11v? z;GI*l!}rRS7*6RQpzUBkV98>oQ0#Jgwwj)0ktOe3VoVAeuY5%O;ZJJD^D}v3nEi@u zYwkO9S6A28Tf+RLFVu<*kmaOpn&@WQQZBt*Tc0knVf3!t9M)@TJTdy^wb+-2p5R=Q z5J~&Yp2HmEq*&f2;{{Hcpq31FU=@`ts|<0m?Ye3Ro$Q>P_c4q3-866k4{77?Ut*h5 z#3v+J?P3DsxpU`Wtt>N)HZB`mUR#Pk^p5K;3fQxL1Gbr+7fOtuU;1LlzugWo~AzlOAp*Yi!kJZjyVU1)Y~^+5T5eDCH>YNv++0slX{VEs?AZ$F0Sh3wfED?IVx+#lK3UQsv-}wDL4zWJK7%Ng5JMf4#BbON0LBrG z_3o*--<0C|jWQwfX|v}^mGbmImlqe&9pKdUunva{k@rwA0>E|_G5y43m)zc|J3&=? z{*z%DlZI9FE}yDcIEFks6$V~-It2WkSI&Pa7ba4QjJ0dYN!Pyexo+jw9elkLg)@rT+f^Iv`p-}n+x@ntt`$1UCFL zTk%COo4fgT<{)U|XU+qek_-292cM}`M^flVwiP`dx;0zqKX!Y6qNvH9wnacv*=PuM z&H(`%_&X+m2!`XOv*M3o8-5A+71e0Hw)X7-4w9^6*Fy}9M80;ggZ*6Ix`8J0*eG*u zuV?JKzS}Dj3df4v-S?H8dR78YLWDtiJe%-CpU7hy;d~5tXNodDJ zBG8Td8!^n58o3e_$~HnyC|47=|iD~cMe6*}jjgxUXF%k76lAkqKhM3os=(Wc6HZG@PC9(}u-88wwcj&CAxRkreQ!<0Vct;_zd_3Z}$S;wEK$r1djfkx6JFd=; zD9-F#ce;x5@IC#ymiy3%Y@tb6*jqz2F&Xb4`YuNl6nGq3VN%OFDeiMq_x`s`QZ~L#`En{``5h*eU{a|e)|b?*smE6W+fuNod5 z{TUk_pWzQ73r|c~-FgqU;=J!D<#09y35R0fN|IIg!#g+7A0dr>MjD14oqoKUZJ zkJ4#!}Ja(5mpuPy)Sl8&xtfNv^Y`p{KtJk%`FmDk3I{P_jnBl7bYt z^5tb94s0+VmN7HeSt7ZsD05&sD=Q131+U&DOVjL*l2j(8!&c(ZF3Oz&z;DYlLz2?c z{aE1I?caZ&sAvNHOHM`xob(1PH5)7I*wdE__#4+@dJPZGsK==FD?ci}-EL>%iim)R z{}~t%_&!Z@0P`7C3Sog^H*Wk*QEoaXA|i5DOKU%9)6j<; z(H76TOb#=xW$0)?;^*csd)xz3C zzk0A4@Hc7m+UoGAWHH(}nuLf=Nj?277C;^t5HP=8@uz9OeD=D?%S@qK>m(6{TzQY$ zgX})VfH0Nj6sJ#cXav8FAieJBcmm>iMBQ^tLbU%8Q)*#ZPoDmjZyclb^=%U;3%ae(W;16ZkBz@Qgv@YDU4| z+o=OLtPEW4dgq7B^0N@cm%*r$bNP>F22(@%J1p*M=>L1_wDWBc_oL$CVy9OWnOIQW zg^X>QqaLAhzgm8Nd378Hhab%VxWYf5OctLXur&PS;;s2N#lEPi*!7_XixIfhA9C8d zj@bRy*4F(-n5Y*VJo2UXf^X5QSN~%7o)$O$?Sv{oN7BHi?my|~=~MC-krqV`DoswG z7ccSdr%}Qk(Yf^z;n&C$n)|)=%bSg^U%T|_2m;4odMo}SFO>D+520_ zE9lZ`fVDZe!z`U){WWO)Bq^cYD&AXo>NO-Fz2rJ>U7`wps&2W~)d85ZXI z^#Mx*yeE<#2yF1`@5)EHlvKn21QGq=o#Hyt3_UBT<$AMObU_?^Om4-S=Bh8_4lA0ZQZ0b^1_dD=W`RH=7of6$bc(z zpCUdRC^7)ZY2VU2;0`gfuP8U%lu~qGP~9F$`*u=}+hLEdXZAJZJuqge;FR9iT=Fg7 zz-G>6WFtJ&7in>JsI1vhY7-Fr67QP*>1JzIdaI+t@`Quw0pc=Jg);Jrg zl0^`OfC+QWyjMi`?R)(At&V$2(3Nw>7sVScUY)pfBt|b$ijq|5)cQ;Q>Koj*1ku&U z#|PxGj*bpoaVl9TKkHV73|>~>JwfB{>DkuOa!=~Q&ZmFwKR(l_LBhp>gum*rW7q5d z0L-8Db)Aa(yFFX;78lmgr&^aKhCU=8@AC7e6N6zwZw% z8q_#5E9>VUSM%XCkjaA9gu3(B!on0l4q;FlDC*o+k=5`<*!0a4Tc5+p%|fPc<~3E; zL3PD&A6W!U1mbwm+J*oNejL~eSjX0u-__n_xUxjJ`IFxX@*Bc+pR7QS_GF(08dk|# z*jW38Lya*=63Hsx9s9H34gjggA>B2sao?Z6Fa6@X=gP zb{l9~SiU}?RVZj^#`T8!Wl!H!T}kDWnC^2PKhTy1D?_nUNNvlKn-45Dp6hQ-{TIKB+A}1fX_ou`-1Vko@eK@ zwTof90#C6vaQ+%4DXUx7eXJoT-9I0RpTrlw=R>6{>)}-+dHUOT?2>g`f7RgqmsmGa z?+S36Z08_-|5W`OgkH7JZ1|X4#Z|DTcP{FfX*6zxKBuCAW(ws57_)ZP-5MDMR+c*E-xVs4PxAB+LOnRLmymw` zD>ooPIw}sOSV^nh)Hf93+reYw)72`aYH!SaKuGJ;%4QoqW~rlm-_CX0&3 z`F9g_;4*&^M#MulO(?^=ukBaC8C9YTiBOExUGFzKkNTeEi_^CdJZb9QAR2HL{>1F3Pt$FbtDxbF>yv=n* zs#FpjC9(k#-3}a>1$ARqYP?(K);2NpZ-Ncl3=K zp^G+tT)Y(03eIA>N*b{`QzPQhhI$mFAi?lpKJKGFE)bA$K@|A>2C&3e&0Xd`VEs!_ zCL(Z|in7|e8GXET{hC@FVJoe#SGsiRC%k1JMVp6Qo+DkLGI{RUt+eK~p<`yYff5aq z|8P7HU+DQ7CTUe!6aFm!j9|(QR4yM~o(Ba5b#--3Cyaw`xIFt|!cCUwC}3wd{<|*W zTix&U>1E&*bR=QZpHx!HKk}|shp5VNB!yIU@9xt;^mPzt-^}JtN=m8#Wa|rou>y)p z1M4-gryd@qDp_znnOFNPFD$%x;bC_5>L#p*LfL{_7?nUZBqbTy+QR45P?tA3Gfbs# z_EC0uZEX#@!d<%;D4>lu{v?(jmpPg#=p=O?1r8J_-%f=(K|sI08S&vQ2~&wVn@J9l2=uVK^u$_>*gN~5C@S`a}S?$i!g zT+vwblD{(6ur*DSnuI(h=Di*)qTFv=JdSB&gd{%V!dhj%e850{?!H!`b(h)XQv_1f zA>7STJVz6Shjqo0%rCO<*mW$@>!L{@LyFHISIjb*3#b0>$>GL%-hZ01^>F@E4}C@WTgJG&_#QQ zGn(&c<0>TyNtB*f-WNm_xX78dagSQsi!8go^+>zC8k^P6;1rp9Wb{EJ|2S>yM-hR9 z?XjY5>MqSWMvCfP)xB{!5RDSJxR9UU7D~>8tLu41MF&%uRfJ8&UB0th z8E7exh%O$Gn!a|{)?<)H%N zDk+h1)vE2U2&bmgNy_#!W<+FJb^+K5k-B|%ZiGM*RNk!$pB7?Ia=$2*<4Kvq{^6wbMd-66R&|wHrZE^7?nxFUcvJ!_D(P*GtDq z+83IK9>}C1?SJ z=my=GWZ{3I{oWxM0WN-1QKQ)%%vbzV;EI@-e9gn2mrP3S_GO=3kNeK^eFhWiqr;55 zpYcHG8Q&em^FW0xw`yZSk8$)nA&JcD*MwUw2txS>qG6HXPSTl}hk1#mPV z3U!%Z{?z!HF_k$$&U8mYbev%L0C#r5^^z9|W>68$3i^-XDZ>ZAX5(of*puhNK6H5wX6J}ow;cQP`+K#Hr^UxN*w?vMbS6vYpoIecqp;zveb0&!WsT5n zu>9xr>;o)z^t+DLz=@8sGv@YfKO|xVHx7Z3_G^KQU*CfMg{Od-1O(TQo*sl(dXDb^ z&AUL8zy}XQh8OZ23`v%oV|ZV9k)N@rd7&zIf;JmKd5m2etjB7@!=~JAE7920MD`j# z?JlStJJXdBA~BK7M}wBCWN|AcqK5!WgCD`tk4bHLeS?;bC<`ET0?u`$9x213prSya zZEb1MUdbKfy3Tz%tD7){07YQz^8V0AIXUA;u|YQ8n_fcIB_<_Rj&;QfST7mPj3A3| zBSA|;Gk}8s5=1g*JSH?N`E9Jai}pnY{BYWiTS0-uufgGP`}XbL9IZ>(Ik3PyR!@#g zNND`EfhsT^j{UEE@a?}v0HfWu4PZ$MfYu@1kLuT_G1D4Ab;xW|z)5Jd0gX0T>ArV` zF{cOwKa7iSTUvY(sqW*!0X)LQ_UQ5BV7-{UNe{ao3^>9rYX)fnW%=GA8`vo0cYVtL z?8nB#8%1+IV1bepdh@1(t!+y`?OCf8Su+)|vyWJ2+*iNhwYn@q4w5#}3Kfd(I$A;p zmj6CE8TREw`ZvO*WRi%`27%zF!yzVG`UZ&ILfNQWu+yl5fXmYi?PDa_=|;+%u|?iC zH&c_k_Jq7xzw-zHqpdVxX7JGY^XEyqMnx9BzOpa>f#L+VS>+8LyW;(|B^HEPfZXoT zZhML3=v~J(g=#@N^Vab>tyDs0g|0Qqht3qH9aYuUtcXZk<79p0F*-tH$zH;_ zcJN@eQOPwO@u=U+UbB@1nTSeqNE}_!4N%9i>W+&CQML*R-Hwc;2!iT17Tgej1ZVrw zVDWB}8+S7m(Led0*@&E)euQx*0vg`t5aR6cu-VWBIoq~ND32U1Hda1f8j6^Qe+2Tf zQVRl?or##3SX1rE)&=h4R|8mh;_Z?(t|uk2ZDRV^5s$Bkop@jdQmgP;jK_lWk!d4! z<>~ET`cRkfJ`Ak{07Rod!BUS3`;SK*HnZ45TE&TW6C%73=LO8{BkMtw3o|hqFtsm* z(ayjbLKcMA+Er0)*v(m4Uhi9nPXh?E?EjbGx$0jUOY;nTbswx)TJkbGfsYP{?ODi3 z|2BUg=YAY>eDj{JKMsg(0O~tASYdHG@8msm>Q`fK#9IL!?|0=&k$Lrdr)RuJJSfgj z63^k812AB-X{n#mRSR5qcocT-2rrGfij5yLU|=VTQYgPzEqL;97AF zG(*dl=17h*lvELoi$Klm>+4k1)c-)k%B0NVt-Q`lgKr>P3KvG&O1N`>o1XpyaqsKo ze}(mBR(^Oh|Rm?eTzW_kh&mestHXpLt9I`{26C(6~(^$te8pZ zsi9uyMcEn;L}7zFRS3ei0zeLUE0--SKErhub~jl48y??j>tooLfCU6dd&NpfYAA!n z%yk!KZ95RfMTCSDRo>%)qP*ujZZMgMv@(c_9#PHxugg&B1Qa)r^N!>{K0KR-N{FBv zc{bHOiqJF@B5_Irq4?oVKo-tOSmUPIjk@Q+;$sv758znHA@u*2u;R}xcnII8rZ_-e zBD__9+QhRQEj58dZBe%G74AyKX2+eqONf7?4afyoT3=Yo(^Gq5Bs!KZ8Pq4d93jEM z#k7b|8S#YCa%h4muM!fT*}mNw8TbjVsYd#D$&&jI9^5fN-t10fHHf#!*0DS>?1zGN z*HIzqcaK1k(U#o4Z5t8B5UDNvrvzr|5xM|^lSEB|&C?O#5M*K(wg#h)1*;*Ljm)XM zPUJeAc|@)y$Zu0aFFf~Wd%>`H#?!@R22c=IXexT{mR~inX3Wmc6qp6doKCCe!OOJ4d^k|+rV`;KYl$ADHYVTef_$vSuN0y%3+S=+rXwH80C^RgL>@|#>(Mc0u zzgjwj3bPm$6f^6t2NnGfYwcXvo(@~nt`wLetBiP>2V8a!pzMcHAO z$YbZ#gkIK$GAy+d+R&;HDvCF>pzi*lbK#*jwy3ev87w)+7En9=j#7wM)ndFksS)9i z#!QK=81T(~d&!YKq2+KdBu=0?^&Ue{iOp#r9~g*(`G~&JcwIK$-!o;MW1;jQiDAeG z=Bl6aG%<4NCiM4KRX?0ALS#9xd-s-@L6_&~y;V8`a_A1_$e*x=>O!ZcaqXSMNR_a? zkt~7M(OpFqggwkH=S5@sS~{=YH#R=L++#v5r0xWb;^BqOzkib3>v{XYqbQxh{*TY< z@Ez`7lAx3JmgNSd-V?g?qd!7=u=?^PMTYdim7xM}x4vAR&^y>bO=kZ*70~;P?Y`1{M}&n!1Vj*x2jAvj76~GdL;pzhbWTL)~X45ysV&lnAy* z+1aDjztHceqXwP#`X1i(=)7OmxdYQE;R^fY^LI?-@*eeRw-m3eu6`iz7uVsTEVCT5 zC0H&nFPKjhCJofV$Z~!l*|n!19~Sv3zB|4L_-G4mzHq?Tbl!dSdx`%>bkb5)Eg5ow z`bkAX{zI!4_pQM}4608cz zKcaD_U0SvA>5Z_kvCGN~p+LdJfs>SEy66|*gM1FAl|=GQ*V58rQ)Ya164iq+U8k19 zav$IUupe?f7Sbt7g^7{z5aK8frSJtK_zoAc z!}!^xxHvLWaGJD$Nxc8F^|S=GEgJ zLxAc=qGpaSrUgl$=CHSV0elt-8*l=D++=BXBPPHr4W#*c&H7jY0`#a;^M(2yuf5l} z+3yvpGdH4Pr*@%<>tO9t4~7whP%Wu()nq=UI3~My?>NjGld^Mwmbllr_0gLF+8@lK zLyYBPS*>s6TjO=Tsz(ay<&RYwFQ}v%Z6r1|HZBAB!8;8|1WTOr$LF&s$9z!fUt_uv zp(Lwgo7TJw=pBe3iKhb(0z2JITiXra0qpk4x}*lNQ0$mT^!Vn+uTGp%kVQb}FY*2u z85x0jtl=6wyLE>?Q%l0~o1xMZyMyKSy12-2wfg%;RagIk!#ZkUY4&5%xGZ>glf(sq znh&CaUwmp-?>`!YL4`b(%D#!)WE>tK!eOK3hz+8hth+9?@C8B{;CUc?+{h?Lu5S41 z=yYSppb^K9bWh%|G~!kInnQ9g!^FY({6QfT#v7e-t`f=dbDz|X9Lr`_2*Dw%Fcu{R znOmM^CJ_V|e5kk%frm%IzWpBhHnH1rlSfrMoBwFu9_H(U`A;iF#Z}`huz$d#iNv(M zJ9Dl^fRFDXioX^K92$Pro#(ubPZx)@z8t66P1veO#W|xQgPdhf7 z>GA}l)e`4weCy3iA(I`ODqOZ!R->SnZwOZjhU3%h4@H-iaY28kquc*lGDbYia4rzO z*}1rmUfbM2i3#Bz1;Pv#y+sT?8Uorgtu>=11-?X;36y|Z6({S+efn6$^lvax$zPru zMinkz(6hkY-j@_GU2<}wh4m+ z4Z;~BBXny(TzD`u$jC^b;dfGGpj>RHZRb1!d%A~*2RuIew9-@W-YtXBiq5g~RbCfx z_B$1RoAnn&Fa)cp7RrL#JDvUwUF3-Qw3-FSBYNsjQ0Fds!zT(&7&sncN5jGkXj|g- zvD@EeDo1Ev+dvK`JuEDw+cjGM-^NBUx<{){yaI*@TL&?tu{*e$j`{w*e-vMimJM+; zGkbxQ2M=mcU^tqG!uG!U<2!d4v;)M8LslJ7HbS%Y!*}4^m!GYH(26+l171S!?mLbU%-2L}VnPk>J4qqujX!l+DWH4r_Y< zwMJLEnc6>J?`mQ5EMqzd9j8=_t%QNsfX57#?R9vd%*!2hz_ugAm?utoUmurMWIz>s z7+@QZi*Uf`Jyx%~*8MYDtr?y%!$Mz3t$u z8f*sWIcUd!sF?%z2zd}VWPJk*zlnr~2Hb5pa+!RhRP82J|4G(5qXldE;99t!=fLj# zteO^+Y6p*uLPsO+5vJ?xt6q?Qg*q9vAS(;YsKWsfk!fTZbZWr8_pYUdf#Ul1+Qlg~ zGB=Lxm|Bz5Ya*IRG-SZGisuywh=D;yQ9pN_i$zM5v}=tA@&;G%5-*7~TPIGI#9$^v z3r#f{oH9zUm~mV*^;JmkG3=We(dT3JSL~WTgYNqY%0dBc-xy;8c&skOBkwD|+ZIoP z#K6Y(yH%tp_4u``AZSs%fA7mb6>Z20{7b8ciApGM?L6_hy88n8o4S=EO^hGVlzWspE$E6^a&h70`eva&D)7p%T7 zHfTP2y)CRU%boi$Z1`$ZSnK1^QW_c>1kNUU;b?$i!m3Nkk~hTj@P}f42^k5+{yz14 zZXi>@NTM+L3V9FZri0;!0_hgRHqb_3qQ{W>6+oAPGO1frUb!xB1-OE348w9`jDTq^ zssq{lK*D;676k+y2|Ouum)?E5)WD=ANNuKC))vcU@1|}ui_{FSy%*Fi*e}Z$TaNzl zl747DgD6LpLs`4csIfmt4xgWqb5W!bRt=%-cIrnIax?eN*W0PCt*_4wmW1e5!P&jS z(KqE4$`JntWxzgFT@{runA1rcKd=4EVhNzf4tc|M-fx0M?EbgZw!ji;9M7*J~D39KY1w{I$TFHUx;OTkt(l{Jjb|xw(`331}9W}{r zTtEz66BFS>+XKEbcmqeMn(iFN8^F!fI7r-vEo`W&rnc?ra`+>94vABV6DT%DgvzNT zmH@**5A*1W-no&1eBo#W8al4@Jd}!)yY-Jmf;j~3qt?1`^OI|{HgCXe_rp!>$ zSnI)@$61h82s8Yu!XhFCnROsEw*;Xg65kgFlZQ>L5C*NSvyOp(5Vf8}p{&wMh}^oQ z&!0X8f+q1@0rIR10bcM8RgHHUcU$&5%R;yR&>@|yZg{$F^zTN9gfu&fZp7gNa(Tob z)VzeL{&isG)!p<(NXUdE7R-$4etcx2LkmFa_w+#L$B#$h!^ybGy9g#?X~_lrCW?U1 z@FpYY?-3{!ZEk3I$ksV9a2Q5mpw6`;cj8E-Ad>1SMSYYyCzp?4&WxPg2MGP^>rD*} z0iZOYZM`xqAi6yjeh=@Z32wB$fIrZ4Cz!*akvr*2zTObB2cR@gw-Jn_*#SC`Dc3 zgYO!bg+mCc9S8zqQvxt5sTSj7!-R-Q`X4`5!{AIf=_=iJe`qtO6a3GF58#MrN>BYD zgE-CpOSht;u;cUiRasPt7H`>Ta0x^r#6IcG%@tIQgYBCa6y6cf$%oY%41|1!X%DAz zde>H0%Uy;RF|8y8O&*J6YI%5c;36Iz3sh24f=^By`8limJGS-Q94`xtG-M(SqM!g~ zrZ&0YNFD^$1nCm|?!#a4x*t%ygZocW-v=e~&Ye3z0(jC_P;2JuX6z*!*SVutU1-{; zKjVV{>4m-V_wQf#@jz7V5NbKOxkbbGgzX(heTSF{00U2_Y|QUr&rV3pXi1$0X~D|+ z9v>W?Tg*nU)_)Y>#Xroh;OXsu5a5B#!TzAGPjw^e{nzi`zvDrR7$|G=4`P7o4uvIc z)W(>Ij^RoOb_hp@FVC4R1sQx$^hzN_Qif0npqRKblKZ~s`~%(mey0M*c@Qq_W`BWf z(UE7s+=#tGJW$X#yYqSA^F(}vMhTP;K+7p^8B}NadiM`7zPSHTFdYBy$EWHFi=y8P z3m4EHiBC&tLD=R4qi!Rgw{EBa#&8L2r89EDm+-vh6%;N7)e6>$x?m@AaB$!;R1_8c z#BcAAsYp1Vv+;rG2O!O(eP2&G?UpHpK#{@KrUdtVwY*b)#beSHH#L(^92XD}>lu5M z@uY`U*l>UT(u}vl_pqBfFWT0f8iohO4IPX(w@>u{d>Ak>=f&30*f=`X^HL(>dtEP-Y-|B}J{1Nhn2+ z>+0#DhPIMPfsbyh|3uER{}GXChwK6Iq&+=#&($T2=)E~>N*Mc$7d$(pE>4o_$NQ3N zW6bF^4}>fScTaj4uI;yYEU%RMV!g{h*daml#E^+nN}$4^y}H1~h8MdMU&NR=i0HP- zMeog6A3fC7qp-*9^J2O%e~e+nKWdhc5L%^8)7(DQWySCOy-`Lx#a;#w(vHFk_z^px-TEKT1b+c^^d#RoLB@m z`5qV>|3pg}TB_5Prk&Pwo0FL+tgz)!hT@qm|N2$%oh5%?yx2cyi^w-y!nc1Eiy+U5 zk~_REltgy4FU6-&Q^D+sqCiq%Ic;m=(1)j$h6Trk z`Z;b+Q~nv&RYRv|4Z<0mF~@*DjEs-lI($w?Aji;8;2*Aw7X4PbFw#hCJG@K=-JzrQ zroO)2{+CAVX;)7++|VIDQiG}i;q>1Vu}UxMYlE{ZAUGkB5VR^PcJ(#E7#l+3BT|yJ zH8Mr>>SYb4A|>S~>?J;5tLPN7%oHUb{(iN{w9nF;HzPnzxo!FYPf5zh z)mOW=X-BSHycbc>{l=QvEZta0wZc{EX&BdZ{Vey}l%t|8m5|-&Nl5w4fjkr8&-rL% zQZ6FXgMYwp`*Ue|_W5-=y>|x3rwpRBjUBG*d2;bKP*8ZK4;u#U-&{(!;~MPAUXjKx7n+wW$$~nIu-$* z8Ew<{><70>myfwyqQ!3>O*o_WPKnAP+xw8`uC7n*e`0vlee4w7@RL{%zmdLW+2}u$ zDVMII=7&DfE^@idp1K-7sP|}b(aq{EJ|XT_^+!N8{MIdBd`r<`33AhSt*!SD*{EQ~ z9?~yS7liEftF=^!3J>2&VFfQFoV@{lcSzaHzb_G{Yf);wj^G=7)?SsLTp>}n7OTkL zcr-@uFhME0u|o@uW~y<6F9q5cf9xEcSv+;~O`Sx0C)A{ z9!G;j2bQvsk|%t!v|`<_j8gXcBr;~rT|)MZo0ZGQ6@rV?sFjGJ^WJiD;gI zyGOy*)V4mOLtZY!gNpoNJPv<)=F_%8qgoPgoh~pg5z`pmW2DLC699Z;P%~S}l&X;3 zvpHsxDlH#zU|hw>ZQm^x7VRc;5r+*b>-0;Ey(f%CA<8pE6TY5>$`fg(vWE;vFVtF} ze5A@3{4^GK;H1ZUae;Sy26kOSf<1kLS{cUSPW$Gl!$g#K=AVho3fNO{qwd!RI25;^ z70{Tj?tXUeK(qV3ulL6>h)B!uQET2AUhns5*3@~DEkhHY6r@#}NJ9nhAAKl7Ag||1 zj-uz6D&)FC^mbbEy-jw;W?~N$ZeJKI9dc=!e;a<$P~o(1a)hRKN1aZ(f#&%}dI?>a z9HzXCQij80Y-0OI)P;OHGRT!%`BrX)kKfKwiDd5vXW2eu=)NKH)L11Wj%wn8+5}^- z1zFHr+jTriY(O;OAF`^aXde%|QenQQ+pScXXPcXopKlWASyxxFrw+s2*)FGi65Vt^ zoBowJSV6CD-aowzlmkf;P7&O;o?vHEiEa`U1c1@#v3CmD`IK&t#+fs+nDF&D${9?g z-d5WIW)ULLSP&auj%)DGCO;Bfsh4~ zW=(CA14RgrAw*QXX;_AFF(L}(g_RYb-CBuuXhOoRNC^qvsC>~?10)*>0r30;!FK1?4lxW_&7&h^bQs5(> zNt^%&^!>Y|w()VlbS+$<&RGON-Y^wXP-%G2(qN&mr_XJ3!*3&%U^D=E3#C)n3$~l% z@uP%sb1Q~@8lY|2KM?brVSGGjQDcmW5EvMzF8F9*8q*n~y>==Qg~mfga#()MwhrC` zEC&_o99$&`v)lCK{>kep>@4v5%i0)IR#QPsTl}tdu=`xB^~%by>AkuIupc z8ZTyofcObb5z!G5u5ceY34NElWZg(ipOMnByKgTieOtruZ{(Row2(#_6*^wm4!7rw zngm6IE>cX2B)0=Yj(JK~H!2&gyQjRsJJ?j$I1l@tPTTAOM+BLF8QPcpRrscO{q-kAiAz@&c z%6Xs?LTN9i^H{%52Bi4htAAJ9eIa|a4A+LYYIHcWE@!#wv4Vr6w7KVcN-MjoicO5b!hOHv?b>2G^5*UhAK&RT7 zqG39b^CnD>psYqJ{U;|uinvt)c?quGZ~?|GL9buGMq32yX=yPr&E!*&cf6 zAfajvQUHW9?p3kWwc~yO!&W4V10eRHxkpq%sE4|@RsFzJuh%B)5f;h6c(~&Er(o`A zd@@4<0oEMF)x0wl7B#~|USNH~`Dif5qrSnAM4%Dq&f9{;QR3h)rI(4y>vS}C-5eNO z6NXTro}`2X9W`QLNR$dOLlBE{FX|f142fx{voE1PqfghB;{;7{vkZ0+M?na7TD z(?>L7FqHW6SyY#I@7~oR#-)6&F-ev{`L&&0oNxbr>Pv6}y_A?L!HENLHIJh9l=_^6 z!#i%ZDJ*|vh)lq$n>y_VM*ms9qYTA73=BnxS99(l%h|-&DS}Y_>|~Q1S-*$LE;EHU zEJFB82e_t>kX}P6BDmr9kneMI(QD5Ro#bCE1OrNVvsNEdQc;1cds`YgC9{QSjmb{3 zfU+#|yy=&`{j`y5MiRdehy%c_RbdQL_y@%EX1XM(3mH0?^Vsf3z;&=qJQQtfS=II@ zS~@YY%>6oQDKgsPe{eSqwxAZ5ywJmMCI=jRl0G=w&Fuv>C8ac^qk;-PD>Ys=UfxZB zfUyEip}(j?{^N>`V;gKGtkkBFZ>&QgG(CVDxHJaaHPe91^Op5c65%c5lA+u6DdQw{ z`-j}}nulBI^n$E*!DT)VnRe^|s@FHVbcOqRZDJH;0@uCTA5Tn8nHU?Rh4D}sK}fJ2 zIJZRnS_ia1Zy5|M9Hby-k=>Vd-=p~jB(B?<)rVaqOx&9SMLLY%;+D@g&^SK$6U)uc z6@Y162t+s62Z-k60lpMBjw+&T?9#vp*3|5A?Z2Y1X3!a(dtz8LfR4cEsQpj3|42{5RHFzm7UhnNyv??=q+)2xYE$SBSkMyHJt-78{iK3sw_Fez$HO?~aBxZAzDGU9D=YK(C6$d1@jE|M%@}Ybyn?T#a6~b(A zpy=hg@a_h9Vn9*jHV%4ma?WgmC8)IS5{s6nPrBdlgc%Qc-#uTShRe$mBq(mC{k8^w zF6_QELt9F>B8;Q4VEi7&gI$H)VYL!m1%uMY#s;)uEupkSHdM!4(dpq^hfZ2Iy5!em z!Nk#N4t=)v!MP}-s@$%P)g%!-Ni*6xiMc|)EAWQ!hhk7KN6V!u=4&TaT6rMVb`Rl#gz%CE9w0QP4& zHBx0`8FupUM9I^8_QMz9v;+KPD8o)(UJEm`Nb-7jGwAo)-Ysw6UOd}FQfGj&X2bY$W#ZqB> zh&p2`)O~sS<6(n5Xc9sD5_)doODNztsjP#xZ=91TepSLL`o6LT{8)VTEx~MpU90hR z_ws`6pa53m3x#Yq9{9hH2l)aAU2zD;f}F}G?>$HW0Xz&?o8A6x{>5W#S2q{epv=EdJoDv&(@Q&%>+v_Z0j75nM${_y{Fasz)qHzyRJwdvYw{6YYTE?Aa z=ZSI@ozf-eo?xaXbS~;LxsyoBDVfYKEdvbDejPm=xMkOXJ5g93@E@-1ofVhLrYGVohwY=lSZt{nWiz+K&g9FUNCbZ&|NYlKzOflccNl_yTeZfrE+BO@If}kJ@2vX>8_8Gj62k5!!cm>)*gqQIU%$41(mAWhd?S z5_rbCvuretT7NL6DgZc_yV6!WKTQDmpx|zM`y(=)8A1ohluyDAQN%kj9 zg0g&YJbsGL6f`VrV>wMC5iAd4rsa_m2&AwXuUC=Xph;-r48RBsJSQV1wS;-sRp^ry zUccc?u}VfoMFpN4AsTax#S|1i*-DheElbm;?!Niv5MVY&WI^V3>CZPnGXVy#m{cJ}uESFThz^>6bN zT)`$GOfy6U0qY$;Mk1ojZW}RgVKjs4v^TVWz-%Lo-ALJruyiD0)3#00zsWvj&z z153J`#M(S?`-fN{EHg?i7)mH}=`-d~5m!z?OztZ@^yN4S;vUpmOAu?wgg=ly|88su zX79STK^WGxwHb78sQV#)Am#i9xs4{BWdI&sweDYcA3W&P9XJ$bAuav^vZLi!ip|?j z$4E68Q;zvegTRy^-SBZMK${>uG2?~+ZUb()3A4%ojiJ8rq`dp4wDDzyk;jN`L$RF4 zk4tZHB(naig6obz0_ZkGw+mw1uA@HaZ+Zgv!o!yMWner)rbktS&dHg+yt{RlXU?8Y z0bujqcbY5+C<#hu-=|N1fkKHcopxu$HP~S>1SAz^Z9M<&R3|};0gb#Sd*T{2nL-<& zphH`if5A*sGa3i~eyFoqbXL3vcT!qf;MW$$K5gEst5$e`Nx1{UL1ahP%V6a3#jHJM z>Mji(9ggM~b;3);vKCvBv$C21tE?Y37Tpo1y2PXg@*adQWzhuvc3 z=EeYVqDu$pNAR>w*g8}s6gy3}5<{~-Y$mHt5&O{#ib;q#+z0;qgZ@$|OrW(kH#gZE zt`TY(TA|_|L3DQg^70epI01nPr<30#o>Yv@j$j1uv-g;GCn`!uf)_O+AtLiv02=5;BAKd)~qe zi$OYLU?#@L@7}+^j<%>PzP?{4CWuOPHH^=ON?BX5n(^27%p;dJRu_N(UqNYyPXXH< z`z96{>tW=i#GV#P?8)C)0bEf9AF4Hka!mgL4a%Zif}n=_?4%aiHfC)3MIZr$yp~W( z6>|6Y-|I#8L(IfULNuboP)Iqsbz;1{`xrW#>u7vyDxIiq4$uT!K062co3I!XQ>TqN z(!Ga9-Jq3T@$;Mb_U#qQLNxt?Tu~aJ@b^tPDTT5e)I(PHDg>!+h+m~)mn7cucMy;JQXn|3Wfmat=F*5hFtnB2GK%6_GFMml*A=qm{IdqhV8cP2&ID!2Ew53p>PVe@ zvw~LIh-TD6!NkZq&X|Ye3A*c0-3~>B0Rdv2!_$oBCUl#c!FEZ0k_1(tl2T6zc_hMpMT%w|9$uH7ouXHcpQN!`ass5 zjLW3dR+Z)CO|d@M;w&5JjxnA*3Fl_GtM=Cqq%K4%@O&~R_)W?KpW}t}ieoYOpIad{ z7K?`Ld}wP#Ld$&#yH=$9Z6q*4h-9UiUL53_y+GD(H_C448ygQ6oj?A*7s~m_^BPpX zQkR9_$S{JGfnEEq-5Ta|$}*!vJOl)mY zO=2!AYvY^*8T$Q=pS?s8bMA=@INJjUwv!+yKJJ|w9LK-InzC?oVeOpqs)GLq+zCMj z7Z$d|6##Q80TR&M&Be`~8Xr&i)(}jtUHgYrZ{1-&v3c=_ekR&nJUu0u(x#0{7*~*QixjIu}9yyFEfRYRk2sTlgCL1GT9Ti+i2lnr8wiJf}lKrBAZ?Z+v zB}`A?qmlIogcor_=aMakC3scg2{O+SgF%y%Z|l4a<|A_8r%yZ)pE>EmFys{hifDiZ z(YA)a&CbpqFdF2Ii603WRH|>o&}%$SfD>j;PJcWMJv9_*sx+VwOE8Up9m4<-jMVq( zojOI~DzSxB{psq;jEQO-Cgz|!A~wgy_%^uos!b5nnyALp(_Lj!(AQV2#_zpu;GBBh z@^d(91~0g|2pr%?vQ$^6d&09jKt0}V_oV-Yf62%X3amwUA~qs4mSrq4FJTDl3wB-VKv&KZ#~M+W674z_8QvLPmizXyD_r9O{y z>LI;TxUH|#QK0L8V!=RP#(061jco9plijuMh48sPT-7)xy51;QC_AOLEkWb@o}gID zX|s#bU1F+5UMI>p1r6xvuc&!H?z9sZG1p;f&apm?^=Ff)6cPD9B%Nhclx^FE2SGyV zPHCh&hDIc$rMsjAq&o+ZPU#Loy1To(89JpqhvvPX@B71_VZoX;_gt~hy^pO})#X6+ zZpXCheY^z|>qprGV7dpa;66Zp4tP>>eK7wIy*6B}rI|)Ya2e0kw58)h zgOj#-!kVre4VZSp3U1e_QD>w3L|SsM`4aqif*q=Q0K6wgbBDsa$uKyPjiV_6T_3N<-~dNIO6|Ge5}ETESdrxmX8uF^0sChm9gwWt2O`8Rz!uA~Q9^}N z^o>$|HJ4Fd1?pW>?rLXaM5ft8mVmAr_XN3C;{HPBnO?LnP#jH&T?vd)5E%wmR<|tLEKYO$=12Pys-bK5)+o zQa{MVUR1PmD>2h)Tk=cW)l8r{0HRp{__+Mw zLs6s16L3KRygtRkr9i8wOl+p-VG(fondN3-EXM#gf(Jl^v`T%axiY!{gbz!Btw(;s z(WxlDsFdJ3hgr6|*ZX9@r73^m3qwND4@^|fKcq_Wx?lOx>1mn&kg!yYN+EC3)5Z@E zqkZnx{-r)*jY5eZ9EjN+lQu!j=vx2Oi0TijewyIJL%~NdNTjtmU50qFAPY@6xwZ^O zf&3J6YrlafkV8mE%Sl8Gj03eO%o=RYT&78vxqq zJqB#?qkA-!V`*6`gtNRvI$1guj-)bLu7KxED!0fv?LB?Hw@S9qfFZp=MS~?vU`|)z zj1`db9ygj?{y{pxkBa!5z$dPqqK`?Fp%~2{fI^u=zjWdnHzdKJz$#xKev7=048o+X zN*}|bu-`sP5YUX4H)E+#7?r!`dPs3Gmt_E5YEN4Isl~na+yEbweSJp?Y-F^Ezgjey zYK%KqMF*lX{(=8UhT9L9hUt*xcI` zrHn-8%M7uK&b`RS&{r{F90$uGBSU98sHpx5N^vyS8$%8{OmUJkz9{O-dPy;1AMm z=-wG&x5D*R2xV5y++)>i1Y1r6)?FL&52th^gP07I={?Gv)*33WFCSQRrM|?X23!WL z$JCm<{JgggD*N-gb6x^@zc&D|r8hjj;x=^rbR`W8EC8l&I03(bMl-XN>|v!Z(_whA zebZZWnh6AZ$7?>Wo8qe9_fwjj$ol%=R$EkYk8B;di5}Oei9+R)478d!0nKPt{Ihat zZuO6mk=%Hzw$^wL`a&89VujPLUjqcJsI9NQ(Ys?Kn&^4ATRAgXzDs{gncW9}>$E3Z zF+`hSrw2j*;^iqUa_^LRt-Z5<|JyY54Q&7*?<1=A8M_`sY!Tk!o*Vom02w`y&qU*s zdNVz=t!tjMLDu_?&dmu1`e+rie1Rb3Ljba+Wf2k4!Qvv3nOO=6->#H|>B)jaqf%Bo zaPb5zdB7YM7>@&MR1(kuoS(ii) z>HVF)>x_dEx8ij(*u5k{3Jz=ixa2~ZWjrCTigJnn1_nlACBW#WL~oDxqeW!(nrmtJ7g@M*cWks{ z#ib)-k6o=bG+>b=T)ehIoIaju7$OuxY~foh@w;8=qdM;M$JaX^A8vN(6zKCd zOkvVMpjX$`HHqAJBT`DNo@qoPw~D6V-%DNYvfH}nK)*^sLV&&AR3h0}ff5E0hj=25 zSVm5L4(}zHF3^Eg)0Bz$W&3=R}HAg%*))PH1R);zytcmHX_pLs_6 zseigYbfpD7_hB_V+|1i0>L;JqHuO+l2>rqR_6?{dRMKxy^gb$E*x#q0N2SztH{f%x zynA^J2Qs3bfKXI2#~8K+TEf;j3b=Vjm-?h32ZDfTimImK$>yQJ+~|SQ;?sZRb)^cH3*bILlLm;G5*7r({?2k zwl=a#x59;rzU6%*)n2_Rf1lMF6P6+00(A*BMzdJLSVK z3~mF1YI?_pps|FH3jckGiawX2xs~j$9$Yk-NzCVqEr) zX2d^0z@=xuIG2vWQ6VaqQ^JONPcMND5Lal6Tx{gVxWkjm>n=v1E=<0HotUE{XFH3QM91#hmeOw+PHW}a( zmX7Z3k7RdYBt)f<>2%l`5AK!lJWx4*x?l5bO9I}6^rRFGvh%ZN<5|1w$+D_U>_rQc zs>kU|Q;#mH^i9@B?-PfUbrG9DCbE3fPBG&CMqN#FK^*zo2BT|z zb_F^O>ytUQM=i2WAcsZchAi8{qth%#PCKS@*lft!BN{v8LJOAnj@pSu^w^(3+Pgfd zE1!cE0o1}{p4@60?@p~wHZFYvt<0nqMEhx4{(1KgT*ik%@CTxWcZ)5;AKlgmm6HYn zeriyMQ*By3!m3>_zyu;U*A+@$`@(An7f~~{N;E@N&Igy~V>6yzCMzy6=^v@;quRe#%&6)SY_ zdh>(55DiWDv}IyG@BNi0zscxPU)Xe`#YBIaZ8hu!Cnc6>u>~V6`kRK+()B@twQtdC zyKs(XMR~)jdF^7-nrGi{i_B=b;b|T;a_Ox3Z6xs!jxbuwi5E1x*Qe`aO${pXfCL5h zqaa|+AaWrw5lkL+42?#5w}VZE<#;#aarO@q&vg?^bf8+Vc{*q(WN$Fvl>B9!MGzev zEnjW;AxK_86NQR07jMUZ*^=Ekj0rv6KnNZ52K6+rjt!3wmo2^dtA&7g2$uv+t?3XT z$Fn%?{^QsUCqvPCAhIAd_8)q0ZIImJbQ(wcynNlN-7XFrH=Z#t4M-@xl63Q&8|Nf&~DTfbaoy^aFGGA1T# zKyC+=#1_Ev3O~~GlZnpk$L>N^iV0kHY0>fCRu*59jV}!9mZg97Bbl;PT0Xk9x3x5+ z^fOwptH!Aw#I$YZNWQKhGsEY$X>l(VK(XX8KpS`g62J2sUAq5o{&W5w)7l%E`n4GMr5-L{Fc%%_y_&iwNUsj9)8rEJG zfJw&(NnD?zfm{S#ai3uw9&8=VD|Ko&hYIL>o3W7%YuM1-2{uuP>)jZVr*1X%$wlG^ zeFa9`jtj-RqY4V~>GI|rO6tqH#iVtwy+^>GO=KYt#S*ut8t!+HqMB-0`)rXBXp$3B{$7)Wy?@;U$$ZrB zFU_P21x!RTpUC-}nWOeo&$} zG-JlAD9lKN@7H5~2Qo+BHqMe92zz2@8zHZof$x#ppLug}$)YD|-VC#sYqz#u*93y; zIAK4LsDn|_<>7L1!5C+YIL3MxGfGv|(kfDBHIlr#@(vF0w#|Fl)0I2^T?}K++Vxy9gv-j&xnOKtZO2#VO%mAD9TjR#>~^ za2o&I#WIoNL46$b(|K&~%|_5-KNU0w(01HKg8VQmwYpC`I@X`=BY|?7!jfzU6Me?nL#6BSQx~+Na0eY(!FiJ6S8sh=ngTsKe z#Q)I}>))deLWfB@mi$)P#l`#`mL)YPyKBaX@59M#fifB#dk`K`nI1#mK-X_d5n~0g zl;{&?vs-XFY1alqAC)lE}hC zR{;TpIK&h^#mS-9wgJE?33YuZ%dEwK_>FPsuRpIiE%L4Z2#YRA-Bo>A_ElG8h%qZJ zdd&3P?IAZb6cpI9t6AGm1rx%tXE<&|aET!kQX>o~WvX%S^(L3KD)IRk=e5y! zSlR+61pAgqisQDMf*8)`9QuY`A4li&k#X}+6zm}L02(S|@eTVMzO2?tT6ICMOMRb{ z)uV{ea2T^l!w5neW$AuZ!M0SCEU@wEs|RWlfCs9Ni;5VYCLoTs?Ker>-1Dh@&H~(K z-(4l<`J{cHyocnHOru&ZP8+CPYNDJ`lOI>DzDf3eXaYvMfNEXefeARc0p8E-@3Eo{ z2TInydJ1M~S~OZd=C;i_lnU4A1J)S6}GU{WF;F&DV1~ig;!1SU0uf zM$;}@H?*dsAUHnuegq5C2nX~WpLZZIX{9}7aZwdMej?zGlC6LmeBC&mDbTdFwd)6( z#h@c>B(brLJ#faxlO+UUp$y@nMX$SQy(4TG@EgEl?Y80o{7MJ?$S^SeNMzOboRTC6 zlqg3w3Xn8n&x?oO>*O$MnNnmh>y)rZ`4qtM)wC5N`nW^zGWnTe)FR$d#^2BvF6n)F zW4GIHCnHo-qOq)wovVqSHHhdhj-fMNELLO@x6^spI*eHl?B^6`*s_HX_o%rB8Tc*8 zNzK`3TCew3*$zk@=BcZzQ~DkneNW{DrkcRiF%zBtZ^y5RceZuFAhsTuk7hb4iaA<7 zz8P0j=OkX;*-5kO5N_2xg9lhb^vpXWXj!;@HfQ)VChmv!H9zR~_X*;`o)IQP(7yHZ zh1EdZ_>=phe|y)JwYC3FwV$P1uTRUG+*Y2AvX+IKVOOhJ&(2`T{^Zb7`&lVN(P!Jn z{HpA1&*$ctxx|GuE=@d0!$DJ;XHzbYR7Aboft9=rr^R3C+ljbs?~|Y=}2tByOcnb&CG}|P+BKfeeCM4p1_3Q2WkbhKm_&{bhnkrj}*b5 zM{?FEZ1HrDMCWp*tZDOEDvW+^meBgx?E4)QN%_=;i>gZ9tT=$iZ?e(tlhA)|FCdp) zGKS6356qxsOiZ?mX#S{LMO%lDQHr4wiWhdxuy0=x!O&XlOqaf>tI{%a^s^@dQ}FA< z5;F|pcw)SSLQ8>u5ySy-9Ic1aTxJP@;q&!E0o$>|pL}a%$_+81K~A_0P?Z0htB0G` zO3do&jp7nOLv{sf2+Ky;D%P5u)`t_;wV2T?GI*AZLRM-+Gn~+m*8$=5XDl{wK)CUO z;p>zIwzz5sqsbA9tN{NJu)$)CWm&FtP0PE1Br0Sd6yu}ul=SnzBbR+$deHUTxxB2%awNp_4Wu3V@}?i9TaT(-ng8|WIDeIT%l6FmM0DcG!qwC@uEjxq zwuSe*Q0{a~;rEn*6AB4lVwR%b96fN8`*^{o$8&deO;2?VzZh{RW+UWf!@~8)@@O-R zfuXEhuwy`!cfL)zrK7>dw7arhKX}0T08XarFHh+5%&h&tFE4*?L=%n|zAk^&X6Dht z&{&<`J~WK}kpjL}^ruz7a*dwIxR&cKwPBhr=N~ygDtN|zxRu+VAjDJLi-fJl&DvFR zUOhdrI2FL!`U1cY;4ws=IgJhYoMwRJ zrBWWReX$RESvv_*6qqVmuW5hSsd}QR6lRuM9=gDRfM7BKYK?|U$k%*<>`N?UCQG1n@{vB_N68YzluXDc0Yyx)?iOpnbntnl!O}u$MqXL(OUN;RJKU1 z?uk@iuRQG+qQq6@k5t2fcoTnU^+A0BSpQ@0$AQ%+`%=+i_OoV2MHBD!7*Ut=+_R8aBqA6!3 z=uvk?N@CKyoY(bODapVKcz9{XIwUFASwYqG^GSc?no^R!``NnVmQ(qG)7X|zI7ypA z?zEHximM2ZAS3g<^W*4!+FHlofjk79enP}vKkWdw}I^#J!qzuJ^ z;^Nf&z+@2#Me9f2^|+tt=OsbjKd3AW=q^tTS2GC{@6HE*F|iM1y{B?kX=yY0 zogO&N5wgD&{)`xwD0kG*ue&QAkP_u>Ix_tej`aqlqpUn+ku_JUqmDwMkn$~yM=p-$ zSIE0JAo0nCs;7c1z@DZDHJ;3Fg@bOXJ%N3+oh&Rxmc^YgCOEs-gu()bj+Ab56M zRyf%o4P}rdE#i}!vWZF6ymk)jI6GRLvA-H?ikBti;^2>!kd+I!Yy1Nc zki>2F&)h{@u1(ccy_U_Rh~@#-qq`xH`m zbXlLvmcxh$L|m{UY`6U=ig(P1hcBh+Z>;NcptKGRfFoa6T@-d0?M1SaJxMc@gZDd( z79D|s6K`(%=a@cMOCL5Xnl1ung7a%9W2k}m*g4ih+4w4to6dygd!PY1U|tKq!ifO4 zYVY*Lb-;qaxs76)REFqn|8HjUhdb)M&8;vlPetUjE`V=vg8YrpL{b?<5ik&%KCfyw zagw|cv!Ng+5hm2JdoQf+)!zPOZ7ho;DZ&nPD$-7f8r}|41L)n*=5gdt8*ZH39%-z} zZe7=zGX=2Ja%D6Jc^VbNit)^6fcdpOtM z-{Crwv@=M)p=FFZeKw=S9zEwUNAM|$mob88y=RZCDdcVtu zhs%Aurr+Z5xgSpxz!?xj<`<`u42R7j`J{!l|4iR!wa?6bm;Vw%D)6s!fPClW#1{eC zgxR>MxMZug6-ncPq zD;a5EdyxuDd@m(^dyGFN`IpLGVfkSL zo=8*AfL%z^@ksw$OHvCX!RdoMPwcDr)7SZ@lYcKvxvT12p7J$BJ@mO^L@_(DW`?}w zcOssfBtFOBdCZ+~TE7{AeLe&-B65Nd+85!jEka zSUghe`y|aaTIKulx_le&)kR#*Q6+PrkpVcmmwf~s_5v*;FumTvn9x{U6i_^8KqF1) z3d*^h7(GYPW*@-_IYdY%mN|k$--2(|`2=8PwBK!2UTD*K5yxPX z4PO#*PVOvkm3p0)HCK}Nvu`ak7wqFHvBg2YT6g1sgNFOZ?RxG^v)+#pWj}UVI51VO zZRmcMkkO@e?kU5OXd{E-@X^{))j)cnq#2Wj#PykB{c%YS?6qRE`lm;dp7(jpz3>O` z-^ye-4Aa?;)rozB-fZdN!1stnH(D{@rz@1j5; zlWsqbVQkbXl%Vg~piL0b-ou(dXND?>rs;<7Lz8@mXLknmnHZJzZhXyogUoZ+59JTvB~GeuGSM%t{;n=QkykKDI` z(i%sP<4PjBzg2^gFrGD>38UZtU8xXleaR|$ZL&WU$6^p?#|m|b4ZvqxYq86^no)Y> zGDk21Wpgp&G_@8zJ$dXIt~_r&2CWw;M{<2NZOX~n0@C(?2YR~mrw<4flzqgEsY$cN z4+1$LAcH@)dGQIou4IO@@$T&E1g;`dZf{3td5rO-v9*J#n3#Uu4>$bjm8ffNHJ@Vy zO8&(32{ylW!nus(S!nEDho$-&K%MxKY}a(!T_9sGh7HdTrQ6z|FX(dPkwar4gJ0@t zEhJ6NBuu|ZVD~;elt`% z(`x2^isX9$Xg1PC6+(z6UlNVy7!8z--S2mPiuZOBhTpH~*1@LE$e*usl&BSAzKOg# zOWZ$B%4_WC-4VTeeKZ_+dH%KT6|Ma4<%_t3c#So~+feS)CH0#nuPpNW+DFkWfsbyS zAa(@{PJ$++^5L-NIqPCg@42yqadOjrua~;#X{0xPK(D%KalTTT#_M)!WR{5ILS0SO zKck1yl-+n-Da&LId6=h%yTr90oOJsW=VS9xqhb%LG4HRksbxZZtTFDB$C3P~$4w*? zdHL*x7(dJh?;a`8isolEwR|yB@Q$B%Mj?l^RxJ9<<6P|br8ZqRBl+!kP=A=C>Ajg`fjG$jYNZtR5ZaTW9y?H0CW0m9ZR{Plk1y4QkKUO-RGGzewrefWO^5$_k!v1y& zEp)wpP736Izu$oD2S-E5-OPIj$O)dWvWD6zl@nuP7CK(LU~5c14+k_LB0p7Gvqj&o zx0^t*s;HSPU!Ud*igFM=t+C2=R}j2$P$dVENxx|X7OdP2stIO7pCwx`X!6m$uTKbM zA`wEfT`j}>EM;od$afm&u~bK(W9aFTTVBM#P_)`o@O&B0N4s=TRK#c6Z|i$FeZy zX1+^Z>)yXT>wxZ64G>u*J0Z$L^#Ql#>%+#grE0;q@IF5@jXs@C`UavOyS26XK93x9 z$i~XbyWa_oCXKM}b{zh3`GG)}ICWn(3$r_z-x zUfmQ99tc&P6`j%Za2+}&mTr1QNkeZ5Dqbs2onOB`Z7aarJ~&PgAn8eS5;LcrHP@Ig z8M(r zhVR2#BQqpLne#T}bc(CoqLrU=)U;*ua3ux$qN3%_&czkS@y-vEDoH=_yrJWSg{dSw zIvU5EZT!B`f#E$q?&<3CY)eA|2kWRm!e>~rEwHO@rN->eI2L-LV zeBAEr1O}R28=&mgbT)1O(>6^6)bME;`N`)SLj#E98PoU(KN}_*tRQ!AJu){>%q3&c zaV!Y!(ZIuOKbU;j*!bs`WSK!AJ0gO0rjK_*^;x=|DDm7?_JQ7VLN65zRY%7~sA0I!BB7joSphoSwdX&qLwiqaD^z9Wu6yXf zRkgI?JCoG|X79(7_J-A#p24&gw82bNQ0hS4!2?M(oxypHn*09X3y}{i&Nu?|h?jwf zc22gwmn~tk6Bpu(^L~LFUwiJ*_hltJT9&0)(XXlQ)bBtOKak|7vmeqfi?9)Y(B<_g0#oLGqw@C#n(Wi>T1Y*@g>oSa&;k*ewoB7xtEq(Qac^Ui|OF+{}+L*7b zy<5>%&(AsSG_Rc!5t`2R+wAUC&_Ma;X{)NK*?pmQ@7QQcPkDQ)w1l4`e^-bDT|_d$ zSPMP1&~_B1+g-IY-2LIqGk&ep%f&x0IKT6ZjNvm%dbb6FcTg>@#>W(k2a(rS2FgzX zkBG&yq%4l%m{1oP?nrQ-7nE{3*$iLBhmwrsL_~t^2G0&Qtb- zs1sUl?)tEW5231bb{#M~E#+7`g+u(so0_&PjEISzu&&HB-8R>_l*ttI8-3cj&6Wjx3ei zwN=+x*(lDlXB<+iO5e(nJl*BAXtKI*7g@e$3f!A>8I61Rb#*x)9FcxxIcR1j{$3|b z^97mhV@Lq7<_t-+dn}Hj)CUL#uN2MX=J^tuAjb?3PZJn2H{I|4D=b9Nhhk9$xbID@ zzf?ffMeQL#(#wY@!*3q`XH|8sJYO>53a=p*1h_k;Mibt-|`rL z^(3&ykT6p~2vc#-I4m0BeCg)gk%)`I9M~m^& zkT6-J2#0ZS(m-_d%YN>!)LpvX4eC!3*rh1xPJqXW=*Q1?OcpOnu5k&@r2?ltv*nr~VyN8?o+hv~wvp~K?j#5iByN;8Sc@|#5 zgDY z>)A@D(~cux-}yYe6mayX1klFheeAve;$-?wLbeQW(4|`QuDaRnEY&G{JoliwR-Bei z5qD#&9J-yBE>=N}pnTS@qC8IOwtzwe0M7YK zh0Fw~{;uk!2(Z%Q0d7^tEkBxmQ3lX3KRezfrL>v9c_R}G{){}u?<8AY9q~DsPGuNu zfh@zrgNGrk<}^UYkN+Nh)(@MgXhO8Z=bFT@TIEeAp>f|Ih~S2nBjG6<9>Z6=t*LCzwBRC^%)LR z>sYJ_kyr{$^PY@)6#PCw#V7zvYSqN^m~EY(`59IA#*nq?U#S8~=|yOfF+&Hw9hk{Q z57zOjUs3WI`9N(#ZuId7)Z$sKK)7V&le#(yaeN4Jw%Q%0j`A1X_KS_XTr;O~!mqbn zw@0Y2PAfSCdF^n?V%c$1^t3-uK~!`uN2;z=qF@PiA0^^4ua%V~G|`6wu=W2e*f|#AN6Q@AxKEGj2 zmWa2s09wKnCSm!U8*QuGGXK08oFeSys;ZuWNg?5B(nCUia!3D>aWAwc6Eh)-(4@GW zN{rBWi*Mpk9tkx39lC!UozL8Sf23^)H8sHX3Pl&V!!D4Y$(ICl900lSIgusZNmkq4 zT}?6SqNGCa`OzAL;CCd?HyA|dblmmu>tOTXC!P(%oYMw&VwhFT{3{gj0ezGM`sjqQ-?SSUp=Av zSU!)uay)kE0hqZ0F+iJDj6c$cPt*@5WLW}~wjs<6=|&dHQ`3-Dx4Shb>4G%%i%e5F%5$XYmICaRQ zPet^LQ=_j>81@MN>vq8G;m%gxe~?Yr^0DDz;BhHXRKsdh9oVMN4g2fuw$TFRYkj$& zodB+(Kfkk4zWIX9z$|q4FD@7mWq|tnNT=-DhfFWmAM2<09j`~J85u>zvqrzw>Mo-M zT|Wr?1*~ulv$M_$xyc~PX#g#|yq4Pb%D40CnB&;HO=y8||3Jf?W+n#mLzyp}|JV+D zVhE=j%fPh>Ba6YOPeQQB%DA`~$g`d&CF06Y%SU-6jv&uEU$VZpZLs3jc~jRH*ApvC zEa&ZkR#u1f{>g2qomSYyGu1RsiU$FK5&(EB|0v~jw-2(n{elfthdUme5eZb~WP?#Lj8dW6>h&by!gF0lW>T{O+sp~C4Z3GS?$YW0-+yz9xRm;c#in;6&yMLn&04@NnI?QJw@Um2Few5cY230zk?fMmXo*S<5XT)K z$Q3Wotj%Jq{Ww3!TkNva%&Q}P%}dICT?7yC*m7JsD%{+LoH%6XpAQtH>0{TByuUnI zZGOw9@VHgUW|xGiXgRi2m%_I?NHkKxbR0KH;M@%BBj9PeNEKn@EmnAK*m-7HWxFeK zd=3gcYf*>ZmOQw+sf`Y1L=g^zsnxmKT=?JT?y(xE)e8+ITXE_v71=M&Gu8@CGqQv} zI|BG>@dXV2uI&pT5gZsCoF(>-|3K*UJB&zXO|$K+-R{)OdP_I7ULw*e=Qaj{(;v?( zd?B$(WT7>||B(bl8Wq-Zwq~HwFY?l8{q0+R@nIMhKWPHrdTUu#4CKx@dVgO{VFu1A zbWrK(u6O>jEWCh?cf?#>lcSr^1chw-goeh$#V%I(qKoT;JLKpQ(4IJYF~yt5M|aP$ zsK4Q}2ymLqBlf=o!Hazk0slM5VPv|At=<)n!=HpF(m5HeeoL5kX3x$3@H!3V9uVU8Ss1r_^b%CA02#rcR5_9@@76zg8_30RvS`*{dbP!lhmfEp- z?iNkjZ{mw^50$P4Zt2}^XM^;v?{JV?+etA2U4OK3goA$=0UKlH)l>+9fuVi}O4Dp+ zN*jY9oEgCqV=4vWIL~s1-uRvnFmesO4=N^6X-i7ZWCrsvM@7$0G zr5XQ;`EUlYE2)5f@c2vOSJeJ8SO>MWv)Ur}T@d8pP*vxVjIpwoD{r=90=)uD3K>~O z@qqvp1*3%Ip!&I}H%0?Xd&ZHD4)R4a^GW>C=tqPJZlEA1fg?>X?GeikDr1_8amwWtD1Ef@E;O6Bb=hZ zh+-w0NuEM?=}ueV0c1+?jc~Rw65^YqiV6I*!fe-w8`#=1FkpB%?-6#f`byL7H+JXU zKb(^ws{gFY{!1_uh9;#eH-AZb5j?OBq(p9oDZi>GsK}V3PpQ=hd}UFS|C_;6f>un@B3be6X=Z zh_^Q`?z;d?ixu1231a)69;VZzD^FS5Hmi{e>=EH-EpaGazN|LcS2)s5=PrGd_>o$Nc5w+LII zh1*Ss$?fyiL2D_!&J(pXE3oz?n|AtI@Es%mH2a>}zyyk!+UxP><`WbTJpTMPQ2M}v zGQVpNrq82PlVAV}g+8W#QKX9A{zP`GmTv%v??u7|2Gduu8{+0KMp&)BKAQvNd_qt5 z=2}{MGDAPD%NH#h;$Uk>WJD?1#mzZ`2??~C`gMZL5rF?X>-^kZzX17HK;E9OhwbX7 z%N1+p$HQ;$K2qC_y_>-Y?@29!xjpu~h2OuG>n7wt)YiT&Wge+E%lo8IH(01#rCSeP zJ=GESo=>?BxwdlS$>RoZ@FvqsmX5-L+t0|Lx~9|=0az+Y%nSfD$GlETc^H|JRdd__ zZE!^!U6*w(L*4cArq zo8i`CBsrDm2L+^8k(P(pPrHH@>5eZav&bPmb{V5aj@T~&T+`!V!R=Aq_O=-T@2@jC z70DeQlmF6Xc3NM^EERw%8F&zTKe#Xb)#iFVylp2$yfTZ%T^62G|J*i=-MrbL!A5sw zfkR*h`azqI!ZQ2Rksm0ZiWJWMv-Kr{1s_}xVcT>+Klt`2v*}WF8%L@nTQ$Y|Znb26 z-F0<-GiI6X07$cage7mOPCx{GnOIQ#QS$RTV{jB{K86pxZG5w%);;ZFs7GKC+TwQC z)}@077@MOwfbv-PJJ zltg*l+in7kK56bsN&ga!FAoMA8v!RY!ymfBQ%h0!JZ#z0|YfB=nP(!gGUbPu^10`;M)K2lS zc5W~z+Lg$#LhRG3de}>Vtv0?+XYP2QSS%rN7Uym=9W3D^v>6Au6f2!MEymAYKnBA1 z$nM_E@nXwtr4Pk&vRbOCHZ(q!iBe}!i4*P-exe2EVDR5Gdq$+He?(v7fgkOLA(;?v z7aRKiNne`g@}+-h_i}EqS9R~35Dlcwb2mAT9AEk_hngqQ_1AU;Ujat9s5(AMiiBJy zQoVcg+OjvdmOkmj(F?sN7mN)5&L7{07{DgW6Q+oi)H=zHQ$66*H_g5 zq!_IJ(}54m3CZ7EUmjja_)c3cho876xZ9o_Au?f@sJ5>!PuAr_6C+&0uig;ts99%y z8sD|*<3zFqL43vMl^m@%b3DFCe!moNMh8dZE93jT2&fH*aC-Sgw#&@R5cLoWzI)d$cvPBC*5V0yg1l}0=6o$C>!Qtp zTXuI@1N&g-ZEpodAsGLX`J+Js)V+@}QQSyMU8-!OGTuw{xneaV3LWwoS*f?*Y>RNv z9$FKgie5x(rWHTAl^sa*4V#PD_3X_nVrVMTKw#J$aJ7z_?3%!L?l5|YdU!%qdY+u! zjF_U9>yQ_A1EdF0xA4<({v^}9jb~WwxWLDYx`Ldssfa1%&|KCfQ&hPxA6)xRwsVlE z^mF{3m$~)TV;}yB_iwZ$V z9z{?KGMII;-ikqD0jd;&+-Lv&B(+$ms!#Q|x%svrONhXbbfBr5u!m4BAt~XHL0d$+ zNtQ1LGH7?~*A*S7f1Z!}J&tG!n8|K__4C<$A>DA%Ym@9sx)=o{Z8itCJ`ffQ^qS4o zCdN>S$L*w_L2l-TvmnEn_AhQc)-yZq;S3R&2ZR0-72dB~vmNU`CjpNSK`z@1NowZ< zqL-ZtG|WZK0<=cW2NB-^bfT(nutJ z@;5Mxbm|C_t1BC$!sE2ci_Qt07R9a35gWY(^Fq(Sn35q+<5qj%sR6t?T^-~ zHbgNggmHBZVs!JHriMJ`Eo*tilu3 z9E_F>%C;5w4zDm+-qvE|dv}6O(y;?#Ds;xqxO0(zewSE6?q9sUJn6myb%K)~VHnHx zWim5D0TiGhOkz%&(t3PNH8o6shTQZk;H&Dx;|2G#f>7d$Pf338ehu8i12!SE5*YPpagWl#` z>o?*+ge4WUv+rR72bideu{#eLf=rORKLEqI{pF;K{G+&km*ve3012G`9S^n~>yP9> z%WPhvOtAoU0>;R-tHpsCb76kw6I6w(Lp=-{<&59aq?CqK8OYf4xi+IauBXeEx7|4tZ3q}4V1F~!wKG><1@`FM{T3y%UhHNax4n(I=L zqE0qr6(%$NAxr#{S>Ex2SE*3g=f7>z+H0l5^C;eN1bY3L( zA*0{-=eN%fL`;m`a&k;h-DFB~a-kuU4R&@Lq<++(!O8;7reg3Oc+mOH%QQUTx<>xm zP-qLHz}mVI&?u#E+s`qT!T#jxhk3k)))RCuMb!v#!852y%n(&(En>^4vVUmnIc+N( zkSs1AmOvjjMfR{jzMy%*qinaavpFeWtZ7;EX*lN!m}TAj&z9tH-r1}TrHv+Ce%zFv zr{inNQP^|KDCOa?Y20=#EZwWI1ACq4DOZ!>$n*v`n25ZRA>;m(@;l}Tt#5A|(JaUH z@IZ#U{J#5PRfsm&B^n7C4q^1D&h?<1!FqH;+8?i8UrF!UOP#6swoH3yV&6%5Nuq^; z6thrz7@d5nMyRN@fL3rATOs6MdD*8qw9AywP<(IE`VsB5)YRu!?|SIb|0p`g@VL4) z3{O1C7fjf=nHY_&#zte?wo`5_GiT(pJnvX7u7zMb7UTb?`@Yk=8MTe8T zNz&0L%zLjEK)WM?fK_^0B#+0}%~&24K`Av^%Aer>^F8aS&qobgX{g5^5XU&ak%A#5 zTw4TrCQ9k8_T=K>Zm@I3RHbgej20(Pieh+*o2J(n_4~`}KYp6J1K~xI`i1JAZr}bz zv65|lN)Tl9{u6sX$&V7k|1g!M4OE+ohs?^7|9coHRidUFS+-D6x&3#S^7pU4-@`C3 zbGN6#QM#>gnXN`r`~oAZpumNrKJ@L^zhA0?cqKp(;`1MD>kr4N`spqzUFY<_!T>VzZN^ts4wmCJRBh4L;gcx$?gOG+JfWD-r zAslsV3*30t>?pA2%;@_}^+qoEf z`#91KE*9ivZVWk6JOh;j*65@|_vsxuH%mStD%F)s&MuOH@?qctG#M!^ zNgi?8gzKI!+%mCzEZtmz?kti#VS~-?*6Lm@Z6Q%J?J1RZw9)c!XEVp-i%)UvZ!;do zeHeDwpTjbvoC4)AcVDZ=y{nZ6Y)4Rn_u?~3v4N1%zjF!qJOu~rj{ga5h?DU{WzNT_Lk&i9aXj*$e;6C8lce+h)9J&190@i zqldS6@0MbOIL}f%YkS!4mBjWS)DwukCKPgAWHCiFgLb~O`-^2Ba@qy9!#@g6YK4OQi#)xo_b z;o;YQaj`g^JNfEc(Q=^4eSVR0h%fT_3SgZL4F_F|BIz1_DLTw{zZCKr#e08cn%O%X zV$^N<`=p|EFQlgXV$(y(Q?1qSK--Y~z2NH^%kJu5z^0utSKEH@K2Z$w$G=>b%Fi6G zFncEKpj76koBg>$V*zkf?nvCmAFv&u5SDZl9q-ZqZ+-`$;lx5Q$w zbQQB4Np*KevPGWG(e_E)c=u2Q>1won<;*6PhTD22qZaeqF_|KtdGy@J1YrW5r3~r2 zM(fkC{*uk_$@Y4NMFQGCS5YB01*7usst&Smi zC+P3>Nt}{fNgJ=Cr{~u&Nvn*M6yWU!86qHSBpfnYFD^`%-p8K5+&-r99}l?RWUVYY z8}f+7X8-OV3l`b9hXU^{yqh0?{*Z8JRiy`ipYOQu?ff)502Tzf1bkH$ zK9+n2+#S*U0=aNlmll2tl&4HnvDqkC(#9^>63Lvy{I!OyQ+V&<3JZN)x%)#DL@O>U zYBZ6+b|zQp40>u%ROKcOIDBD}VKUo`AByjKpoT3n7Bx&hln@i(3xL zjK49mGC12fh=NN>OMk64%79(>EE-pT-LwsDN3*BE2v&o9D9Xl1WbQSt!Zm}xPSC>+i)jrrfg{b14WE&J~O&rrH#97 zE@J)fH4t9Z^sA|(YIXCl0GHWGNDVH0LUb9z50y=P{ClHrAlIZZAB5Y&1_gedqje`v z;I7#J)mNI~&hG=QK9Fv)tbMiCpYMTi?VU$|uzT(|I?n6Jl|m zvC`^usJ%5~F&M=4n zW1zZ{P~rWu@kM)kHb*QU$RwnDk=sb7h;xAOeXd5co2KN|$Kg@>OV-@2RDb#SIotZ~ z9ExlKe&s*0#3CP^2N1{GBiv#1YEnbHSx#%X?_`X8OVY;hD$ihI?9Y1l=iCZ4?dI2H z+7F$XYJQQE63Oa~4$85zEZ_GWlK+pNgN-xFp8}*(I8iRB6c`ZJ4i8J$OY!=V@E>ap zo!L8|Yw(r2LUV6jBF%2#+~9lh8ZwG^dEYTHKKT1zT03J*11gA3+%t=p zp`N{D*KY|nnMMBc6tHZ(4~)n5?Lt#$X5}lt!I9l3ukG_46Hb88%<;?(fG~K+`Mu`_ zBhc^#W^&hdM!^n`H7A|x+8eJMy3N zf$YX7SHm$Ve)m63qWMUt&N9(z8um(Ym|=u_#7j*=07ciEVBU1W;T?`9P4HUYOlHsD!@-;B`E=@i?oDhm6aa^Qf1UnO*kN@w+Z~mudE3^#Qm+#k6$LoA&(6;5?Cr6|!zs!xx#XooApO)3E(!i>H%+)OYI_{> zu8N9+LU?3!Hi{7Z&{2+xnwpLF`OHi`Z-BQk7#4*Jl^YwYG^`x)7G@p5>3?RvERe#k zr^wP%K}GwfNcpIb7JM}*`V}=cO0-U^NCHA3-9B-+hYr6Jtw75B-QSxGf~Y_z3^(Q ziKFnJEO#3*gmG+A6jO^Gnd6$<&P-bigUKmef1m3sdty$OX%Mvo9Ce>Ki*2yO?h zyjGnA^caBu3cWU3S;|ORLd+H`N@Z^wEAtH7ym#Nt_sU{@!eT7+g`KJ;AAD+{@$~=@ z`QyCkQmVki20k8!?}VJLdb)XvsyFnf2<>lq#&mcf2(&tXDr;P{sK3)?VpxIb87dG& z>+6>~Iy%P3Drd9x}q;#HT1v!80K2c9F#Z9l-=@+Xrq&;c8 zi*k^`pNr=YBiM@A$%~c_g*fnJ4fW55mB!tc?j%3ur}NoMf9IwOcAx6n%ETbK7kV&= zd%B7v`Y5Dw*|krDJ%v3?^WR6o>wiG$gYMhx_2dxJQ(|ILA4O=eFid9p5fxcqns(X4 zkTS-em;CR&O6x6}yBf~&oz*Znp(wKc(u?=-*gEIi%~l8ess#^EM{Do3!?HW;hF)`t zv$%-que+MXF7ttAf# zasieb03psG%D`yGz^B0%dG{vt^80kp4S`+9LZOPLCBKwnIaS|41HGZeX|tfX*iZA$ zM<{dCDK0!*2S8CjqVCq$-xMwvQ!>1Fd^w3;;7q(2>;FI=#H)`7i$SKJ|MabAZu~VF z9R!ZyLnS=@%sN2``n9RwU@B2CxT~R=oI$i*qR?c+J_|i?eHI0?%k!7$r1EZh=`RAk zotstbU19$-SR~D9lZnd?Cj+fgwvDoUgV*`E*0m?rMiRGA@|PdiP4(7k#Uv^9{PL2B z=IE|0r`a~iC~8LHw>pGW4F-?>C}Bra7aYG~-Fm0#$q8E>(i)n^DosPPD|gX<;0n(V zYmKLCJL(?dKfh#6gjsF7`XBwA$gpuBR2yy%MW01mL^`DPx>kH;@j#E;@6oGBruZ57 z>B%8;>JNTeHAOn2%wI2Xu*B$c3ja@t)_#Rt9gT`KQrv%BOgq^?6p92LSxVJx&m7qK z?%RzBbDUv^Ef=UYkF9I$R@q$hIMlA>k|#OE)gc^3L`VU&jOb{VK$OL$C9kV3iTDIX zi`L86o0&M)yx(rJ2L7Utw`2B@JqL_Y*M5%7>FYT=QG@WR5keGTCT3^RKCoXg%N1E) zppbSlA0$ZnUymr-6t?)~oSM40xX*NMG`ol0(@|4!pFBwVuURneVID@6Fk+xappoP{zD#xbXcPY+nY&!;hNI?{$Mw@~R)^m$5H3VLM(QN;55P2+=6Iz3*I>&m-e5OGYAl7?Y zT59=3>S0>HbN(FVtVvoH)e6QMv^u)kuwSAtfl(dC)F%>}p8ZQi5+D_AZ5ak`;+V|R zv4(-b>22<}%L4DPaOZvj22oCLoyuZd#Yl10(4+4M`2jeW(<2~IdA0COx480VBI=-l zpFq>*QX>@#-RpaENystFanFmC;K#a{_sqJE@;*Mg=<*~SZZo*k{E=9T&MRD&t76Y% zydu5bg*P*SfQtjz3%L2TIW%H7W~5Il{rxMilm3BZiT4-ZJpbzN^ZU}ZHDm3c-kMhl zb{x4`SAvOgQo8!39291{b z2Y3Z~!bVPK#Xm*t?b%x?H9B;v)h@U}IECxu;O1cI2waueN=Xd?8Z``Lgke5L#_{tz zK!zAT>sO&Yka%74uM}SaR^#XC+{CX}hc~;SkSs$NQl;O@!pr*(1d&#{i&5}mg+r83 z|E?yFkTKVPxj=!$nLHW#lZmui6>f&|wBs)_=FYNa?>|_1%l$|>{UvG(^FHp z-iuSL8#P6X!JLp%Iy$d%fs)Hew9?bWs`Bg#f3%!)Z~K^-r3Ef~J3Y_WLR0fn${h0N z6DoaW%;me%=g((uszTQPJ1$v|ncF+}t{_QKKFz z=|FD~8&GrU4)Ajp79Xuj$ELu{Rs%4PAd_fpaX=6SSn+f$Ee{jssN@pl6wuNXC$f-1 z$?CIBA3|v1(G{qfn=IVFMofGA)tN&jvoL;b@4bP=KrUOp75l@M&RYhJ<|ATBsAXkg z?87(C4tX1@*WH32HzyFGqUo}7`y+J6`WULJverfsopGcU|BgCKgMz}pzKnNq-!6&E z_&RKB8!7fayHQR+y~73FvnMUKXX<{3+bi|zI)?{u)^6tsI3%)vjCDIIhpjyQ>?NMp zz;_K{14)Tp?CEX3({CO}()ZQpXuChw_2x79`!_U-fb8=_@8)GsoWFiD+iV}Qp2li6ikgWoq>iAz#M{xni{~$yG#LqZB!_J3ce|IRY!-PQ~!p5 zcFv?p6pxflMv>Y@1;!RK;;5uz;cEg620$FZ(C8b0u+KKklBSS)=)}KC>`TR-)W|t) z6^*2(!l(!W1%Tuv#WcL-9r>vf;EZI8=IDJIKZo6auJsrGVPesN`AUJc zt1Z`ipaxR8)cvd+mrJR%*!d;S>)M5y^S_jtKH1RLwarTHNSqP+|8AI@&NLxl?DL@6 z>|u~z-4LkVO&%`)>qsn&wf6_DI>3`ZTF`KHJ#i%3PmTccx#X-eGo*r*EL@7 zNEJs$Qevs5Dk>{Ker|&m4y><)Cni=Ic7(hzn8~~vnDo(z>Qy=NGMPHXFv!AbWLa>0 zBDIN(+81afc1#T^4uSKPWOvyWx^dR+VSJ3PKtlPiN8d`hQ`RLX)A-xu^frGR49?ON zv&-u|Rruv%{_+t(TvEmsWEc@Syx2r-la&osYl6MK7V^bxy|>9`s0hs`(ZSgW0T#`a zA=G|t=(nTKr+8_fq7wmvPyRve>n>1tPtqtOaLt@n^g-ZuDb z_&e=FKQ$+*TD`lb#z++%fOCNhMn^{_!Z3%1hvAPEtpj$m2AnU6yyC#$f~}q$%QgA< zTH*BQvBffF&z~H34ui~P=q*8YW z+eI}@MJ797zu9;O?F}_K%jJ^A;3?JA ziPeNfw)xbz7r(85^UD*n=>^XiNCZL{0n0)v`XygZ*#%R3zX+S*gI{-Kp(wgX%(BZO59=h;$S2asr7|*2*x8K(9_tYQ%CA{et@s z0H`2}OH=Hag2~oY<(QjN4Q5_R_xK&Zq+!OAWeBxDG%x2y#LI_MBVSvy55F+DczCj{ z88s(qESR=3woRwE70}$FfIxhpl6rRA26YUh22te{HccDNMG~qF!?2@B@ZUX&&git* z)+4WD(6h@SVA7GGnl{b80sR&)P{fF(X=@((rH3)*EOoh0icKj`Mpc|De%|(7LN zx^$B;`Co-QF7HW+o!tJNuT7on&@r22yioc`wpJt=4g`=^aysV} z;lF;A?!3=)yj@9Fgr~{-dSN=ALE+Nev!ujO1oM?QTzD7MxMCgKC^OU^QmDpN^%eQ1 zp+<#kC+@{PUkG79)3@HDiV9$*mh^zV-l_s)AWY06g_o&r6MtN?QQ|7gvJ^c?6|B^8{Z8vf)($jvx-Tl8TOnJr_ ziQP=tctbeF)B8<-2Q@z|(R?-TM)BKJ9DKN?CSm6;E1D^= zIiHz~Kdi65O-)wTx6c0~i1eTl(<-OnDhvW56s0t>p=aUyb3R|sl8Z|_ zUW_qgP=f1vL2~}dn;bvJs?t$uC20fT_`)4pQhzE~SgJ_(Z5VYgRNWPPccD{OGBk`5CK z0mRSK@$|D2GnYMoKr`1&k5X zJg7tfb~IF^YF|4tQHY1v|3ZGOuj7egGZJ~ zGn_KEZ8LXx7#ZNJur1Y@%lzAc#;mTQ8f{MSx6-FV&}`JQ_NrYe=LUC8R8`n5wv6h9%Q!{`m=l&p*(B1% zlT))3+{XDhAqe1zMnkb$-2Q8+V7H^24nR3`?(czhzDWXIII%q;UoRP`O0T|m>yFi4 zEHFf27YIZO;Ft^LEhJ6)~&0h4b&y-DL7GY z0adRSwl8O%o+hm%Ai}-kHBA7?LfR(sM_cG!h&eQ*mOE`tB|qEGeO;-Xvnj?SJpZ;S z8?LY5pG2?5vzof@ets;M|5^-!6FS3Zco49N1&sw}B9Kh*epP>*l-gt^&J#8Yaq<46 zm$L+!mu0K)(vlf9RZT!$lt39lC)AiAO;{mLSmtz{qxLs?YaXf`M(=L(=_@^mNRslL zH@RWl5m5s=#u2Dy-li^*tK?2RnQH%OBU$}nVLyvc`8N2P{2UW*wD2Qarwt_&Yav&F@DPK;%Ng#p_r6pwP zQ*~lUp7-rMs@Ctmls!$TI;ooQGvwj3btz-NOb4qV9d}I`T662LAHyaX+zl2qaI7OZ z*-6+lL}`KX?a+u$r^Qe#`J}%7#}IZIS$jKxb9b=<28|Z+l*cFzxE|@$7NM#RM{_KC z#zCvOb|Y*TupO;Lnhx} zIB~4%%qYuZ**+)I%)@ojp-x_j9Coggn$e4E$;r`2$miWb?9B&rt~bU2ZB+VsutF%! z$9C`7Rpou4GPE`$>o)q)u6^C6=ba4YWBjvAnH>_ck4a&a#$DGTZg)b4~8P0oTK*;^3tsD3XqS9BGk=)2f_}@WX5{D>`Evn!Umq{I~dcXLOmyJLn@K2r~2@g&OPGVwpID8`Ecjo;DC6W0F(a=-8=X*Hy2^jr07t#?+eB_bj0Xr&~ymCsIbRqRD5P4j&}oo zj0lzZmsLvEbk#-&rZGpBqAdB0u8vX9UBWZYg$_fe72=nc&~E=0QKVk-m)arU?@n84#wTgVxUJI{wIU_#jOB+xg6b{qip(AGHJ0u(dvPN{T1 zyyoO{dL1MWmUKiXVm}8a?KCz+K?fd%qq4?R9qOK?Atj=aS-x)Nw6jPyX^L3TO)2fB z1CJ~Y3Mg%&gg%mbV&~Yk|I>lY`WkQ4PjqO%$(wfg8S&><$ksc9kB!W3dtG;%8Vj@; z0^I&fRSEqgf)}p+;DjAL4$N^|!g`0>Bi`G=@Bf0siva#27LCZHfImk+^;n{g9)}2b zrMAiZe~SeCz{3b0B58shG#Pw_^`{?BsAFV51mD1l^MhM-yfxB^B^<3KN|ixXNI2%zEYu?2_C~xYF-H zu6=x(@&x%cWLz46T~>j>M&|zqwDco5T->M9MT%e)P}ra)8-3&lcg)3_GkSMC@$kj~ zx&_|o)RwoFAFWz1FCX{8_FBj-f3sRXA~5qTSiKu>h)^rF&7@z8Rk82sRc=b&fq#PR zb0l=g9CWQeZpCs>E02G4w%@#rJ7v_K4fDNkWM@G=GgR4=oAI37zYIhz}ftrh-wp%LY$vIY_F& z&?@@U5X32>+Y`|`exBGfVp0sWn~G5bzJ2`zYM5JU zayZ(rYKIVTL^*cIjT{E(`x^s)&0*Sed z^Wcdh$(OLfQj7GYMj>D6_1`X5q5L>fN^Lt8T z?|oAONq>83W@kJrF0~`WEkla4S5OqL-?LvYz!CdK1x5__FQ+q#Kmi5SuV1V-loUH} zUYcAr@SHAG(R_AG9gJR(KU#uGTu>z-%VNcWfrY9uih5ZI&EO2=$hd z+P}=X%hJLi66vwKDU2+c$d=gqXXuNI^}$#r+v~a-!e#C|)56Xx>GJ!fA7NwiA-s>j z2bYkuw8YZr3wNmo7x^z89DZgW1HsHURLM$GP9IN+SC@0Agg#NWnhICoTIFz@X%H@2 zxy*dC<0Tk*NGgPmzgwxVUlbh#0q?lLlsjiHdADv(PHM6dKu7_9{$Qh`lEP_cz&JNc zSDvftQkJnwv%I=6`punK+CqC&`+Vo779yXez=y9O5TL7CYgW-ja&?W*#gyKUFwE=4 zgrCFZde=R~c%mF;R_{8+V7=ABn4^kSh-Td}a(dH2Q`DY;eqtG&7#6#My?>Uw zk><`aB5A4AEA&lv8ti1(l#YRlYNtV6`tdyr;?TKUZ(2)9-8RgU>LAXF75~qtfc$*w zeq2bN z$A@SH7o~YdCi@NY|3Kb0m?80E9EdG4AQwC3-T>t6BIB>CIl1HrkeK+MZ@q%F(#WDl zXNwt5npJR-4yOlb~#-{bBJ(7cpps6zrMe-r6{uLrJ593lWcW znDMtvZ{Wn}OD-?JR-m&TM4=`!V<_sE8q>dH6otq&H8nI_a-x2nOy#1jK};x;KklMgy@Qt@eCzmXd-3?})gmKk?(4 z{hae|ZsOAaF@uwNy#e4X>rG5khU>*Q0z?rW$uSHplqoUIK=%BFksSf`+h8KGa7^&z zeiK{(0$Zy-+Hc3eg$v~9`VUkg<;xQcG_<%}$1)$~`@*S!x*}s>hl|;b+GJ_s3%-Bi z2UKM00%>caTHcJRLS41K?02_HNLZ+P8rC*__$HR*Qh@Vrm~7r z4#op8S*X0GA+NL)HWXx+>C~(T>MQbRD5G7)bIA)sXUI1Yz5f1-_g!mIM$1Yo!Z-sK z_O@PPE!!s)7E~g_5;)X-8-;ddewKixV;-F<39##wM^3sTs1Mr=FzG0ia`Tu;^*3y& z74KzphJa<(6;qgC-uHZL9pbf(m#mpsSv9so4p7?e%TY|e&k8=Q%0&H>l5|@GzsU#I zD8k)*+>@LpSnEsV*Xy6peP(Vt?DR1!a390HxVBr!Lm6yrs64Khv^z9lfY^p(Y0H)sC@I-MbDm0?+(l!EAkRxZBqpH6_@xD8Ap=uU*ib=WNo~2JdFTD zbIyntffZ|?6v0Pm!{#6xH>7Q3f0q)a~#6K58RY`@XGsk?6P7 ziYcj0?C-LIJKWfSdh}T7FjD4dQdleH4{Z410QS9yW$@}G-3T|gsnpGBOW@?zmH80Z zRtaNv!|mbvDY6?MV+A+EMQS?v6@S(+>MTkcmZ8g+>wfuZ;s%S=hPr>ysnix((=MQ; z`@X;S@3A<_@3z$Uax-CQBay5apF3oFceh7~vV_UthTEnzQ9wMwG3?(1eY&F|WKi?~ zV&Y8tMP@6`|8H&_Bbn=-Q16s2?V4yZsW3?mSl%6*aAbh`4&jy5k$Nk=m!C*6yF4Gn zU9MHbkzEy6nbWcUm9e+AdOYDS%6}*ipoSASVUq==tGyFP#?z4B`Gx_N&x>QY)`G_m zezVFPrpNGLb4g1fGBXQ#A109V!vQlPH&#A8TRlx74V0y*oBBc2gx9l`*7{DK2#KKx zP3^qvyH>U;Gq)BP(|Vs2zDN7pjDg_y91#YA#F6G=WY@NKr$x+Zv~ZG1gTTotzVB*F zYvlp%Q}&z9#PeWUfg+F)Q)SksGXDtJHEhtNl}W1-6=b*BOv$GV0r!WvU#gJt41MR@ z>m$SD75{0$;W|Zz3EC+fRm*AJvZ_O3!3#Eh&A$#v(Q^@9&?A)#Z(F{%4P15NQN5r) zb)3{u!qCtXS5;k|E72z*R(#kN0mPKIy}I7Tn#jvAb|xPwwqY;`SNsD({d&2l=l61Y zQ^XSTI13(>|6Bu6Tmg`FE{yT$h7y!xCz)utRT_RmKTbceQq*2_K}k0<-Mi-b4iV$F zeDz&^hUr0V|IW^*goz=234OYvebGwq??)v0AthpR!67nTl( z77;E}=Nq10!NQ{RL3~z+lfWrNWgH^z=}8C$&LI>bXJ;OUC`qX9QvfTKl8*)O0qdbz z283|EBeb~IM~r!<58fvgS4P-=35{R%%T#`9#CUydn^Rlqd^bL>xY-$I>?-wHBl*W$ z>v?PlTiZ^a-Y#Y~)zrE7ukzm`_Kd`I12O~bXNiMZwb|}ywH{Dfae+;8e>Vub?mLjhw zLOyJTpWaXT9>$5-#l)O)TAS(C==SDzY@TWxMSuh2n+FM@7RpL7C)>s=SAc7_WFl4S z=#b`9rbgq}FSAcqBe6ud30Xi>J`wTayWe{n9uQa!UWKH<<(rpBop24e;bPc586ULM zs*};Naisb8bbMZXFAw^$5Gxi)^z+=yKR9R~p>#OVY)>Ul9hVZs5uuv{0FNJnL@23J zMrA|Qko$0PF(M*d1xljpb$uHL&`tLSOrqq%(^19jRKt(^ztKLFjr`tuXJ=}K+$(oU zNIT(GI=T-hFE-ysJu7N4W-MLAuZ*~k7=)oeNtpeOJ(CE7?I?Q${r$&-i&mraSH09;mrTQwzT2ku|0h?iV&%9f5Qc+5!dP1W`YI8Nb~g-d2g7S7UcB~ zP?52EhPdEC&YEzJ{DaU>*irbD_$?C;fslMWwvYJ6L4+jsTPXP@{q>8w0_5^fo8T5^ zL{|Cm&kJ*u!$ggMh}znc9VE>lZnLBB{C6vUE?e-bm>et7QVf5_tQ{LlA-6}uNke=8 z`2ZVP)Kl2*Wac*-8bmyrouYVEc8%iEq?GiTve&;xZ*J~17v0B@OB!Bj+S&qn{h-QZt7Zy@7GDfOJx-!+E_n)FdPjsx{9}Nit zQ;n@Ed2J{5W@7WV#IN?z(`8G5{2^~77WR)I8OltaCOhPNM_3<``CfKQRao`M6PJJ_7@dm3d~1zf1p4o6RzId&ZrBI8(O!cd%IC{q(@}mKiCe zSera))YpE?$mU7n@e3r+OG*j}ikNp`6x56VEqNp&&DO>yKRK36u%*cOcmWQ zQP{@8!C_?T>)DNrZ#HBV8J$o?Q&S3jlYx^d@r%0C`|6|H!Du{G!lA_~t5=uj>0cPa ziAFk;($_xKBqUzK6K{#&9^l9JNTQCoMI&$H#tsfp z%Hfo7QBf6X@+&{9uMjY0{2 zf9k#c#kyG!2x+Y?88lN>=k?8UD7oX|o60pp@kn8kp18-gxKMJF;2&qZ zn{jB?{H0+mQZOe(j#*RMjaD0TsM+pFNmoq$^QVXfGp57qLetNa*?}*fEP+PIu)lHn zY`ukXreE$l4-J#eqz`oY$xDdr8hC^%G?yI!THvyy@`dxNZ9i7q_{&|=r;lT!qu`H5 zuoOR!xRW}vat>+@p1dneNRX8b2!wDy+q}1t*1)16L%sblSHY29kr0+4j!WiBCAH(zIIJSJCnRxWT?tn~+=WzKnj@5gjc%!ApN)Y$QGDLL#|PrUm7IL#x7AfYs8L&D2GrL&n1grHvJ zML@>K#sGV!vw&UAXTHym>rjb~4IlHhU@YWVGT#eskJFL}V6k{^b$#oocuqnhntnF- zJ;XYujeQ_vX4!g~{&t@(Oj1|~Z+-c^ECQp$`pz`1z&&K0*&y(Ew=P}Tw4lld*TE+Bm_bDl`uzUc%6Fsqf6l?iwRO-nLcZH;`@`3HIy+Kc?jP(&SNEvn_c_MRzx zuTN^f-l!xb=U58vJKiLuj*4ol7g>BLXQsDjBuU^AFM-LjTpu#a}}! z0A<3kOVM`AID>Y@*{9SXQPa4c0IKM^VJ`3PEWtLbnGm;kqzvLjH0cg3E2LOAnWd+S z?}Vf%sL#;W>V&={x{Iq^9^BMHv473I@;hxR@h@s9r5^5NdecQtM(DE-)0bS(3kJn| z7%p>Fm)x{2QZ(<_^5?RX{$`va99Wt3{du>{WSw1LOAL~2$bQ>?<01RF@sMH7rFmyL z*UUDHte_b^_fp%t^yh_@-?@jc^`ug4w>zU$Jvuuybp5?=F#!IVFQ-4daoLHSZQ5T< zBY^h5Kq<|BtH@Tl{_CIUUe?}e64i-6BdiylRzhAW~5(sdB!rew91zCQ%?w1?L=&eMm4(uUT(|A}KE#`bDEc$K!x`XjEh+xv`$rXsU8;r^J^r3P zU#`7+yPZg9mz0$?o`uw>aY|E<=^JUjUZOOANs1&R2^+?eZ!A+c>8h}O!2RUWdABtB zKin8eD=C+#LpYcXxMr=lOobS2YJSRa3PO z?tS;}y}El{7wcukvI#-d$K7zGN7O}&LXQvt09^KQuj-tlsAUo{fT`yrvDX%S|d9Y$aR# zmQ;ToGGLX|BMogL+vE8h-4GfzNm<6nXM5Ey^zk^+dBa_8w&pLHUxHNR7qEfLCK8Kb zVz??n>Aho8kR*EK@7*RS<6kZ;Z&tg`wMei)YewF9P`4N_99@aoauiTJK#^A*UREnV z3YLWUFE;E@Kq$QGPXBK_qNjAbqIwlKa+q9{gjZfTMb@%k<*c&BqUpt3Wq#b^HWJON zBTgyr%hMWuOV@hSPUCFY4-`oDTdLvt6GzMGvu@MZjylq#MJhX>@${rE^DXF-ECS$eM0?-GUk{j8j;6z%^P^DF1`jv0PdV8E};9;PXx@3<9m9c zlUPMFjiQ#%Fc}%8&|m=9*!roXYQyw90DfR0vnVO#@s#IkoTqZVRap-%N++hx8%>2H zFq{|fPPoKpg#Sy)y>Ivsc|R{wV;b zn{Y1|;$mZrf31juo+X7H<3j^`;*aAIGWG;}{cj;jI?NisaK?t`&2W#*AwVVA6zX`Qd z9hw?CnyLYyDVMp(CIjL#Pl{F`MFN2FenY`)p%4NLyNp|TmS4o_+^mUhO=n$*jfqs= z3i`+$IYvPsu)8IGLq8vGAL?@%RCc&7H1hLMA3l^(qm{3=ly_#MYJ2Rvt`Rc2`xJ4u zn z1)MixU|{fjFlIkx{}LH{?cbTZCWxw~lHK(Eda8YQxn_vS54P`Lim+^juFDDoflhZQv596w=uT z&VM((eGQV{1e2M~#xwTz_SV*P2eo8q6e9@8lNPMk!c6&8l70yNFH=Xqd2?@wd82Jo2?h$BSBT=iwTK~W$p$lP~1NuxMYh^#6|Gp z?RHY+^X)d){RTW%zPr0i>UmbH%38B4rjE@^1l0Yq|1Y~*Ph|VW7x_h|;?T}wtDzI7YvUu9Yh}5jg5sTTJG$yUaTTk5{yrccW9{Tzu(mRe4C$~9L|`YzFg;* zr0H<6gok;*8^GVkF7BMQ!i|y9pFYCSWCwWF%36*bDBGmOo(Nj&V|+uj49>fdyy z9wLQ#JQ0E{DSUM*Q@Q07ovzmVK6=t0X#)lBK(fK^U;Wixoi8Q1qZ>Z2@`I&A1sU>S zmmE(p&UG=A&Lk#QhBvOYA(U_>cp6l$i?uu!9x$3@FE zI(ltJaprh26x_EQACFfBo_iwhzfHx;ffP=D6*va-Jhhh{lo#`*G_o`FiF>{bihbnS zA6rQ;Zfo?aYVStLmM;H()}jac=bAx?mLQkNRZh$TeMsqK!Z4%M%c~f5pDz(UV_2ob z29bK$y7f;wE+S9O?lRaoO-rS@qF8rD;k`cJV;1!V?z*A+W@ZvN75>k`;AC26QP0a^ zr9T|Ze)tKAm)O?XSs|yE3MVa?ryQN9YF>WSKN12@M=MQ?BZYnDW zHQ{c>)%uBhC6aV_^$IlPL{!!nc;t%rQ(#31$MMjE9=q(3y$7FSTJ* z*`iLDvCKZ2$PzmTR1t&HgA*Uvg3pjQeau;c8@rp8Ierik5mA!s^Rjnuc-fB-x)j9l<;$3vQ>dI%KK)z>3L)6Y zpB|lUae8*Q+H{WmZp}aT;~dx$Nc$Tk(&0u_G`H{AqFWV8;!#>KG!wpwurmE2P`y}j z`m*xRYOzXhbvki6d^7OnjAuOx#ax|i;9x^NE0 zWn-a(h=r)LpKL_o%gVoSE@1xiDnVjE=tm%}_qC_}z&Zj71iF>3+vf5*{FG4=rTw5& z#6rks7G&mKZ~bq5ks#G{TjBu4FE1cbECYUWD(BX#X5(TV*PY2=)TCYhvZHE$Oeyhf z*hod^d`w?c=bPsLw=ZwgH^})P;Z>)^`rECTzR>ykCPR(Tw_JN>f9u(2atZP;K*zKC z7qK_ZqBfuypLoSAfHR|VM2T^f!0F=$jC}?x@6)cJ;vKYhoyiCo8EmS3k#z9k{NK63 z?S8Wd0R`)99g2H>r;y1FPVR8C8}7`5@`~Eq+y6TToR4vw^WE0K51QX@qdmqaRq@S7 zQbq<1kJXQD!QYuarxSb})8=~B6&w~|c9E)G^ODC)OG|%CxC2W7#U&*igZwVh zvk*g^VMndZvNPrC6*;~i&ownQF(lP!;At-KebV!?V|Oqblk)5c-DPJ9Xz*X^VApzF zJUsmMiQ%~1pJ&d+dn}DjTU(p>T^jtDOUugm@W)3!;hps^SDU~s-8swCP{;jt5=KUU zs!pS7V@SSoWDP6S{rMxd=%Tl%)EqU+r-=oF_e;&{H?-(%gRy=j(6YdQAEq1d70;(# zpT&I4L>vGMDCD3q`fHIv1^Ow6abfJhpfHPPj+gBgo(!dFvI(LPr;2j<#%By519jtl zfB&UvI1!&u2B0a10I|bPaYZ|U$8w{>i_sZD;5E4VD{;qBv1P&;AR1(^2NwiAf)vV15u}O;2m*+c z{RT>gxdADHEHZ#j@Dy6T^5$RE5rnTZ)~8d82moM-v=tSQ6`1c=1$F4h3LDi@<%F8x z?A%@11rH8x_ROV8CrJ*l9~+Y>C@8dV*dM%#RIz<{NaH&}SG=f15Ib2;ThNK2nnGT` z7nUhiCiAku>|Sv&l6wLKA{6oQk{i#3fbKqEcT#UJ>S!h2|Km>6|Ds z!;L0YDD`91>9LJY$=!Mua2@3BVc$|6{u~(nw4X)zv>yT zErZxbaj?uEuXFVC%W}#Z2Y0IRUky!$`1mYJaW^A!Dw=?4K+gr{e0)i+exu29Nu=&fui0m+-@Tt-<-{H;M#Fs_f&R%>lE|AwFZS zd!2I7q>JgOMDR2$)t=9NHl*wmK=L93Z}lc`0l03wj!$8OrWlUF`(ju-1txIn@p3(+ zrzEJXrda|(7UZyeV>%%*0Kav$o$>ir(onVF2b>|A80__u90t4G=U!^V!*4mWC1+7Q z<@16a`;K3sHaFXD^r4iUQ!9=-9-T{6v!-8yyP?M74JnL{doDj7jx<@SUN`=u?xt4% zm`^kC16W!vr5ZSHA(N}!-%j4IHc|Qw!g8CVvVu!~T&FxqzOPJ+_uw0RGiWr7isPF? zf`$3~gwZGQgRjm9nlU4KxC4!I2f&?Z5lNimM}E|ad}?^R^XJT~=7t>=Y|!=gKqN@B za>ozBNG_%OX8$V%k;~70QqGz`&~qjl>YpZZ>-m zk?SiCZ^K`A{V%$DL{eDM@=`Rb3RVv_pav`73p`LVq$Bc!$I82WvGH;9bE5eTk-#kT zc02U4<554BL`XcSxtpe#LoYrlzcrt*v)Rbdi`k*#?p6GxP}A%3AwfO+q(S3H2{@RCBjL76M@RHM?R0o9 z`aZq~&lc;6fMWoheiy6nc12fb>tDBJ;=qd|j0Kn=<%Hv(q&R^VRt%^>X4537)LLYC zc;MiXK<4I;oG!1)N>2=H1TI1znp_^X)0I=xKu&X1p7d#!E>`W7-|x3B<_;ryJez{L zj#s3gbl&^POBGeUL}4X8*#34};}geEk4p<{ zwCnSJy*^SdIN|=AI(87nP-Rcbw^LRgakMV5f^|Pp=&FHT>0xw^6#4ir;%Pu^Ad~#? znN@q5vDPeCU{hFDc%fD*p<^kEgtaJzyBR!Sq^C~D!syZ4H|gf$#%t+fd@a4@aLoN+`tmmGJ{Hx>S#k{CVk}{z0Jxt>zGzaytWQBf zlUAf{@V2+w&LSfCF>+(ENtNNjmCJuzULMi)=#$aw`HheR=+t3ZM9Q}p zYCkiG_lfur7iv5qbBMy6W!`(lF_ADbG1*Y|d5^V=cb z#rOR`nVlULzhBN4L%Wt~g&5hWuX1x!p+tuZWk~}4ab&F=`pd8?Y+Mx z2bH?dtnIXEMQMOCAC&UhT<5SUY-+;E0M@){NzfR71Zk&~ZMLYQ<7eA{N}Ss?4&~B~ zjjOLa$PfTWRa6Y#KOl{N06=V-%$ZySV8g`dppvstVM%JzDbD(bn-enA?_F0ukT_Gm z(d36t92OCc%}n(glOERH1ri0J8Mn-wmm)x3LjaCcxOtTWIT;`hTDS7w@3|Xgnxf;9 zWvhEcg#$pHDFYO|#ZTik1%;|&Gl1FJ_+7CLr`T5`#C}1$5f&=EXktntVR;}1K7L6= zc-d;Bf*I!*N=hB2?3;#0$Y8e~0e;)N1$Ujp!bx|pp+|VXzhu0j#X=-)ai-eh$#%WP|h|TKmMQ7;tk&x$BKf>M9 zZjcfF^e*~3v%)ui$aZf+4Cv-#8VdNZFbZ-iw=q1%pVhb;mEyC-KP8oIQkg3-c3cn; z+)>=s8&CL-ov2Ko1!UMiF3xyKRoOXJ?JBx_LGPDQ-U5`2k+|(SA%koJjhQwmFsZi} z9gjX@z``3!z*R77VH}QJeQxgaTO@~p7u)kKdMzak0Ja&1(uOw2vtEmD$!*pdG;nK! zdz(P&ZSCkhFtnM=+}14}6v9m+GO>DI>vYK6%&{BTC7}-0elsdZ+J2MzArmA2c_Rm? zLB*?iNK|@>CO(yvck)(ulDd^#?iZ%_=1#TwxEc%Pq20*BNShI8e%vbUVSB+VSM?3k z!m6bkz(FSqG5eJV(S62w$-L^_@N%?&z<{Vog4}~*CGBm$8Q4L|zWzIwf{yX+;22A) z(Y6N_&SldZ Im6KE`nrB7 znXq@(IX%CMrq=lyOJ4DDeC3gnt@F^tX7@9|ACNzFnZ@EOrJrCHfuP@DH2t?hyWVpW z-9HQ`v-T4JkXQCYxT}eKOQx2qZ~Nvd)ONTG_abC(zL1RvfH+xPbva=zXSNT*>wx~g ze`EI(cOBbgVsWd#{!4}gheeTWC`fj|Mr~Jf(!k7)5fs;c0hV;$&DCRNAaOO`w{8z{C zJJdXv8wRzXhWkB{3b=auN}Fl=89P!2(QW@UW5?v3vk3nm0LSk!Qmo5#$L92}zCms_cN%#K+3^HU4-XMQS%;#EMR6>toI_te*>}%Ui+9rN z1m|L^k_#ihD)Kn1ZR@o9XSjtB(p487mKmpz9zUhR8B z3wEU=0RaO$Ha1Np>)6YGb#4iGTz|CHJ(77E`8QRL>!}N{Qo1ds4zT5~8lhJ{Kcl=p z%#tB19p8^T^3(S9m(3et(k@3tB746WicLKp&;EXCs4tdACsBkyfT@m35z2Tyf_f(% zwi;ViZZ(#hPN|;sW&)DaGMY<{g{i4pmeVH}wPK9lH@@T~n(Ir#=3V?n@NUe)2&D1yd>F19Bgt|}$AUy8m}W-Trl5N* zbl(k?_Z<>&{#{*GPT&H?3SSfR~Y_8-~adG+d;2|fHt+N($1yef5>K%t3`#_udlIXC^6uB-=3>WJwB^8wwCBdTSWGUjNk5AHa9xN_uwO>~g+; zJ3EN=v+}CW4Z?3vyc;ikdhd~9`bnHO zh=ObXS-|#H_qs{K*Xg>7g~rz(!VkCoJY{Q}ZQpcfP~j-Y698^^<55)v6G?NHuKQbF zoK|tq!seu#ZE!kkTlSaO^gKM4vIJ;0BR;P2qeBC^fzYW#j?XH4Njd?5n52Q0S#oyh z6{Hu=^9^*~U*FwI1-?vq+H)lDt(6{elv*q*iVjb;@svzM#}a8bdACuEh&Z2VMdVBQ z<05S1F$*vX2rv=?#uS~zcU>b5~b62sGJPZy$=o6BAWhDuNZ1IFRFkbAk|ZgEP7rw#=r zN&uOPnkKh=-6~QB5{``)C!^&N;qC@BWa{Q?oV0i^R4m*csyG0W1^KuSd2}vUU_5ve z8QDS2>$U7*gjRJo{ZGq_Dy?&PEh8q;ewiM24&8U<>2$P0E_G{e={n}uW%}9_<4K`G z(hip;<5+0zwUW%l(0^Wf%|F$5cI`$u9Rb|{Eg&lnx|Gx}N{R;bf}b)FezT{78ct`T z1OA~+!ddtk&sW`{LqoBept+eEcYhfyHFY)LCvf`C5)=|Lz;&jL3q1YV`+2Y*_1Y=e z_|&6Yi3;CSH8^+_jKsw@4Qc1OKi~R%J6|wF6-hDcWv_G|g3_0=p+qQgvbr^1E&k~- zRP{6^IkaJ_XeZu1e4RJuW${?wMZe1KMb% z!Gfo;_};hwtFZo$+%6xT>&Ee0ZVs$If$Z_uN13rSu@O^|s=<0Qe{pj1?p8P1ehN?x zPLt_$h?J<|Yrr|t0AD77)neshQL*%7cI2;&M~o11Z%?EW)V?GOIxO+VuzzsRn zmA??HD+yl0?*3@Ft{bvcK^a=l%%uH=P`!nu4kZN`6Lhe;+HrpF4+Te1S5F^N{&Gxf z{JW8#CIq%S$PgWq5mFSq4ejT)dXxi;lKFLfe=9xb)Vr8Rf7?weq{qahfvpHEGBS?p zXs0{eTD3D;vDVJ(@&0v@-|fR^WIW`t{#I9gq+?4C3)K@;r{7{tz%7qM0z>xqht$A8 zygyX9pT$)xObLhm6q!FRK90s4dg&eDtG9e;&yU*4AtJ6h?(5s@;hScZ~%8S}{`zBHT)N}v$I|bU3u%Ai3Lgl*JQEcR~V z4}nYx1&9xW2BN!n=jEAz&q}Z#-}|tiOcWwzKCnj;2rVxu2`|OQNiF;+zCWkaN27S6 z(_}o4|q6FW9TmNAzH=uM#@Gu2l2I5&DM5j~bP|A*{oeu2zv&js5BZ=SuK=klVEktHC zF~;-12u{ALSX0$#54{&cVMb)^1ii^b{56b@J>ms1-xQ=|6iV_x!=0lwvF9z-`g=jQ-WTuGN-W`I1e|@B^ zPhayD2}^^luGneAj^{9EhlN7vZL}FSqcq0(32D8a5?z-ksz}4alj`mLak@X78mV;_ z;QAcjFIS?JD-5nKvs6zM#0q~&9PuHveXevu?ILTGqHttvs&reui3Bx`uNLQ-hUF79 z+8Seh(NOHVUqte!u^dS-i9(lh3HO!N6zMvA=80){FdBJo*mAxNuE*Z2lI`?tSHV%l z+O<$(T7Tmj{yCbd+Nq6QhBRZ2mQ}ha)ua=_^G^+BVN_-SOPXMn@0-5skLEe<}6w{~2D6G0EtK?55rGj@8{>Dd$$?Zy`ym2H>auYk>(BKLQLimslw zj!FDiv%0pP_S{BwEp=B@6 zpQTfEu$u zbdZSvyP&RD856E7Jp>shB{i2LbhYN>$3_k&9<%M||xWw=5= zQH=U&=D2ls^1}V$HGLC~4vN*UsYoh2i6}1J;mH0Yr835@Q?|$=8;xbBFU-qxdo{aA zKx4h`3lndL_5C*q%iKBK91XV^Kg;-G)q9l28IH37&`nnVZGGrLQDNdLEHcX%vk-;4 zMXcJYKEnGs;peMYrd+a~lPDw^`a;IKwa?#s zc64xkT^paKs-U{#K*=Z;^tUsgY-Il~uo6aK7R$Cw(AMi_|Gh)~d1$cS+Ku_$@f8LR zWz>u}0sxSz#exGsck8eBHlC9~Y}^03SM59A3S<0I(&cd`_e0_1`rVsgHQDLV6Mi^j z6a#yhSU>}+f8No|)S@To^$kMfj_IXf^+`4!ZQnq67$myGfJ9=Mz4Y}fFbezEmUpP%mykxqL*q6Y^6 zrnoYFR0akuogDq(=jZPpH~ZcnF;j{o{M*hdrR@P573Y$8#JICSY{mBo0&OfGFWKtJi!S z);Q|HLW7H@n>{H&;J3DfczVIWh0~#~cA#U&P4~<02&##Bp$D|I=l}A#RjSegGZ{OHick{A*q$f|omV?KJSM)5gEDeBYS8*&>tpy_ul&StfgCIvQ@Z@#0`2zE3+q zs$_7dYoA~B`w>QJ#Y9zC*|W@Hv8+Lc1PEkunw#T1TT68YS!0yi(zfLOtrh0Kqz&Hr z9TmU$Gd9a5l)02-`vr4eLiqjZsR>lTUa!2G3>rEdC%qZ@sr@viw+}bE^6iuDni4&d zGv8NHTz07F<$KPdC!LlUP%GuVW4_@P=irH0rDy&+)yTnx3 zG-9H|y=7LyHz_H9y5E@g$m2S3heL_NBzVa>zE63SU91-uZS>%%N;&oFnphws zR!uuwEN0CJn@5d+k4K)`(vS%A{QOV@O?xCjYw7 z&7Tigym02>?jOsIHE|RJioAri9@Ajpzy^Hf14Tr?zgFZIAmC&(dn~ip*^>ta0poFr zu=lrBOGeeoM;}J9-5E62TM;oCEmE!O5}l`ZuXXDev~Cc5UTG$IkVy|?e@Vdewa4IB zm@ytVndrXpvRrO!X*($!4L4s$7|xzn6T_n#U33blW6?*PY#AZ|YnrStt>6rFL_GDN zF+DymKd1U>s5b>RRGXdG^`A4|t6n^Y){U`!KP%D_@KB_Y{1ma&^l2TkTJT0JSU4Xi zwgAC}X^s+P&cEUrjLui`r4IGrdH(I*t$mf1+IY537nnrYH#pC!o<_+x!%R2VF!1|1A+um!&W0Zkg z@$HXeFq_q1T^bsp&zTn{`ac%d&J;HE6?P)+%!V3uQ#iMiE<6gk6qYUxi$CuheVPb3h?yH@!nNh$pe_{C>Zo2uJ{deW|*@`l`dKy_LBqu4$B z@q{ka1L8I)_rnE}z}8-SmF9x-4$JM<;L8|qbLX|ALCR2U596zVXRNIZ%Q`Eb@(RlE z@L$su7{t*KzW~x$;T6T-&~f-}zr5b#ZaHHVjR1v>+NX3`B4aWH)4d+>ciekfqip-B zZU+$6tFa2uiwfh8@a_hob-x$3*q1&IO=1o2c;3r~jxm1W41ztvLWE|;VZ&hq2GL1Z zaG96@{Fs?gBJ&wIFmDC(=@cC6S~VRH{2&f*{AB$y%>z0*N4)F}o|x(_2vV2_ykGMA zC6PsWE>^%ootXDapqsHdtG*i`A62{ zcy`#PBx+W_%BIGfMnnpEQ2D;YzTVi)XlXD+Xtf3nwMWHQHaGA1Gdf0Mc>@a{5Hwk9Se`51|ANG8O(?pZ@Fok4&1ndby_7=+Xyfs0l_y zJ=sN7o@$rQvUIh>d947A*T%-ctJCr2j*px@=dp%Mq*5(N#E=yhg}8jBx%T~BGkV)& zhaDAd-J8gkz|$VIwEODHqUX7(_w7%tFatd+8x2|TIr>;H*_W3;!@?1GB<=)ZpeznE z@+l)gX&zog#0aKRm>^m{pNOqWm>`$yd22(VB%4q+ylv;xE}DXIF&;9iQs!;2YsPDd zl3et1I74{{S85rC)7IRdw44K!Zyz@bUaLfGOhVs)_NTRh+p}hl6P3fPcR5^<2wP)u z@}yzMkABb18|_rG72tzseW%fj)}X&GOWOJMld?HGQY z^(a#FwZ+E`*%2Ndu>A~O1-w4%=5c_!!FB>hxdCdr0ImtUY^!oUSw<|eDQCt)BJIBp z4!l(DX#(E|-L*LXAeHiyM%!?G`!;d^o!yaIV^-_5;d1p9r;=oHu^=QOVWLKBa#>lr z7F=0HlZt4~sl2^=t0#^j{Dk)4NFmH>StUeyc~+V)T73IMidtAOQRqNP)rjG*fIPq) z8yj?9hu&H{s?Zy&ba&VSx2d(D#BPC14M<8M&5!L*r6x0>(+fQXCHmHSTl_Ne?V&Lv zg_2XC;XmogoM?n82APDnTRPmraekbrk1OX`bnWU6fq zF>Sgrnw_`S;P3{~Yw_ag0k8Y*biu+-!qhcpcFY0K#$Vlr;j}CaYIq7M7dlIP%2k8T z&-=h?A(5M4|7i(=#kXakRIu>le9LZ$@N(ZhA1c24AvVTzHt-jGLzx7hh zTSH|AjFS_*;cY&9fI^vM()zqv%Kd5ArI(0x@yj-c+j}F0=~D1{uhDK}XP=zB*r>1l z{5(rh;B;Y0hJ;_mXBmfUs!w|GWS8NNSLE4$uO)I2`E{b9`pykZDm|Dp*VrgGO74J6 zm7i`~Giq4~%XU!baC~T+fRAC^qO$2d@6el_vx6hT&OetGj ziXyz|yz3^JyQtv`djDXT#~TBm_-EElQGe3aE8w4=<_(Owh|v9URo(jPq1_O4JtJ-F zdQ4qioNOZcmI9&jd*xF&w zKdyG1kKBx4!j1w!@O&X9@YS@3Jb$%va+KY~43RmtsoXgz+F z-y*x%K&0IWihwh03K?6_3>h+j4vRz)3sFr6$;b$rE}NVo^EyT7Nwi@xnq=8|Z>)$o zU5pDf!EE!`0K(V)OU}KXl!KL)^h;hI{_ED!MHmNi0NE5Hqh7>dEG@s#y4xP!z*I5B zJ0W5JJKV7hX%FB(E&6 z!W10*SAX+#TLzEs{b3W4lfd$Nhf}!_h*1=ROf+syzjU)VK`Wfu>0Da(x!raV8zDfO z`w|-{Gm3U5^zXU^Tdd~~Sw!RL7wG2upXs~96H5*j2n8WC1lV-y06&O0{Y(z!j|;So zNL`J;ZRxY-o+no7G_xF&iUcAS$KjL?P$er+p(u@6A}k>x)Kqx+JI24ymc8w4iR~S1 z+*2cj4Y;2|)h%Iih(!1)Ijy8eqke08N_k3Y>n!gVgI-4;E882uGq~)PvZjd2!FPyX zMLxlBK|PUjf%3lbr%zkCXGR&D-nHbicGlIhc*XldJ39 zJk!2M!lQN1C%Z01Wjg18Asz38ewx@m^1!cq?dJxUM8Hm;?-y9eQBCv3a+5RADJtDQ z2AXwECbthT0(()qJ0QYw7%TP!!8O@rQK*hP7PBnuu zUXm9J+&z{Ex%|WswpJ1hn)$C6l#ECeEE$tIv01{|NQn7>%Ox?s?3!QkVg3$D$=4cI zPf_?--6I)^z5-x|PSfAH(T2~M)R)&nLdspQ4kDk?PmARiQ=LQGWb%;jNkw^i&c`!z z=DDW(bphKC;P=S|YiS9?o)%Vo6Oljm{e$+LBr((9*> zY$d_xLY1;c^+mnsQv*qsa2+q1?*>AvR-D9Kuvi^=wxITec>PEWGoSE2^b zP2Q`S;+D+4sI{Razb%nw_|3&m@-Dv5PNv7ov}^AM;UC=;64MV6h|76ZbxpaJ4QrdFYIcU-1a7Q)y2 zM5!Q)mqJ2z4hR4|8Q@dxzJ`1!MQ;Vdm|ZuT32TTP8oF;7y~d2`?wC>qxe^|!50$|` zPpsu>ys4DUI8Iz_?%F|=JcA}E@4M+-@mnPYj&_6`8+A!q0_{Tv-TKUS^)r5pulJkZb|x;|{*) z4B&O{XJN5I;FckBHz(-|OY1rWA0MDMo^r0m>1u2i&kx<^;UEyMuUOH@v<}v@Pbl-! z-F!^|Lu>D|)Gv^bdY7uwvvd2+0QGGjRAvBM4ha)LuW!%8{orvW<}T_7T7KB@oOj(Q zQ&Nb=fsO)-g%#7SmRxF9@hRW$s45kmU|RWjFx4rAu^LUTtsd8ON4ReI!a(hHy=o%X zFjKRW3C+#L1~+{s z&3-9*esUN{qL;hK=>S(t=2LGeU>{=B*!uJ}AOKKK0W0>_)5{F_i8EZXvfGKYNVd3? ziPUg3xfZ;j@5n4y?cEY3nqh$B{g;pJ_t((JM}?O8`5(*re6H_nB{_QbN(%)s>jFu# zWiM++rmq_Uo*P%&ZAOm4XE!JRq83UK82^F3ATl*8PK3I(2FT2s*oAZP+PrX>X>_TKi~fH9&`CQ>bpF5#={_f_nsAm zW#RIVd87HMXq&|}=x!S(2cv)HvJ0WhJwcETPjs!5*wifH$t_L=B>=#PG(Ud?sy1a& z@^H1Bfd>~vY^R`jV|MLz)sCMzH$oKX z5^CgS#dB`PUO_bcl!Dm<&HebZ0zx%gk#;p?K?+%TgkT`c@MKA1BDww57C0lVB^$+7 z%N5=m!{>CSLq~v+Uzx~4Az~%l09qXqlk`xU@3oTwPJVd>0971EqvE2X;*dA{b%>ZW zV(%YvZAnFHViM71R57>Rr7O^-yspUPb3-70=_orG+E!6~ArO6g&`C;1NKe@3^0ydWPCG}+bMTUTQu+e$YRbj3MN zVdDMqTrbe1^XU6(+CJ54Y<*GVsr|)f)%ek1xq;c}Hpjgxf#USpPU6)u;|OmB5Av&l zzG_=%k=g77z0E{1I$HtBSgQ~{okUOBEdh%M1FEok1K;v<3m(5~UrsHb>w@HDw&v9f zgIHh404-G%k%05=U^d#`#3B{7CZoZHP1E28x6`$Er3~t!sUlH;Q~|Tl`ubeq6k+e2 zk$w-upUE1v$0b>ndI@n>njmp3g=w1Pk%QX(kbPmk9Tg4nIl5Ba_5cKBFE>n~Fnvc~= zrT#`=9lNLcKW@HsBt#mmU%w=!g0G%1$s&UCd zyBxPCR$=Ej!6)E;?aB`&qSW9JJlMO#JwyPjzz=S0>mRB$jT;)d1W6tS6W-qxwN04$ z)jchZr={!vxqSb*P;Z>_e$|n7Qz-b7qBEMpq&`S}l!y_Ej1!xir=kK69GzXWW?@*o z|MpdO5SKY%4wS4>A*9Jn85HGM5TMSn?k`pXxhf9rf1k~Wu-EerA#%!A_fYw>CH{f?CU}PU#SE?2|xab>`yRC`cN*D z;*C9WhV#9mbZnCV(+@%w%k*{$$42+=$}coj8tvX=oe1>ra39~vQ7t}Rba5j#d^5}V z%m8@*@R-4+5DbSGa^z8h?R|&-o6ktDVxfwI0RBq+&#=GM*utPpaSkhs6x9oA&Hl> zov}laM^6ARB$kZ`I%!h~M;wZb_{k}DtVjk#UR`Q3&&3+PH%PwRUDG@BB)sJ0QX3;d zpeD7Wb$SLP3WJ7XDk(kEyei{nt>&6SwlS?7Nxh{58-~@buOtaEFTD6rgT3}@S7@}HT@{a zz7h`$5RZqVs$vDREq6jilc4y~b#`!hop5RtSzA&J#r*NSAjb>*RiCMp6{go5r&9Wh zp!0*EsA#8Ozu|eOsC68Sqa-MaRI>NTp4C*iB%XVXX0wCO7o{X1r@D7|f@wWq*SjUM zke<1)Vg#{^C=oMHfASE1F8n5f5Qp+!9Bd1bLea_USUSD0$!d2zCota3$|IMMM2D~K zd`XQcQ~igJ|33ihKoq}G9*-!KF>ZI$z4uM32krsf_i9f&J|C#CLz{!0^k4#Ou zyl!)@nl)Zt_kQ-Q{>m{Q?kzd`_ftz6EzL7~Pd~hHNB@Wcl7;2(NJ@YewM_fSz$fm` zxo^QwQx4{TGBfRtpz#y)UVbHd&YZNQ1cxGF*?Uj?>7$DYk33qnd^u3iJ6$KJQjQ-x zFeaf!5gMTg4)Qo`22&7AaSiq+z1-m2fCBu$sJ=y|5G4}vRNblPhCEAAlqw`N?a8ND zwfw-Q6QXitN>XrJ-X-zuU*tD0E~|)b*!TLnZ|{$a8Ps;^;+4O=mi+wdq~#E>@cb+C zUa|JO7N_UG|DiTLBX;gUq|7ZyGd+ z_X;eBHg1HF5Ynp`@H`*{EDL-0!mge8xj$1H4b<*FMKg>hGHmlE;mjHG!pp2+ZZ75I2+L-*c`8MUhU;6X~M)d zf-~M4_m&_EzW3#Svq0q9cAtc`{OPBkIF9={O)RyBoH%hJ!~6>F$Zyy`4v0uZj^-ZS z{-~N$zck_{p7(H^%zWmIqr4(~`~<-(GMw<i< ztCI&;Zrh!CTUvGT)(2mX&*;QyIag{=PyQ1nvh>?X2f%kzc{A-QvSu~pY8wZOQ0o7|6VCie=sMlln5D( zht5$PYcLfcTKM4mu;x1R%5@_g6^*PZFm!k#YQ6MD=}(`Y`1^a~KNuH3ju&`_x{eDf zlgXrB5j~DeTDrQrIv^n6x8bFZ-G~_Oi!)ywsyejswvD1FuncD_D{I=nKW@+&6@Br2#FVMZ;J{U@#O6lw{EMusrq)qg8!~RR zGBothyPn-wbznxyj2DK#5UGm<0AAoFEGGiwi3K7EF2H4|e1G4$<=2nK+h9K3lm%FIL}A^OZrP|ATooH4%AQYpPI zS1U^&F%)BVnlCk7dLs8vgy&n190?ja9AlE7PezX>gdpB(HS`~d03SL?bUMh$AR?w5 zMFp%*XUNP%AXHRPhYyj*pQ0Ug_2!euqGr$Hl&U>td!F9?^rFl~_xHbF5W6~Y{yQsj zwF&=o`cjf{GiT2Hm-n<-EWgUOzt>&Hb?l}AL@IcG=g2!X)|xf5)`+6OvNA_yC9O~- zJ^V0YISRp^J^3qESib*0Stc_g1Qit}ix-#nN`SwAGTe2kBLAmnQXd#oTHL(&sib}b zwQs!>RK0Rz!PfXJ9>%Lk)^Qi>hL)GtEld$-%2{##^d#SC(l@(y! z+IrT2u_t2uP2uXWI(w~TS4jCwa+QHCY5NU@>j#DBQh-`6ob@y{0ic~f9W;f+^hyq{ z-&agAlp--@!*)cqt$OScFBU_b`(Hy1wF6!mPHE$ou8e&BmBN#kb8vo<&FV~;KNqT2 z-+y*vp|x@C?6ooa7+&P1zS#f%(p-J=)qaP6t@XR_zPn(-f@|K2=Xse-_8Syh6Cem7 zUgSB3+gZBvtMgxNp1WDis6@by>S||IRn)9mM8p8#^`dXSfyIjvMS&E9`{o<4w&TYh zr`+{*tyQ%l!-gn>jbD5&0KkI}vtCPk-L|cfGp5S}0vD~j(=B@T-o97P$N_*CcqyX# zvriHb0V0UP%ftWr*!IU?+W+$V6W-S*C)Hfa^EzEJl{_^SpF2;D83WpswAPcy6(PYf z(X>)Y>S|GF7~%{@TX`u&L`CQ#j#MAIyoeel(Vum3sepMxR?b-X+O^IUw7zJ%B0MrlW$*I$2~W!Y{#yQmvRIrWveX?9yQk*H2vNc zQ$N4?lj*55BtL>eloWJ_f4x^0A_Py$ym#r3%SWDnNuK?-tFqi$RIKZjFlwY}?N5EC zl&{go+-W|l5FF0lSs?~Z$%&GHi6b>y*Pq*jNdOS43dwIPBtS@qE8*8Un-Btk5KxHn z3f>}f@oS@A1B5iOaRJFG4XySoC+q+GLNWoQjEWlc{j&UHhwr+7@Y`=Zq=|HTPkn}J z_a*dKP}E3=UHtgN!a4W!HC|eK=9_JUKlrez@Y9F(e?SKh-g?^x8LNwhbS?n$K&}q-{jXNDQCqo+=coJ&+dM9*_35Qg;6A!BB*x5hKOm?Shbq>iY!O1 zT#0jYQB)KX1fXg7{BvL!^3WrUDB`LOo5LqfP(?=m^Pi$h0k_}DwC5K(YpY`x&Szzc zKW%@)AvjmvvI;si<8GXnPD`EIB zLb0^ya5(B3wb2n_p<;PCgoY8s$($7xXu!ZQO_et)c+0*+>l@ zhN7dOc=?ggK?6lBKDG17vkm9g&0ZI4i0w!+{14s3e_vlpGVa-DpZ$fqtz5aXy1M$; z3MgOSoP$3;^LWki*AKt`)zq(yN+ZUYrf6qXRm{A3%CImj5Q=vE`OlYn^^z}KmSzyd_~%QLzHuu^!)zF)S=YjGKSD@(|Q~=62FG8`fhH-!w+wXo)^knn<;+4!vo( zAP@$rhYr>4-C2#ecx6IvPuk1AQG>hzxf{Ru zde)TEHy&?3`j=0ldj%G@ygzpJV`*u3eLrm($H)ay@Co#PIpzOdocI^G`PT{z|C_ud zm7}&gTNiCy^y2UrGea{4k;gWBc4o_4FXFkhRR%*qrGZhoMoy>#U*E2ZsV7>eoG z z+c|?VuwPDjIk8x&5hE}G%28U{bm$PP*Ha)|xk75{VQg+o`P#K{lc#VAaTC6q(Clj2 zwQ!dt;C#yRe?&jGWLKXf&BbD|@I3GFc)Z=o<#PSYKI#8cx{flA_-(IsrTNMOTORm) z^5=;GiBc6|+1FpoOeS4wD(@8-7M3qBC@SKgdoBebIBw<6OYFyIcUhOX5Js>m(V_dUgJ#IIA>!yk3Kid`wGP_s6Dggbe`&-0b45GSu*->Guvk9hkW<^EhArh zamb&%A}^uCezPp<)ye~YTLA|E4zFX*+BtV+-!(OPs@LmgXtrVBK0*j*FbKSW5HvNB zty^KgCB&22w=o`z)lpFqnB9lSRPVegCM3elTj=(plG-hsBBo4MhebTS z^QlH>(+@L$kaU!_ zbHdkShDHxvK7BcZm@Z}cud=BB|N2r&s{S1Px*T=AWL%flr5KCbvT)tPrv^Wj8=33% zxLKBKJ$qL4cvR8R0?)G?bLC3ur=Qln@=8o-sA=80lYe=&;+}h$dl&aES-P~Xuq^e_ zS=$fP-~Xudu}5|HKQN?bbU9!Z)T?G25Zw)fL0UFiAyE(k=R5cR((81|h%G2y0j z^`~p19$+|LU%$kXcaZ=yV|w_-vvqAYqayo$+JcP1>&dIZn$+7lyUM^hA~j0g)EH?b!FfQqS)8!!p;ysl4%F}qLI zxdL@a_Q(+tA}^Rvo(Ue&pV7$7uK1U~&Kxm_ms^geKJnz?kkDKAe6{khI5 zV70F9~-HEW}$O%V;@fBUQ0w=YbcMqA4(JuQu4l@RYNFGUEY_a<&PvRZ-DsPq{bA}B;{ zM4z(!537o~E;6pGX%Zl}=)QIJt#@VLH9LK_$Lr=~^7c!YXqhbG?z@4a5XCqgtxrB# zI(>TZuwn6^d~*8LSDN2?C-}3^WP5ME#a>YzKBC|0)nC2*LG;Tn1mHA0Hx@YXzq>WbCBWN9F<5{=vuc zEjyAEkXA;ISXH+6Py0SvIdl0yQ{bG{voRuTX05>(i$wh2X%6Xpk%)wlS!uI=xUvc%q)+edsI2nZ zorKV-srb}MAcQF65-kOV2tjxlDJutrkSr*OlFI-xDdLb-he)ICcc5rvo8~cYC>j0As)Ygy51A>T)qP zBD+qhO~~VFrhIwR^TS_wp#K9B2<|^vSzZ?zCov_3D2k%)-F$DVDfQl*dnEw6v#zf4 zhacp@!GI7Jfu@D`-@l~Muunc2@!tDqmo9Do_|w?&H)UALT0=(+Pkiv^8My`C#OC+ zQFG$M2_K4Bq=3R6k2OC(amf-^qalD87L}DjQ4uUyK#Gddr=OAq3m~T-)p{wv?ZO39 zzrHO_&0BB5#ECF;7^**T*kf+$wfJruaozOIm>_lV(kV-&Xv6={+l~;T5lw(hOPXeO znXB3=X@*rqMYNqgix4vCvALBXgs|7+s;M#aIb(~ zflAZCs>6t3w7t_Dl@$a~PCxnO%aqG(mYIUhhYtZE*?ogNZd6%T&&cG}n>GPJyfMC{ zwL}VhR=*Y8W5m{lt{&^7=7X|qX0K~($D*!rS5$!{J0$Jevgr+uW z??0F+d#nxtLR0kTXFgw%w_^3I)oM;H3IfeAO$QDLb~`PXi@X4U91gO4IlBLTxO^FX z^(9%dgi21PD%bo(L^0uxg{39pd+&&IZzDPVXiHh8*V1Cj$f;SCh|nHhozDixv`)Y?L1 z!NJD9IS8R%fyuSDT1i#{_;Oc(5TSrV1f8uvdvnSZBH+dY2WXm9RTHHO;u08-#g1r7 zlaN?mf?XaI5s5^esG}lf!668QJ$tCI7}P%*AClYV|8VPPQ@@y;G?^E9mS+F61sq&2 z83zOaEFMd-x%l3`_e#Mj>~!V7`z|1)Nk|ZQo@SWZ+S(63XuI=H^6|&5M~_&3_%S&x zDZKG~VaTZQLAe7CfB(v%J;meiT{yb0sd(w)6;bhhrGP3T^6Vc9y6986cQ317LM~SdMS-k z-zO6x6r+!6bG7;35Oucyq$sM&Ql(;*A?lF!f)Z1xqm}M^^israw-^X0%D-MFp8N~A z+FD!5sGM4YXiZXT=$PS@Wou#0@P=CNpmcFtYM3p$?7ewA(v5M_CfVc{cQg1;C7Z|g z$)rzADibg8EXz5nsyKroWb9ZhiUiR#O_nc5H{C=^O3=z5@RLuHs3@`RN)e;g1!Sb1 zIfIvdiSD|aN=v6J*KKrEmPJpQcDCi*)E}nI?KAK3+{Z;M{_o=Kmb5(XHM}p2LPNrNk)wz0I2QU8BJ0$MNtj) z=-?54sCUN|#{1Z%zq~f|)!{M2y@FTbB`GI%qnGzO(shw>60d})L(blNwqJNZEQ&14 zS@QGsy?g6YQbe9d6eEh(zx<`RSFeVB`@Cf(-jzRQg~wdSrj^0sZ9wQ5ZK*k4P)br1r{#Tc6~uEJ7TT7OOv8g9tx z%RB5r!NE~c?WfKn!kB5bzUk-@AXHjfU|9)Iz#G_qAa8Dq3ruWvnS00`Uzoh4twbl& z(TJ*Aw~DI%Zr|=og-pom8|$oZ#}ykj`n<}TUZLvZAoXe6i50CU4#-_UwOIoG{sT9h z-FxKt`m;4}-ZAj~{;}gYM4|4%zW)Y72ubYp#Gog72lp05k)c`4^Hr-?tK;JV0As{3 zaPS~9nLrfL_U+`^=V_ynF8*S<;IM~}8@*>Y*}8>1^(4f{xa$udQYXcQjU2V+(&~9@ z=Y2Tg!w366=oP%ugZ))_$rs`671&F~sdkp`q-a_m9_Fg5AcPDUND2yQLWnA%m*B7= zgkob!c{v~iM8uYo;xKI_!Pq8MKz9T^001x{2$6hqK8u(TWpMeXjY_?&#Z1g*NJvD2 zmsf^`1rHlyw_~@PN=QVa*JI7g69r6Buwg4TeH;jZj)i;f{cG+Elag0y--rprC|7EGL+2+Rt4?6zZF8s%i>EBDS78(|Y<;d_XV2#N~BT zh`PExLqSVHQeZM5r0SK<47hX09yT}E$S^XXy)i>D3hCR>s?$aFCh`QT{1aXK%fx)* z^x>i??`JXUi-n#a?(Xx5DrBIS_mIx_@Hc?k{(h*#&;SszD2gH?1PgrY*|RYV7RZc7 zj0we{+FEkz6mdGq=~MW*=P1U}+<4?z$k@@QetlQ}D4se+o_>zj8>s5-`;o{8q^AGl z_=j&Fd*|C3-wux%em(rPPBuPMI z7`iYY>-C^gQC_DfXxuoZ$_`O9Bk|1oo~*|KuM{t#7w55k{-NN+My1Yh>HW851QoUyZ!Os-;Q@W4 zl5DQ3_NwHNG~8~FoN$xV8QEOlHg0?n!MI`TRyKZGyKKhs!-X;0Q^-@RNFJpPB1joi zR^D)L&e5Ggd+nk9BYG_q1TRf7lJx9yg#SAvU57a%rE?IX7^Z&fR(pB5A}B}{1cHG= zWc_-muSX0`9({}kz_mN~FiNE+Ci=@|g4qnuJWJ!YmZD|LOapR4MvnT^_9s?eUbSuh zww$mWUlXcd*I$zoZU8VTagxnzD>4_ULc%cb^;+8X0Te>S+(HG9&~jb$JOMHx3a%oJOflNdX|2 zE>o$gKnS#*JSn=|Ku~!#go^GTPrh#_!n1>)@wKIq6#IVzBVEV5Dj|gCI9o{x5R&xB zBeY6I2v8JbHaEWZS{)Ycvu1IBeSHXVKwCk6!o1tu(GRTq7N_?SXWasBv)R$moc8cD z#9guGr*SrQ@A)G?;1Ebvnb#vkD!CY#;21+35v;4Ki@ESbq4XPDVM#=+wy|zqg;_=6 zTHfKd*VSv}Ttu9#r0AkT36xApfXG<|bkFunjPB;$4Ys=Uu*{k*8(jrg${N~xr-;3J z1v)CLybcf3d-SPOZClUADwDS{j}Y zGX7sB={k%oio#xR?UpT?#Kf4}Zo{HTfMQs*W(}-gkA@A!bLY{H>YB>cKSfQS7Lb+k z$iNbW@hEY~F^0zv^QW2>tb0C1<|#RO9bA%rU1 zE0Rnpj><}Ct1qr1QIUk@5O1?%uU8Rda=CDI4J4-k0OIsobxbq?+)#zErsyke;Y)*` zh0Z2P9oY2$4o13OzA6A9gxW7%s#vi?85v0{6j%@tMNyRfz4r>6nrfbZUjD=r!z;dB zUA}Tv!u(tE_>_PAQ<;=x9Y4Oje9iXK?|z7ye6vT~`R@A3%J^A-yJbzPF$QCNomxNk z82k&vUo34YgorU&^9JRz4T^$CL=ticF`%$)mhWLyTTa)eI01pv|l zQ!_#`5rCw?0U;^jaH;8%T&C0`1z{OFEad2c;?bjZ43g1WP0Vd`k-O!LA|N0lrOtt%PWJ*BifM7QI0R>eJcJGlGYsgPe*q#Rfe_Ss?!2qX41}CN2Yq@I zibA~8NpUh-E=LH8_qBa=aL%OO^D~0erRD;@knVp8BV8X~l@LTx9<%x6-FFA{?=LeL zM4rb$(KPw+L+8?^x))yz9Y0~bqqfmwv-Y~^7go#zuug2mnaIIJ@&7ofb+5Y==iZMj`|-i{iquMeNavmMN`pHk@XFAAdfXh zwQ6F+03nSMx#a1zV{=%J5+UT|?MhZjq%0T!bS7#qH(w4?1%n`lj2O}6r7xZKq-OTT zJYTb6Bd1b&o2rT{Wy6M=2tkZm6FejvXjyZyr03b6ov)SF!;%HF@hX73z zf<+NY+bASI`urVU=~ew-cmt zIYFnV5F)DRsKCL45rVpU6cGW0fUTs|V>TloZVPq5d5G>8`FOv_#LjJY{R`Cm>q-Ap z8R`1SID~)@TA@gL;RRz(4#61FGy)(5nM{fYABY(_a%}NuUzpEdNWTBR<7dk5zPn<< zLjAoDkIY}AZ>>`&+sc?BP3dnZXR=K@i0ShWtg$SY$h5Rhp{H80t zLesn#ODxBCf-?kMw&H zi`|;1LuUbOM^_5Ty74P`zM5;Vg?qHS5zr7}ghr{eSw5__{QhX&OXYd7A@${9`o*4Y(^TVF!MIHhA4KV$_ES8#6 z8|B6W75fPxv_a=-YNBZ*afsYx(x#*UAryrW5X3M+!$%+nIXw;yr-hCM?SRMuf&7;I zAXN~BXjVmA+EGITiH)Z`t!*twk8mov$4xvQh>AuCA!S&oK06x#3ac&00^WRi>RXgl z9M<{%_=FloQ9W!rxQdZ(^z#4z(shi-F)CH~#EEi~siV-20*uLRx2axuQGIy!9BW}g z)TGh-_MCt8k%|W&3%UQktoDLxd7o*~3t#=m>D_-l_VJ3DD+Y$73nI_ZT{XJbRme#< z$AAG~Sme;0*uEMA)v)_u#i=tTi|Mz-xD`ly+78D_bRC2kO=P|@uFO^%oE&`yI zsinR#5{pQY-6~6EpRkPf;zC_g!sWcih$tf=JQoxaI({M`*ZDkWWMr^btMr)L%!iH& zp0=if2Gh|mZ<&5i2p5R4=z}*yXV@LOn{PcB)c^Q@jQ7zo8R>28=mJ1629^b50Oe}2 zxN9l{GJ1P4_x^igRyH+lI$gP8v!&ot!n_5$s&~)-aqeG7zcwp<)~}2=`d36cd~7lx ze87Q<1Bej0$>eTsB7Q~9jeGZ!&N*VQ*HcqV06Tf7Uas$)dsnBf{O0_$klwh>7CLrx zYda7E97{08DkCB&nzC4k-420)$m?)5A3T64KnSgG`p4~~`u7XU<#9(jD){A6e}R#@ z9{c}&F1nuB5sXDa=(&>%Mi|pE3+AUh{P6aj4UaumdGCEecik3y{>8rm5@Kggd3o;( zKVJHJ`}{3wL210m(|%opeuKx+VJjjO6OdpJ^}a|VV41&Rb!mRzYWDy!PG z!`{@S4G8o&ZIr86mpjwWw^Bd@fCwVSqO?~HMNtl~Lm)VuHR*;AWa6{yE*JLe#{g2= zuAJj^CL|P{EKsHO4z@ep`OCg6-@V>yGheb;3zGSM114dDCC|cNqat6u7(&F?ci4xR z>tWgbXL%pKCjijV?tl=4F~|)1=vlLzpnm5qF)0ZqOn|ohLS;m7#Mp7)UtIC%_D8>& z{*8p`|BWkreM>VmdT5QUR-`C}*640(?wSkBs$ycGbEUAryQ-?aqF2DYjFyQVjD$%? zBOBCYYfO#IvYt5Ot*a`lLD`v@5b!iM=+b%<0%he891Mhj=yCyoT&Hhyww|az`s|SB zd?^bbFZrV^e_3Vz2l%kp%Z`)&rRc7`g*u54f+KFe*{+G6cU$oMMSbqLqhHP8i{T?j z=#x^GtiStg!}-l~w+5+#{L@~)B}r~ztW`1a%Zu%2^Mj(Qep+qhmP%d znizLu&`#fmbVPCf$8N0u3BN0b_7_Z@4I@E9RuCdyJdXzsq9;zGD>rPh6&INL_Ivl} zyC0tX$J*Izv%|7~djZ!mPC$f^2wfyALT%9n22m8lNa|l|7s=}N(o6^;v`iK}WS9%P zS;U%DCYqvIhNUT*rf7y{yrS3YwIyj{nZ#ISWOQ{IiH{>vd4t#PKnR^Y32CVqA>^v5 z4jz9K04zKAW$&OqCY4FVqJN*L?!v2Ms8>rm=)N}){Lh0SzZ02w1NK)%2w_pwsnD)n zY?O-0d-IK?#Y-U2F!hJ2Mul&fCxeXtWIWjlzhKDAxGJ~XR{DJfFObq zW9StdIhqfRjt({$6>3@4rmc?CyRN)=^SP#T2P+TCD4ANORufN_UBCrGb&BTpUM0m=GDw6-CwHN{O!fT|8}(y1%wckI}j+* zUcLE1aP9zkkm<2)kDhNhzir+&gTl}Sg1Zqj60xXYgU6a7ve!CJzjh zYMWb*9ML2uB7}gZy%vl4;34_kNiB}1?IqhoG$9H`p<-1^R%uiQ)L5$3N^KyoG`AMX z;s*$BtkO~l5!0YS^58&1$ozSr)q=aV)mB=pOHZ{6&P_#|zL@d_Azj<+_&!`m<`O_P@DK|wCj8(}=J0DCMIr)8h-;eccH*mEq8qm-1 z>dVpL?WN_}Z@zhduaxAfHQ%uMx$=+uN(xtPUJ9gwyFwLx<9xjpGr+Fgh(Sc$yu}U!VUPA>M6eKpZeR=r3 ztkA4p0llQ=*#11z{ok%OIdIk1GuIGy-FFT6xu=Q{l5=wKlb$z_Zh(xFo&h1mjg-lM zdZs1OaLd=D=BCX4)8Hqi-GC_SMz*p-gkT_u5Cg4iYR(Ih*~91E7mz#R=tuvs*VVLn z68W+V=-8K)I+aVMlCzhhZn`fyHRNvh!ygQPi(mqTxJ7r1)9mJ5FYb9hJ31#WWVlN- zL(3+^Ez<+^0j(8{L49Qq5M{w-k#=**r=Pisw;D2rR=2sdBzcv3ZpLX{3{*!OtoL0If`m5UR1(?5*7U{iW}}nEXYgE>hArzjGo45Jia? z2j=v(gJ;rr6GAni-%R}y2nhIwc>|qeBT|4jP#IWXUxI=Xb?NDvgak&fCtgqew(W>v zylrh1!|2k}5yP4Wuv zyvU!dI=QWU+sK%aqhm*R7<4y?Zj6jW2nkqF1_l&QPMq;+)(^LSKP_dNSMdHXHp}?c zNeW{Df_hi;XS?5iKW!?filzW`p&`Ad-)gAZeesi`=$RRp^3Js#JW{8zhGz!2S8tCn zH{zj~AVgD?f>uUwN(2xb7B{-j&EB&PJ$1QF5gQk801;|dyq-L7_~kW4`j-00-iz!J zO+kT9?Ja*=U+|r&eVieqPe-Q3Z%gWH3b<<;X#Q;1Q3vF6F?{Fvz5)OM0Sf@I`pW8) zhfl6QwSH8>$f-$Fl&q3mt7W{D%aH2T{ByoHkR)+UAtYd7(VF>#qXs`e?0G>Hz=y}a zkz+%MAOy&yG&MV$L8bDxw+kMRJTMRe#4lc4_RTl);9$XSZ@+l4;ow1;)!j5ES@e1Y zEHE^KXo>?aR2K>WVY+Zo%2A`)Rad7DkHCnf09cKN)#)sSh32D20|pMH<#J`O#Q3@M z_S7BHRO>UtvP3NUOZlJi06KW>nqiF|Hr>y?p3-TOQaoIJ_`w4YiWYHj%;4TZz5Vwc z{<#aU>sx*XQ&#?>~^X z>;|^n%BXQH6&~=g7js z+-ENp*Ic%h6*p|!EDsB_78C^L=C)qWi_95vsqk2iGRYfUYb`2Jn1VcJv*2(v?AoPG zPBsl5tn1xdmy+VFtJ9^X0>D>CKAD%X06L1ms2+X*TxHqy80|IJ^q2Eg{(pw6!=CH= zOtdam`m+bJLL!lf0v0gF-TFUXA4zwEeAUkWOPLXUGb8$nSo|F}c*zLvV-@O61JZQo zc{k6$`pT<_QySxK^?R$e*@FuUiy01Lh%LYnd-6W2uphp8=3`#LV;VaKfY5T8ppqe; zS1~$cw1bvug;GQm@~{cxwaPVIxS=dCXkd1l*xFvdd%JR4@Ar#-FuNP0jIkJFNuCj( zn1im6uT<&)*J9Ql@sw`P2y_!_(0Li^+?fmj#-vUh961ySxSN|rL=?urt5d5YCAPX- z%~tcN`crX+xB-y^q%OxddZ9Y;m=x1HxVMj&ekUPCX$&Fi&?f6P03v!kg3HBfwUV-F zIeb`g>#dSXqZM*}dhZ}pv|-nUh{;oV(W{J$Ar!5Oi4hsFxGh>hB4EyF#9l9>Qn6}v z^z7NX^mGKEj)}p7(0u$@XzuXiji=i@?W5yHi=s#YbxjxbYDwq!C-^n3`^8HnhV;wB zO1zX1k`k?pwc9KdFK`+yMbrLVBz@K@v_N&iI7b{h(LK|8RoM92irNo-0sz&gjgzp)Rs8TUtH7@tN@+CGasQuNlefl!91f4e=CzH98vGEB}^cMP!#2B2+_q8oeI;32Z1h?;8U~UlXt!;_)EGTDx?PpPH?y= znUXOE_x<1lg;ov(BbxTfN2%}j&G9#nhS6RNPXis{jDX)wdqrQU=ojKXH;@P-8cst1 zti&T%YekpKH$+u*v@`>Zi7G0J4UHu3!sC@E=B%E>W3PfyYB_DFCL~xH++eL&Ik6%k zj$&C+5GabWqY*!l+G190@vh0x80>L%=< zTSx?YxRO0C_m8MhG)u`1wDtbCPw|6j#~beB+g@LH&CpD;pf7kVVX%eM@v$ zxZ8ckc-t$VU)V7qtS>@H0y=bL+$GJ~nXB*08<5UD>HO08z8a?to4C%S`?Km`lHBLIf#UC55OK zcZ)W_#5?T1qEwFB+e`yOhysN4>FJ2$e8647>s80bZK>E7stFBJ2MJOIKIvR*)MNF#IidckRVj=C zehK+&rcgT%O7jE1Ye5qouFc8-;CZhAh+-Jjxmz7TXRYJ|gC6jgY1fR|{aSQ`W!&$U z5JCop!S1#=2%pTCu16H*_IO-2TS8nNN(gH`)HFD}e~dmRMjzw5 z!|J?MOY^_&)2FWkMl_F-Ee@ys$ zhc_YZR@m9++Slv2hp3Ve30(UggeCzLMWO=BB{LpFR(R9XnN_dbu&)m>`N>7@{7& z>NWHCsI$Y8&viu|pkMIU+Sh={`4}7k05B#1uxod{(^VWDsTwmnKF}nOj6?+cCu0!+ zsAFS^QfbJ|RmH_~Y84?wsy6$Bewi@Ah(an>)nKj9(j?*rbH{F50HWLNTRug2I4jq* zoL#S#>j)tp!9yb&1EwjuthG$RDj13h95DPfU~BCaq;C*wTsYTx<*Vd{S0|2zOA(}yf0gyWon@q@Gz(FVD zx$Ej>0fC@UcpIBl;StyM`Trr~u1z8q8Jg1(cb&B?TsvB5X=IghjH504(Z3{3W#Vqjg$qafud-NMzo7}%}kyiP#5jCPRu)BSXybV*}54nHLTkpe(}w; zJI5C{oG1xh^5~YlZ*NVT4N~VRzxH!JF?UV<)+3(c3!M3%*Il~#^mdn9BK=F#6ad_L zN6O;62K(OtV?t5g^s}?10JSw!94jtwt0{`sLonT>oYMgAOs8x7(@HT zi-xQ$EMUY?ZKu!3!s6YO5XR~y_#sWv2oR0vI$NDl7X(dKTj3?QHhT3+eDB?&wpTBt z(qA}Fl2eE>Ax6pRmS6c|Xv}aGr;Lf7y}ChpUySpEU>F7|Qo;7dJ@) zH#P?P^+y150KlCHP2zVW0MI8Ts8lMDDbiUfP6?Fo@s$1fD@@5y!y zgZTVW(z#D(_s&cH`k7JaFh5W2+%AbMC74i$Yk_*Tw1vf zd~q=X2qCk}9HI#YkC$O-r<>@FlxS^l+`h%h!^x9`09ekP1&%FlE*u><+8;mUYl`U` zhVRa#i@HV$U5WT^wfBDRYDf1%1O$L|K%9N*RkG#)iRM;wQAyjS%c9+mD2jB4m>`C! zyHs1a|E+oDSi-D$76YxN8#yfDp=%(G?<_WohT>##Y|gSjG{r^5SFn8n$fR zv2A<`yB#$gmw7Y?8jjy5+|v>D=#)1O-f7~DSZQYi18wc~`bbWuQsLNWd7C%4-^HNb z@zx>3slcFtrrv5!2~vN3)a4I!u?6bDAM{KB(+OrOSRHJEe-tSJfsK)>P1Gp6_R;537MLP(SW01!pr=!N<}LtfD< zqd6p3a1>wN00n2sj9b8b;fgvo=I|k83WFHEjMD4fUZ+l`gN^_i5KI7|)!i1XHbIEN zXv)g329=U9DrG>wY^xRIa*%O=tu6<4xOiu@F1n-l*3aN2eVHo%gWdBFK0GDrY~t19 zlxxh+-<`!?9@o?mnx<81pc$z<6zRN>q7V^;lXWQ?z0950>M?r*GeM!ej!*s%8Fvlp zh*=Pltv8AB(=K?%I6C>W^QPdRj_pEn0x*FwC^<4AH5ejN1Nx)_fGCQ- zf^A90c1w5D|nITXc_Qc0+fsciUL9i=6QsmxCkQ!6Igeg z%MJB{pp|K*3#H0ALWtSfk{+3DJAQ^T@Gd%7rNl%K7zJmwfMg>E^zGm7weun$tPUof zdlf>DI7Vki%jXt!hlX_gpdaIPi`?}9i}Zjfbt|)B7$&1nK&Q$UMJ$PHKWH&boy04Z zETWAFj6WBU`L*Z|U$rN7t`%F z3?8cP-*;T-@Hk^UB8Wmr!nk<^j~DYHis0&X8%S+|n~5PTUnu*3p@lAEkg3AsM zq8LPYbNlJ@t=@JDQ7p~+Iw^GK-#fwLgh0oRgI%J{uO~6_B_BHchYpYRXIp0a$WO%r z0MIlbAhp%ry?ex_CM=l?UG2UI0Dw%zQ8b0RP{-Gi{;=b&O#~1S(k^)76aj*{RZuE) zvcmS(HX_p+QJ)%l>)qmruM)_rVqRh6`xg#AZ^d|0i3>_*6V*>=H$t5IOd9`i z0!I43Y@AHS`z`>#Ho!U(KqUJ$f`D#~EoX*Yb zZ3JJPC7V&0wlkVqyNzBPo7#&IYqb4MMgA z^Xqeg0~AT$Ilk(TSe;-#qhT_TseLf-zY~LkuF}jbP~5)@*3-`#2wLnI7MZsz26w2) zC?~^Td(NavzdX=#w{)}MhHtO~HCcvATFgq@n*YB#iG7WSVMWYONyaNRRjS~Ss~45vNIQ@thIfB5|_!S026K>0rml) zbvl)Wx~LLV0>dEem9OsPJQ`pF#j&H5TsEA_mo{x%rz3_Br;dX_iiX^wad~ut#IQ ze;d@BwM}G(k}IrJaY!35BYWJ#1kj)%zVERyO&X?Ntl02n=~(nyHkF!Lda=nBnM#ta z7%9mjtQ!n^e+9n|%&GjTk@b%7l5B;4daOW1mi9y23njfCUpFXyIsr}o#$81`jahe3 zt!%=P$kc-Z_|aehxa{(tJ0B(l-fkxp$O!~ql1Rbtelf2^&dZD03@*T4g7hJy`Z~Je zVHhkCvc6^rBJ3SVSmAT+lB{0CKNoxk2#e$@Tz!9xkukk#6(uExO&}1AlQ{BYpp7U` z1MUyx|LNB8<-8epNJANb7%;J_u}7W3daIN0TP;Ak~cyaN5st*nlSB`uXDS@ zDALe!nzh>~nwlkap-<@6j^h#vMopR%+jxGr09VgptIhJSzqqV5hNFSPymTF}!IEX~ z7E%LOQ1id?2}}?-m91#$tT;Z6a!XQT(j@mcs{z5KunftOeX#6Xrcy&TO2vD&q zICU2le^H1JGyo(Ma?;zTopA{UidUO`9(7(eu*`48Av$p2k4PLw2&UGdS-|eC#G@2c zv!*32Yn+ckBA~tbj*@=h(hlAzSDjt+qeT9sMuV2@ppcuXGUJejHdMrJuvU*qc&zNH zBWrFZDMWC}VTp+D$3%1Z5aJ92|B~wagqK_ZAoUQg@q0PgzWQyMz7UrL1^|b!qS5ZN zp}D!A?RPt8-4?%@3V^EHDtTm!A(VMa=z<@61koJh2K3 z&>s^+13nxH_=rjMb2119cWei#O@xC)ANZaHJ5 zL#Oo!W-eBzSO9pyNFpp@c`jyFwJd*qcCreGw(T`*kL2yWb~h9=6YWOI^XM55r%a8f z96o25dYfwE%9p{YUi3HS%l>TL%M~tt5b}}^;~`d><4&y9KOP7kk%7m{_*4-q z6~m$Bk%UP~x9mW$Nks0IQs<>NYxMC-tVd@&xm53~0!R+M>S9EfU2rwCpXd!i& zi>Kh==a4R#M>JwLx6Zz;{=BE1hHMk9_iB?xpm$TrCIWwR17`Fx!#i2FkIW;`X>M%9 z1PFv-Bw&_#@f%o|#5OrMDJq}P)zRs`1YFwbF7kv57;BgMJF0Awr7jNr2}J1Uu>A;N zZrkHeCkyZYu0jp`6)6`sWhwo4qtm$jx2ly?XcP#xzjC2jU`8VV}1GIh?uJaBXU&dIDb*_5Hg8)k=}_(+}k zg_c#Yrn%=P+*kpE?iRZTGAR$6?N>*I?z>cZ6(SOB~_)_5Vp@0P}!4;$e* zkRq;tBY`;b^)gTKW~Ft~EfzUH8vlPwzml|N0bzoqJ;ozPyH0*lRMK{7ZLSsbrmA+t zalV`Trls71rh-JjJ|o`UfU$(~DHuv8Kg;48qPb;Y|ClAHfOKPM1MZAE}uQjuy^bSl5b6#+dWXvr#1Oaq7DeJOBetE{k(HbiK%A3 zyqK6G>$0;>l7QDdqs!5DHBSdlFe|LP<{;)|9?Ob63y1pV9^VxwlDpCnO6(Wb{<=2< z=b?X$pDWcVAIIn;xZF!c?2gPEK@$DP*iA~ISPrr!3b4_HtMc~J#dEdvzmC>pr}pCm zj_TV0Ji-oB?lm!}#Dz&?oIzd}VroUwpW7bq2(BCF&$39JG=j=PbHCmZFSpjafB0zV zlk(bB@p7lnn^#Xustx_6jYuJbz?2A#h%KUILIeCFT@}I#{DHctLRk8F*dvL)%aQw> zRHOQ&1;hPrVKBV=4QsC7j;R=`Qv^*&<-qQK)2E-LY4DMuL_+L&7|;=hz}R2(ez;>d zNDqyG5wzB@_m0A(N%sh2Q3VmQXON#9B@f~E1mKPi((k3~pS$9}pUo&P{f2>t`BQ5! zU=GgimUV_!0s_FXc+>VW`lR=*OpBrKt0rrcw7LLuXN@R!7=>m6Y9f%#3k{GQ8Qw%0 zUF8M~h6(nxg61EN=tIb_i4xWd0KmDZFmBMgx4>KxR@Fuulg++%6p;=&^xSkh*ez-w zNg*_(u9t@~_q_NLdiahe=2*BjKE~znsKUTCYYG9L>>WiUb?Z!U@-9w-pEFtdxBEX3 z4#N%}RpY;$uKPJy02l<_(-z=-j=Q?A>t0y@+ZX&X{kyOjdCB&c;GOa4#4oV>Y4+r& z(r-FwV(W1PvM>O%gJ_9!16nbhyOvgdusy8Do>Qm-c`}nx)T+)o8I(dw=V7`$u>=sC zWvXCYLv1ocm=-oHUnL*i`l8-aRRpcq&*Gz2Q_c0S?3osBN8Rq^#Sd=`=SrhHrldF& zSGu(vGHzO%%p8>FjR*h@fB*ob>Voz?s`|;RWj|E(tz5lPwZs7LzU^f6!xGLlp368AF`o;F| zF-?ioB6T|}D=dZqYaARoIMY->D6*>3s=j%O0#f&)@0x9B&drNFRyRU_?_G-#U;C)v zm5y$BygJT&`A=>w-qioZ3ef`M{xl#VH$8i}T4ytMqK;2*m1-929;TC}!wAF8Wu8i{ zN@aKad#2s^aCB<)`$tRs=jU$^ddpxqGH4_i`a=gYd?|AyW^yI2uY$k&BFeaUgqKsA zS4n3j9>9JyU`X(VCQPL%b|+D&qFo7t837`p-dr=TCr^e8=Zw5N8hm!LN~6O9>PJ~* zT>n^rfSh>6K%bnU@6_LfPx2i-PG3OsS`*cd88f}rJgRg1-ycP=)1Y&lQ9GtneB;4-Z;*ZwoZB)OS?$}XS*-QlD{MqCKgvJ>nF41jN?hXE8pU1 zvU^)l6cBv$p45aO2!ftjVlgQft7d$zj1Q}}w33w8zjl~L4DWt(3vr{tCMr=$byJ?N zyBDh6z$NAakG7FXgW|A0+g+hFN+ZIuIN&p9^-Ds7;?HN?-NnrRm0H0kXRfT2W@};%an2kG~wqSfOXOX5Q#wJ(=#cXYSW*5>CAV^Lk0 zBlXN8%?$Vsxxf%nNJJ7E=vv0@jM_3*BlS_tI<9u{3sb;~Hs!Pk24{W(vsV%DY>DGRBtwG30xq084T+u_DCiXl?01vI#q32$ zf!TME#94K)i)A!hlQ@h&s&&^{BJ0lbo0BddE^tMu;YF)vdzd1p^_njx7JJ)eTxVw% zmdN=$~R zns}Jvr<`X^I18cxa@pBq%tzQ5iiuZ}Sg;)-p z)LJrZH@8kl^kLV&ssIR1L4@Kl7EiV?yzklJRb^tg+4dz|!Nq%?))ftUsBnO=XtZ4` zUH~2?VlO6@y$8${Y2jL(Zg@umS-=QO-2{jhQW!8?9dCs|Hn1FFUdZxYw3OU1u}lU* zM!!5W#P9V8OUA)TFrHKT%RIXEzf0a(f5{W!f%u8ETBdKHw|)4a6XtQRYhz!u3IV3T zFZ|VsXaMs)O*+w!U{VbAAc@vEB7zM@-zONu3%da zF}By^(G4MvK*m6ARG;=W=L2SM5YU;Ybg|6!qNRwf&Qpay{1M-Bo$*p=Mkt!oFa9e& zlI{i!@He}sj8QJy4yN^yhYjwN9`$Zpuq@A}?^?D{F`JHVsP30NF4v7?^9E{^$_jfl z|HcDcM}L27YuSKWM($tX*MK3Sap#|xApk&DhZ2JlOn@Ih3;65mm5$Db!3`Md`*`p$ zB1naWqf_v}E{C1mJB$wK?6=7ix|l3^{ak?Qv13d8 z?D+Ki#OOYLUUBtInuRd$}@{xoM zFsBmy=B|nNlUW)SEQ0uG zc8F?`D@{}%sg8?n{%h^H_9ZrS^fGO9ljJwTf+GB#MDlEwaK@V18U@8~HFcGm`So~x za!%;o@?QIXxav@uZtSOrD4A9CwsKlj?mtb zG{LLqx}HVfg`8SxVnhUqP(WsOf)}J%Ww_dDrh@BVCF0mfHYk_v`5kUhuq&a#=EV^S ze^#`5@r1c0BWbk~1!aL3T)owi8_6UA)yvEnFa6?Nqs(ef0O=n!KQ1 zzJ8j)sh$aH%q^VrCsE=oXNYIX3j48#Sc<+UMfxZT^dUcU_)bCnJ1~;>+6nl?7WC++=nA5VA>il&|166)E7_D2qNqcv@Q+yks+_8Zka=OKCA%+CWO6|-o4b5T! zfq(V^0s8#jq8#pcP~4!jFm=Hta<`@Mpn3se8#)y7Kjl+x7IYAV7*+EiH?HlACI}EZ;nq{im zv<}bixaHL^`T*I@PEsnqSr)b3K&m5fFXw2GVonHtHn+G9`~iY`j@U_ZhCA|*5!qz3 zZR*qGYZw$u^PFyEIZDX~CJs8z5Yo&85G{Yz<;(9*$GRHftF(7nzr34rDvQDf#ECOQ z=@B~4@$D|NBr(D$K|_gCX;S005X!c&`7Xdjov3BbM(44)ZewVJaCAsgNk0eBINjbq z)l8=ACcb_O5@32lXsJe)pggGaJf~Cr;YSK4(v`2I1<(&36n(1A<~zmi)Sa$0PI^B` z=s%UGZP@C&<8?s`_^CQ&*)uzH^Re=Bb43?Xb*VJJ?m9^VtwD+qNh8&>1x>~1M+$85 ziwGDkBR}HmsxI8+`WIeEeiVlINEh2>!$#N79M}?o2+Ht-Fg^h>f`G6i!VB##uJcdt zWu7lCkZK42Ai(`40_ub6;(7Hxq@2*~>oP66a9YU3nol&EzO~=*FDVv9Uw?O%pL~_hi5hnL-wY**V5$FKSYAoR}ZE|Hiwc?O0@oi zow?ym3nT1N@KxdZd_$+%!)mJ!(ciDiN~XB-Eig=)0?dw*1tCvM%c!CdOelS6JyZ#P zOcmf|{IbM$hqba7bkodFtu|5mLWtu2&~hG5F{YJi%^v=niP-|F-*lpu8`Pn~5r&>w z(|j<_H89KA3=1Vpn;b@wj`5=Dpe;3E9~>F&B>Jy94nq?7sO%fTmhLEvP(io!GfSMp zVoDzX?*J>zN+n$}1x?gSHXQ7pbuemoD;Qjq>&!`(P{Oh-n%QLgK!MA=v3@}D&Y`E0 zy`ZIV7E#1J1S-&23XlYCYmB#QjSXM~5)*o}W6DJhuQ4THLV*+{3KH@I>-Sn@#jG&2 zm2E(vLhB@0f&gUBK^u5+JjJ}d{ihQcP6(;@dUm3UR%k+iWKX3EQQ0B^rgeA_+5|T! zLfT+MUP;oy_40s$qq1wNtlDo=h!)ODgy)PmslXUX245hb(Qc&Bt-ybh0z;4nq<~z& z=B}CE$wKB}s!9>9?FENBI<7_EVXEFm8XCKRr)^9()sSZ*erikGM&g0r;h z+uWt^1F=FfG~!!JA*1?X95JV*2vV+GP#AJ=h?5$Rf%LzZPyY<*O0v0;6zhe3oJJrU zC^vuba=78md(Qz52RoX9R!F~_`Yk)ngf?6C(INr2^IcFwaANLoqd{VfG#&sPm=KJv zueaMp>}(R9PbDTv(C8W9AE#s?dGxJzHEo&${?*2ehKz01aUy-*`6V&y588%c0sS9H z2WI$+1xTK!+f6bxm1gI*oZWh4-Nyf73zbk%oG7m6luW96xHHpwM41+q$0G-Bx%J_v zr8P~$jc4F#JAE}oqhU959~OYHkDFV6)7R#h;ng1q&%_{@g65w$BagU16-Q|&0C5H@ zwL_qEAUR<=bVB4~nfGV?P9+!8JUb!WA+OH~Oel=*p1v|5G6@V+x*Txs$@#@GSsL=EfGl3) z;g0J`GzV`gBw3bB9{#tdN&ywaNz<24H+bf-v7$~qbw zyZsi?Dw-KRHwR_3bOr ztHlIhiy%)704zuWflGkb>@nh00LE|Y7h5tvjO9mmeRBZO^L=!W+^Ua!^=8wzhtfkI zc$sn&wGg649}00=>=(&Q<4mWOVnE#Mp#Tr2`3Aw{0D``plnnRjnPq6bzIj0RPcyq= zx#HIOx4khtHuE{Cw6uhu_!nIvl8k4&Ayk)l=3Ol#sRxXZN-kJaIQWum3na)j)>{MW zsM_=qs^t=-tbnlBq%I8qq|f~3XhNg|+fyoI7ZN>sHJ4UZ^&Cr-o^EjDs}o~E(px<` z=B~UP(Xn6Tv>CO_5WxK_6wdGGkqY2yc#B53yrMkhCw4yZLrTI8;Iz6+n_C^LJjiZaNZgADQdZ~ucelfkvTu18W3{Rj&YbSOrndZGyap1l|?kc zU2HFn-&XB7f&If`m-ZLOZrDS`FMc?MJz;$$1VD1187B-G=s>pR9xU<+^o7eIM!PDD z%5DM(Esnur6BCvNQw+)zE7BDl!Y5-NvMo#hP*}2>#p;{)k~YjrjxpWAGfJRHPD&c7 zTe}YQ!h+m29cAj?%ZkKls>-XNBP+5-af>oX01U8;eZ)%YA*ILXQJTUbkcGS{Cy8~C zj?;baH(WS_Gu*&8OA@W`9>CrNr5e82EVvC=YrPEnDM>z@uTqb)@}b16W7CVXlS{9O zRY7h;ayuQxl~_EiyXq~vjP;=x=I3v7q3;tA5^~6RzCRu2`o2F^+e8MF3t3w;9G@(O z$U}}~4gHe_UBOXbz1~0rI;LZ33d+jH^~*ah(p5u|I1F?Q4BlJ*@OAa|_(t|$76}rq zW!%DPLseBR9AzZSZ7(-U>gq1Y#kkK-tZFV@cpmq@!mD_zfG9m!L$iMGg?_MZrQUW^ zP_fYNjxnxw(|qjo*TtG%)B9==9uQLxWuARELId; zE;X5lF82k({_Hr(M{V5D@}{D4*Y$ESIx*n`8H6Tz5eiY3sy{HPqOnoN#H8FT+q(Js zVI#19B4Ey#PhQDPPOf?MzO>uNL%18kTk&OJPs}K*)%t>RHKk0t^K5QZFr`7Han6R? zrBtTZWHhf9t=(CzVTg(k@776fy#xRB^eW3R{CKRpbS-f}7P{^E&%^SRC_*4z)X0rI-K z8}6}O(np!!E_3dHAjYRlIW;+kU*#(3EG-5wp!cm+m%C6UtJ;#rP2IZN1nRDAa{d=p zAOEm0t1hjl<)YCp+OL&A1~Pn^hB~psxQvEYQuS6M!j@X#n;C>Yo+n2SWIpUH`4(L7 z00W`SID>*!zBOIXbbyZMlgco>&%tBZu={SEx&S;@V<70HZDU#HlV4w7Uo}ZB(1{pb z;2p19Jv~VPBH>=&=M(v?PWuTZ+W){tUB#sP%3GT70%C>m@ z#XHwN!h*wI1v0`AFuuxpBXK z*_o^Bm6LMZ<#Hm{%4gl(xgS+wyQo&9!GG1f*rn5Sk$joLl<^u%!ADyLuO<1#4X>`k zz7b}qf}27DgMBKPSM}Pd_uFb28W0vBxae%)W-X(8v~+FJI4)?Mc{{#nse&z=D_gPX zJBi)u`7w)ArE~Qp_Z3%GqpUoG`#NO8IrMBvyYZ=d-<6@|anNbzm(QY{rcS5lmOj9F zoM5d1-8P+!BS{Um0+kZ=Oi$%k*JJl<2w_jjRK>V}ux3P}B-`ebbqMcn&h|r)FkOS_ zamj75sWaiRJcqWnyd35F)?1}?!u6B{6ok0f^U^1-8%B=!)sLbuAZ>vXgdZ=JPb~N-!p{UfBB8eY|xsKFwx++~K{ki;Jo_|+eGPB83!--5cN3aWs zZb>>`X4}6IzAcsW$trnogxD^+esPQ^pH@;Gl9JrI-&8V&RDyEwF%rpWmm})w&z4Mg zcs*^YQI0f>A&PD^zixolW_FFfET&h}I8g9V@%Z6MJi%=T4i;K8=MCO0-*ockTl_hS zpBm=QmIcH)Iy$0~d|z5xI=Xg?@y=W!A_pFdM7wo09mz>G*Nu2(uptJ21fSCfDGGvP zo;u08zrEj1p2q}%6H=dED8I>_30Nq<)2OJK(1}p6OCGk#{O)eK_>_C#X>(Rjl=ogK zXTM3^y0}g>mpI8FPT)J8e`Pcg zo!9+lD3H+1`}%4$^;Iu`PsproWY=&kd*ZHLpvzNqJ8EZbC$IT>O0AeXu~fWHS*d?R z1Nz$r_@~Ei@1#^cQ>*fQ3uFZ%f02F81`)qnHc~gIma5gj^LGgh%spWquub$)K!r5h z{_|oh0m4sDaq>?0B2^=j;f&}r0|7S8gn+%Os;V!(ufHoQ=)QhE9cik}hK=IkYeKh~ zIS4lITRW18;%61*8tyo)P6S-&_{dq;+6>j=6U-XZ5hjkuuVTHlucmhC-S_&|JTr+G z`JCdo>UNpk5&?XM&#i`|?ZH%UZ+1J)%vHkt$MlzUgO8xs1a>EIALHAEf@XT5d%WRB}6gaj%Hv;8=A%?bmr5` zHL*Ifg67;GQZ@!FXp(Nv>3jYfI`Ja4IoFj@jenRDo@bbzUFl;&IAN0SSFLl%o zGdxs2k*0t1l{bVoBWt`=!GticY$eLEL-2Qz7QNdcOoA5|>!}wnNB0l5nXcEb@5gT( zI>pML;g=%e8pJJZgZ47pXNHnqGbR}NQ4<@$_uYy~`79oqidxY-H9EgvbI{=OgX7oG){MQ29gjhMb~dR0-I zgfM`!VhCqsWVl?crDtbn=j6x&<2~=s5CHI~BD*7tRz3i(1EjDYA)6XRRnc(iqvF8C?YN z2SP+v2F1j75YCDp?aa4l$Tevad~qSmac6%jVj4iOd5Y;u-xCfxFdyL_c01c@4H}9P z#4=I$UBflo8yAETl+-8e*^IQOZFbv-_}kaFz>5BDOxROYl)1;_f$EcppQH3yqM01O z&b&(Vj)%?K_i%BZMcRCW)Vl-Zu*I`lQQm5lhdm{aFcKA2)%VwbljLGC($dmkFgF_; zCgA>j6&{_XMrqnb+G@?;#=G5v>pMyhQ7VC!1kf9gAGdLES_o;0{E!c%oE8)p&?=W- zU0IPakIDb&ciL7EA{-=0Q-lucysg9ZgA2@8y6A#I6Y1S5Ec^lw312{hP9)T~bsU-H z@qbasP(;3`G`Ot|HGp=Q;KW3{9{Nr7WLN;9cWZh?o-EEthBE!IBX=S-i^?)f(n&V! zPC77vRK(|pPU$4ZRTD`*wj->+WpL2 zza9p1s2GAPUdkVgrl_M69hRP+o}{R=xIZf=uzv26nv~STvTbi+VL`1;1cEY5C07V? zV2_YWG1~ElKy`5q*oyBd;ldY_P{ypLscNLiaS1~M22Cw%$4$MWM9W1#7vhrJ7FUf7 zi^!iS2&eE?B8L1auBt);+}zy2N+tjM_m7Q@O-fP{qKZN0ixLcwtDJQmZr3{thMI$a z=_}Gl2mj!P{saEA&mDK6`h_YfmB^5R^Q{jZzy$@}1BDQmhWVp^`6mE94w^DZQ8I&A z8O45Yv}H^f%oLK$1wv##6r>O?l$DNR3zr;DSPa8Uo|#e9NQvzQd2tDXOJ_Co^ge2Q z-zuZILLpD&l?a#EO1+2#u?IAG%-;r!vHJv}#iMau^lJXLCLI_5k+NrcN6xYW4bhh= zBhm%Dtt_boWj(!1Cm!Cwl&GG3RQ^W*fU#u@8tv!swq2IXiUqIh#af-oXc#&y>*+^IFS7Bpg*PBf; zGBS2SyyW1Iwh_fv^U;Et^f%_Ly5LhlJmqrfv!!<`$Vs@yFVgJ5kySnNki@bQQ z(gWA0+iqpc!Y1=&X;dkrB$|jDqn)P&8$g6I9PkOzNhh}bv{zjEl<#3JF+2MWGTscn zRTz)Fzgm>W24u-%QGx>_D2DG-n!)C+$AM&Gc>7poU`iEK`yN3RliKQq^=8!FF{)Me zl4I@IpX4AaXi~sb8Rxt9hoigqQ+1mT%Cf!beR^SAimv->=EnEk86$Y&VZWaac1Tw{TMyblywif+xr*ffdK@CYNJbfx5X3&t^3POJ$3c8 zloYBG5vm}FM7YCR4Eo^|%KIBvM62$~6be_$n#?A1#J+etL1G37lM{QYUXiZyY0Y`| zS|-e454Qsti#w*kw8zP(XuDmQzKSHo_LOg4cJD-dg+wI< z2~lBBPfu^pcUp~>^IhIA9(N~{PDI2nk4;9J!82kl=TBbzGPBAm9PG624!XJyx=TsM zcu%OM&en^^aB;Y^@&M=wKP78<>e~z;z*&u|_2%Zx0YqisU#TBwC0|XBdp7>XpSgz* zPzSkyJArR@KL2>6twv!YH<$yp4riWG!j=tP+@bgM`6#DxHa)qLO($hf#*=dynp0;s zIM!2ad}6zB)JPLKHgw?zhR|6v(n6;67&Z6JE^-quebtW~H-gzH9V}?%AC>YkV}@SX zR%5?eNchFueMhQp{NmxFwjDFU*6hi;K+(%%UU}mCF8jj+`R#aA@!jIb`|Kj}nt;kX<}M zh;-0};gAW8@fJ*m)D;zHj;0H7>D90HQ}twX`H6W}0`4POeu2W&e)56aWWY(-8jsg( z#y#Yx6is4gYXyc;Zfifi;m{nkvo*yi`nW`({r|q*ldQWXh>6Jjm@j=nVwrjc?$LPV z`2{S&53W7o_6VjJ*IbN?kH}*MOC|xx*cqI6Fehx5>*z5hjU_ft*E3TysQ+M?X_q*t zv1BkR|H04f zB%EC+1H>ID5ek{}!3By)WCsYXWIzg9|~A~z#n;aQT(}Ug`xDL zS!i?;(Ho)~@tzcSq=FR3X_~(^Wsj!&<33kbRHQKKWMpSEAfXs;ly8+Nl}JPRE$VuX zUvZr~AZFxmN=2w=hR!~6EmAjlsLF; zgj1mZNgJq@9cQ->QS(yXHIStn)FAZbndQEL96Q7z=0^O+rCbq9|_uq9nn;F=2zZqfM7T*pf9$$zqK1o zIg~!ssBA_DJct1%wRt{JQr(%$*)HI$}7=#w@{wscYgHMdC8S z`JGm`Ph2I#pttnlIr3yTue)SU9(rDOJ|J;dkN#+7#HWv0GjSvX{V;#rhQ~qrtSC=W z-2ZDGItZ=%;9=>z%O8?(3YdcjDMnmU;l05!)P+N;!yI-e_Zb*;l@hvyfG{HYYd4Be zk*xHuO6m3s_ze_Gm4D~3#3a?pf=2>`EbIR;;yN%A)OK6=${M0HnDyJQ39A!AK=n*a zAekK^9;YJ>v+PeLT-ugtK&I|jhs^Q&v--(ruP(+1)FV1HVVA}84b4UsEvm+q#t;1a5E>$lAnU!p>bS0OQcx^9^7$=ZdK@fO- z>JuVeHXp#j;bBxLK9I*UUh~9jL`Rz+cBIm2lbH#=QLtzr7oV75R?ir{8dn3xZM|Q)BG|-Xv?1E-|sv?z1{g6V$&hY z84wTvan@2tKMm7eMz}qf$Yh3~5POU=GuPHe?gocNQ~{t2(h{*hL8X;I zkwwtM1e912w;N%*CPJJ7I$2>L1pE`g%<#&U{ud0AHtU0VYWZ6M2426sNk~YDgdi=q z*c(~F!;uh%LEXN**)+H3<>~i_AY1Q=T>caXgOZM<;Q^r0K5tBbu>@RDivY#oK; zL?L=y5Srw4l8|l+k5#=_l719~*i&7-#r>aY?&P6E>q;CQ_$65FvPz?##C+|h#!3m2JAiQ+x+y&yH@&!ivZ$oNs z%eKn}^OOR6wC*!FeX}T}Fog-!cp@EO)^0Hq?w9}3ip2p+}RW9Y-UZ;?I1!uSY9tkzBz?S@4dPE?fhnx$(4&} zc&3LAT>2vt$NTMWF~xMfPodAWK3F*ai_c>|;yxGRd2rmWaDLo)QW7O25}9nuerc{x zTXprt{i?0Nev;AvXF+-x!99RAoBW&o4j7QeW)>q38<6cqMJ6g+snzY%IeYv%*rmb6 zzwN~HLw<{e%d-|4sA+9(espqOYu8{7c||{k3F|C!AFei8Lm~o6DXG;)D_kRKtc zDpGg|k1HMS{~(UEHsj*AO)qi>W@nc$kZ3?BBy`~Uei|NZ1oPX6ih^Rx-pW`lnO9kLT3x*TJ~uaRG~= z-GOyV36HB@_`|Vu$3bj0dfg7Li%EL*TK#+}Qm*6?y*l&hg36jMkEuK%tHo+(E+TvD zqH*aU;RYB$_ol$lrCMTMmyV}{%-K>o+~(7cJN(StaEtAMAEEG?&A4uO_AfZHz=7!1 z;Sr(Tpt!$(|FTVR6&4ii?8XX^@Vfl8`bn2Ej9;)u4PpMd9>j%UUP9D|>f5))sXdLE ztWStIZLo0l7@4F0_27DE5aLi`J;7?N+Hfj0AXL+$c;NB6oPBw}3Wb43(RN*TuBZs0 zwAO(1y^<`5`G>;6cxE#sw8|F=<-eO#61~4^!BD54WCaU>Z=bv{sibLWXZaKbne*josKe8pV-DP(TErprYz}e?H~&xJ`ou zI*=nI`q4T&M*rQj1`9+S`m8l(wwCkN=8uDK-3V}jasqEo3JMC3+CdASY92l1>J4oM zBd{^4WtCN&k7tS#sbnk}V!I%Nq<4ZFJ1eWJyV;3k+Nd??Cw5(9vEAtCzkUw8d^A-Z zizeZFPB+Wh&XUXIv|g?=siQ|6)~_w%{;wtL7*cgfiRs@YT0+jBEw?jL(Jga&bD53% zYf?b;L;f|7(_fJI=-#9A+nQbf$$TaD4;Hal*E>>(gtzyC(t9~LIVaLs3L6@n#?n|m zAjinTW`DL}=cMXQ$0Iy@YZ=1KvRpUZ!&-hW*HKEPAmK6V_xM4zRU=YarjtU{Wa@X1 zcm;1hQDs$C$IaibsSH}2UXNEMqbcs+Nuns^c{I6Y*Mt)&hL--LZiZ~Ti;IhpL7Zp1 zZYW^=AZj#`aKxckKN7^tl45SrX?M|aS^l4Ghs|`1_p-yNsOP)%afJ%EEU9P)@5k%E zb(;rBXwd*s$)cj7f&!3&jd4*uEDvNJAV3V90bwBcKP!z^)N(nHg>!JdJN$7d@F5{1 zW9|CAZ~M?;D+|%8pU}3$pm`Nae<%uHMn)#A%pyafbd**0zv%#n^zy;x3JrYQaO*#I zg6@&|wvL4a$C5B#qT_W?>qeBZl;t+0rsqB1Y^w*rJvA0k( z`uQN!7R=Fm98)C(dHiE?9;GY9scZXqe+ho%H2Iqp5%D={uWx{SpZa{)eo4Cm>&rrw z2Ajj4_;l6g$NK}ME7%;)I?XOMkk(=Tzb7$2Gm@dQ4C#8`&A+~S0mP-GJdcZ0!6v;T zF{XM;HY8>Z7&1s{j{LswuV-gJr@N?g+zRUIX3Qbwk2v-9`rC^Lffm4vl8Op@3y60A z`0+znwhdw!Mn^}{<5^69!pDZ#5+z#^W^}|sZT0xW$nihzeXZ%3nVU;WOk{IDnS)sC z`_n}^Wt?%2ToOa>4E5S8D}bnB0c<5J^af;P{162&%W*edZ*vAP+pM*Ol6aK0wBUj8 zcX%iUw#?UEqZbg(Z~#lcjfU4+9rZkpDHRM>8Z2f@WC-~@8v3`*vRZFRs*4FEQuYmnx#b(Su}Yo(6ICsdRxT zoZq99iyE7mQBzVP6LB#M2nf*9mTNWJK)eGZEfa`YoPaxgg$T`GP=B#n2jUy5sHh|- zCqvB9O1o=~LTOo<`OZN2;NYO}F&z4MGFmu~#M*I^-E-RdR{&l~NJ&Y_W%q{<#B@RI z^?w2yiYDYORQqq$ki=zX@|sWOy}rI8GPdNEm6eIbk`!n!eIYF>wv6Cy;kONb+{+cmL&s?P}Bb>79#i;(yj?8_mn|YO5Ck0Ri&=*yQBo z#DuJr)acllw1fo9;+(Ti8dgkt@%2*i4^|S2N8;kSGqL}Sej-Mg`Ci5uhCl?P_}hX5 zwblRpHXP`imb@_o$nT3mootcM?<0~@@4>NPRz)!mW)xg@;*uvEP|2AO#_Wv8ys|}Jt P8UQ4I$bzed^#lGNXJPO0 literal 0 HcmV?d00001 diff --git a/lanelet2_ml_converter/test/test_map_data_interface.cpp b/lanelet2_ml_converter/test/test_map_data_interface.cpp index c7afbe9d..41ad323b 100644 --- a/lanelet2_ml_converter/test/test_map_data_interface.cpp +++ b/lanelet2_ml_converter/test/test_map_data_interface.cpp @@ -79,9 +79,7 @@ TEST_F(MLConverterTest, MapDataInterface) { // NOLINT // y.resize(mat.rows()); // Eigen::VectorXd::Map(&x[0], mat.rows()) = mat.col(0); // Eigen::VectorXd::Map(&y[0], mat.rows()) = mat.col(1); - // matplot::plot(x, y, "r")->line_width(3); - // } - // for (const auto& mat : compoundLaneDividers) { + // matplot::pltrafficRulesuto& mat : compoundLaneDividers) { // std::vector x; // std::vector y; // x.resize(mat.rows()); From aa3b02361f4775d7b8df52905b25410a5963946b Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 1 Feb 2024 19:45:44 +0100 Subject: [PATCH 63/64] add pre-release note --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 4229f3db..14049c31 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,8 @@ https://pypi.org/project/lanelet2/) Convert Lanelet2 maps directly to instance labels for map perception and other map-based learning tasks! `lanelet2_ml_converter` provides local instance labels of various abstraction layers and representations accessible as numpy arrays from python. Check out the `lanelet2_ml_converter` module for more information and usage examples. +**Note:** As the module is not released officially yet, to try it out you need to either build Lanelet2 with the python bindings yourself or manually install the pip wheel files from the CI artefacts. + ## Overview Lanelet2 is a C++ library for handling map data in the context of automated driving. It is designed to utilize high-definition map data in order to efficiently handle the challenges posed to a vehicle in complex traffic scenarios. Flexibility and extensibility are some of the core principles to handle the upcoming challenges of future maps. From 07bc92fc43375780306efc1afca657a9adac78b6 Mon Sep 17 00:00:00 2001 From: Fabian Immel Date: Thu, 1 Feb 2024 19:50:17 +0100 Subject: [PATCH 64/64] add download link --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 14049c31..7c243d3a 100644 --- a/README.md +++ b/README.md @@ -17,7 +17,7 @@ https://pypi.org/project/lanelet2/) Convert Lanelet2 maps directly to instance labels for map perception and other map-based learning tasks! `lanelet2_ml_converter` provides local instance labels of various abstraction layers and representations accessible as numpy arrays from python. Check out the `lanelet2_ml_converter` module for more information and usage examples. -**Note:** As the module is not released officially yet, to try it out you need to either build Lanelet2 with the python bindings yourself or manually install the pip wheel files from the CI artefacts. +**Note:** As the module is not released officially yet, to try it out you need to either build Lanelet2 with the python bindings yourself or manually install the pip wheel files from the CI artefacts (download [here](https://github.com/immel-f/Lanelet2/actions/runs/7714926040/artifacts/1206339616)). ## Overview