ml_converter 6d pose support, naming convention and bugfixes

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; \

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
 

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! 
+- **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! 

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 <class Archive>
-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 <class Archive>
-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 <class Archive>
-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 <class Archive>
-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 <class Archive>
-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 <class Archive>
 void serialize(Archive& ar, lanelet::ml_converter::LaneData& feat, const unsigned int /*version*/);

lanelet2_ml_converter/include/lanelet2_ml_converter/MapData.h

@@ -11,7 +11,7 @@
 #include <type_traits>
 
 #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<Id, std::shared_ptr<T>> getValidElements(const std::map<Id, std::shared
 
 class LaneData {
  public:
-  struct TensorFeatureData {
+  struct TensorInstanceData {
    public:
     const std::vector<MatrixXd>& roadBorders() { return roadBorders_; }
     const std::vector<MatrixXd>& laneDividers() { return laneDividers_; }
@@ -70,11 +70,11 @@ class LaneData {
     const std::vector<int>& compoundLaneDividerTypes() { return compoundLaneDividerTypes_; }
     const std::vector<MatrixXd>& 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<int> compoundLaneDividerTypes_;
     std::vector<MatrixXd> compoundCenterlines_;
     std::string uuid_;
-    std::map<size_t, CompoundLaneLineStringFeaturePtr>
+    std::map<size_t, CompoundLaneLineStringInstancePtr>
         pointMatrixCpdRoadBorder_;  // relates point matrix index to compound features
-    std::map<size_t, CompoundLaneLineStringFeaturePtr>
+    std::map<size_t, CompoundLaneLineStringInstancePtr>
         pointMatrixCpdLaneDivider_;  // relates point matrix index to compound features, tfData_ must be initialized
-    std::map<size_t, CompoundLaneLineStringFeaturePtr>
+    std::map<size_t, CompoundLaneLineStringInstancePtr>
         pointMatrixCpdCenterline_;  // relates point matrix index to compound features, tfData_ must be initialized
   };
 
   LaneData() noexcept : uuid_{boost::lexical_cast<std::string>(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 <class Archive>
   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<ConstLanelets>& paths,
                 ConstLanelet start, ConstLanelets initPath = ConstLanelets());
 
   LineStringType getLineStringTypeFromId(Id id);
-  LaneLineStringFeaturePtr getLineStringFeatFromId(Id id, bool inverted);
+  LaneLineStringInstancePtr getLineStringFeatFromId(Id id, bool inverted);
   std::vector<internal::CompoundElsList> computeCompoundLeftBorders(const ConstLanelets& path);
   std::vector<internal::CompoundElsList> 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<Id, std::vector<size_t>>
       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<TensorFeatureData> tfData_;
+  Optional<TensorInstanceData> 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