diff --git a/Dockerfile b/Dockerfile index 83e2ab5c..62a084f8 100644 --- a/Dockerfile +++ b/Dockerfile @@ -135,7 +135,7 @@ 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_ENABLE_COVERAGE=1"; \ else \ export CMAKE_ARGS="-DCMAKE_BUILD_TYPE=Release"; \ fi; \ diff --git a/README.md b/README.md index 7c243d3a..65159f74 100644 --- a/README.md +++ b/README.md @@ -13,11 +13,11 @@ 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` +## New experimental module :test_tube: : `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. -**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)). +**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/7820288630/artifacts/1228128122)). ## Overview diff --git a/lanelet2_ml_converter/README.md b/lanelet2_ml_converter/README.md index f78ca7e5..4b710c13 100644 --- a/lanelet2_ml_converter/README.md +++ b/lanelet2_ml_converter/README.md @@ -2,6 +2,8 @@ Converter module to convert Lanelet2 maps into local instance labels for machine learning tasks. +**Note:** This module is experimental, so the documentation is still sparse and there may be breaking API changes in the future! + ![](doc/summary_flowchart.png) ## Usage Examples @@ -16,27 +18,36 @@ 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 +mDataIf.setCurrPosAndExtractSubmap2d(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 +tfData = lData.getTensorInstanceData(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" +#include "lanelet2_ml_converter/MapInstances.h" -using TensorData = lanelet::ml_converter::LaneData::TensorFeatureData; +using TensorData = lanelet::ml_converter::LaneData::TensorInstanceData; 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 +mDataIf.setCurrPosAndExtractSubmap2d(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 +TensorData tData = lData.getTensorInstanceData(True, False); // get the local instance labels as Eigen mats ``` +## Features + +- Access lanelet map information as numpy arrays from python, in a representation directly usable for machine learning tasks +- Compound labels for independence from map annotation artifacts +- Full traceability to the underlying map element for all instances, including the compound instances +- Real-time capable optimized C++ implementation (around 3 ms for one set of local instance labels) +- Save and load generated labels in both binary and human-readable XML format + + ## Components ### `MapDataInterface` @@ -47,15 +58,15 @@ Main interface class that is used to generate `LaneData` objects. `LaneData` objects hold all local instance labels of a local reference frame pose. -### `TensorFeatureData` +### `TensorInstanceData` -Subclass of `LaneData` that holds all local instance label in a convenient numpy array / Eigen matrix form. +Subclass of `LaneData` that holds all local instance labels in a convenient numpy array / Eigen matrix form. -### `LaneLineStringFeature` / `CompoundLaneLineStringFeature` / `LaneletFeature` / ... +### `LaneLineStringInstance` / `CompoundLaneLineStringInstance` / `LaneletInstance` / ... 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 +- **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 `getTensorInstanceData`). 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/include/lanelet2_ml_converter/Forward.h b/lanelet2_ml_converter/include/lanelet2_ml_converter/Forward.h index c4464472..dcc3b1ab 100644 --- a/lanelet2_ml_converter/include/lanelet2_ml_converter/Forward.h +++ b/lanelet2_ml_converter/include/lanelet2_ml_converter/Forward.h @@ -33,11 +33,11 @@ inline int relationToInt(lanelet::routing::RelationType type) { enum class LaneletRepresentationType; enum class ParametrizationType; -class MapFeature; -class LineStringFeature; -class LaneLineStringFeature; -class CompoundLaneLineStringFeature; -class LaneletFeature; +class MapInstance; +class LineStringInstance; +class LaneLineStringInstance; +class CompoundLaneLineStringInstance; +class LaneletInstance; class LaneData; } // namespace ml_converter @@ -47,19 +47,20 @@ namespace boost { namespace serialization { template -void serialize(Archive& ar, lanelet::ml_converter::MapFeature& feat, const unsigned int /*version*/); +void serialize(Archive& ar, lanelet::ml_converter::MapInstance& feat, const unsigned int /*version*/); template -void serialize(Archive& ar, lanelet::ml_converter::LineStringFeature& feat, const unsigned int /*version*/); +void serialize(Archive& ar, lanelet::ml_converter::LineStringInstance& feat, const unsigned int /*version*/); template -void serialize(Archive& ar, lanelet::ml_converter::LaneLineStringFeature& feat, const unsigned int /*version*/); +void serialize(Archive& ar, lanelet::ml_converter::LaneLineStringInstance& feat, const unsigned int /*version*/); template -void serialize(Archive& ar, lanelet::ml_converter::CompoundLaneLineStringFeature& feat, const unsigned int /*version*/); +void serialize(Archive& ar, lanelet::ml_converter::CompoundLaneLineStringInstance& feat, + const unsigned int /*version*/); template -void serialize(Archive& ar, lanelet::ml_converter::LaneletFeature& feat, const unsigned int /*version*/); +void serialize(Archive& ar, lanelet::ml_converter::LaneletInstance& feat, const unsigned int /*version*/); template void serialize(Archive& ar, lanelet::ml_converter::LaneData& feat, const unsigned int /*version*/); diff --git a/lanelet2_ml_converter/include/lanelet2_ml_converter/MapData.h b/lanelet2_ml_converter/include/lanelet2_ml_converter/MapData.h index ac8112e1..6282a704 100644 --- a/lanelet2_ml_converter/include/lanelet2_ml_converter/MapData.h +++ b/lanelet2_ml_converter/include/lanelet2_ml_converter/MapData.h @@ -11,7 +11,7 @@ #include #include "lanelet2_ml_converter/Forward.h" -#include "lanelet2_ml_converter/MapFeatures.h" +#include "lanelet2_ml_converter/MapInstances.h" #include "lanelet2_ml_converter/Types.h" #include "lanelet2_routing/RoutingGraph.h" #include "lanelet2_routing/internal/Graph.h" @@ -60,7 +60,7 @@ std::map> getValidElements(const std::map& roadBorders() { return roadBorders_; } const std::vector& laneDividers() { return laneDividers_; } @@ -70,11 +70,11 @@ class LaneData { const std::vector& compoundLaneDividerTypes() { return compoundLaneDividerTypes_; } const std::vector& compoundCenterlines() { return compoundCenterlines_; } const std::string& uuid() { return uuid_; } - CompoundLaneLineStringFeaturePtr pointMatrixCpdRoadBorder( + CompoundLaneLineStringInstancePtr pointMatrixCpdRoadBorder( size_t index); // get feature associated to point matrix index - CompoundLaneLineStringFeaturePtr pointMatrixCpdLaneDivider( + CompoundLaneLineStringInstancePtr pointMatrixCpdLaneDivider( size_t index); // get feature associated to point matrix index - CompoundLaneLineStringFeaturePtr pointMatrixCpdCenterline( + CompoundLaneLineStringInstancePtr pointMatrixCpdCenterline( size_t index); // get feature associated to point matrix index friend class LaneData; @@ -87,46 +87,47 @@ class LaneData { std::vector compoundLaneDividerTypes_; std::vector compoundCenterlines_; std::string uuid_; - std::map + std::map pointMatrixCpdRoadBorder_; // relates point matrix index to compound features - std::map + std::map pointMatrixCpdLaneDivider_; // relates point matrix index to compound features, tfData_ must be initialized - std::map + std::map pointMatrixCpdCenterline_; // relates point matrix index to compound features, tfData_ must be initialized }; LaneData() noexcept : uuid_{boost::lexical_cast(boost::uuids::random_generator()())} {} static LaneDataPtr build(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); - bool processAll(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints); + bool processAll(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints, double pitch = 0, + double roll = 0); - const LaneLineStringFeatures& roadBorders() { return roadBorders_; } - const LaneLineStringFeatures& laneDividers() { return laneDividers_; } + const LaneLineStringInstances& roadBorders() { return roadBorders_; } + const LaneLineStringInstances& laneDividers() { return laneDividers_; } - const CompoundLaneLineStringFeatureList& compoundRoadBorders() { return compoundRoadBorders_; } - const CompoundLaneLineStringFeatureList& compoundLaneDividers() { return compoundLaneDividers_; } - const CompoundLaneLineStringFeatureList& compoundCenterlines() { return compoundCenterlines_; } + const CompoundLaneLineStringInstanceList& compoundRoadBorders() { return compoundRoadBorders_; } + const CompoundLaneLineStringInstanceList& compoundLaneDividers() { return compoundLaneDividers_; } + const CompoundLaneLineStringInstanceList& compoundCenterlines() { return compoundCenterlines_; } - LaneLineStringFeatures validRoadBorders() { return getValidElements(roadBorders_); } - LaneLineStringFeatures validLaneDividers() { return getValidElements(laneDividers_); } + LaneLineStringInstances validRoadBorders() { return getValidElements(roadBorders_); } + LaneLineStringInstances validLaneDividers() { return getValidElements(laneDividers_); } - CompoundLaneLineStringFeatureList validCompoundRoadBorders() { return getValidElements(compoundRoadBorders_); } - CompoundLaneLineStringFeatureList validCompoundLaneDividers() { return getValidElements(compoundLaneDividers_); } - CompoundLaneLineStringFeatureList validCompoundCenterlines() { return getValidElements(compoundCenterlines_); } + CompoundLaneLineStringInstanceList validCompoundRoadBorders() { return getValidElements(compoundRoadBorders_); } + CompoundLaneLineStringInstanceList validCompoundLaneDividers() { return getValidElements(compoundLaneDividers_); } + CompoundLaneLineStringInstanceList validCompoundCenterlines() { return getValidElements(compoundCenterlines_); } - CompoundLaneLineStringFeatureList associatedCpdRoadBorders( + CompoundLaneLineStringInstanceList associatedCpdRoadBorders( Id mapId); // get features associated to map element with id - CompoundLaneLineStringFeatureList associatedCpdLaneDividers( + CompoundLaneLineStringInstanceList associatedCpdLaneDividers( Id mapId); // get features associated to map element with id - CompoundLaneLineStringFeatureList associatedCpdCenterlines( + CompoundLaneLineStringInstanceList associatedCpdCenterlines( Id mapId); // get features associated to map element with id - const LaneletFeatures& laneletFeatures() { return laneletFeatures_; } + const LaneletInstances& laneletInstances() { return laneletInstances_; } 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); + TensorInstanceData getTensorInstanceData(bool pointsIn2d, bool ignoreBuffer); template friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::LaneData& feat, @@ -135,27 +136,28 @@ class LaneData { 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, + void initLaneletInstances(LaneletSubmapConstPtr& localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph); - void updateAssociatedCpdFeatureIndices(); + void initCompoundInstances(LaneletSubmapConstPtr& localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph); + void updateAssociatedCpdInstanceIndices(); void getPaths(lanelet::routing::RoutingGraphConstPtr localSubmapGraph, std::vector& paths, ConstLanelet start, ConstLanelets initPath = ConstLanelets()); LineStringType getLineStringTypeFromId(Id id); - LaneLineStringFeaturePtr getLineStringFeatFromId(Id id, bool inverted); + LaneLineStringInstancePtr getLineStringFeatFromId(Id id, bool inverted); std::vector computeCompoundLeftBorders(const ConstLanelets& path); std::vector computeCompoundRightBorders(const ConstLanelets& path); - CompoundLaneLineStringFeaturePtr computeCompoundCenterline(const ConstLanelets& path); + CompoundLaneLineStringInstancePtr computeCompoundCenterline(const ConstLanelets& path); - LaneLineStringFeatures roadBorders_; // auxilliary features - LaneLineStringFeatures laneDividers_; // auxilliary features + LaneLineStringInstances roadBorders_; // auxilliary features + LaneLineStringInstances laneDividers_; // auxilliary features - CompoundLaneLineStringFeatureList compoundRoadBorders_; // auxilliary features - CompoundLaneLineStringFeatureList compoundLaneDividers_; // auxilliary features - CompoundLaneLineStringFeatureList compoundCenterlines_; + CompoundLaneLineStringInstanceList compoundRoadBorders_; // auxilliary features + CompoundLaneLineStringInstanceList compoundLaneDividers_; // auxilliary features + CompoundLaneLineStringInstanceList compoundCenterlines_; - LaneletFeatures laneletFeatures_; + LaneletInstances laneletInstances_; std::map> associatedCpdRoadBorderIndices_; // relates map element id to the "unfiltered" compound features! @@ -167,7 +169,7 @@ class LaneData { Edges edges_; // edge list for centerlines std::string uuid_; // sample id - Optional tfData_; + Optional tfData_; }; /// TODO: finish TE support @@ -176,8 +178,8 @@ class TEData { TEData() noexcept {} private: - LaneletFeatures laneletFeatures; // node features lanelets - TEFeatures teFeatures; // node features traffic elements + LaneletInstances laneletInstances; // node features lanelets + TEInstances teInstances; // node features traffic elements Edges edgeList_; // edge list /// @brief adjacency matrix (sparse). Node indices are assigned as diff --git a/lanelet2_ml_converter/include/lanelet2_ml_converter/MapDataInterface.h b/lanelet2_ml_converter/include/lanelet2_ml_converter/MapDataInterface.h index 102a188e..cf87fb27 100644 --- a/lanelet2_ml_converter/include/lanelet2_ml_converter/MapDataInterface.h +++ b/lanelet2_ml_converter/include/lanelet2_ml_converter/MapDataInterface.h @@ -35,26 +35,37 @@ class MapDataInterface { const Configuration& config() { return config_; } - void setCurrPosAndExtractSubmap(const BasicPoint2d& pt, double yaw); + void setCurrPosAndExtractSubmap2d(const BasicPoint2d& pt, double yaw); + void setCurrPosAndExtractSubmap(const BasicPoint3d& pt, double yaw); + void setCurrPosAndExtractSubmap(const BasicPoint3d& pt, double yaw, double pitch, double roll); LaneDataPtr laneData(bool processAll); TEData teData(bool processAll); - std::vector laneDataBatch(std::vector pts, std::vector yaws); - std::vector laneTEDataBatch(std::vector pts, std::vector yaws); + std::vector laneDataBatch2d(std::vector pts, std::vector yaws); + std::vector laneDataBatch(std::vector pts, std::vector yaws); + std::vector laneDataBatch(std::vector pts, std::vector yaws, + std::vector pitches, std::vector rolls); + std::vector laneTEDataBatch(std::vector pts, std::vector yaws, + std::vector pitches, std::vector rolls); private: - LaneDataPtr getLaneData(LaneletSubmapConstPtr localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph, - const BasicPoint2d& pos, double yaw, bool processAll = true); + LaneDataPtr getLaneData(LaneletSubmapConstPtr localSubmap, const OrientedRect& bbox, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph, double pitch, double roll, + bool processAll); - TEData getLaneTEData(LaneletSubmapConstPtr localSubmap, lanelet::routing::RoutingGraphConstPtr localSubmapGraph, - const BasicPoint2d& pos, double yaw, bool processAll = true); + TEData getLaneTEData(LaneletSubmapConstPtr localSubmap, const OrientedRect& bbox, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph, double pitch, double roll, + bool processAll); LaneletMapConstPtr laneletMap_; LaneletSubmapConstPtr localSubmap_; std::unordered_map teId2Index_; routing::RoutingGraphConstPtr localSubmapGraph_; - Optional currPos_; // in the map frame - Optional currYaw_; // in the map frame + Optional currPos_; // in the map frame + Optional currRoll_; // in the map frame, [rad] + Optional currPitch_; // in the map frame, [rad] + Optional currYaw_; // in the map frame, [rad] + Optional currBbox_; Configuration config_; traffic_rules::TrafficRulesPtr trafficRules_; }; diff --git a/lanelet2_ml_converter/include/lanelet2_ml_converter/MapFeatures.h b/lanelet2_ml_converter/include/lanelet2_ml_converter/MapFeatures.h deleted file mode 100644 index 18d717fa..00000000 --- a/lanelet2_ml_converter/include/lanelet2_ml_converter/MapFeatures.h +++ /dev/null @@ -1,269 +0,0 @@ -#pragma once -#include -#include -#include -#include - -#include -#include - -#include "lanelet2_ml_converter/Forward.h" -#include "lanelet2_ml_converter/Types.h" - -namespace lanelet { -namespace ml_converter { - -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); } - bool initialized() const { return initialized_; } - bool valid() const { return valid_; } - bool wasCut() const { return wasCut_; } - - virtual std::vector computeFeatureVectors(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::ml_converter::MapFeature& feat, - const unsigned int /*version*/); - - virtual ~MapFeature() noexcept = default; - - protected: - bool initialized_{false}; - bool valid_{true}; - bool wasCut_{false}; - Optional mapID_; - - MapFeature() = default; - MapFeature(Id mapID) : mapID_{mapID}, initialized_{true} {} -}; - -class LineStringFeature : public MapFeature { - public: - const BasicLineString3d& rawFeature() const { return rawFeature_; } - - 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 - friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::LineStringFeature& feat, - const unsigned int /*version*/); - - virtual ~LineStringFeature() noexcept = default; - - protected: - BasicLineString3d rawFeature_; - LineStringFeature() {} - LineStringFeature(const BasicLineString3d& feature, Id mapID) : MapFeature(mapID), rawFeature_{feature} {} -}; - -class LaneLineStringFeature : public LineStringFeature { - public: - LaneLineStringFeature() {} - 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; - 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_); } - const Ids& laneletIDs() const { return laneletIDs_; } - void addLaneletID(Id id) { laneletIDs_.push_back(id); } - - template - friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::LaneLineStringFeature& feat, - const unsigned int /*version*/); - - protected: - BasicLineStrings3d cutFeatures_; - BasicLineStrings3d cutAndResampledFeatures_; - BasicLineStrings3d cutResampledAndTransformedFeatures_; - LineStringType type_; - bool inverted_{false}; // = inverted compared to element with that Id in lineStringLayer - Ids laneletIDs_; -}; - -using LaneLineStringFeaturePtr = std::shared_ptr; -using LaneLineStringFeatures = std::map; -using LaneLineStringFeatureList = std::vector; - -class TEFeature : public LineStringFeature { - public: - TEFeature() {} - TEFeature(const BasicLineString3d& feature, Id mapID, TEType type) - : LineStringFeature(feature, mapID), teType_{type} {} - virtual ~TEFeature() noexcept = default; - - bool process(const OrientedRect& bbox, const ParametrizationType& paramType, - int32_t /*unused*/) override; // not implemented yet - 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_; } - - private: - TEType teType_; -}; - -class LaneletFeature : public MapFeature { - public: - LaneletFeature() {} - LaneletFeature(LaneLineStringFeaturePtr leftBoundary, LaneLineStringFeaturePtr rightBoundary, - LaneLineStringFeaturePtr centerline, Id mapID); - LaneletFeature(const ConstLanelet& ll); - virtual ~LaneletFeature() noexcept = default; - - bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; - std::vector computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const override; - - void setReprType(LaneletRepresentationType reprType) { reprType_ = reprType; } - - LaneLineStringFeaturePtr leftBoundary() const { return leftBoundary_; } - LaneLineStringFeaturePtr rightBoundary() const { return rightBoundary_; } - LaneLineStringFeaturePtr centerline() const { return centerline_; } - - template - friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::LaneletFeature& feat, - const unsigned int /*version*/); - - private: - LaneLineStringFeaturePtr leftBoundary_; - LaneLineStringFeaturePtr rightBoundary_; - LaneLineStringFeaturePtr centerline_; - LaneletRepresentationType reprType_{LaneletRepresentationType::Centerline}; -}; - -class CompoundLaneLineStringFeature : public LaneLineStringFeature { - public: - CompoundLaneLineStringFeature() {} - // features for this constructor are required to be given in already sorted order - CompoundLaneLineStringFeature(const LaneLineStringFeatureList& features, LineStringType compoundType); - - virtual ~CompoundLaneLineStringFeature() noexcept = default; - bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) override; - - 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_; } - - template - friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::CompoundLaneLineStringFeature& feat, - const unsigned int /*version*/); - - private: - LaneLineStringFeatureList individualFeatures_; - std::vector pathLengthsRaw_; - std::vector pathLengthsProcessed_; - std::vector processedFeaturesValid_; - double validLengthThresh_{0.3}; -}; - -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; - 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) { - 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!"); - } - 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(); }) == - 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!"); - } - MatrixXd featureMat(featureVectors.size(), featureVectors[0].size()); - for (size_t i = 0; i < featureVectors.size(); i++) { - featureMat.row(i) = featureVectors[i]; - } - return featureMat; -} - -template -std::vector getPointMatrices(const std::map>& mapFeatures, bool pointsIn2d) { - std::vector> featList; - for (const auto& pair : mapFeatures) { - featList.push_back(pair.second); - } - return getPointMatrices(featList, pointsIn2d); -} - -template -std::vector getPointMatrices(const std::vector>& mapFeatures, bool pointsIn2d) { - 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!"); - } - std::vector individualMats = feat->pointMatrices(pointsIn2d); - pointMatrices.insert(pointMatrices.end(), individualMats.begin(), individualMats.end()); - } - return pointMatrices; -} - -template -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)) { - allValid = false; - } - } - return allValid; -} - -template -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)) { - allValid = false; - } - } - return allValid; -} - -MatrixXd toPointMatrix(const BasicLineString3d& lString, bool pointsIn2d); -VectorXd toFeatureVector(const BasicLineString3d& line, int typeInt, bool onlyPoints, bool pointsIn2d); -} // namespace ml_converter -} // namespace lanelet \ No newline at end of file diff --git a/lanelet2_ml_converter/include/lanelet2_ml_converter/MapInstances.h b/lanelet2_ml_converter/include/lanelet2_ml_converter/MapInstances.h new file mode 100644 index 00000000..f8d6597b --- /dev/null +++ b/lanelet2_ml_converter/include/lanelet2_ml_converter/MapInstances.h @@ -0,0 +1,278 @@ +#pragma once +#include +#include +#include +#include + +#include +#include + +#include "lanelet2_ml_converter/Forward.h" +#include "lanelet2_ml_converter/Types.h" + +namespace lanelet { +namespace ml_converter { + +using VectorXd = Eigen::Matrix; +using MatrixXd = Eigen::Matrix; + +using BasicLineStrings3d = std::vector; + +class MapInstance { + public: + 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 std::vector computeInstanceVectors(bool onlyPoints, bool pointsIn2d) const = 0; + virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints, + double pitch = 0, double roll = 0) = 0; + + template + friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::MapInstance& feat, + const unsigned int /*version*/); + + virtual ~MapInstance() noexcept = default; + + protected: + bool initialized_{false}; + bool valid_{true}; + bool wasCut_{false}; + bool processedFrom2d_{false}; + Optional mapID_; + + MapInstance() = default; + MapInstance(Id mapID) : mapID_{mapID}, initialized_{true} {} +}; + +class LineStringInstance : public MapInstance { + public: + const BasicLineString3d& rawInstance() const { return rawInstance_; } + + virtual std::vector computeInstanceVectors(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, + double pitch = 0, double roll = 0) = 0; + + template + friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::LineStringInstance& feat, + const unsigned int /*version*/); + + virtual ~LineStringInstance() noexcept = default; + + protected: + BasicLineString3d rawInstance_; + LineStringInstance() {} + LineStringInstance(const BasicLineString3d& feature, Id mapID) : MapInstance(mapID), rawInstance_{feature} {} +}; + +class LaneLineStringInstance : public LineStringInstance { + public: + LaneLineStringInstance() {} + LaneLineStringInstance(const BasicLineString3d& feature, Id mapID, LineStringType type, + const std::vector& laneletID, bool inverted) + : LineStringInstance(feature, mapID), type_{type}, laneletIDs_{laneletID}, inverted_{inverted} {} + virtual ~LaneLineStringInstance() noexcept = default; + + virtual bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints, + double pitch = 0, double roll = 0) override; + virtual std::vector computeInstanceVectors( + bool onlyPoints, + bool pointsIn2d) const override; // uses processedInstance_ when available + virtual std::vector pointMatrices( + bool pointsIn2d) const override; // uses processedInstance_ when available + + const BasicLineStrings3d& cutInstance() const { return cutInstances_; } + const BasicLineStrings3d& cutAndResampledInstance() const { return cutAndResampledInstances_; } + const BasicLineStrings3d& cutResampledAndTransformedInstance() const { return cutResampledAndTransformedInstances_; } + LineStringType type() const { return type_; } + bool inverted() const { return inverted_; } + int typeInt() const { return static_cast(type_); } + const Ids& laneletIDs() const { return laneletIDs_; } + void addLaneletID(Id id) { laneletIDs_.push_back(id); } + + template + friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::LaneLineStringInstance& feat, + const unsigned int /*version*/); + + protected: + BasicLineStrings3d cutInstances_; + BasicLineStrings3d cutAndResampledInstances_; + BasicLineStrings3d cutResampledAndTransformedInstances_; + LineStringType type_; + bool inverted_{false}; // = inverted compared to element with that Id in lineStringLayer + Ids laneletIDs_; +}; + +using LaneLineStringInstancePtr = std::shared_ptr; +using LaneLineStringInstances = std::map; +using LaneLineStringInstanceList = std::vector; + +class TEInstance : public LineStringInstance { + public: + TEInstance() {} + TEInstance(const BasicLineString3d& feature, Id mapID, TEType type) + : LineStringInstance(feature, mapID), teType_{type} {} + virtual ~TEInstance() noexcept = default; + + bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/, double pitch = 0, + double roll = 0) override; // not implemented yet + std::vector computeInstanceVectors(bool onlyPoints, + bool pointsIn2d) const override; // currently uses raw feature only + virtual std::vector pointMatrices(bool pointsIn2d) const override; + + const TEType& teType() { return teType_; } + + private: + TEType teType_; +}; + +class LaneletInstance : public MapInstance { + public: + LaneletInstance() {} + LaneletInstance(LaneLineStringInstancePtr leftBoundary, LaneLineStringInstancePtr rightBoundary, + LaneLineStringInstancePtr centerline, Id mapID); + LaneletInstance(const ConstLanelet& ll); + virtual ~LaneletInstance() noexcept = default; + + bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints, double pitch = 0, + double roll = 0) override; + std::vector computeInstanceVectors(bool onlyPoints, bool pointsIn2d) const override; + + void setReprType(LaneletRepresentationType reprType) { reprType_ = reprType; } + + LaneLineStringInstancePtr leftBoundary() const { return leftBoundary_; } + LaneLineStringInstancePtr rightBoundary() const { return rightBoundary_; } + LaneLineStringInstancePtr centerline() const { return centerline_; } + + template + friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::LaneletInstance& feat, + const unsigned int /*version*/); + + private: + LaneLineStringInstancePtr leftBoundary_; + LaneLineStringInstancePtr rightBoundary_; + LaneLineStringInstancePtr centerline_; + LaneletRepresentationType reprType_{LaneletRepresentationType::Centerline}; +}; + +class CompoundLaneLineStringInstance : public LaneLineStringInstance { + public: + CompoundLaneLineStringInstance() {} + // features for this constructor are required to be given in already sorted order + CompoundLaneLineStringInstance(const LaneLineStringInstanceList& features, LineStringType compoundType); + + virtual ~CompoundLaneLineStringInstance() noexcept = default; + bool process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints, double pitch = 0, + double roll = 0) override; + + LaneLineStringInstanceList features() const { return individualInstances_; } + const std::vector& pathLengthsRaw() const { return pathLengthsRaw_; } + const std::vector& pathLengthsProcessed() const { return pathLengthsProcessed_; } + const std::vector& processedInstancesValid() const { return processedInstancesValid_; } + + template + friend void boost::serialization::serialize(Archive& ar, lanelet::ml_converter::CompoundLaneLineStringInstance& feat, + const unsigned int /*version*/); + + private: + LaneLineStringInstanceList individualInstances_; + std::vector pathLengthsRaw_; + std::vector pathLengthsProcessed_; + std::vector processedInstancesValid_; + double validLengthThresh_{0.3}; +}; + +using CompoundLaneLineStringInstancePtr = std::shared_ptr; +using TEInstancePtr = std::shared_ptr; +using LaneletInstancePtr = std::shared_ptr; +using CompoundLaneLineStringInstanceList = std::vector; +using TEInstances = std::map; +using LaneletInstances = std::map; + +template +MatrixXd getInstanceVectorMatrix(const std::map>& mapInstances, bool onlyPoints, + bool pointsIn2d) { + std::vector> featList; + for (const auto& pair : mapInstances) { + featList.push_back(pair.second); + } + return getInstanceVectorMatrix(featList, onlyPoints, pointsIn2d); +} + +template +MatrixXd getInstanceVectorMatrix(const std::vector>& mapInstances, bool onlyPoints, + bool pointsIn2d) { + std::vector featureVectors; + for (const auto& feat : mapInstances) { + if (!feat->valid()) { + throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); + } + std::vector individualVecs = feat->computeInstanceVectors(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(); }) == + 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!"); + } + MatrixXd featureMat(featureVectors.size(), featureVectors[0].size()); + for (size_t i = 0; i < featureVectors.size(); i++) { + featureMat.row(i) = featureVectors[i]; + } + return featureMat; +} + +template +std::vector getPointMatrices(const std::map>& mapInstances, bool pointsIn2d) { + std::vector> featList; + for (const auto& pair : mapInstances) { + featList.push_back(pair.second); + } + return getPointMatrices(featList, pointsIn2d); +} + +template +std::vector getPointMatrices(const std::vector>& mapInstances, bool pointsIn2d) { + std::vector pointMatrices; + for (const auto& feat : mapInstances) { + if (!feat->valid()) { + throw std::runtime_error("Invalid feature in list! This function requires all given features to be valid!"); + } + std::vector individualMats = feat->pointMatrices(pointsIn2d); + pointMatrices.insert(pointMatrices.end(), individualMats.begin(), individualMats.end()); + } + return pointMatrices; +} + +template +bool processInstances(std::map>& featMap, const OrientedRect& bbox, + const ParametrizationType& paramType, int32_t nPoints, double pitch = 0, double roll = 0) { + bool allValid = true; + for (auto& feat : featMap) { + if (!feat.second->process(bbox, paramType, nPoints, pitch, roll)) { + allValid = false; + } + } + return allValid; +} + +template +bool processInstances(std::vector>& featVec, const OrientedRect& bbox, + const ParametrizationType& paramType, int32_t nPoints, double pitch = 0, double roll = 0) { + bool allValid = true; + for (auto& feat : featVec) { + if (!feat->process(bbox, paramType, nPoints, pitch, roll)) { + allValid = false; + } + } + return allValid; +} + +MatrixXd toPointMatrix(const BasicLineString3d& lString, bool pointsIn2d); +VectorXd toInstanceVector(const BasicLineString3d& line, int typeInt, bool onlyPoints, bool pointsIn2d); +} // namespace ml_converter +} // namespace lanelet \ No newline at end of file diff --git a/lanelet2_ml_converter/include/lanelet2_ml_converter/Serialize.h b/lanelet2_ml_converter/include/lanelet2_ml_converter/Serialize.h index 37bfdafc..87fbde66 100644 --- a/lanelet2_ml_converter/include/lanelet2_ml_converter/Serialize.h +++ b/lanelet2_ml_converter/include/lanelet2_ml_converter/Serialize.h @@ -51,7 +51,7 @@ void serialize(Archive& ar, lanelet::ml_converter::LineStringType& type, const u } template -void serialize(Archive& ar, lanelet::ml_converter::MapFeature& feat, const unsigned int /*version*/) { +void serialize(Archive& ar, lanelet::ml_converter::MapInstance& feat, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_NVP(feat.initialized_); ar& BOOST_SERIALIZATION_NVP(feat.wasCut_); ar& BOOST_SERIALIZATION_NVP(feat.valid_); @@ -59,37 +59,38 @@ void serialize(Archive& ar, lanelet::ml_converter::MapFeature& feat, const unsig } template -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_); +void serialize(Archive& ar, lanelet::ml_converter::LineStringInstance& feat, const unsigned int /*version*/) { + ar& make_nvp("MapInstance", boost::serialization::base_object(feat)); + ar& BOOST_SERIALIZATION_NVP(feat.rawInstance_); } template -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_); +void serialize(Archive& ar, lanelet::ml_converter::LaneLineStringInstance& feat, const unsigned int /*version*/) { + ar& make_nvp("LineStringInstance", + boost::serialization::base_object(feat)); + ar& BOOST_SERIALIZATION_NVP(feat.cutInstances_); + ar& BOOST_SERIALIZATION_NVP(feat.cutAndResampledInstances_); + ar& BOOST_SERIALIZATION_NVP(feat.cutResampledAndTransformedInstances_); ar& BOOST_SERIALIZATION_NVP(feat.type_); ar& BOOST_SERIALIZATION_NVP(feat.inverted_); ar& BOOST_SERIALIZATION_NVP(feat.laneletIDs_); } template -void serialize(Archive& ar, lanelet::ml_converter::CompoundLaneLineStringFeature& feat, +void serialize(Archive& ar, lanelet::ml_converter::CompoundLaneLineStringInstance& feat, const unsigned int /*version*/) { - ar& make_nvp("LaneLineStringFeature", - boost::serialization::base_object(feat)); - ar& BOOST_SERIALIZATION_NVP(feat.individualFeatures_); + ar& make_nvp("LaneLineStringInstance", + boost::serialization::base_object(feat)); + ar& BOOST_SERIALIZATION_NVP(feat.individualInstances_); ar& BOOST_SERIALIZATION_NVP(feat.pathLengthsRaw_); ar& BOOST_SERIALIZATION_NVP(feat.pathLengthsProcessed_); - ar& BOOST_SERIALIZATION_NVP(feat.processedFeaturesValid_); + ar& BOOST_SERIALIZATION_NVP(feat.processedInstancesValid_); ar& BOOST_SERIALIZATION_NVP(feat.validLengthThresh_); } template -void serialize(Archive& ar, lanelet::ml_converter::LaneletFeature& feat, const unsigned int /*version*/) { - ar& make_nvp("MapFeature", boost::serialization::base_object(feat)); +void serialize(Archive& ar, lanelet::ml_converter::LaneletInstance& feat, const unsigned int /*version*/) { + ar& make_nvp("MapInstance", boost::serialization::base_object(feat)); ar& BOOST_SERIALIZATION_NVP(feat.leftBoundary_); ar& BOOST_SERIALIZATION_NVP(feat.rightBoundary_); ar& BOOST_SERIALIZATION_NVP(feat.centerline_); @@ -109,7 +110,7 @@ void serialize(Archive& ar, lanelet::ml_converter::LaneData& lData, const unsign 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.laneletInstances_); ar& BOOST_SERIALIZATION_NVP(lData.associatedCpdRoadBorderIndices_); ar& BOOST_SERIALIZATION_NVP(lData.associatedCpdLaneDividerIndices_); ar& BOOST_SERIALIZATION_NVP(lData.associatedCpdCenterlineIndices_); @@ -122,21 +123,22 @@ void serialize(Archive& ar, lanelet::ml_converter::LaneData& lData, const unsign // prevent unneccessary boost serialization xml tags // 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_CLASS_IMPLEMENTATION(lanelet::ml_converter::LaneletInstance, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::LaneLineStringInstance, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::CompoundLaneLineStringInstance, // boost::serialization::object_serializable); -// 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::LineStringInstance, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::MapInstance, 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::ml_converter::LaneLineStringFeatures, boost::serialization::object_serializable); -// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::LaneLineStringFeatureList, +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::LaneLineStringInstances, // boost::serialization::object_serializable); -// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::CompoundLaneLineStringFeatureList, +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::LaneLineStringInstanceList, +// boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::CompoundLaneLineStringInstanceList, // boost::serialization::object_serializable); -// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::LaneletFeatures, boost::serialization::object_serializable); +// BOOST_CLASS_IMPLEMENTATION(lanelet::ml_converter::LaneletInstances, boost::serialization::object_serializable); // BOOST_CLASS_IMPLEMENTATION(lanelet::BasicLineString3d, boost::serialization::object_serializable); diff --git a/lanelet2_ml_converter/include/lanelet2_ml_converter/Types.h b/lanelet2_ml_converter/include/lanelet2_ml_converter/Types.h index ed91db15..d70a5317 100644 --- a/lanelet2_ml_converter/include/lanelet2_ml_converter/Types.h +++ b/lanelet2_ml_converter/include/lanelet2_ml_converter/Types.h @@ -25,14 +25,15 @@ enum class LineStringType { enum class TEType { TrafficLight, TrafficSign, Unknown }; struct OrientedRect { - BasicPoint2d center; + BasicPoint3d center; double yaw; + bool from2d{false}; 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); + friend OrientedRect getRotatedRect(const BasicPoint3d& center, double extentLongitudinal, double extentLateral, + double yaw, bool from2dPos); private: OrientedRect() noexcept {} diff --git a/lanelet2_ml_converter/include/lanelet2_ml_converter/Utils.h b/lanelet2_ml_converter/include/lanelet2_ml_converter/Utils.h index c66943aa..9e1b6924 100644 --- a/lanelet2_ml_converter/include/lanelet2_ml_converter/Utils.h +++ b/lanelet2_ml_converter/include/lanelet2_ml_converter/Utils.h @@ -13,7 +13,8 @@ namespace lanelet { namespace ml_converter { -OrientedRect getRotatedRect(const BasicPoint2d& center, double extentLongitudinal, double extentLateral, double yaw); +OrientedRect getRotatedRect(const BasicPoint3d& center, double extentLongitudinal, double extentLateral, double yaw, + bool from2dPos); LaneletSubmapConstPtr extractSubmap(LaneletMapConstPtr laneletMap, const BasicPoint2d& center, double extentLongitudinal, double extentLateral); @@ -66,7 +67,8 @@ BasicLineString3d resampleLineString(const BasicLineString3d& polyline, int32_t std::vector cutLineString(const OrientedRect& bbox, const BasicLineString3d& polyline); -BasicLineString3d transformLineString(const OrientedRect& bbox, const BasicLineString3d& polyline); +BasicLineString3d transformLineString(const OrientedRect& bbox, const BasicLineString3d& polyline, double pitch, + double roll); void saveLaneData(const std::string& filename, const std::vector& lDataVec, bool binary); // saves all in one file diff --git a/lanelet2_ml_converter/src/MapData.cpp b/lanelet2_ml_converter/src/MapData.cpp index 76d554b6..61a50213 100644 --- a/lanelet2_ml_converter/src/MapData.cpp +++ b/lanelet2_ml_converter/src/MapData.cpp @@ -12,9 +12,9 @@ LaneDataPtr LaneData::build(LaneletSubmapConstPtr& localSubmap, LaneDataPtr data = std::make_shared(); data->initLeftBoundaries(localSubmap, localSubmapGraph); data->initRightBoundaries(localSubmap, localSubmapGraph); - data->initLaneletFeatures(localSubmap, localSubmapGraph); - data->initCompoundFeatures(localSubmap, localSubmapGraph); - data->updateAssociatedCpdFeatureIndices(); + data->initLaneletInstances(localSubmap, localSubmapGraph); + data->initCompoundInstances(localSubmap, localSubmapGraph); + data->updateAssociatedCpdInstanceIndices(); return data; } @@ -25,19 +25,19 @@ void LaneData::initLeftBoundaries(LaneletSubmapConstPtr& localSubmap, BasicLineString3d bound = ll.leftBound3d().inverted() ? ll.leftBound3d().invert().basicLineString() : ll.leftBound3d().basicLineString(); if (isRoadBorder(ll.leftBound3d())) { - LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(boundID); + LaneLineStringInstances::iterator itRoadBd = roadBorders_.find(boundID); if (itRoadBd != roadBorders_.end()) { itRoadBd->second->addLaneletID(ll.id()); } else { - roadBorders_.insert({boundID, std::make_shared( + roadBorders_.insert({boundID, std::make_shared( bound, boundID, bdTypeToEnum(ll.leftBound3d()), Ids{ll.id()}, false)}); } } else { - LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(boundID); + LaneLineStringInstances::iterator itLaneBd = laneDividers_.find(boundID); if (itLaneBd != laneDividers_.end()) { itLaneBd->second->addLaneletID(ll.id()); } else { - laneDividers_.insert({boundID, std::make_shared( + laneDividers_.insert({boundID, std::make_shared( bound, boundID, bdTypeToEnum(ll.leftBound3d()), Ids{ll.id()}, false)}); } } @@ -58,19 +58,19 @@ void LaneData::initRightBoundaries(LaneletSubmapConstPtr& localSubmap, BasicLineString3d bound = ll.rightBound3d().inverted() ? ll.rightBound3d().invert().basicLineString() : ll.rightBound3d().basicLineString(); if (isRoadBorder(ll.rightBound3d())) { - LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(boundID); + LaneLineStringInstances::iterator itRoadBd = roadBorders_.find(boundID); if (itRoadBd != roadBorders_.end()) { itRoadBd->second->addLaneletID(ll.id()); } else { - roadBorders_.insert({boundID, std::make_shared( + roadBorders_.insert({boundID, std::make_shared( bound, boundID, bdTypeToEnum(ll.rightBound3d()), Ids{ll.id()}, false)}); } } else { - LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(boundID); + LaneLineStringInstances::iterator itLaneBd = laneDividers_.find(boundID); if (itLaneBd != laneDividers_.end()) { itLaneBd->second->addLaneletID(ll.id()); } else { - laneDividers_.insert({boundID, std::make_shared( + laneDividers_.insert({boundID, std::make_shared( bound, boundID, bdTypeToEnum(ll.rightBound3d()), Ids{ll.id()}, false)}); } } @@ -82,16 +82,16 @@ void LaneData::initRightBoundaries(LaneletSubmapConstPtr& localSubmap, } } -void LaneData::initLaneletFeatures(LaneletSubmapConstPtr& localSubmap, - lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { +void LaneData::initLaneletInstances(LaneletSubmapConstPtr& localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { 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()}, ll.centerline3d().inverted()); - laneletFeatures_.insert( - {ll.id(), std::make_shared(leftBoundary, rightBoundary, centerline, ll.id())}); + LaneLineStringInstancePtr leftBoundary = getLineStringFeatFromId(ll.leftBound().id(), ll.leftBound().inverted()); + LaneLineStringInstancePtr rightBoundary = getLineStringFeatFromId(ll.rightBound().id(), ll.leftBound().inverted()); + LaneLineStringInstancePtr centerline = std::make_shared( + ll.centerline3d().basicLineString(), ll.centerline3d().id(), LineStringType::Centerline, Ids{ll.id()}, + ll.centerline3d().inverted()); + laneletInstances_.insert( + {ll.id(), std::make_shared(leftBoundary, rightBoundary, centerline, ll.id())}); } } @@ -132,8 +132,8 @@ void LaneData::getPaths(lanelet::routing::RoutingGraphConstPtr localSubmapGraph, LineStringType LaneData::getLineStringTypeFromId(Id id) { LineStringType bdType; - LaneLineStringFeatures::iterator itRoadBd = roadBorders_.find(id); - LaneLineStringFeatures::iterator itLaneBd = laneDividers_.find(id); + LaneLineStringInstances::iterator itRoadBd = roadBorders_.find(id); + LaneLineStringInstances::iterator itLaneBd = laneDividers_.find(id); if (itLaneBd != laneDividers_.end()) { bdType = itLaneBd->second->type(); } else if (itRoadBd != roadBorders_.end()) { @@ -145,15 +145,15 @@ LineStringType LaneData::getLineStringTypeFromId(Id id) { return bdType; } -LaneLineStringFeaturePtr makeInverted(const LaneLineStringFeaturePtr& feat) { - return std::make_shared( - BasicLineString3d(feat->rawFeature().rbegin(), feat->rawFeature().rend()), feat->mapID(), feat->type(), +LaneLineStringInstancePtr makeInverted(const LaneLineStringInstancePtr& feat) { + return std::make_shared( + BasicLineString3d(feat->rawInstance().rbegin(), feat->rawInstance().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); +LaneLineStringInstancePtr LaneData::getLineStringFeatFromId(Id id, bool inverted) { + LaneLineStringInstances::iterator itRoadBd = roadBorders_.find(id); + LaneLineStringInstances::iterator itLaneBd = laneDividers_.find(id); if (itLaneBd != laneDividers_.end()) { return (inverted == itLaneBd->second->inverted()) ? itLaneBd->second : makeInverted(itLaneBd->second); } else if (itRoadBd != roadBorders_.end()) { @@ -205,14 +205,14 @@ std::vector LaneData::computeCompoundRightBorders(const ConstLa return compoundBorders; } -CompoundLaneLineStringFeaturePtr LaneData::computeCompoundCenterline(const ConstLanelets& path) { - LaneLineStringFeatureList compoundCenterlines; +CompoundLaneLineStringInstancePtr LaneData::computeCompoundCenterline(const ConstLanelets& path) { + LaneLineStringInstanceList compoundCenterlines; for (const auto& ll : path) { - compoundCenterlines.push_back(std::make_shared(ll.centerline3d().basicLineString(), ll.id(), - LineStringType::Centerline, Ids{ll.id()}, - ll.centerline3d().inverted())); + 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); + return std::make_shared(compoundCenterlines, LineStringType::Centerline); } std::map::const_iterator findFirstOccElement(const CompoundElsList& elsList, @@ -226,9 +226,18 @@ std::map::const_iterator findFirstOccElement(const CompoundElsList& return searchMap.end(); } -void insertAndCheckNewCompoundFeatures(std::vector& compFeats, - const std::vector& newCompFeats, - std::map& elInsertIdx) { +bool hasElementNotInOther(const CompoundElsList& elsList1, const CompoundElsList& elsList2) { + for (const auto& el : elsList1.ids) { + if (std::find(elsList2.ids.begin(), elsList2.ids.end(), el) == elsList2.ids.end()) { + return true; + } + } + return false; +} + +void insertAndCheckNewCompoundInstances(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()) { @@ -237,17 +246,48 @@ void insertAndCheckNewCompoundFeatures(std::vector& compFeats, elInsertIdx[el] = compFeats.size() - 1; } } else if ((compFeats[firstOccIt->second].ids.size() < compEl.ids.size()) && - compFeats[firstOccIt->second].type == compEl.type) { + compFeats[firstOccIt->second].type == compEl.type && + !hasElementNotInOther(compFeats[firstOccIt->second], compEl)) { compFeats[firstOccIt->second] = compEl; for (const Id& el : compEl.ids) { elInsertIdx[el] = firstOccIt->second; } + } else if (compFeats[firstOccIt->second].type == compEl.type) { + std::vector leftoverIds; + std::vector leftoverInverted; + bool lastLeftover{false}; + for (size_t i = 0; i < compEl.ids.size(); i++) { + if (!elInsertIdx.count(compEl.ids[i])) { + leftoverIds.push_back(compEl.ids[i]); + leftoverInverted.push_back(compEl.inverted[i]); + lastLeftover = true; + } else if (lastLeftover) { + CompoundElsList leftover(leftoverIds, leftoverInverted, compEl.type); + compFeats.push_back(leftover); + for (const Id& el : leftover.ids) { + elInsertIdx[el] = compFeats.size() - 1; + } + leftoverIds.clear(); + leftoverInverted.clear(); + lastLeftover = false; + } + } + if (lastLeftover) { + CompoundElsList leftover(leftoverIds, leftoverInverted, compEl.type); + compFeats.push_back(leftover); + for (const Id& el : leftover.ids) { + elInsertIdx[el] = compFeats.size() - 1; + } + leftoverIds.clear(); + leftoverInverted.clear(); + lastLeftover = false; + } } } } -void LaneData::initCompoundFeatures(LaneletSubmapConstPtr& localSubmap, - lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { +void LaneData::initCompoundInstances(LaneletSubmapConstPtr& localSubmap, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph) { std::vector compoundedBordersAndDividers; std::map elInsertIdx; @@ -262,30 +302,30 @@ void LaneData::initCompoundFeatures(LaneletSubmapConstPtr& localSubmap, for (const auto& path : paths) { std::vector compoundedLeft = computeCompoundLeftBorders(path); - insertAndCheckNewCompoundFeatures(compoundedBordersAndDividers, compoundedLeft, elInsertIdx); + insertAndCheckNewCompoundInstances(compoundedBordersAndDividers, compoundedLeft, elInsertIdx); std::vector compoundedRight = computeCompoundRightBorders(path); - insertAndCheckNewCompoundFeatures(compoundedBordersAndDividers, compoundedRight, elInsertIdx); + insertAndCheckNewCompoundInstances(compoundedBordersAndDividers, compoundedRight, elInsertIdx); compoundCenterlines_.push_back(computeCompoundCenterline(path)); } for (const auto& compFeat : compoundedBordersAndDividers) { - LaneLineStringFeatureList toBeCompounded; + LaneLineStringInstanceList toBeCompounded; 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]); + LaneLineStringInstancePtr cmpdFeat = getLineStringFeatFromId(compFeat.ids[i], compFeat.inverted[i]); toBeCompounded.push_back(cmpdFeat); } LineStringType cmpdType = toBeCompounded.front()->type(); if (cmpdType == LineStringType::RoadBorder) { - compoundRoadBorders_.push_back(std::make_shared(toBeCompounded, cmpdType)); + compoundRoadBorders_.push_back(std::make_shared(toBeCompounded, cmpdType)); } else { - compoundLaneDividers_.push_back(std::make_shared(toBeCompounded, cmpdType)); + compoundLaneDividers_.push_back(std::make_shared(toBeCompounded, cmpdType)); } } } -void LaneData::updateAssociatedCpdFeatureIndices() { +void LaneData::updateAssociatedCpdInstanceIndices() { for (size_t i = 0; i < compoundRoadBorders_.size(); i++) { const auto& cpdFeat = compoundRoadBorders_[i]; for (const auto& indFeat : cpdFeat->features()) { @@ -314,15 +354,16 @@ void LaneData::updateAssociatedCpdFeatureIndices() { } } -bool LaneData::processAll(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t 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); +bool LaneData::processAll(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints, double pitch, + double roll) { + bool validRoadBorders = processInstances(roadBorders_, bbox, paramType, nPoints, pitch, roll); + bool validLaneDividers = processInstances(laneDividers_, bbox, paramType, nPoints, pitch, roll); + bool validLaneletInstances = processInstances(laneletInstances_, bbox, paramType, nPoints, pitch, roll); + bool validCompoundRoadBorders = processInstances(compoundRoadBorders_, bbox, paramType, nPoints, pitch, roll); + bool validCompoundLaneDividers = processInstances(compoundLaneDividers_, bbox, paramType, nPoints, pitch, roll); + bool validCompoundCenterlines = processInstances(compoundCenterlines_, bbox, paramType, nPoints, pitch, roll); - if (validRoadBorders && validLaneDividers && validLaneletFeatures && validCompoundRoadBorders && + if (validRoadBorders && validLaneDividers && validLaneletInstances && validCompoundRoadBorders && validCompoundLaneDividers && validCompoundCenterlines) { return true; } else { @@ -330,9 +371,9 @@ bool LaneData::processAll(const OrientedRect& bbox, const ParametrizationType& p } } -LaneData::TensorFeatureData LaneData::getTensorFeatureData(bool pointsIn2d, bool ignoreBuffer) { +LaneData::TensorInstanceData LaneData::getTensorInstanceData(bool pointsIn2d, bool ignoreBuffer) { if (!tfData_.has_value() || ignoreBuffer) { - tfData_ = LaneData::TensorFeatureData(); + tfData_ = LaneData::TensorInstanceData(); tfData_->uuid_ = uuid_; tfData_->roadBorders_ = getPointMatrices(validRoadBorders(), pointsIn2d); tfData_->laneDividers_ = getPointMatrices(validLaneDividers(), pointsIn2d); @@ -347,21 +388,21 @@ LaneData::TensorFeatureData LaneData::getTensorFeatureData(bool pointsIn2d, bool } size_t pointMatrixIdxRb = 0; for (const auto& cpdFeat : compoundRoadBorders_) { - for (const auto& lString : cpdFeat->cutResampledAndTransformedFeature()) { + for (const auto& lString : cpdFeat->cutResampledAndTransformedInstance()) { tfData_->pointMatrixCpdRoadBorder_[pointMatrixIdxRb] = cpdFeat; pointMatrixIdxRb++; } } size_t pointMatrixIdxLd = 0; for (const auto& cpdFeat : compoundLaneDividers_) { - for (const auto& lString : cpdFeat->cutResampledAndTransformedFeature()) { + for (const auto& lString : cpdFeat->cutResampledAndTransformedInstance()) { tfData_->pointMatrixCpdLaneDivider_[pointMatrixIdxLd] = cpdFeat; pointMatrixIdxLd++; } } size_t pointMatrixIdxCl = 0; for (const auto& cpdFeat : compoundCenterlines_) { - for (const auto& lString : cpdFeat->cutResampledAndTransformedFeature()) { + for (const auto& lString : cpdFeat->cutResampledAndTransformedInstance()) { tfData_->pointMatrixCpdCenterline_[pointMatrixIdxCl] = cpdFeat; pointMatrixIdxCl++; } @@ -370,30 +411,30 @@ LaneData::TensorFeatureData LaneData::getTensorFeatureData(bool pointsIn2d, bool return tfData_.value(); } -CompoundLaneLineStringFeatureList associatedCpdFeats(Id mapId, const CompoundLaneLineStringFeatureList& featList, - const std::map>& assoIndices) { - CompoundLaneLineStringFeatureList assoFeats; +CompoundLaneLineStringInstanceList associatedCpdFeats(Id mapId, const CompoundLaneLineStringInstanceList& featList, + const std::map>& assoIndices) { + CompoundLaneLineStringInstanceList assoFeats; for (const auto& idx : assoIndices.at(mapId)) { assoFeats.push_back(featList[idx]); } return assoFeats; } -CompoundLaneLineStringFeatureList LaneData::associatedCpdRoadBorders(Id mapId) { +CompoundLaneLineStringInstanceList LaneData::associatedCpdRoadBorders(Id mapId) { return associatedCpdFeats(mapId, compoundRoadBorders_, associatedCpdRoadBorderIndices_); } -CompoundLaneLineStringFeatureList LaneData::associatedCpdLaneDividers(Id mapId) { +CompoundLaneLineStringInstanceList LaneData::associatedCpdLaneDividers(Id mapId) { return associatedCpdFeats(mapId, compoundLaneDividers_, associatedCpdLaneDividerIndices_); } -CompoundLaneLineStringFeatureList LaneData::associatedCpdCenterlines(Id mapId) { +CompoundLaneLineStringInstanceList LaneData::associatedCpdCenterlines(Id mapId) { return associatedCpdFeats(mapId, compoundCenterlines_, associatedCpdCenterlineIndices_); } -CompoundLaneLineStringFeaturePtr pointMatrixCpdFeat( - size_t index, const std::map& assoFeats) { - CompoundLaneLineStringFeaturePtr feat; +CompoundLaneLineStringInstancePtr pointMatrixCpdFeat( + size_t index, const std::map& assoFeats) { + CompoundLaneLineStringInstancePtr feat; try { feat = assoFeats.at(index); } catch (const std::out_of_range& e) { @@ -402,15 +443,15 @@ CompoundLaneLineStringFeaturePtr pointMatrixCpdFeat( return feat; } -CompoundLaneLineStringFeaturePtr LaneData::TensorFeatureData::pointMatrixCpdRoadBorder(size_t index) { +CompoundLaneLineStringInstancePtr LaneData::TensorInstanceData::pointMatrixCpdRoadBorder(size_t index) { return pointMatrixCpdFeat(index, pointMatrixCpdRoadBorder_); } -CompoundLaneLineStringFeaturePtr LaneData::TensorFeatureData::pointMatrixCpdLaneDivider(size_t index) { +CompoundLaneLineStringInstancePtr LaneData::TensorInstanceData::pointMatrixCpdLaneDivider(size_t index) { return pointMatrixCpdFeat(index, pointMatrixCpdLaneDivider_); } -CompoundLaneLineStringFeaturePtr LaneData::TensorFeatureData::pointMatrixCpdCenterline(size_t index) { +CompoundLaneLineStringInstancePtr LaneData::TensorInstanceData::pointMatrixCpdCenterline(size_t index) { return pointMatrixCpdFeat(index, pointMatrixCpdCenterline_); } diff --git a/lanelet2_ml_converter/src/MapDataInterface.cpp b/lanelet2_ml_converter/src/MapDataInterface.cpp index 7bc8f621..866def2b 100644 --- a/lanelet2_ml_converter/src/MapDataInterface.cpp +++ b/lanelet2_ml_converter/src/MapDataInterface.cpp @@ -15,11 +15,27 @@ namespace lanelet { namespace ml_converter { -void MapDataInterface::setCurrPosAndExtractSubmap(const BasicPoint2d& pt, double yaw) { +void MapDataInterface::setCurrPosAndExtractSubmap2d(const BasicPoint2d& pt, double yaw) { + setCurrPosAndExtractSubmap(BasicPoint3d{pt.x(), pt.y(), 0}, yaw, 0, 0); + currBbox_ = getRotatedRect(*currPos_, config_.submapExtentLongitudinal, config_.submapExtentLateral, *currYaw_, true); +} + +void MapDataInterface::setCurrPosAndExtractSubmap(const BasicPoint3d& pt, double yaw) { + setCurrPosAndExtractSubmap(pt, yaw, 0, 0); + currBbox_ = + getRotatedRect(*currPos_, config_.submapExtentLongitudinal, config_.submapExtentLateral, *currYaw_, false); +} + +void MapDataInterface::setCurrPosAndExtractSubmap(const BasicPoint3d& pt, double yaw, double pitch, double roll) { currPos_ = pt; currYaw_ = yaw; - localSubmap_ = extractSubmap(laneletMap_, *currPos_, config_.submapExtentLongitudinal, config_.submapExtentLateral); + currPitch_ = pitch; + currRoll_ = roll; + localSubmap_ = + extractSubmap(laneletMap_, currPos_->head(2), config_.submapExtentLongitudinal, config_.submapExtentLateral); localSubmapGraph_ = lanelet::routing::RoutingGraph::build(*localSubmap_, *trafficRules_); + currBbox_ = + getRotatedRect(*currPos_, config_.submapExtentLongitudinal, config_.submapExtentLateral, *currYaw_, false); } MapDataInterface::MapDataInterface(LaneletMapConstPtr laneletMap) @@ -32,20 +48,19 @@ MapDataInterface::MapDataInterface(LaneletMapConstPtr laneletMap, Configuration config_{config}, trafficRules_{traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle)} {} -LaneDataPtr MapDataInterface::getLaneData(LaneletSubmapConstPtr localSubmap, - lanelet::routing::RoutingGraphConstPtr localSubmapGraph, - const BasicPoint2d& pos, double yaw, bool processAll) { +LaneDataPtr MapDataInterface::getLaneData(LaneletSubmapConstPtr localSubmap, const OrientedRect& bbox, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph, double pitch, + double roll, 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, pitch, roll); } return laneData; } -std::vector MapDataInterface::laneDataBatch(std::vector pts, std::vector yaws) { +std::vector MapDataInterface::laneDataBatch2d(std::vector pts, std::vector yaws) { if (pts.size() != yaws.size()) { - throw std::runtime_error("Unequal sizes of pts and yaws!"); + throw std::runtime_error("Unequal sizes of pts and rotation angles!"); } std::vector lDataVec; for (size_t i = 0; i < pts.size(); i++) { @@ -53,12 +68,37 @@ std::vector MapDataInterface::laneDataBatch(std::vector MapDataInterface::laneDataBatch(std::vector pts, std::vector yaws) { + return laneDataBatch(pts, yaws, std::vector(yaws.size(), 0), std::vector(yaws.size(), 0)); +} + +std::vector MapDataInterface::laneDataBatch(std::vector pts, std::vector yaws, + std::vector pitches, std::vector rolls) { + if ((pts.size() != yaws.size()) || (pts.size() != pitches.size()) || (pts.size() != rolls.size())) { + throw std::runtime_error("Unequal sizes of pts and rotation angles!"); + } + std::vector lDataVec; + for (size_t i = 0; i < pts.size(); i++) { + LaneletSubmapConstPtr localSubmap = + extractSubmap(laneletMap_, pts[i].head(2), config_.submapExtentLongitudinal, config_.submapExtentLateral); + routing::RoutingGraphConstPtr localSubmapGraph = + lanelet::routing::RoutingGraph::build(*localSubmap, *trafficRules_); + OrientedRect bbox = + getRotatedRect(pts[i], config_.submapExtentLongitudinal, config_.submapExtentLateral, yaws[i], false); + lDataVec.push_back(getLaneData(localSubmap, bbox, localSubmapGraph, pitches[i], rolls[i], true)); } return lDataVec; } -std::vector MapDataInterface::laneTEDataBatch(std::vector pts, std::vector yaws) { +std::vector MapDataInterface::laneTEDataBatch(std::vector pts, std::vector yaws, + std::vector pitches, std::vector rolls) { throw std::runtime_error("Not implemented yet!"); } @@ -67,9 +107,9 @@ bool isTe(ConstLineString3d ls) { return type == AttributeValueString::TrafficLight || type == AttributeValueString::TrafficSign; } -TEData MapDataInterface::getLaneTEData(LaneletSubmapConstPtr localSubmap, - lanelet::routing::RoutingGraphConstPtr localSubmapGraph, const BasicPoint2d& pos, - double yaw, bool processAll) { +TEData MapDataInterface::getLaneTEData(LaneletSubmapConstPtr localSubmap, const OrientedRect& bbox, + lanelet::routing::RoutingGraphConstPtr localSubmapGraph, double pitch, + double roll, bool processAll) { throw std::runtime_error("Not implemented yet!"); } @@ -82,7 +122,15 @@ LaneDataPtr MapDataInterface::laneData(bool processAll) { throw InvalidObjectStateError( "Your current yaw angle is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); } - return getLaneData(localSubmap_, localSubmapGraph_, *currPos_, *currYaw_, processAll); + if (!currPitch_) { + throw InvalidObjectStateError( + "Your current pitch angle is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); + } + if (!currRoll_) { + throw InvalidObjectStateError( + "Your current roll angle is not set! Call setCurrPosAndExtractSubmap() before trying to get the data!"); + } + return getLaneData(localSubmap_, *currBbox_, localSubmapGraph_, *currPitch_, *currRoll_, processAll); } TEData MapDataInterface::teData(bool processAll) { diff --git a/lanelet2_ml_converter/src/MapFeatures.cpp b/lanelet2_ml_converter/src/MapInstances.cpp similarity index 51% rename from lanelet2_ml_converter/src/MapFeatures.cpp rename to lanelet2_ml_converter/src/MapInstances.cpp index f5626ae0..85f00c6c 100644 --- a/lanelet2_ml_converter/src/MapFeatures.cpp +++ b/lanelet2_ml_converter/src/MapInstances.cpp @@ -1,23 +1,23 @@ -#include "lanelet2_ml_converter/MapFeatures.h" - #include #include +#include "lanelet2_ml_converter/MapInstances.h" #include "lanelet2_ml_converter/Utils.h" namespace lanelet { namespace ml_converter { struct LStringProcessResult { - BasicLineStrings3d cutFeatures; - BasicLineStrings3d cutAndResampledFeatures; - BasicLineStrings3d cutResampledAndTransformedFeatures; + BasicLineStrings3d cutInstances; + BasicLineStrings3d cutAndResampledInstances; + BasicLineStrings3d cutResampledAndTransformedInstances; bool wasCut_{false}; bool valid_{true}; }; LStringProcessResult processLineStringImpl(const BasicLineString3d& lstring, const OrientedRect& bbox, - const ParametrizationType& paramType, int32_t nPoints) { + const ParametrizationType& paramType, int32_t nPoints, double pitch, + double roll) { LStringProcessResult result; if (paramType != ParametrizationType::LineString) { throw std::runtime_error("Only polyline parametrization is implemented so far!"); @@ -45,45 +45,48 @@ LStringProcessResult processLineStringImpl(const BasicLineString3d& lstring, con } for (const auto& line : cutLines) { - result.cutFeatures.push_back(line); + result.cutInstances.push_back(line); BasicLineString3d lineResampled = resampleLineString(line, nPoints); - result.cutAndResampledFeatures.push_back(lineResampled); - result.cutResampledAndTransformedFeatures.push_back(transformLineString(bbox, lineResampled)); + result.cutAndResampledInstances.push_back(lineResampled); + result.cutResampledAndTransformedInstances.push_back(transformLineString(bbox, lineResampled, pitch, roll)); } return result; } -bool LaneLineStringFeature::process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints) { - LStringProcessResult result = processLineStringImpl(rawFeature_, bbox, paramType, nPoints); +bool LaneLineStringInstance::process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints, + double pitch, double roll) { + LStringProcessResult result = processLineStringImpl(rawInstance_, bbox, paramType, nPoints, pitch, roll); if (result.valid_) { - cutFeatures_ = result.cutFeatures; - cutAndResampledFeatures_ = result.cutAndResampledFeatures; - cutResampledAndTransformedFeatures_ = result.cutResampledAndTransformedFeatures; + cutInstances_ = result.cutInstances; + cutAndResampledInstances_ = result.cutAndResampledInstances; + cutResampledAndTransformedInstances_ = result.cutResampledAndTransformedInstances; } else { valid_ = result.valid_; } wasCut_ = result.wasCut_; + processedFrom2d_ = bbox.from2d; return result.valid_; } -std::vector LaneLineStringFeature::computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { +std::vector LaneLineStringInstance::computeInstanceVectors(bool onlyPoints, bool pointsIn2d) const { std::vector featVecs; - for (const auto& split : cutResampledAndTransformedFeatures_) { - featVecs.push_back(toFeatureVector(split, typeInt(), onlyPoints, pointsIn2d)); + for (const auto& split : cutResampledAndTransformedInstances_) { + featVecs.push_back(toInstanceVector(split, typeInt(), onlyPoints, (pointsIn2d || processedFrom2d_))); } return featVecs; } -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 +std::vector TEInstance::computeInstanceVectors(bool onlyPoints, bool pointsIn2d) const { + VectorXd vec = (pointsIn2d || processedFrom2d_) + ? VectorXd(2 * rawInstance_.size() + 1) + : VectorXd(3 * rawInstance_.size() + 1); // n points with 2/3 dims + type if (pointsIn2d == true) { - for (size_t i = 0; i < rawFeature_.size(); i++) { - vec.segment(2 * i, 2) = rawFeature_[i].segment(0, 2); + for (size_t i = 0; i < rawInstance_.size(); i++) { + vec.segment(2 * i, 2) = rawInstance_[i].segment(0, 2); } } else { - for (size_t i = 0; i < rawFeature_.size(); i++) { - vec.segment(3 * i, 3) = rawFeature_[i].segment(0, 3); + for (size_t i = 0; i < rawInstance_.size(); i++) { + vec.segment(3 * i, 3) = rawInstance_[i].segment(0, 3); } } vec[vec.size() - 1] = static_cast(teType_); @@ -94,40 +97,42 @@ std::vector TEFeature::computeFeatureVectors(bool onlyPoints, bool poi } } -std::vector LaneLineStringFeature::pointMatrices(bool pointsIn2d) const { +std::vector LaneLineStringInstance::pointMatrices(bool pointsIn2d) const { std::vector pointMatrices; - for (const auto& split : cutResampledAndTransformedFeatures_) { - pointMatrices.push_back(toPointMatrix(split, pointsIn2d)); + for (const auto& split : cutResampledAndTransformedInstances_) { + pointMatrices.push_back(toPointMatrix(split, (pointsIn2d || processedFrom2d_))); } return pointMatrices; } -std::vector TEFeature::pointMatrices(bool pointsIn2d) const { - return std::vector{toPointMatrix(rawFeature_, pointsIn2d)}; +std::vector TEInstance::pointMatrices(bool pointsIn2d) const { + return std::vector{toPointMatrix(rawInstance_, (pointsIn2d || processedFrom2d_))}; } -bool TEFeature::process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/) { +bool TEInstance::process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t /*unused*/, + double pitch, double roll) { 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} {} +LaneletInstance::LaneletInstance(LaneLineStringInstancePtr leftBoundary, LaneLineStringInstancePtr rightBoundary, + LaneLineStringInstancePtr centerline, Id mapID) + : MapInstance(mapID), leftBoundary_{leftBoundary}, rightBoundary_{rightBoundary}, centerline_{centerline} {} -LaneletFeature::LaneletFeature(const ConstLanelet& ll) - : leftBoundary_{std::make_shared(ll.leftBound3d().basicLineString(), ll.leftBound3d().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, Ids{ll.id()}, false)} {} +LaneletInstance::LaneletInstance(const ConstLanelet& ll) + : leftBoundary_{std::make_shared(ll.leftBound3d().basicLineString(), ll.leftBound3d().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, Ids{ll.id()}, false)} {} -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); +bool LaneletInstance::process(const OrientedRect& bbox, const ParametrizationType& paramType, int32_t nPoints, + double pitch, double roll) { + leftBoundary_->process(bbox, paramType, nPoints, pitch, roll); + rightBoundary_->process(bbox, paramType, nPoints, pitch, roll); + centerline_->process(bbox, paramType, nPoints, pitch, roll); if (leftBoundary_->wasCut() || rightBoundary_->wasCut() || centerline_->wasCut()) { wasCut_ = true; @@ -135,6 +140,7 @@ bool LaneletFeature::process(const OrientedRect& bbox, const ParametrizationType if (!leftBoundary_->valid() || !rightBoundary_->valid() || !centerline_->valid()) { valid_ = false; } + processedFrom2d_ = bbox.from2d; return valid_; } @@ -151,9 +157,9 @@ VectorXd stackVector(const std::vector& vec) { return stacked; } -std::vector LaneletFeature::computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { +std::vector LaneletInstance::computeInstanceVectors(bool onlyPoints, bool pointsIn2d) const { if (reprType_ == LaneletRepresentationType::Centerline) { - std::vector vecCenterlinePts = centerline_->computeFeatureVectors(true, pointsIn2d); + std::vector vecCenterlinePts = centerline_->computeInstanceVectors(true, pointsIn2d); if (onlyPoints) { return vecCenterlinePts; } @@ -167,9 +173,9 @@ std::vector LaneletFeature::computeFeatureVectors(bool onlyPoints, boo } return featureVecs; } else if (reprType_ == LaneletRepresentationType::Boundaries) { - std::vector vecLeftBdPts = leftBoundary_->computeFeatureVectors(true, pointsIn2d); + std::vector vecLeftBdPts = leftBoundary_->computeInstanceVectors(true, pointsIn2d); VectorXd leftBdPts = stackVector(vecLeftBdPts); - std::vector vecRightBdPts = rightBoundary_->computeFeatureVectors(true, pointsIn2d); + std::vector vecRightBdPts = rightBoundary_->computeInstanceVectors(true, pointsIn2d); VectorXd rightBdPts = stackVector(vecRightBdPts); VectorXd vec(leftBdPts.size() + rightBdPts.size() + 2); // pts vec + left and right type @@ -189,27 +195,27 @@ std::vector LaneletFeature::computeFeatureVectors(bool onlyPoints, boo } } -CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStringFeatureList& features, - LineStringType compoundType) - : individualFeatures_{features}, +CompoundLaneLineStringInstance::CompoundLaneLineStringInstance(const LaneLineStringInstanceList& features, + LineStringType compoundType) + : individualInstances_{features}, pathLengthsRaw_{std::vector(features.size())}, pathLengthsProcessed_{std::vector(features.size())}, - processedFeaturesValid_{std::vector(features.size())} { + processedInstancesValid_{std::vector(features.size())} { type_ = compoundType; for (size_t i = 0; i < features.size(); i++) { - if (features[i]->rawFeature().empty()) { - throw std::runtime_error("Feature with empty rawFeature() supplied!"); + if (features[i]->rawInstance().empty()) { + throw std::runtime_error("Instance with empty rawInstance() supplied!"); } // checking if the linestring is correctly if (i == features.size() - 1) { - rawFeature_.insert(rawFeature_.end(), features[i]->rawFeature().begin(), features[i]->rawFeature().end()); + rawInstance_.insert(rawInstance_.end(), features[i]->rawInstance().begin(), features[i]->rawInstance().end()); } else { - rawFeature_.insert(rawFeature_.end(), features[i]->rawFeature().begin(), features[i]->rawFeature().end() - 1); + rawInstance_.insert(rawInstance_.end(), features[i]->rawInstance().begin(), features[i]->rawInstance().end() - 1); } double rawLength = - boost::geometry::length(features[i]->rawFeature(), boost::geometry::strategy::distance::pythagoras()); + boost::geometry::length(features[i]->rawInstance(), boost::geometry::strategy::distance::pythagoras()); if (i > 0) { pathLengthsRaw_[i] = pathLengthsRaw_[i - 1] + rawLength; } else { @@ -218,21 +224,21 @@ CompoundLaneLineStringFeature::CompoundLaneLineStringFeature(const LaneLineStrin } } -bool CompoundLaneLineStringFeature::process(const OrientedRect& bbox, const ParametrizationType& paramType, - int32_t nPoints) { +bool CompoundLaneLineStringInstance::process(const OrientedRect& bbox, const ParametrizationType& paramType, + int32_t nPoints, double pitch, double roll) { bool valid = false; - LaneLineStringFeature::process(bbox, paramType, nPoints); - for (size_t i = 0; i < individualFeatures_.size(); i++) { - individualFeatures_[i]->process(bbox, paramType, nPoints); + LaneLineStringInstance::process(bbox, paramType, nPoints, pitch, roll); + for (size_t i = 0; i < individualInstances_.size(); i++) { + individualInstances_[i]->process(bbox, paramType, nPoints, pitch, roll); double processedLength = 0; - for (const auto& line : individualFeatures_[i]->cutFeature()) { + for (const auto& line : individualInstances_[i]->cutInstance()) { processedLength = processedLength + boost::geometry::length(line, boost::geometry::strategy::distance::pythagoras()); } if (processedLength > validLengthThresh_) { - processedFeaturesValid_[i] = true; + processedInstancesValid_[i] = true; valid = true; } @@ -242,6 +248,7 @@ bool CompoundLaneLineStringFeature::process(const OrientedRect& bbox, const Para pathLengthsProcessed_[i] = processedLength; } } + processedFrom2d_ = bbox.from2d; return valid; } @@ -260,7 +267,7 @@ MatrixXd toPointMatrix(const BasicLineString3d& lString, bool pointsIn2d) { return mat; } -VectorXd toFeatureVector(const BasicLineString3d& line, int typeInt, bool onlyPoints, bool pointsIn2d) { +VectorXd toInstanceVector(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) { diff --git a/lanelet2_ml_converter/src/Utils.cpp b/lanelet2_ml_converter/src/Utils.cpp index 4e124ee8..0b2a1305 100644 --- a/lanelet2_ml_converter/src/Utils.cpp +++ b/lanelet2_ml_converter/src/Utils.cpp @@ -14,7 +14,8 @@ namespace fs = boost::filesystem; namespace lanelet { namespace ml_converter { -OrientedRect getRotatedRect(const BasicPoint2d& center, double extentLongitudinal, double extentLateral, double yaw) { +OrientedRect getRotatedRect(const BasicPoint3d& center, double extentLongitudinal, double extentLateral, double yaw, + bool from2dPos) { BasicPoints2d pts{BasicPoint2d{center.x() - extentLongitudinal, center.y() - extentLateral}, BasicPoint2d{center.x() - extentLongitudinal, center.y() + extentLateral}, BasicPoint2d{center.x() + extentLongitudinal, center.y() + extentLateral}, @@ -37,6 +38,7 @@ OrientedRect getRotatedRect(const BasicPoint2d& center, double extentLongitudina trans2Rect.center = center; trans2Rect.yaw = yaw; + trans2Rect.from2d = from2dPos; return trans2Rect; } @@ -124,13 +126,28 @@ std::vector cutLineString(const OrientedRect& bbox, const Bas 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); +boost::geometry::strategy::transform::matrix_transformer getYPRMatrix(double yaw, double pitch, + double roll) { + Eigen::AngleAxisd rollAngle(-roll, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd pitchAngle(-pitch, Eigen::Vector3d::UnitY()); + Eigen::AngleAxisd yawAngle(-yaw, Eigen::Vector3d::UnitZ()); + Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle; + Eigen::Matrix3d rotM = q.matrix(); + boost::geometry::strategy::transform::matrix_transformer ypr( + rotM(0, 0), rotM(0, 1), rotM(0, 2), 0, rotM(1, 0), rotM(1, 1), rotM(1, 2), 0, rotM(2, 0), rotM(2, 1), rotM(2, 2), + 0, 0, 0, 0, 1); + return ypr; +} + +BasicLineString3d transformLineString(const OrientedRect& bbox, const BasicLineString3d& polyline, double pitch, + double roll) { + boost::geometry::strategy::transform::translate_transformer trans1(-bbox.center.x(), -bbox.center.y(), + -bbox.center.z()); + boost::geometry::strategy::transform::matrix_transformer ypr = getYPRMatrix(bbox.yaw, pitch, roll); BasicLineString3d transPolyline; BasicLineString3d rotatedPolyline; boost::geometry::transform(polyline, transPolyline, trans1); - boost::geometry::transform(transPolyline, rotatedPolyline, rotate); + boost::geometry::transform(transPolyline, rotatedPolyline, ypr); return rotatedPolyline; } diff --git a/lanelet2_ml_converter/test/test_map.h b/lanelet2_ml_converter/test/test_map.h index 065ef97d..237347b5 100644 --- a/lanelet2_ml_converter/test/test_map.h +++ b/lanelet2_ml_converter/test/test_map.h @@ -177,12 +177,12 @@ class MLConverterTest : public ::testing::Test { 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 BasicPoint3d centerBbox{5, 5, 0}; const double extentLongitudinalBbox{15}; const double extentLateralBbox{10}; const double yawBbox{M_PI / 2.0}; OrientedRect bbox; - MLConverterTest() : bbox{getRotatedRect(centerBbox, extentLongitudinalBbox, extentLateralBbox, yawBbox)} {} + MLConverterTest() : bbox{getRotatedRect(centerBbox, extentLongitudinalBbox, extentLateralBbox, yawBbox, true)} {} }; } // namespace tests diff --git a/lanelet2_ml_converter/test/test_map_data.cpp b/lanelet2_ml_converter/test/test_map_data.cpp index c87abb5a..8873f879 100644 --- a/lanelet2_ml_converter/test/test_map_data.cpp +++ b/lanelet2_ml_converter/test/test_map_data.cpp @@ -3,7 +3,7 @@ // #include #include "lanelet2_ml_converter/MapData.h" -#include "lanelet2_ml_converter/MapFeatures.h" +#include "lanelet2_ml_converter/MapInstances.h" #include "lanelet2_routing/RoutingGraph.h" #include "test_map.h" @@ -26,8 +26,8 @@ TEST_F(MLConverterTest, LaneData) { // NOLINT 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->laneletInstances().find(2007) != laneData->laneletInstances().end()); + EXPECT_EQ(laneData->laneletInstances().find(2007)->second->leftBoundary()->mapID(), 1012); EXPECT_TRUE(laneData->roadBorders().find(1001) != laneData->roadBorders().end()); EXPECT_EQ(compoundRoadBorders.size(), 3); @@ -35,19 +35,19 @@ TEST_F(MLConverterTest, LaneData) { // NOLINT EXPECT_EQ(compoundCenterlines.size(), 4); EXPECT_EQ(laneData->associatedCpdRoadBorders(2001).size(), 1); - CompoundLaneLineStringFeaturePtr assoBorder = laneData->associatedCpdRoadBorders(2001).front(); + CompoundLaneLineStringInstancePtr assoBorder = laneData->associatedCpdRoadBorders(2001).front(); EXPECT_EQ(assoBorder->features().back()->mapID(), 1000); EXPECT_EQ(assoBorder->features().back()->laneletIDs().front(), 2002); EXPECT_EQ(laneData->associatedCpdLaneDividers(2004).size(), 2); EXPECT_EQ(laneData->associatedCpdLaneDividers(2009).size(), 1); - CompoundLaneLineStringFeaturePtr assoDivider = laneData->associatedCpdLaneDividers(2009).front(); + CompoundLaneLineStringInstancePtr assoDivider = laneData->associatedCpdLaneDividers(2009).front(); EXPECT_EQ(assoDivider->features().front()->mapID(), 1015); EXPECT_EQ(assoDivider->features().front()->laneletIDs().size(), 2); EXPECT_EQ(laneData->associatedCpdCenterlines(2003).size(), 2); EXPECT_EQ(laneData->associatedCpdCenterlines(2000).size(), 1); - CompoundLaneLineStringFeaturePtr assoCenterline = laneData->associatedCpdCenterlines(2000).front(); + CompoundLaneLineStringInstancePtr assoCenterline = laneData->associatedCpdCenterlines(2000).front(); EXPECT_EQ(assoCenterline->features().front()->mapID(), 2000); EXPECT_EQ(assoCenterline->features().front()->laneletIDs().front(), 2000); diff --git a/lanelet2_ml_converter/test/test_map_data_interface.cpp b/lanelet2_ml_converter/test/test_map_data_interface.cpp index 41ad323b..b4bf4144 100644 --- a/lanelet2_ml_converter/test/test_map_data_interface.cpp +++ b/lanelet2_ml_converter/test/test_map_data_interface.cpp @@ -5,7 +5,7 @@ #include "lanelet2_ml_converter/MapData.h" #include "lanelet2_ml_converter/MapDataInterface.h" -#include "lanelet2_ml_converter/MapFeatures.h" +#include "lanelet2_ml_converter/MapInstances.h" #include "lanelet2_routing/RoutingGraph.h" #include "lanelet2_traffic_rules/TrafficRulesFactory.h" #include "test_map.h" @@ -36,7 +36,7 @@ TEST_F(MLConverterTest, MapDataInterface) { // NOLINT 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.laneDataBatch2d(pts, yaws); // std::cerr << "Elapsed(micros)=" << since(start).count() << std::endl; for (size_t i = 0; i < lDataVec.size(); i++) { @@ -49,20 +49,20 @@ TEST_F(MLConverterTest, MapDataInterface) { // NOLINT 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()); + LaneletInstances::const_iterator itLL0 = lDataVec[i]->laneletInstances().find(2000); + EXPECT_TRUE(itLL0 != lDataVec[i]->laneletInstances().end()); + LaneletInstances::const_iterator itLL1 = lDataVec[i]->laneletInstances().find(2002); + EXPECT_TRUE(itLL1 == lDataVec[i]->laneletInstances().end()); break; } case 1: { - LaneletFeatures::const_iterator itLL = lDataVec[i]->laneletFeatures().find(2011); - EXPECT_TRUE(itLL != lDataVec[i]->laneletFeatures().end()); + LaneletInstances::const_iterator itLL = lDataVec[i]->laneletInstances().find(2011); + EXPECT_TRUE(itLL != lDataVec[i]->laneletInstances().end()); break; } case 5: { - LaneletFeatures::const_iterator itLL = lDataVec[i]->laneletFeatures().find(2004); - EXPECT_TRUE(itLL == lDataVec[i]->laneletFeatures().end()); + LaneletInstances::const_iterator itLL = lDataVec[i]->laneletInstances().find(2004); + EXPECT_TRUE(itLL == lDataVec[i]->laneletInstances().end()); break; } } @@ -116,7 +116,7 @@ TEST_F(MLConverterTest, 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::vector lDataVec = parser.laneDataBatch(pts, yaws); + std::vector lDataVec = parser.laneDataBatch2d(pts, yaws); saveLaneData("/tmp/lane_data_save_test.xml", lDataVec, false); std::vector lDataLoaded = loadLaneData("/tmp/lane_data_save_test.xml", false); diff --git a/lanelet2_ml_converter/test/test_map_features.cpp b/lanelet2_ml_converter/test/test_map_features.cpp deleted file mode 100644 index 982e73b1..00000000 --- a/lanelet2_ml_converter/test/test_map_features.cpp +++ /dev/null @@ -1,122 +0,0 @@ -#include - -#include "lanelet2_ml_converter/MapFeatures.h" -#include "test_map.h" - -using namespace lanelet; -using namespace lanelet::ml_converter; -using namespace lanelet::ml_converter::tests; - -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); - - feat.process(bbox, ParametrizationType::LineString, 4); - 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(MLConverterTest, 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, 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, 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, Ids(1234), false); - - LaneletFeature llFeat(leftBdFeat, rightBdFeat, centerlineFeat, Id(1234)); - llFeat.setReprType(LaneletRepresentationType::Boundaries); - - llFeat.process(bbox, ParametrizationType::LineString, 4); - 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(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); - BasicLineString3d p2{BasicPoint3d{-5, 0, 0}, BasicPoint3d{0, 0, 0}}; - LaneLineStringFeaturePtr feat2 = - 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, Ids(1234), false); - BasicLineString3d p4{BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}}; - LaneLineStringFeaturePtr feat4 = - 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, Ids(1234), false); - - CompoundLaneLineStringFeature cpdFeat(LaneLineStringFeatureList{feat1, feat2, feat3, feat4, feat5}, - LineStringType::Solid); - - cpdFeat.process(bbox, ParametrizationType::LineString, 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()[0], boost::geometry::strategy::distance::pythagoras()); - EXPECT_NEAR(cpdFeatLength, cpdFeat.pathLengthsProcessed().back(), 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_ml_converter/test/test_map_instances.cpp b/lanelet2_ml_converter/test/test_map_instances.cpp new file mode 100644 index 00000000..62929b58 --- /dev/null +++ b/lanelet2_ml_converter/test/test_map_instances.cpp @@ -0,0 +1,122 @@ +#include + +#include "lanelet2_ml_converter/MapInstances.h" +#include "test_map.h" + +using namespace lanelet; +using namespace lanelet::ml_converter; +using namespace lanelet::ml_converter::tests; + +TEST_F(MLConverterTest, LaneLineStringInstance) { // NOLINT + BasicLineString3d polyline{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}, + BasicPoint3d{20, 0, 0}}; + LaneLineStringInstance feat(polyline, Id(123), LineStringType::Solid, Ids(1234), false); + + feat.process(bbox, ParametrizationType::LineString, 4); + EXPECT_EQ(feat.cutAndResampledInstance().size(), 1); + EXPECT_EQ(feat.cutAndResampledInstance()[0].size(), 4); + EXPECT_NEAR(feat.cutAndResampledInstance()[0][0].x(), 0, 10e-5); + EXPECT_NEAR(feat.cutAndResampledInstance()[0][1].x(), 5, 10e-5); + EXPECT_NEAR(feat.cutAndResampledInstance()[0][3].x(), 15, 10e-5); + + std::vector vec = feat.computeInstanceVectors(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(), 2); + 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(MLConverterTest, LaneletInstance) { // NOLINT + BasicLineString3d leftBd{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}, + BasicPoint3d{20, 0, 0}}; + LaneLineStringInstancePtr leftBdFeat = + 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}}; + LaneLineStringInstancePtr rightBdFeat = + 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}}; + LaneLineStringInstancePtr centerlineFeat = + std::make_shared(centerline, Id(125), LineStringType::Centerline, Ids(1234), false); + + LaneletInstance llFeat(leftBdFeat, rightBdFeat, centerlineFeat, Id(1234)); + llFeat.setReprType(LaneletRepresentationType::Boundaries); + + llFeat.process(bbox, ParametrizationType::LineString, 4); + EXPECT_EQ(llFeat.centerline()->cutAndResampledInstance()[0].size(), 4); + EXPECT_NEAR(llFeat.centerline()->cutAndResampledInstance()[0][0].x(), 0, 10e-5); + EXPECT_NEAR(llFeat.centerline()->cutAndResampledInstance()[0][1].x(), 5, 10e-5); + EXPECT_NEAR(llFeat.centerline()->cutAndResampledInstance()[0][3].x(), 15, 10e-5); + + std::vector vec = llFeat.computeInstanceVectors(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(MLConverterTest, CompoundLaneLineStringInstance) { // NOLINT + BasicLineString3d p1{BasicPoint3d{-10, 0, 0}, BasicPoint3d{-5, 0, 0}}; + LaneLineStringInstancePtr feat1 = + std::make_shared(p1, Id(123), LineStringType::Solid, Ids(1234), false); + BasicLineString3d p2{BasicPoint3d{-5, 0, 0}, BasicPoint3d{0, 0, 0}}; + LaneLineStringInstancePtr feat2 = + std::make_shared(p2, Id(123), LineStringType::Solid, Ids(1234), false); + BasicLineString3d p3{BasicPoint3d{0, 0, 0}, BasicPoint3d{5, 0, 0}}; + LaneLineStringInstancePtr feat3 = + std::make_shared(p3, Id(123), LineStringType::Solid, Ids(1234), false); + BasicLineString3d p4{BasicPoint3d{5, 0, 0}, BasicPoint3d{10, 0, 0}}; + LaneLineStringInstancePtr feat4 = + std::make_shared(p4, Id(124), LineStringType::Solid, Ids(1234), false); + BasicLineString3d p5{BasicPoint3d{10, 0, 0}, BasicPoint3d{20, 0, 0}}; + LaneLineStringInstancePtr feat5 = + std::make_shared(p5, Id(125), LineStringType::Solid, Ids(1234), false); + + CompoundLaneLineStringInstance cpdFeat(LaneLineStringInstanceList{feat1, feat2, feat3, feat4, feat5}, + LineStringType::Solid); + + cpdFeat.process(bbox, ParametrizationType::LineString, 5); + EXPECT_EQ(cpdFeat.cutAndResampledInstance().size(), 1); + EXPECT_EQ(cpdFeat.cutAndResampledInstance()[0].size(), 5); + EXPECT_NEAR(cpdFeat.cutAndResampledInstance()[0][0].x(), -5, 10e-5); + EXPECT_NEAR(cpdFeat.cutAndResampledInstance()[0][1].x(), 0, 10e-5); + EXPECT_NEAR(cpdFeat.cutAndResampledInstance()[0][3].x(), 10, 10e-5); + + EXPECT_NEAR(cpdFeat.pathLengthsProcessed().front(), 0, 10e-5); + EXPECT_EQ(cpdFeat.processedInstancesValid().front(), false); + double cpdFeatLength = + boost::geometry::length(cpdFeat.cutInstance()[0], boost::geometry::strategy::distance::pythagoras()); + EXPECT_NEAR(cpdFeatLength, cpdFeat.pathLengthsProcessed().back(), 10e-5); + + std::vector vec = cpdFeat.computeInstanceVectors(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(), 2); + 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_ml_converter/test/test_utils.cpp b/lanelet2_ml_converter/test/test_utils.cpp index 3fffdb17..c0f78cdb 100644 --- a/lanelet2_ml_converter/test/test_utils.cpp +++ b/lanelet2_ml_converter/test/test_utils.cpp @@ -19,7 +19,8 @@ TEST_F(MLConverterTest, GetRotatedRect) { // NOLINT } TEST_F(MLConverterTest, ExtractSubmap) { // NOLINT - LaneletSubmapConstPtr submap = extractSubmap(laneletMap, centerBbox, extentLongitudinalBbox, extentLateralBbox); + LaneletSubmapConstPtr submap = + extractSubmap(laneletMap, centerBbox.head(2), extentLongitudinalBbox, extentLateralBbox); EXPECT_TRUE(submap->laneletLayer.exists(2000)); EXPECT_TRUE(submap->laneletLayer.exists(2001)); EXPECT_TRUE(submap->laneletLayer.exists(2002)); diff --git a/lanelet2_python/python_api/ml_converter.cpp b/lanelet2_python/python_api/ml_converter.cpp index 6493fdad..34e309bc 100644 --- a/lanelet2_python/python_api/ml_converter.cpp +++ b/lanelet2_python/python_api/ml_converter.cpp @@ -5,7 +5,7 @@ #include "lanelet2_ml_converter/MapData.h" #include "lanelet2_ml_converter/MapDataInterface.h" -#include "lanelet2_ml_converter/MapFeatures.h" +#include "lanelet2_ml_converter/MapInstances.h" #include "lanelet2_ml_converter/Utils.h" #include "lanelet2_python/internal/converter.h" #include "lanelet2_python/internal/eigen_converter.h" @@ -14,111 +14,119 @@ using namespace boost::python; using namespace lanelet; using namespace lanelet::ml_converter; -class MapFeatureWrap : public MapFeature, public wrapper { +class MapInstanceWrap : public MapInstance, public wrapper { public: - std::vector computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { - return this->get_override("computeFeatureVectors")(onlyPoints, pointsIn2d); + std::vector computeInstanceVectors(bool onlyPoints, bool pointsIn2d) const { + return this->get_override("computeInstanceVectors")(onlyPoints, pointsIn2d); } - bool process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { - return this->get_override("process")(bbox, paramType, nPoints); + bool process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints, double pitch, + double roll) { + return this->get_override("process")(bbox, paramType, nPoints, pitch, roll); } }; -class LineStringFeatureWrap : public LineStringFeature, public wrapper { +class LineStringInstanceWrap : public LineStringInstance, public wrapper { public: - std::vector computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { - return this->get_override("computeFeatureVectors")(onlyPoints, pointsIn2d); + std::vector computeInstanceVectors(bool onlyPoints, bool pointsIn2d) const { + return this->get_override("computeInstanceVectors")(onlyPoints, pointsIn2d); } - bool process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { - return this->get_override("process")(bbox, paramType, nPoints); + bool process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints, double pitch, + double roll) { + return this->get_override("process")(bbox, paramType, nPoints, pitch, roll); } std::vector pointMatrices(bool pointsIn2d) const { return this->get_override("pointMatrices")(pointsIn2d); } }; -class LaneLineStringFeatureWrap : public LaneLineStringFeature, public wrapper { +class LaneLineStringInstanceWrap : public LaneLineStringInstance, public wrapper { public: - LaneLineStringFeatureWrap() {} + LaneLineStringInstanceWrap() {} - LaneLineStringFeatureWrap(const BasicLineString3d &feature, Id mapID, LineStringType type, - const std::vector &laneletID, bool inverted) - : LaneLineStringFeature(feature, mapID, type, laneletID, inverted) {} + LaneLineStringInstanceWrap(const BasicLineString3d &feature, Id mapID, LineStringType type, + const std::vector &laneletID, bool inverted) + : LaneLineStringInstance(feature, mapID, type, laneletID, inverted) {} - std::vector computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { - if (override f = this->get_override("computeFeatureVectors")) return f(onlyPoints, pointsIn2d); - return LaneLineStringFeature::computeFeatureVectors(onlyPoints, pointsIn2d); + std::vector computeInstanceVectors(bool onlyPoints, bool pointsIn2d) const { + if (override f = this->get_override("computeInstanceVectors")) return f(onlyPoints, pointsIn2d); + return LaneLineStringInstance::computeInstanceVectors(onlyPoints, pointsIn2d); } - std::vector default_computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { - return this->LaneLineStringFeature::computeFeatureVectors(onlyPoints, pointsIn2d); + std::vector default_computeInstanceVectors(bool onlyPoints, bool pointsIn2d) const { + return this->LaneLineStringInstance::computeInstanceVectors(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 process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints, double pitch, + double roll) { + if (override f = this->get_override("process")) return f(bbox, paramType, nPoints, pitch, roll); + return LaneLineStringInstance::process(bbox, paramType, nPoints, pitch, roll); } - bool default_process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { - return this->LaneLineStringFeature::process(bbox, paramType, nPoints); + bool default_process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints, double pitch, + double roll) { + return this->LaneLineStringInstance::process(bbox, paramType, nPoints, pitch, roll); } std::vector pointMatrices(bool pointsIn2d) const { if (override f = this->get_override("pointMatrices")) return f(pointsIn2d); - return LaneLineStringFeature::pointMatrices(pointsIn2d); + return LaneLineStringInstance::pointMatrices(pointsIn2d); } std::vector default_pointMatrices(bool pointsIn2d) const { - return this->LaneLineStringFeature::pointMatrices(pointsIn2d); + return this->LaneLineStringInstance::pointMatrices(pointsIn2d); } }; -class CompoundLaneLineStringFeatureWrap : public CompoundLaneLineStringFeature, - public wrapper { +class CompoundLaneLineStringInstanceWrap : public CompoundLaneLineStringInstance, + public wrapper { public: - CompoundLaneLineStringFeatureWrap() {} + CompoundLaneLineStringInstanceWrap() {} - CompoundLaneLineStringFeatureWrap(const LaneLineStringFeatureList &features, LineStringType compoundType) - : CompoundLaneLineStringFeature(features, compoundType) {} + CompoundLaneLineStringInstanceWrap(const LaneLineStringInstanceList &features, LineStringType compoundType) + : CompoundLaneLineStringInstance(features, compoundType) {} - std::vector computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { - if (override f = this->get_override("computeFeatureVectors")) return f(onlyPoints, pointsIn2d); - return CompoundLaneLineStringFeature::computeFeatureVectors(onlyPoints, pointsIn2d); + std::vector computeInstanceVectors(bool onlyPoints, bool pointsIn2d) const { + if (override f = this->get_override("computeInstanceVectors")) return f(onlyPoints, pointsIn2d); + return CompoundLaneLineStringInstance::computeInstanceVectors(onlyPoints, pointsIn2d); } - std::vector default_computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { - return this->CompoundLaneLineStringFeature::computeFeatureVectors(onlyPoints, pointsIn2d); + std::vector default_computeInstanceVectors(bool onlyPoints, bool pointsIn2d) const { + return this->CompoundLaneLineStringInstance::computeInstanceVectors(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 process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints, double pitch, + double roll) { + if (override f = this->get_override("process")) return f(bbox, paramType, nPoints, pitch, roll); + return CompoundLaneLineStringInstance::process(bbox, paramType, nPoints, pitch, roll); } - bool default_process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { - return this->CompoundLaneLineStringFeature::process(bbox, paramType, nPoints); + bool default_process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints, double pitch, + double roll) { + return this->CompoundLaneLineStringInstance::process(bbox, paramType, nPoints, pitch, roll); } std::vector pointMatrices(bool pointsIn2d) const { if (override f = this->get_override("pointMatrices")) return f(pointsIn2d); - return CompoundLaneLineStringFeature::pointMatrices(pointsIn2d); + return CompoundLaneLineStringInstance::pointMatrices(pointsIn2d); } std::vector default_pointMatrices(bool pointsIn2d) const { - return this->CompoundLaneLineStringFeature::pointMatrices(pointsIn2d); + return this->CompoundLaneLineStringInstance::pointMatrices(pointsIn2d); } }; -class LaneletFeatureWrap : public LaneletFeature, public wrapper { +class LaneletInstanceWrap : public LaneletInstance, public wrapper { public: - LaneletFeatureWrap() {} + LaneletInstanceWrap() {} - LaneletFeatureWrap(LaneLineStringFeaturePtr leftBoundary, LaneLineStringFeaturePtr rightBoundary, - LaneLineStringFeaturePtr centerline, Id mapID) - : LaneletFeature(leftBoundary, rightBoundary, centerline, mapID) {} - LaneletFeatureWrap(const ConstLanelet &ll) : LaneletFeature(ll) {} + LaneletInstanceWrap(LaneLineStringInstancePtr leftBoundary, LaneLineStringInstancePtr rightBoundary, + LaneLineStringInstancePtr centerline, Id mapID) + : LaneletInstance(leftBoundary, rightBoundary, centerline, mapID) {} + LaneletInstanceWrap(const ConstLanelet &ll) : LaneletInstance(ll) {} - std::vector computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { - if (override f = this->get_override("computeFeatureVectors")) return f(onlyPoints, pointsIn2d); - return LaneletFeature::computeFeatureVectors(onlyPoints, pointsIn2d); + std::vector computeInstanceVectors(bool onlyPoints, bool pointsIn2d) const { + if (override f = this->get_override("computeInstanceVectors")) return f(onlyPoints, pointsIn2d); + return LaneletInstance::computeInstanceVectors(onlyPoints, pointsIn2d); } - std::vector default_computeFeatureVectors(bool onlyPoints, bool pointsIn2d) const { - return this->LaneletFeature::computeFeatureVectors(onlyPoints, pointsIn2d); + std::vector default_computeInstanceVectors(bool onlyPoints, bool pointsIn2d) const { + return this->LaneletInstance::computeInstanceVectors(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 process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints, double pitch = 0, + double roll = 0) { + if (override f = this->get_override("process")) return f(bbox, paramType, nPoints, pitch, roll); + return LaneletInstance::process(bbox, paramType, nPoints, pitch, roll); } - bool default_process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints) { - return this->LaneletFeature::process(bbox, paramType, nPoints); + bool default_process(const OrientedRect &bbox, const ParametrizationType ¶mType, int32_t nPoints, + double pitch = 0, double roll = 0) { + return this->LaneletInstance::process(bbox, paramType, nPoints, pitch, roll); } }; @@ -174,86 +182,97 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT class_("OrientedRect", "Oriented rectangle for feature crop area", no_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); - def("saveLaneDataMultiFile", &saveLaneDataMultiFile); - def("loadLaneDataMultiFile", &loadLaneDataMultiFile); - - 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("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("computeFeatureVectors", pure_virtual(&LineStringFeature::computeFeatureVectors)) - .def("process", pure_virtual(&LineStringFeature::process)) - .def("pointMatrices", pure_virtual(&LineStringFeature::pointMatrices)); - - class_, LaneLineStringFeaturePtr, boost::noncopyable>( - "LaneLineStringFeature", "Lane line string feature class", + def("getRotatedRect", &getRotatedRect, + (arg("center"), arg("extentLongitudinal"), arg("extentLateral"), arg("yaw"), arg("from2dPos"))); + def("extractSubmap", &extractSubmap, + (arg("laneletMap"), arg("center"), arg("extentLongitudinal"), arg("extentLateral"))); + def("isRoadBorder", &isRoadBorder, (arg("lstring"))); + def("bdTypeToEnum", &bdTypeToEnum, (arg("lstring"))); + def("teTypeToEnum", &teTypeToEnum, (arg("te"))); + def("resampleLineString", &resampleLineString, (arg("polyline"), arg("nPoints"))); + def("cutLineString", &cutLineString, (arg("bbox"), arg("polyline"))); + def("transformLineString", &transformLineString, (arg("bbox"), arg("polyline"), arg("pitch"), arg("roll"))); + def("saveLaneData", &saveLaneData, (arg("filename"), arg("lDataVec"), arg("binary"))); + def("loadLaneData", &loadLaneData, (arg("filename"), arg("binary"))); + def("saveLaneDataMultiFile", &saveLaneDataMultiFile, (arg("path"), arg("filenames"), arg("lDataVec"), arg("binary"))); + def("loadLaneDataMultiFile", &loadLaneDataMultiFile, (arg("path"), arg("filenames"), arg("binary"))); + + class_("MapInstance", "Abstract base map feature class", no_init) + .add_property("wasCut", &MapInstance::wasCut) + .add_property("mapID", &MapInstance::mapID) + .add_property("initialized", &MapInstance::initialized) + .add_property("valid", &MapInstance::valid) + .def("computeInstanceVectors", pure_virtual(&MapInstance::computeInstanceVectors), + (arg("onlyPoints"), arg("pointsIn2d"))) + .def("process", pure_virtual(&MapInstance::process), + (arg("bbox"), arg("paramType"), arg("nPoints"), arg("pitch") = 0, arg("roll") = 0)); + + class_, boost::noncopyable>("LineStringInstance", + "Abstract line string feature class", no_init) + .add_property("rawInstance", + make_function(&LineStringInstance::rawInstance, return_value_policy())) + .def("computeInstanceVectors", pure_virtual(&LineStringInstance::computeInstanceVectors), + (arg("onlyPoints"), arg("pointsIn2d"))) + .def("process", pure_virtual(&LineStringInstance::process), + (arg("bbox"), arg("paramType"), arg("nPoints"), arg("pitch") = 0, arg("roll") = 0)) + .def("pointMatrices", pure_virtual(&LineStringInstance::pointMatrices), (arg("pointsIn2d"))); + + class_, LaneLineStringInstancePtr, boost::noncopyable>( + "LaneLineStringInstance", "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("cutResampledAndTransformedFeature", - make_function(&LaneLineStringFeature::cutResampledAndTransformedFeature, + .add_property("cutInstance", + make_function(&LaneLineStringInstance::cutInstance, return_value_policy())) + .add_property("cutAndResampledInstance", make_function(&LaneLineStringInstance::cutAndResampledInstance, + return_value_policy())) + .add_property("cutResampledAndTransformedInstance", + make_function(&LaneLineStringInstance::cutResampledAndTransformedInstance, return_value_policy())) - .add_property("type", &LaneLineStringFeature::type) - .add_property("inverted", &LaneLineStringFeature::inverted) - .add_property("typeInt", &LaneLineStringFeature::typeInt) + .add_property("type", &LaneLineStringInstance::type) + .add_property("inverted", &LaneLineStringInstance::inverted) + .add_property("typeInt", &LaneLineStringInstance::typeInt) .add_property("laneletIDs", - make_function(&LaneLineStringFeature::laneletIDs, return_value_policy())) - .add_property("addLaneletID", &LaneLineStringFeature::addLaneletID) - .def("computeFeatureVectors", &LaneLineStringFeature::computeFeatureVectors, - &LaneLineStringFeatureWrap::default_computeFeatureVectors) - .def("process", &LaneLineStringFeature::process, &LaneLineStringFeatureWrap::default_process) - .def("pointMatrices", &LaneLineStringFeature::pointMatrices, &LaneLineStringFeatureWrap::default_pointMatrices); - - class_, LaneletFeaturePtr, boost::noncopyable>( - "LaneletFeature", "Lanelet feature class that contains lower level LaneLineStringFeatures", - init()) + make_function(&LaneLineStringInstance::laneletIDs, return_value_policy())) + .def("addLaneletID", &LaneLineStringInstance::addLaneletID, (arg("id"))) + .def("computeInstanceVectors", &LaneLineStringInstance::computeInstanceVectors, + &LaneLineStringInstanceWrap::default_computeInstanceVectors, (arg("onlyPoints"), arg("pointsIn2d"))) + .def("process", &LaneLineStringInstance::process, &LaneLineStringInstanceWrap::default_process, + (arg("bbox"), arg("paramType"), arg("nPoints"), arg("pitch") = 0, arg("roll") = 0)) + .def("pointMatrices", &LaneLineStringInstance::pointMatrices, &LaneLineStringInstanceWrap::default_pointMatrices, + (arg("pointsIn2d"))); + + class_, LaneletInstancePtr, boost::noncopyable>( + "LaneletInstance", "Lanelet feature class that contains lower level LaneLineStringInstances", + init()) .def(init()) .def(init<>()) - .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("computeFeatureVectors", &LaneletFeature::computeFeatureVectors, - &LaneletFeatureWrap::default_computeFeatureVectors) - .def("process", &LaneletFeature::process, &LaneletFeatureWrap::default_process); - - class_, CompoundLaneLineStringFeaturePtr, - boost::noncopyable>("CompoundLaneLineStringFeature", + .add_property("leftBoundary", make_function(&LaneletInstance::leftBoundary)) + .add_property("rightBoundary", make_function(&LaneletInstance::rightBoundary)) + .add_property("centerline", make_function(&LaneletInstance::centerline)) + .def("setReprType", &LaneletInstance::setReprType, (arg("reprType"))) + .def("computeInstanceVectors", &LaneletInstance::computeInstanceVectors, + &LaneletInstanceWrap::default_computeInstanceVectors, (arg("onlyPoints"), arg("pointsIn2d"))) + .def("process", &LaneletInstance::process, &LaneletInstanceWrap::default_process, + (arg("bbox"), arg("paramType"), arg("nPoints"), arg("pitch") = 0, arg("roll") = 0)); + + class_, CompoundLaneLineStringInstancePtr, + boost::noncopyable>("CompoundLaneLineStringInstance", "Compound lane line string feature class that can trace back the individual features", - init()) + init()) .def(init<>()) - .add_property("features", make_function(&CompoundLaneLineStringFeature::features)) - .add_property("pathLengthsRaw", make_function(&CompoundLaneLineStringFeature::pathLengthsRaw, + .add_property("features", make_function(&CompoundLaneLineStringInstance::features)) + .add_property("pathLengthsRaw", make_function(&CompoundLaneLineStringInstance::pathLengthsRaw, return_value_policy())) - .add_property("pathLengthsProcessed", make_function(&CompoundLaneLineStringFeature::pathLengthsProcessed, + .add_property("pathLengthsProcessed", make_function(&CompoundLaneLineStringInstance::pathLengthsProcessed, return_value_policy())) - .add_property("processedFeaturesValid", make_function(&CompoundLaneLineStringFeature::processedFeaturesValid, - return_value_policy())) - .def("computeFeatureVectors", &CompoundLaneLineStringFeature::computeFeatureVectors, - &CompoundLaneLineStringFeatureWrap::default_computeFeatureVectors) - .def("process", &CompoundLaneLineStringFeature::process, &CompoundLaneLineStringFeatureWrap::default_process) - .def("pointMatrices", &CompoundLaneLineStringFeature::pointMatrices, - &CompoundLaneLineStringFeatureWrap::default_pointMatrices); + .add_property("processedInstancesValid", make_function(&CompoundLaneLineStringInstance::processedInstancesValid, + return_value_policy())) + .def("computeInstanceVectors", &CompoundLaneLineStringInstance::computeInstanceVectors, + &CompoundLaneLineStringInstanceWrap::default_computeInstanceVectors, (arg("onlyPoints"), arg("pointsIn2d"))) + .def("process", &CompoundLaneLineStringInstance::process, &CompoundLaneLineStringInstanceWrap::default_process, + (arg("bbox"), arg("paramType"), arg("nPoints"), arg("pitch") = 0, arg("roll") = 0)) + .def("pointMatrices", &CompoundLaneLineStringInstance::pointMatrices, + &CompoundLaneLineStringInstanceWrap::default_pointMatrices, (arg("pointsIn2d"))); class_("Edge", "Struct of a lane graph edge", init()) .def(init<>()) @@ -283,49 +302,69 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .add_property("validCompoundRoadBorders", &LaneData::validCompoundRoadBorders) .add_property("validCompoundLaneDividers", &LaneData::validCompoundLaneDividers) .add_property("validCompoundCenterlines", &LaneData::validCompoundCenterlines) - .def("associatedCpdRoadBorders", &LaneData::associatedCpdRoadBorders) - .def("associatedCpdLaneDividers", &LaneData::associatedCpdLaneDividers) - .def("associatedCpdCenterlines", &LaneData::associatedCpdCenterlines) - .add_property("laneletFeatures", - make_function(&LaneData::laneletFeatures, return_value_policy())) + .def("associatedCpdRoadBorders", &LaneData::associatedCpdRoadBorders, (arg("mapId"))) + .def("associatedCpdLaneDividers", &LaneData::associatedCpdLaneDividers, (arg("mapId"))) + .def("associatedCpdCenterlines", &LaneData::associatedCpdCenterlines, (arg("mapId"))) + .add_property("laneletInstances", + make_function(&LaneData::laneletInstances, 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); + .def("getTensorInstanceData", &LaneData::getTensorInstanceData, (arg("pointsIn2d"), arg("ignoreBuffer"))); - class_("TensorFeatureData", "TensorFeatureData class for LaneData", init<>()) - .add_property("roadBorders", make_function(&LaneData::TensorFeatureData::roadBorders, + class_("TensorInstanceData", "TensorInstanceData class for LaneData", init<>()) + .add_property("roadBorders", make_function(&LaneData::TensorInstanceData::roadBorders, return_value_policy())) - .add_property("laneDividers", make_function(&LaneData::TensorFeatureData::laneDividers, + .add_property("laneDividers", make_function(&LaneData::TensorInstanceData::laneDividers, return_value_policy())) - .add_property("laneDividerTypes", make_function(&LaneData::TensorFeatureData::laneDividerTypes, + .add_property("laneDividerTypes", make_function(&LaneData::TensorInstanceData::laneDividerTypes, return_value_policy())) - .add_property("compoundRoadBorders", make_function(&LaneData::TensorFeatureData::compoundRoadBorders, + .add_property("compoundRoadBorders", make_function(&LaneData::TensorInstanceData::compoundRoadBorders, return_value_policy())) - .add_property("compoundLaneDividers", make_function(&LaneData::TensorFeatureData::compoundLaneDividers, + .add_property("compoundLaneDividers", make_function(&LaneData::TensorInstanceData::compoundLaneDividers, return_value_policy())) - .add_property("compoundLaneDividerTypes", make_function(&LaneData::TensorFeatureData::compoundLaneDividerTypes, + .add_property("compoundLaneDividerTypes", make_function(&LaneData::TensorInstanceData::compoundLaneDividerTypes, return_value_policy())) - .add_property("compoundCenterlines", make_function(&LaneData::TensorFeatureData::compoundCenterlines, + .add_property("compoundCenterlines", make_function(&LaneData::TensorInstanceData::compoundCenterlines, return_value_policy())) .add_property("uuid", - make_function(&LaneData::TensorFeatureData::uuid, return_value_policy())) - .def("pointMatrixCpdRoadBorder", &LaneData::TensorFeatureData::pointMatrixCpdRoadBorder) - .def("pointMatrixCpdLaneDivider", &LaneData::TensorFeatureData::pointMatrixCpdLaneDivider) - .def("pointMatrixCpdCenterline", &LaneData::TensorFeatureData::pointMatrixCpdCenterline); + make_function(&LaneData::TensorInstanceData::uuid, return_value_policy())) + .def("pointMatrixCpdRoadBorder", &LaneData::TensorInstanceData::pointMatrixCpdRoadBorder, (arg("index"))) + .def("pointMatrixCpdLaneDivider", &LaneData::TensorInstanceData::pointMatrixCpdLaneDivider, (arg("index"))) + .def("pointMatrixCpdCenterline", &LaneData::TensorInstanceData::pointMatrixCpdCenterline, (arg("index"))); } { + void (MapDataInterface::*setCurrPosAndExtractSubmap2d)(const lanelet::BasicPoint2d &, double) = + &MapDataInterface::setCurrPosAndExtractSubmap2d; + void (MapDataInterface::*setCurrPosAndExtractSubmap4d)(const lanelet::BasicPoint3d &, double) = + &MapDataInterface::setCurrPosAndExtractSubmap; + void (MapDataInterface::*setCurrPosAndExtractSubmap6d)(const lanelet::BasicPoint3d &, double, double, double) = + &MapDataInterface::setCurrPosAndExtractSubmap; + + std::vector (MapDataInterface::*laneDataBatch2d)(std::vector, std::vector) = + &MapDataInterface::laneDataBatch2d; + std::vector (MapDataInterface::*laneDataBatch4d)(std::vector, std::vector) = + &MapDataInterface::laneDataBatch; + std::vector (MapDataInterface::*laneDataBatch6d)(std::vector, std::vector, + std::vector, std::vector) = + &MapDataInterface::laneDataBatch; 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); + .def("setCurrPosAndExtractSubmap2d", setCurrPosAndExtractSubmap2d, (arg("pt"), arg("yaw"))) + .def("setCurrPosAndExtractSubmap", setCurrPosAndExtractSubmap4d, (arg("pt"), arg("yaw"))) + .def("setCurrPosAndExtractSubmap", setCurrPosAndExtractSubmap6d, + (arg("pt"), arg("yaw"), arg("pitch"), arg("roll"))) + .def("laneData", &MapDataInterface::laneData, (arg("processAll"))) + .def("teData", &MapDataInterface::teData, (arg("processAll"))) + .def("laneDataBatch2d", laneDataBatch2d, (arg("pts"), arg("yaws"))) + .def("laneDataBatch", laneDataBatch4d, (arg("pts"), arg("yaws"))) + .def("laneDataBatch", laneDataBatch6d, (arg("pts"), arg("yaws"), arg("pitches"), arg("rolls"))) + .def("laneTEDataBatch", &MapDataInterface::laneTEDataBatch, + (arg("pts"), arg("yaws"), arg("pitches"), arg("rolls"))); class_("Configuration", "Configuration class for MapDataInterface", init<>()) .def(init()) @@ -342,8 +381,8 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT converters::VectorToListConverter>(); converters::VectorToListConverter(); - converters::VectorToListConverter(); - converters::VectorToListConverter(); + converters::VectorToListConverter(); + converters::VectorToListConverter(); converters::VectorToListConverter< boost::geometry::model::ring>(); converters::VectorToListConverter>(); @@ -358,16 +397,16 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) { // NOLINT .fromPython>() .fromPython>() .fromPython>() - .fromPython() - .fromPython(); - converters::MapToDictConverter(); - converters::MapToDictConverter(); + .fromPython() + .fromPython(); + converters::MapToDictConverter(); + converters::MapToDictConverter(); converters::MapToDictConverter(); - DictToMapConverter(); - DictToMapConverter(); + DictToMapConverter(); + DictToMapConverter(); class_>("BoolList").def(vector_indexing_suite>()); - def("toPointMatrix", &toPointMatrix); + def("toPointMatrix", &toPointMatrix, (arg("lString"), arg("pointsIn2d"))); implicitly_convertible(); } diff --git a/lanelet2_python/test/test_lanelet2_ml_converter.py b/lanelet2_python/test/test_lanelet2_ml_converter.py index e9aac30f..dc0af324 100644 --- a/lanelet2_python/test/test_lanelet2_ml_converter.py +++ b/lanelet2_python/test/test_lanelet2_ml_converter.py @@ -22,9 +22,9 @@ def test_map_data_interface(self): mymap = get_sample_lanelet_map() pos = BasicPoint2d(1, 1) mDataIf = MapDataInterface(mymap) - mDataIf.setCurrPosAndExtractSubmap(pos, 0) + mDataIf.setCurrPosAndExtractSubmap2d(pos, 0) lData = mDataIf.laneData(True) - tfData = lData.getTensorFeatureData(True, False) + tfData = lData.getTensorInstanceData(True, False) self.assertEqual(len(tfData.compoundCenterlines), 2) self.assertEqual(tfData.compoundLaneDividerTypes[-1], 1)