add map/lanelet2.

src/map/lanelet2/include/lanelet2_core/Attribute.h

@@ -0,0 +1,379 @@
+// this is for emacs file handling -*- mode: c++; c-basic-offset: 2;
+// indent-tabs-mode: nil -*-
+
+#pragma once
+#include <boost/variant/variant.hpp>
+#include <memory>
+#include <string>
+
+#include "lanelet2_core/Forward.h"
+#include "lanelet2_core/utility/HybridMap.h"
+#include "lanelet2_core/utility/Optional.h"
+
+namespace lanelet {
+namespace internal {
+template <typename ValueT>
+struct ValueOf {
+  using Type = std::decay_t<ValueT>;
+};
+template <typename ValueT>
+struct ValueOf<Optional<ValueT>> {
+  using Type = ValueT;
+};
+}  // namespace internal
+   /**
+    * @brief An attribute represents one value of a tag of a lanelet primitive.
+    *
+    * They are internally represented as strings but can be interpreted as numbers.
+    *
+    * The result of the last as... is cached, therefore repeated calls to the same
+    * as.. function are very cheap.
+    */
+class Attribute {
+  template <typename T>
+  using ValueOfT = typename internal::ValueOf<T>::Type;
+
+ public:
+  using Cache = boost::variant<bool, double, Id, int, Velocity>;
+
+  Attribute() = default;
+  Attribute(const std::string& value) : value_{value} {}        // NOLINT
+  Attribute(std::string&& value) : value_{std::move(value)} {}  // NOLINT
+  Attribute(const char* value) : value_(value) {}               // NOLINT
+  Attribute(bool value);                                        // NOLINT
+  Attribute(Id value);                                          // NOLINT
+  Attribute(int value);                                         // NOLINT
+  Attribute(double value);                                      // NOLINT
+  Attribute(const Velocity& value);                             // NOLINT
+
+  /**
+   * @brief interpret this attribute as bool value
+   * @throw ConversionError if the attribute could not be interpreted as bool
+   * @return bool value
+   */
+  Optional<bool> asBool() const;
+
+  /**
+   * @brief interpret this attribute as double value
+   * @return double if possible to convert the value
+   */
+  Optional<double> asDouble() const;
+
+  /**
+   * @brief interpret this attribute as an id
+   * @return id if possible to convert the value
+   */
+  Optional<Id> asId() const;
+
+  /**
+   * @brief interpret this attribute as an int
+   * @return int if possible to convert the value
+   */
+  Optional<int> asInt() const;
+
+  /**
+   * @brief interpret this attribute as Velocity
+   * @return Velocity if possible to convert the value
+   *
+   * The function can understand and correctly parse units in the attribute. "2.
+   * km/h", "2.mps", etc. To obtain the velocity as a double, divide it by the
+   * desired unit:
+   *
+   * double v = attr.asVelocity() / KMH;
+   */
+  Optional<Velocity> asVelocity() const;
+
+  /**
+   * @brief templated version. Works ony for the as.. above
+   */
+  template <typename T>
+  Optional<ValueOfT<T>> as() const;
+
+  /**
+   * @brief gets the value of this attribute
+   * @return value
+   */
+  const std::string& value() const { return value_; }
+
+  /**
+   * @brief set the value of this attribute
+   * @param value new value
+   */
+  void setValue(const std::string& value);
+
+ private:
+  std::string value_;                     //!< internal value of this parameter
+  mutable std::shared_ptr<Cache> cache_;  //!< cache for the last queried value
+};
+
+template <>
+inline Optional<double> Attribute::as<double>() const {
+  return asDouble();
+}
+
+template <>
+inline Optional<double> Attribute::as<Optional<double>>() const {
+  return asDouble();
+}
+
+template <>
+inline Optional<int> Attribute::as<int>() const {
+  return asInt();
+}
+
+template <>
+inline Optional<int> Attribute::as<Optional<int>>() const {
+  return asInt();
+}
+
+template <>
+inline Optional<bool> Attribute::as<bool>() const {
+  return asBool();
+}
+
+template <>
+inline Optional<bool> Attribute::as<Optional<bool>>() const {
+  return asBool();
+}
+
+template <>
+inline Optional<Id> Attribute::as<Id>() const {
+  return asId();
+}
+
+template <>
+inline Optional<Id> Attribute::as<Optional<Id>>() const {
+  return asId();
+}
+
+template <>
+inline Optional<Velocity> Attribute::as<Velocity>() const {
+  return asVelocity();
+}
+
+template <>
+inline Optional<Velocity> Attribute::as<Optional<Velocity>>() const {
+  return asVelocity();
+}
+
+template <>
+inline Optional<std::string> Attribute::as<std::string>() const {
+  return value();
+}
+
+template <>
+inline Optional<std::string> Attribute::as<Optional<std::string>>() const {
+  return value();
+}
+
+template <>
+inline Optional<const char*> Attribute::as<const char*>() const {
+  return value().c_str();
+}
+
+template <>
+inline Optional<const char*> Attribute::as<Optional<const char*>>() const {
+  return value().c_str();
+}
+
+inline bool operator==(const Attribute& lhs, const Attribute& rhs) { return lhs.value() == rhs.value(); }
+inline bool operator!=(const Attribute& lhs, const Attribute& rhs) { return !(lhs == rhs); }
+
+/**
+ * @brief Typical Attributes names within lanelet
+ */
+enum class AttributeName {
+  Type,
+  Subtype,
+  OneWay,
+  ParticipantVehicle,
+  ParticipantPedestrian,
+  SpeedLimit,
+  Location,
+  Dynamic
+};
+
+using AttributeNamesItem = std::pair<const char*, const AttributeName>;
+
+/**
+ * @brief Lists which attribute strings are mapped to which enum value
+ *
+ * Needs to be in a class because we want external linkage
+ */
+struct AttributeNamesString {
+  static constexpr const char Type[] = "type";
+  static constexpr const char Subtype[] = "subtype";
+  static constexpr const char OneWay[] = "one_way";
+  static constexpr const char ParticipantVehicle[] = "participant:vehicle";
+  static constexpr const char ParticipantPedestrian[] = "participant:pedestrian";
+  static constexpr const char SpeedLimit[] = "speed_limit";
+  static constexpr const char Location[] = "location";
+  static constexpr const char Dynamic[] = "dynamic";
+  static constexpr const char Color[] = "color";
+
+  // attributes not used in fast lookup
+  // on points
+  static constexpr const char Ele[] = "ele";
+
+  // on linestrings
+  static constexpr const char LaneChange[] = "lane_change";
+  static constexpr const char LaneChangeLeft[] = "lane_change:left";
+  static constexpr const char LaneChangeRight[] = "lane_change:right";
+  static constexpr const char Name[] = "name";
+  static constexpr const char Region[] = "region";
+
+  // on lanelets/areas
+  static constexpr const char SpeedLimitMandatory[] = "speed_limit_mandatory";
+  static constexpr const char Participant[] = "participant";
+  static constexpr const char Area[] = "area";
+  static constexpr const char Fallback[] = "fallback";
+  static constexpr const char Width[] = "width";
+  static constexpr const char Height[] = "height";
+  static constexpr const char Temporary[] = "temporary";
+
+  // on regulatory elements
+  static constexpr const char SignType[] = "sign_type";
+
+  static constexpr AttributeNamesItem Map[] = {{Type, AttributeName::Type},
+                                               {Subtype, AttributeName::Subtype},
+                                               {OneWay, AttributeName::OneWay},
+                                               {ParticipantVehicle, AttributeName::ParticipantVehicle},
+                                               {ParticipantPedestrian, AttributeName::ParticipantPedestrian},
+                                               {SpeedLimit, AttributeName::SpeedLimit},
+                                               {Location, AttributeName::Location},
+                                               {Dynamic, AttributeName::Dynamic}};
+};
+
+//! parts of tag that have to be combined with either Participants:, OneWay: or SpeedLimit to generate an override.
+struct Participants {
+  //! Obtain the tag for the participant override
+  static std::string tag(const std::string& participant) {
+    return AttributeNamesString::Participant + (":" + participant);
+  }
+  static constexpr const char Vehicle[] = "vehicle";
+  static constexpr const char VehicleBus[] = "vehicle:bus";
+  static constexpr const char VehicleCar[] = "vehicle:car";
+  static constexpr const char VehicleCarElectric[] = "vehicle:car:electric";
+  static constexpr const char VehicleCarCombustion[] = "vehicle:car:combustion";
+  static constexpr const char VehicleTruck[] = "vehicle:truck";
+  static constexpr const char VehicleMotorcycle[] = "vehicle:motorcycle";
+  static constexpr const char VehicleTaxi[] = "vehicle:taxi";
+  static constexpr const char VehicleEmergency[] = "vehicle:emergency";
+  static constexpr const char Pedestrian[] = "pedestrian";
+  static constexpr const char Bicycle[] = "bicycle";
+  static constexpr const char Train[] = "train";
+};
+/**
+ * @brief Common values for attributes are defined here.
+ *
+ * This is for convenience when comparing attribute values
+ */
+struct AttributeValueString {
+  // lanelet types
+  static constexpr const char Node[] = "node";
+  static constexpr const char Way[] = "way";
+  static constexpr const char Relation[] = "relation";
+  static constexpr const char Lanelet[] = "lanelet";
+  static constexpr const char RegulatoryElement[] = "regulatory_element";
+  static constexpr const char Multipolygon[] = "multipolygon";
+
+  // line types
+  static constexpr const char LineThick[] = "line_thick";
+  static constexpr const char LineThin[] = "line_thin";
+  static constexpr const char Curbstone[] = "curbstone";
+  static constexpr const char GuardRail[] = "guard_rail";
+  static constexpr const char RoadBorder[] = "road_border";
+  static constexpr const char Wall[] = "wall";
+  static constexpr const char Fence[] = "fence";
+  static constexpr const char Zebra[] = "zebra_marking";
+  static constexpr const char PedestrianMarking[] = "pedestrian_marking";
+  static constexpr const char BikeMarking[] = "bike_marking";
+  static constexpr const char Keepout[] = "keepout";
+  static constexpr const char StopLine[] = "stop_line";
+  static constexpr const char Virtual[] = "virtual";
+  static constexpr const char Visualization[] = "visualization";
+  static constexpr const char ZigZag[] = "zig-zag";
+  static constexpr const char LiftGate[] = "lift_gate";
+  static constexpr const char JerseyBarrier[] = "jersey_barrier";
+  static constexpr const char Gate[] = "gate";
+  static constexpr const char Door[] = "door";
+  static constexpr const char Trajectory[] = "trajectory";
+  static constexpr const char Rail[] = "rail";
+  static constexpr const char Bump[] = "bump";
+
+  // line subtypes
+  static constexpr const char Solid[] = "solid";
+  static constexpr const char Dashed[] = "dashed";
+  static constexpr const char DashedSolid[] = "dashed_solid";
+  static constexpr const char SolidDashed[] = "solid_dashed";
+  static constexpr const char SolidSolid[] = "solid_solid";
+  static constexpr const char Straight[] = "straight";
+  static constexpr const char Left[] = "left";
+  static constexpr const char Right[] = "right";
+  static constexpr const char StraightLeft[] = "straight_left";
+  static constexpr const char StraightRight[] = "straight_right";
+  static constexpr const char LeftRight[] = "left_right";
+  static constexpr const char High[] = "high";
+  static constexpr const char Low[] = "low";
+
+  // Node types
+  static constexpr const char Arrow[] = "arrow";
+  static constexpr const char Pole[] = "pole";
+  static constexpr const char Post[] = "post";
+  static constexpr const char Symbol[] = "symbol";
+  static constexpr const char Start[] = "start";
+  static constexpr const char End[] = "end";
+  static constexpr const char Dot[] = "dot";
+
+  // Color / traffic light types
+  static constexpr const char RedYellowGreen[] = "red_yellow_green";
+  static constexpr const char RedGreen[] = "red_green";
+  static constexpr const char RedYellow[] = "red_yellow";
+  static constexpr const char Red[] = "red";
+  static constexpr const char Yellow[] = "yellow";
+  static constexpr const char White[] = "white";
+
+  // Lanelet types
+  static constexpr const char Road[] = "road";
+  static constexpr const char Highway[] = "highway";
+  static constexpr const char PlayStreet[] = "play_street";
+  static constexpr const char EmergencyLane[] = "emergency_lane";
+  static constexpr const char BusLane[] = "bus_lane";
+  static constexpr const char BicycleLane[] = "bicycle_lane";
+  static constexpr const char Walkway[] = "walkway";
+  static constexpr const char SharedWalkway[] = "shared_walkway";
+  static constexpr const char Crosswalk[] = "crosswalk";
+  static constexpr const char Stairs[] = "stairs";
+
+  // Lanelet location tag
+  static constexpr const char Nonurban[] = "nonurban";
+  static constexpr const char Urban[] = "urban";
+  static constexpr const char Private[] = "private";
+
+  // Area types
+  static constexpr const char Parking[] = "parking";
+  static constexpr const char Freespace[] = "freespace";
+  static constexpr const char Vegetation[] = "vegetation";
+  static constexpr const char Building[] = "building";
+  static constexpr const char TrafficIsland[] = "traffic_island";
+  static constexpr const char Exit[] = "exit";
+
+  // Regulatory elements
+  static constexpr const char TrafficLight[] = "traffic_light";
+  static constexpr const char TrafficSign[] = "traffic_sign";
+  static constexpr const char SpeedLimit[] = "speed_limit";
+  static constexpr const char RightOfWay[] = "right_of_way";
+  static constexpr const char AllWayStop[] = "all_way_stop";
+};
+
+inline std::ostream& operator<<(std::ostream& stream, const Attribute& obj) { return stream << obj.value(); }
+
+using AttributeMap = HybridMap<Attribute, decltype(AttributeNamesString::Map)&, AttributeNamesString::Map>;
+
+inline std::ostream& operator<<(std::ostream& stream, const AttributeMap& obj) {
+  for (const auto& o : obj) {
+    stream << o.first << ": " << o.second << " ";
+  }
+  return stream;
+}
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/Exceptions.h

@@ -0,0 +1,84 @@
+#pragma once
+#include <stdexcept>
+#include <vector>
+
+namespace lanelet {
+
+/**
+ * @brief Generic lanelet error class.
+ * All errors lanelet2 will throw derive from this type
+ */
+class LaneletError : public std::runtime_error {
+  using std::runtime_error::runtime_error;
+};
+
+/**
+ * @brief Thrown when multiple errors occur at the same time
+ *
+ * Construction is not exception-save, don't throw this on memory errors!
+ */
+class LaneletMultiError : public LaneletError {
+ public:
+  using ErrorMessages = std::vector<std::string>;
+  explicit LaneletMultiError(const std::string& err) : LaneletError(err), errorMessages{{err}} {}
+  explicit LaneletMultiError(ErrorMessages messages = {})
+      : LaneletError(combineErrors(messages)), errorMessages(std::move(messages)) {}
+  const ErrorMessages errorMessages;  //!< The individual error strings
+
+ private:
+  static std::string combineErrors(const ErrorMessages& m) {
+    std::string result;
+    for (const auto& message : m) {
+      result += message;
+      result += '\n';
+    }
+    return result;
+  }
+};
+
+/**
+ * @brief Thrown when an attribute has been queried that does not exist.
+ */
+class NoSuchAttributeError : public LaneletError {
+  using LaneletError::LaneletError;
+};
+
+/**
+ * @brief Thrown when an element is not part of the map
+ */
+class NoSuchPrimitiveError : public LaneletError {
+  using LaneletError::LaneletError;
+};
+
+/**
+ * @brief Thrown when a function was called with invalid input arguments.
+ */
+class InvalidInputError : public LaneletError {
+  using LaneletError::LaneletError;
+};
+
+/**
+ * @brief Thrown when a geometric operation is not valid.
+ */
+class GeometryError : public LaneletError {
+  using LaneletError::LaneletError;
+};
+
+/**
+ * @brief Thrown when the state of a lanelet object is invalid
+ * E.g. when an linestring has no points or member pointers are NULL
+ */
+class InvalidObjectStateError : public LaneletError {
+  using LaneletError::LaneletError;
+};
+
+/**
+ * @brief SharedPtrs in lanelet2 must never point to null.
+ * If this is violated, this exception is thrown (usually checked at object
+ * construction).
+ */
+class NullptrError : public LaneletError {
+  using LaneletError::LaneletError;
+};
+
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/Forward.h

@@ -0,0 +1,213 @@
+#pragma once
+#include <boost/units/physical_dimensions/acceleration.hpp>
+#include <boost/units/physical_dimensions/velocity.hpp>
+#include <boost/units/quantity.hpp>
+#include <boost/units/systems/si/base.hpp>
+#include <cstdint>
+#include <memory>
+#include <vector>
+
+/**
+ * @namespace lanelet basic namespace for everything in lanelet2
+ */
+namespace lanelet {
+
+// PrimitiveData
+class PrimitiveData;
+using PrimitiveDataPtr = std::shared_ptr<PrimitiveData>;
+using PrimitiveDataConstPtr = std::shared_ptr<const PrimitiveData>;
+using PrimitiveDataPtrs = std::vector<PrimitiveDataPtr>;
+using PrimitiveDataConstPtrs = std::vector<PrimitiveDataConstPtr>;
+
+// PointData
+class PointData;
+using PointDataPtr = std::shared_ptr<PointData>;
+using PointDataConstPtr = std::shared_ptr<const PointData>;
+using PointDataPtrs = std::vector<PointDataPtr>;
+using PointDataConstPtrs = std::vector<PointDataConstPtr>;
+
+// Point
+class Point3d;
+class Point2d;
+class ConstPoint3d;
+class ConstPoint2d;
+using Points3d = std::vector<Point3d>;
+using ConstPoints3d = std::vector<ConstPoint3d>;
+using Points2d = std::vector<Point2d>;
+using ConstPoints2d = std::vector<ConstPoint2d>;
+
+// GPSPoint
+class GPSPoint;
+using GPSPoints = std::vector<GPSPoint>;
+
+// LinestringData
+class LineStringData;
+using LineStringDataPtr = std::shared_ptr<LineStringData>;
+using LineStringDataConstPtr = std::shared_ptr<const LineStringData>;
+using LineStringDataPtrs = std::vector<LineStringDataPtr>;
+using LineStringDataConstPtrs = std::vector<LineStringDataConstPtr>;
+
+// Linestring
+class LineString3d;
+class ConstLineString3d;
+class LineString2d;
+class ConstLineString2d;
+class ConstHybridLineString2d;
+class ConstHybridLineString3d;
+using LineStrings3d = std::vector<LineString3d>;
+using ConstLineStrings3d = std::vector<ConstLineString3d>;
+using LineStrings2d = std::vector<LineString2d>;
+using ConstLineStrings2d = std::vector<ConstLineString2d>;
+using ConstHybridLineStrings2d = std::vector<ConstHybridLineString2d>;
+using ConstHybridLineStrings3d = std::vector<ConstHybridLineString3d>;
+
+// CompoundLineData
+class CompoundLineStringData;
+using CompoundLineStringDataPtr = std::shared_ptr<CompoundLineStringData>;
+using CompoundLineStringDataConstPtr = std::shared_ptr<const CompoundLineStringData>;
+using CompoundLineStringDataPtrs = std::vector<CompoundLineStringDataPtr>;
+using CompoundLineStringDataConstPtrs = std::vector<CompoundLineStringDataConstPtr>;
+
+// CompoundLineString
+class CompoundLineString3d;
+class CompoundLineString2d;
+class CompoundHybridLineString2d;
+class CompoundHybridLineString3d;
+using CompoundLineStrings2d = std::vector<CompoundLineString2d>;
+using CompoundLineStrings3d = std::vector<CompoundLineString3d>;
+using CompoundHybridLineStrings2d = std::vector<CompoundHybridLineString2d>;
+using CompoundHybridLineStrings3d = std::vector<CompoundHybridLineString3d>;
+
+// CompoundPolygon
+class CompoundPolygon3d;
+class CompoundPolygon2d;
+class CompoundHybridPolygon2d;
+class CompoundHybridPolygon3d;
+using CompoundPolygons3d = std::vector<CompoundPolygon3d>;
+using CompoundPolygons2d = std::vector<CompoundPolygon2d>;
+using CompoundHybridPolygons2d = std::vector<CompoundHybridPolygon2d>;
+using CompoundHybridPolygons3d = std::vector<CompoundHybridPolygon3d>;
+
+// LaneletSequenceData
+class LaneletSequenceData;
+using LaneletSequenceDataPtr = std::shared_ptr<LaneletSequenceData>;
+using LaneletSequenceDataConstPtr = std::shared_ptr<const LaneletSequenceData>;
+using LaneletSequenceDataPtrs = std::vector<LaneletSequenceDataPtr>;
+using LaneletSequenceDataConstPtrs = std::vector<LaneletSequenceDataConstPtr>;
+
+// LaneletData
+class LaneletData;
+using LaneletDataPtr = std::shared_ptr<LaneletData>;
+using LaneletDataptr = std::weak_ptr<LaneletData>;
+using LaneletDataConstPtr = std::shared_ptr<const LaneletData>;
+using LaneletDataConstWptr = std::weak_ptr<const LaneletData>;
+using LaneletDataPtrs = std::vector<LaneletDataPtr>;
+using LaneletDataConstPtrs = std::vector<LaneletDataConstPtr>;
+using LaneletDataConstWptrs = std::vector<LaneletDataConstWptr>;
+
+// Lanelet
+class Lanelet;
+class ConstLanelet;
+class WeakLanelet;
+class ConstWeakLanelet;
+using Lanelets = std::vector<Lanelet>;
+using ConstLanelets = std::vector<ConstLanelet>;
+
+// LaneletSequence
+class LaneletSequence;
+using LaneletSequences = std::vector<LaneletSequence>;
+
+// Area
+class Area;
+class ConstArea;
+class WeakArea;
+class ConstWeakArea;
+using Areas = std::vector<Area>;
+using ConstAreas = std::vector<ConstArea>;
+
+// AreaData
+class AreaData;
+using AreaDataPtr = std::shared_ptr<AreaData>;
+using AreaDataptr = std::weak_ptr<AreaData>;
+using AreaDataConstPtr = std::shared_ptr<const AreaData>;
+using AreaDataPtrs = std::vector<AreaDataPtr>;
+using AreaDataConstPtrs = std::vector<AreaDataConstPtr>;
+
+// LaneletOrArea
+class ConstLaneletOrArea;
+using ConstLaneletOrAreas = std::vector<ConstLaneletOrArea>;
+
+// Polygon
+class BasicPolygon2d;
+class BasicPolygon3d;
+class Polygon3d;
+class ConstPolygon3d;
+class Polygon2d;
+class ConstPolygon2d;
+class ConstHybridPolygon2d;
+class ConstHybridPolygon3d;
+using Polygons3d = std::vector<Polygon3d>;
+using ConstPolygons3d = std::vector<ConstPolygon3d>;
+using Polygons2d = std::vector<Polygon2d>;
+using ConstPolygons2d = std::vector<ConstPolygon2d>;
+using BasicPolygons2d = std::vector<BasicPolygon2d>;
+using BasicPolygons3d = std::vector<BasicPolygon3d>;
+using ConstHybridPolygons2d = std::vector<ConstHybridPolygon2d>;
+using ConstHybridPolygons3d = std::vector<ConstHybridPolygon3d>;
+
+// polygon with holes
+class BasicPolygonWithHoles3d;
+class BasicPolygonWithHoles2d;
+using BasicPolygonsWithHoles3d = std::vector<BasicPolygonWithHoles3d>;
+using BasicPolygonsWithHoles2d = std::vector<BasicPolygonWithHoles2d>;
+
+// LaneletMap
+class LaneletMap;
+using LaneletMapPtr = std::shared_ptr<LaneletMap>;
+using LaneletMapUPtr = std::unique_ptr<LaneletMap>;
+using LaneletMapConstPtr = std::shared_ptr<const LaneletMap>;
+using LaneletMapConstUPtr = std::unique_ptr<const LaneletMap>;
+using LaneletMapPtrs = std::vector<LaneletMapPtr>;
+using LaneletMapConstPtrs = std::vector<LaneletMapConstPtr>;
+
+// LaneletSubmap
+class LaneletSubmap;
+using LaneletSubmapPtr = std::shared_ptr<LaneletSubmap>;
+using LaneletSubmapUPtr = std::unique_ptr<LaneletSubmap>;
+using LaneletSubmapConstPtr = std::shared_ptr<const LaneletSubmap>;
+using LaneletSubmapConstUPtr = std::unique_ptr<const LaneletSubmap>;
+using LaneletSubmapPtrs = std::vector<LaneletSubmapPtr>;
+using LaneletSubmapConstPtrs = std::vector<LaneletSubmapConstPtr>;
+
+// RegulatoryElementData
+class RegulatoryElementData;
+using RegulatoryElementDataPtr = std::shared_ptr<RegulatoryElementData>;
+using RegulatoryElementDataConstPtr = std::shared_ptr<const RegulatoryElementData>;
+using RegulatoryElementDataPtrs = std::vector<RegulatoryElementDataPtr>;
+using RegulatoryElementDataConstPtrs = std::vector<RegulatoryElementDataConstPtr>;
+
+// RegulatoryElement
+class RegulatoryElement;
+class GenericRegulatoryElement;
+using RegulatoryElementPtr = std::shared_ptr<RegulatoryElement>;
+using RegulatoryElementPtrs = std::vector<RegulatoryElementPtr>;
+using RegulatoryElementConstPtr = std::shared_ptr<const RegulatoryElement>;
+using RegulatoryElementConstPtrs = std::vector<RegulatoryElementConstPtr>;
+class RegulatoryElementFactory;
+
+using Id = int64_t;
+constexpr Id InvalId = 0;  //!< indicates a primitive that is not part of a map
+using Ids = std::vector<Id>;
+
+// Velocity definitions
+namespace units {
+using MPS = boost::units::unit<boost::units::velocity_dimension, boost::units::si::system>;
+using MPS2 = boost::units::unit<boost::units::acceleration_dimension, boost::units::si::system>;
+using MPSQuantity = boost::units::quantity<MPS>;
+using MPS2Quantity = boost::units::quantity<MPS2>;
+
+}  // namespace units
+using Velocity = units::MPSQuantity;
+using Acceleration = units::MPS2Quantity;
+
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/LaneletMap.h

@@ -0,0 +1,608 @@
+#pragma once
+
+#include <unordered_map>
+
+#include "lanelet2_core/Forward.h"
+#include "lanelet2_core/primitives/Area.h"
+#include "lanelet2_core/primitives/BoundingBox.h"
+#include "lanelet2_core/primitives/Lanelet.h"
+#include "lanelet2_core/primitives/RegulatoryElement.h"
+#include "lanelet2_core/utility/Utilities.h"
+
+namespace lanelet {
+namespace internal {
+template <typename T>
+struct SearchBox {
+  using Type = BoundingBox2d;
+};
+template <>
+struct SearchBox<Point3d> {
+  using Type = BasicPoint2d;
+};
+template <typename T>
+using SearchBoxT = typename SearchBox<T>::Type;
+}  // namespace internal
+   /**
+    * @brief Each primitive in lanelet2 has its own layer in the map.
+    *
+    * This template class defines the common interface for these layers.
+    * It is only implemented for lanelet primitives and should not be
+    * instanciated with any other type.
+    *
+    * Layers are an integral part of LaneletMap, they can not be created
+    * or modified without a LaneletMap object.
+    *
+    * Internally, the elements are identified by their id, therefore it is
+    * absolutely important that an id is unique within one layer.
+    */
+template <typename T>
+class PrimitiveLayer {
+ public:
+  using PrimitiveT = T;
+  using ConstPrimitiveT = traits::ConstPrimitiveType<T>;
+  using Map = std::unordered_map<Id, T>;
+  using ConstPrimitiveVec = std::vector<ConstPrimitiveT>;
+  using PrimitiveVec = std::vector<PrimitiveT>;
+  using OptConstPrimitiveT = Optional<ConstPrimitiveT>;
+  using OptPrimitiveT = Optional<PrimitiveT>;
+
+  //! iterator that gives the primitive when dereferencing, not a std:pair
+  using iterator =  //  NOLINT
+      internal::TransformIterator<typename Map::iterator, PrimitiveT, internal::PairConverter<PrimitiveT>>;
+
+  //! iterator that gives a const primitive, not a std::pair
+  using const_iterator =  // NOLINT
+      internal::TransformIterator<typename Map::const_iterator, const ConstPrimitiveT,
+                                  internal::PairConverter<const ConstPrimitiveT>>;
+  PrimitiveLayer(const PrimitiveLayer& rhs) = delete;
+  PrimitiveLayer& operator=(const PrimitiveLayer& rhs) = delete;
+
+  /**
+   * @brief checks whether an element exists in this layer
+   * @param id the id identifying the element
+   * @return true if element exists
+   */
+  bool exists(Id id) const;
+
+  /**
+   * @brief returns an element for this id if it exists
+   * @param id the id identifying the element
+   * @throws NoSuchPrimitiveError if the element does not exist
+   * @return the const version of the element
+   */
+  ConstPrimitiveT get(Id id) const;
+
+  /**
+   * @brief returns an element for this id if it exists
+   * @param id the id identifying the element
+   * @throws NoSuchPrimitiveError if the element does not exist
+   * @return the non-const version of the element
+   */
+  PrimitiveT get(Id id);
+
+  /**
+   * @brief find returns an iterator to an element if it exists
+   * @param id id to look for
+   * @return iterator to the element or end()
+   *
+   * Note that dereferencing the iterator will give you only the primitive, not
+   * a std::pair as usual with normal std::maps!
+   */
+  const_iterator find(Id id) const;
+
+  /**
+   * @brief finds usages of an owned type within this layer
+   * @see utils::findUsages
+   *
+   * Finds e.g. points owned by linestrings in the lanelet layer.
+   *
+   * The relations are stored by a map internally, so this is just a fast map
+   * lookup.
+   */
+  std::vector<ConstPrimitiveT> findUsages(
+      const traits::ConstPrimitiveType<traits::OwnedT<PrimitiveT>>& primitive) const;
+
+  /**
+   * @brief finds usages of an owned type within this layer
+   *
+   * This is the non-const version to find usages of a primitive in a layer.
+   */
+  std::vector<PrimitiveT> findUsages(const traits::ConstPrimitiveType<traits::OwnedT<PrimitiveT>>& primitive);
+
+  /**
+   * @brief iterator to beginning of the elements (not ordered by id!)
+   * @return the iterator
+   */
+  const_iterator begin() const;
+
+  /**
+   * @brief iterator to end of the elements
+   * @return the iterator
+   */
+  const_iterator end() const;
+
+  /**
+   * @brief non-const version of finding an element
+   * @param id the id to look for
+   * @return iterator to the matching element or end()
+   */
+  iterator find(Id id);
+
+  /**
+   * @brief iterator to beginning
+   * @return the iterator
+   */
+  iterator begin();
+
+  /**
+   * @brief iterator to end
+   * @return the iterator
+   */
+  iterator end();
+
+  /**
+   * @brief returns whether this layer contains something
+   * @return true if no elements
+   */
+  bool empty() const { return elements_.empty(); }
+
+  /**
+   * @brief returns number of elements in this layer
+   * @return number of elements
+   */
+  size_t size() const { return elements_.size(); }
+
+  using ConstSearchFunction = std::function<bool(const internal::SearchBoxT<T>& box, const ConstPrimitiveT& prim)>;
+  using SearchFunction = std::function<bool(const internal::SearchBoxT<T>& box, const PrimitiveT& prim)>;
+
+  /**
+   * @brief searches for elements within a search area
+   * @param area the search area in the map
+   * @return a immutable list of matching elements
+   *
+   * The search is done by comparing bounding boxes, therefore this function
+   * might return false positives because the elements do not intersect with the
+   * area, but their bounding boxes do. To sort these out use
+   * lanelet::geometry::intersects or lanelet::geometry::overlaps.
+   *
+   * Note that this function is also implemented for the point layer for
+   * consistency, even if it does not make sense for this type. It will always
+   * return an empty vector.
+   */
+  ConstPrimitiveVec search(const BoundingBox2d& area) const;
+  PrimitiveVec search(const BoundingBox2d& area);
+
+  /**
+   * @brief searches within search area until a search function returns true.
+   * @return the element for which the function returned true
+   * @see nearestUntil
+   *
+   * Starting by the object with the closest bounding box, searchUntil will pass
+   * primitives to func until the result is true. This is the returned object.
+   * If no such object exists, the Optional will be empty.
+   */
+  OptConstPrimitiveT searchUntil(const BoundingBox2d& area, const ConstSearchFunction& func) const;
+  OptPrimitiveT searchUntil(const BoundingBox2d& area, const SearchFunction& func);
+
+  /**
+   * @brief search for the n nearest elements to a point
+   * @param point the point for the query
+   * @param n number of elements returned
+   * @return the element whose *bounding boxes* are closest to the point
+   * @see nearestUntil, lanelet::geometry::findNearest
+   *
+   * Note that this function does not yield accurate results for all primitives
+   * except points. For all other primitives, "nearest" is determined by the
+   * bounding box of the object. For regulatory elements, this will be the
+   * bounding box of all parameters of the regulatory element. For Lanelets, it
+   * will be the bounding box alround the polygon.
+   *
+   * If you are not happy with this, have a look at nearestUntil, or one of the
+   * free functions, like geometry::findNearest.
+   */
+  ConstPrimitiveVec nearest(const BasicPoint2d& point, unsigned n) const;
+  PrimitiveVec nearest(const BasicPoint2d& point, unsigned n);
+  ConstPrimitiveVec nearest(const Point2d& point, unsigned n) const { return nearest(point.basicPoint(), n); }
+  PrimitiveVec nearest(const Point2d& point, unsigned n) { return nearest(point.basicPoint(), n); }
+
+  /**
+   * @brief repeatedly calls a user-defined predicate until it returns true.
+   * @return the element for which the function returned true. If it was never
+   * true, an empty Optional is returned.
+   *
+   * Starting at the primitive with the closest bounding-box distance to the
+   * query point iteratively calls func until true is returned. This can be used
+   * for iterative queries.
+   */
+  OptConstPrimitiveT nearestUntil(const BasicPoint2d& point, const ConstSearchFunction& func) const;
+  OptPrimitiveT nearestUntil(const BasicPoint2d& point, const SearchFunction& func);
+  OptConstPrimitiveT nearestUntil(const ConstPoint2d& point, const ConstSearchFunction& func) const {
+    return nearestUntil(point.basicPoint2d(), func);
+  }
+  OptPrimitiveT nearestUntil(const Point2d& point, const SearchFunction& func) {
+    return nearestUntil(point.basicPoint2d(), func);
+  }
+
+  /**
+   * @brief returns a unique id. it is guaranteed that the id is not used within
+   * this layer
+   * @return an unused id
+   *
+   * Several calls to uniqueId might or might not produce the same Id, as long
+   * as no element with this Id was added to the layer
+   */
+  Id uniqueId() const;
+
+ protected:
+  friend class LaneletMap;  // only the map can create or modify layers
+  friend class LaneletMapLayers;
+  friend class LaneletSubmap;
+  explicit PrimitiveLayer(const Map& primitives = Map());
+  PrimitiveLayer(PrimitiveLayer&& rhs) noexcept;
+  PrimitiveLayer& operator=(PrimitiveLayer&& rhs) noexcept;
+  ~PrimitiveLayer() noexcept;
+
+  void add(const PrimitiveT& element);
+  void remove(Id element);
+
+  // NOLINTNEXTLINE
+  Map elements_;  //!< the list of elements in this layer
+  struct Tree;
+  // NOLINTNEXTLINE
+  std::unique_ptr<Tree> tree_;  //!< Hides boost trees from you/the compiler
+};
+
+// we need this ifndef for LaneletMap.cpp
+#ifndef LANELET_LAYER_DEFINITION
+extern template class PrimitiveLayer<Area>;
+extern template class PrimitiveLayer<Polygon3d>;
+extern template class PrimitiveLayer<Lanelet>;
+extern template class PrimitiveLayer<LineString3d>;
+extern template class PrimitiveLayer<Point3d>;
+extern template class PrimitiveLayer<RegulatoryElementPtr>;
+#endif
+
+//! Specialized map layer for Area.
+class AreaLayer : public PrimitiveLayer<Area> {
+ public:
+  using PrimitiveLayer::findUsages;
+  AreaLayer() = default;
+  ~AreaLayer() = default;
+  AreaLayer(const AreaLayer&) = delete;
+  AreaLayer operator=(AreaLayer&) = delete;
+  Areas findUsages(const RegulatoryElementConstPtr& regElem);
+  ConstAreas findUsages(const RegulatoryElementConstPtr& regElem) const;
+
+ private:
+  friend class LaneletMap;
+  friend class LaneletMapLayers;
+  friend class LaneletSubmap;
+  using PrimitiveLayer<Area>::PrimitiveLayer;
+  AreaLayer(AreaLayer&& rhs) noexcept = default;
+  AreaLayer& operator=(AreaLayer&& rhs) noexcept = default;
+};
+
+//! Specialized map layer for Lanelet.
+class LaneletLayer : public PrimitiveLayer<Lanelet> {
+ public:
+  using PrimitiveLayer::findUsages;
+  LaneletLayer() = default;
+  ~LaneletLayer() = default;
+  LaneletLayer(const LaneletLayer&) = delete;
+  LaneletLayer operator=(LaneletLayer&) = delete;
+  Lanelets findUsages(const RegulatoryElementConstPtr& regElem);
+  ConstLanelets findUsages(const RegulatoryElementConstPtr& regElem) const;
+
+ private:
+  friend class LaneletMap;
+  friend class LaneletMapLayers;
+  friend class LaneletSubmap;
+  using PrimitiveLayer<Lanelet>::PrimitiveLayer;
+  LaneletLayer(LaneletLayer&& rhs) noexcept = default;
+  LaneletLayer& operator=(LaneletLayer&& rhs) noexcept = default;
+};
+
+using PolygonLayer = PrimitiveLayer<Polygon3d>;
+using LineStringLayer = PrimitiveLayer<LineString3d>;
+using PointLayer = PrimitiveLayer<Point3d>;
+using RegulatoryElementLayer = PrimitiveLayer<RegulatoryElementPtr>;
+
+/**
+ * @brief Container for all layers of a lanelet map. Used by both LaneletMap and LaneletSubmap
+ */
+class LaneletMapLayers {
+ public:
+  LaneletMapLayers() = default;
+  LaneletMapLayers(LaneletMapLayers&& rhs) noexcept = default;
+  LaneletMapLayers& operator=(LaneletMapLayers&& rhs) noexcept = default;
+  LaneletMapLayers(const LaneletMapLayers& rhs) = delete;
+  LaneletMapLayers& operator=(const LaneletMapLayers& rhs) = delete;
+  ~LaneletMapLayers() noexcept = default;
+
+  /**
+   * @brief Construct from already initialized layers
+   * @param lanelets new lanelet layer
+   * @param areas new area layer
+   * @param regulatoryElements new regulatoryElement layer
+   * @param polygons polygon layer
+   * @param lineStrings linesting layer
+   * @param points point layer
+   *
+   * Constructs a map from its individual elements for the layers.
+   * Unlike with the add functions you are responsible that e.g. all points of
+   * a linestring are in the point layer. However, this is the most efficient
+   * way to create a map because this will result in the most efficient RTree
+   * structure for fastest lookups.
+   */
+  LaneletMapLayers(const LaneletLayer::Map& lanelets, const AreaLayer::Map& areas,
+                   const RegulatoryElementLayer::Map& regulatoryElements, const PolygonLayer::Map& polygons,
+                   const LineStringLayer::Map& lineStrings, const PointLayer::Map& points);
+
+  //! Returns whether all layers of this object are empty
+  bool empty() const noexcept {
+    return laneletLayer.empty() && areaLayer.empty() && regulatoryElementLayer.empty() && polygonLayer.empty() &&
+           lineStringLayer.empty() && pointLayer.empty();
+  }
+
+  //! Returns the total number of elements in all layers
+  size_t size() const noexcept {
+    return laneletLayer.size() + areaLayer.size() + regulatoryElementLayer.size() + polygonLayer.size() +
+           lineStringLayer.size() + pointLayer.size();
+  }
+
+  LaneletLayer laneletLayer;                      //!< access to the lanelets within this map
+  AreaLayer areaLayer;                            //!< access to areas
+  RegulatoryElementLayer regulatoryElementLayer;  //!< access to regElems
+  PolygonLayer polygonLayer;                      //!< access to the polygons
+  LineStringLayer lineStringLayer;                //!< access to the lineStrings
+  PointLayer pointLayer;                          //!< access to the points
+};
+
+/**
+ * @brief Basic element for accessing and managing the elements of a map.
+ *
+ * The map is divided in individual layers, one for each primitive in lanelet2.
+ * Each layer offers efficient functions to find elements by its id or spacial
+ * position. A LaneletMap can not be copied because maps are unique. You can
+ * only std::move them.
+ *
+ * A LaneletMap is designed to be *always* self contained. This means it always contains all elements that are used by
+ * any element in the map. If you add a Lanelet, all its LineString boundaries, all RegulatoryElements, all things
+ * referenced by the regulatory elements are added to the map as well. This also means that if a lanelet has regulatory
+ * elements that in turn also reference lanelets, these lanelets will also be added to the map. If this behaviour is not
+ * what you want, consider using a LaneletSubmap.
+ */
+class LaneletMap : public LaneletMapLayers {
+ public:
+  using LaneletMapLayers::LaneletMapLayers;
+
+  /**
+   * @brief adds a lanelet and all the elements it owns to the map
+   * @throws InvalidInputError if lanelet has a reglatory element without
+   * members
+   *
+   * If the lanelet or elements owned by the lanelet have InvalId as Id, they
+   * will be assigned a new, unique id. Otherwise you are responsible for making
+   * sure that the id has not already been for a different element.
+   */
+  void add(Lanelet lanelet);
+
+  /**
+   * @brief adds an area and all the elements it owns
+   * @param area the area to add
+   * @throws InvalidInputError if area has a reglatory element without members
+   *
+   * If the area or elements owned by it have InvalId as Id, they
+   * will be assigned a new, unique id. Otherwise you are responsible for making
+   * sure that the id has not already been for a different element.
+   */
+  void add(Area area);
+
+  /**
+   * @brief adds a new regulatory element and all elements it owns to the map.
+   * @throws NullptrError if regElem is a nullptr
+   * @throws InvalidInputError if regElem has no members
+   *
+   * If the regulatory elemnet or elements owned it lanelet have InvalId as
+   * Id, they will be assigned a new, unique id. Otherwise you are responsible
+   * for making sure that the id has not already been for a different element.
+   */
+  void add(const RegulatoryElementPtr& regElem);
+
+  /**
+   * @brief adds a new polygon and all elements it owns to the map.
+   *
+   * If the polygon or elements owned by it have InvalId as Id, they
+   * will be assigned a new, unique id. Otherwise you are responsible for making
+   * sure that the id has not already been for a different element.
+   */
+  void add(Polygon3d polygon);
+
+  /**
+   * @brief adds a new lineString and all elements it owns to the map.
+   *
+   * If the lineString or elements owned by it have InvalId as Id, they
+   * will be assigned a new, unique id. Otherwise you are responsible for making
+   * sure that the id has not already been for a different element.
+   */
+  void add(LineString3d lineString);
+
+  /**
+   * @brief adds a new point and all elements it owns to the map.
+   *
+   * If the point or elements owned by it have InvalId as Id, they
+   * will be assigned a new, unique id. Otherwise you are responsible for making
+   * sure that the id has not already been for a different element.
+   */
+  void add(Point3d point);
+};
+
+/**
+ * @brief A LaneletSubmap only contains the elemets that have be expleicitly added to it.
+ *
+ * This class is similar to a LaneletMap but with one fundamental difference: It is not self-contained. When add is
+ * called with an object *only this object* is added to the map and nothing else like e.g. its subobjects. But you can
+ * always convert this object back to a laneletMap by calling "LaneletSubmap::laneletMap", which is of course a costly
+ * operation.
+ *
+ * If you want to have a function that can operate on both LaneletMap and LaneletSubmap, consider using their common
+ * base class LaneletMapLayers as an input parameter for it.
+ */
+class LaneletSubmap : public LaneletMapLayers {
+ public:
+  LaneletSubmap() = default;
+  using LaneletMapLayers::LaneletMapLayers;
+
+  //! Constructs a submap from a moved-from LaneletMap
+  explicit LaneletSubmap(LaneletMap&& rhs) noexcept : LaneletMapLayers{std::move(rhs)} {}
+  LaneletSubmap& operator=(LaneletMap&& rhs) {
+    static_cast<LaneletMapLayers&>(*this) = std::move(rhs);
+    return *this;
+  }
+
+  /**
+   * @brief adds a lanelet to the submap
+   * @throws InvalidInputError if lanelet has a reglatory element without
+   * members
+   *
+   * If the lanelet or elements owned by the lanelet have InvalId as Id, they
+   * will be assigned a new, unique id. Otherwise you are responsible for making
+   * sure that the id has not already been for a different element.
+   */
+  void add(Lanelet lanelet);
+
+  //! adds an area
+  void add(Area area);
+
+  //! adds a new regulatory element to the submap.
+  void add(const RegulatoryElementPtr& regElem);
+
+  //! adds a new polygon to the submap
+  void add(Polygon3d polygon);
+
+  //! adds a new lineString the submap
+  void add(LineString3d lineString);
+
+  //! adds a new point to the submap
+  void add(Point3d point);
+
+  //! Converts this into a fully valid lanelet map
+  LaneletMapConstPtr laneletMap() const;
+  LaneletMapUPtr laneletMap();
+
+  //! In order to let areas and lanelets get out of scope, this function ensures they stay alive. LaneletSubmap::add
+  //! already handles this for you, so there is usually no need to call this.
+  void trackParameters(const RegulatoryElement& regelem);
+
+ private:
+  //! In order to not let lanelets/areas referenced by regelems get out of scope, we keep a reference to them here
+  std::vector<boost::variant<ConstLanelet, ConstArea>> regelemObjects_;
+};
+
+namespace utils {
+template <typename PrimitiveT>
+using ConstLayerPrimitive = typename PrimitiveLayer<PrimitiveT>::ConstPrimitiveT;
+
+//! Efficiently create a LaneletMap from a vector of things. All elements must have a valid id!
+LaneletMapUPtr createMap(const Points3d& fromPoints);
+LaneletMapUPtr createMap(const LineStrings3d& fromLineStrings);
+LaneletMapUPtr createMap(const Polygons3d& fromPolygons);
+LaneletMapUPtr createMap(const Lanelets& fromLanelets);
+LaneletMapUPtr createMap(const Areas& fromAreas);
+LaneletMapUPtr createMap(const Lanelets& fromLanelets, const Areas& fromAreas);
+LaneletMapConstUPtr createConstMap(const ConstLanelets& fromLanelets, const ConstAreas& fromAreas);
+
+//! Efficiently create a LaneletSubmap from a vector of things. All elements must have a valid id!
+LaneletSubmapUPtr createSubmap(const Points3d& fromPoints);
+LaneletSubmapUPtr createSubmap(const LineStrings3d& fromLineStrings);
+LaneletSubmapUPtr createSubmap(const Polygons3d& fromPolygons);
+LaneletSubmapUPtr createSubmap(const Lanelets& fromLanelets);
+LaneletSubmapUPtr createSubmap(const Areas& fromAreas);
+LaneletSubmapUPtr createSubmap(const Lanelets& fromLanelets, const Areas& fromAreas);
+LaneletSubmapConstUPtr createConstSubmap(const ConstLanelets& fromLanelets, const ConstAreas& fromAreas);
+
+/**
+ * @brief returns a unique id by incrementing a counter each time this is
+ * called.
+ *
+ * Useful for creating a new map with primitives. As long as all the ids are
+ * generated using this function, it is guaranteed that there will be no
+ * duplicates. This is different from PrimitiveLayer::uniqueId, which only
+ * guarantees that an Id is unique within this layer.
+ *
+ * This is threa safe.
+ */
+Id getId();
+
+//! Register an id as being in use. This id will no longer be returned by getId.
+//! Function is thread safe.
+void registerId(Id id);
+
+template <typename PrimitiveT>
+std::vector<ConstLayerPrimitive<PrimitiveT>> findUsages(const PrimitiveLayer<PrimitiveT>& layer, Id id);
+
+ConstLanelets findUsagesInLanelets(const LaneletMapLayers& map, const ConstPoint3d& p);
+ConstAreas findUsagesInAreas(const LaneletMapLayers& map, const ConstPoint3d& p);
+
+}  // namespace utils
+
+namespace traits {
+template <typename T>
+struct LayerPrimitive {
+  using Type = typename T::PrimitiveT;
+};
+template <typename T>
+struct LayerPrimitive<const T> {
+  using Type = typename T::ConstPrimitiveT;
+};
+
+template <typename T>
+using LayerPrimitiveType = typename LayerPrimitive<T>::Type;
+}  // namespace traits
+
+namespace geometry {
+/**
+ * @brief returns the nearest n primitives to a point.
+ * @return vector of the n closest primitives together with their distance in 2D space in
+ * ascending order.
+ * @see findWithin2d, findWithin3d
+ *
+ * Other than than LaneletLayer::nearest, this returns the actually closest
+ * primitives, not only the closest bounding boxes.
+ * This comes at a slightly higher cost, because more primitives from the R-Tree
+ * need to be checked.
+ *
+ * Example: `std::vector<std::pair<double, Lanelet>> closeLanelets = findNearest(laneletMap.laneletLayer,
+ * BasicPoint2d(1,0), 5);`
+ */
+template <typename PrimT>
+std::vector<std::pair<double, PrimT>> findNearest(PrimitiveLayer<PrimT>& map, const BasicPoint2d& pt, unsigned count);
+template <typename PrimT>
+std::vector<std::pair<double, traits::ConstPrimitiveType<PrimT>>> findNearest(const PrimitiveLayer<PrimT>& map,
+                                                                              const BasicPoint2d& pt, unsigned count);
+
+#ifndef LANELET_LAYER_DEFINITION
+// clang-format off
+// NOLINTNEXTLINE
+#define EXTERN_FIND_NEAREST(PRIM) extern template std::vector<std::pair<double, PRIM>> findNearest(PrimitiveLayer<PRIM>&, const BasicPoint2d&, unsigned)
+// NOLINTNEXTLINE
+#define EXTERN_CONST_FIND_NEAREST(PRIM) extern template std::vector<std::pair<double, traits::ConstPrimitiveType<PRIM>>> findNearest(const PrimitiveLayer<PRIM>&, const BasicPoint2d&, unsigned)
+// clang-format on
+EXTERN_FIND_NEAREST(Area);
+EXTERN_FIND_NEAREST(Polygon3d);
+EXTERN_FIND_NEAREST(Lanelet);
+EXTERN_FIND_NEAREST(LineString3d);
+EXTERN_FIND_NEAREST(Point3d);
+EXTERN_FIND_NEAREST(RegulatoryElementPtr);
+EXTERN_CONST_FIND_NEAREST(Area);
+EXTERN_CONST_FIND_NEAREST(Polygon3d);
+EXTERN_CONST_FIND_NEAREST(Lanelet);
+EXTERN_CONST_FIND_NEAREST(LineString3d);
+EXTERN_CONST_FIND_NEAREST(Point3d);
+EXTERN_CONST_FIND_NEAREST(RegulatoryElementPtr);
+#undef EXTERN_FIND_NEAREST
+#undef EXTERN_CONST_FIND_NEAREST
+#endif
+}  // namespace geometry
+
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/geometry/Area.h

@@ -0,0 +1,151 @@
+#pragma once
+
+#include "lanelet2_core/geometry/Polygon.h"
+#include "lanelet2_core/primitives/Area.h"
+#include "lanelet2_core/primitives/Lanelet.h"
+
+namespace lanelet {
+namespace geometry {
+/**
+ * @brief Checks whether a point is within or at the border of a area
+ * @param area for the check
+ * @param point point to check
+ * @return true if the point is within or at the border, false otherwise
+ */
+template <typename AreaT>
+IfAr<AreaT, bool> inside(const AreaT& area, const BasicPoint2d& point);
+
+/**
+ * @brief calculates an up-right 2d bounding box
+ * @param area area to calculate it from
+ * @return the bounding box.
+ *
+ * Linear on number of points.
+ */
+template <typename AreaT>
+IfAr<AreaT, BoundingBox2d> boundingBox2d(const AreaT& area);
+
+/**
+ * @brief calculates 3d bounding box
+ * @param area area to calculate it from.
+ * @return the bounding box
+ */
+template <typename AreaT>
+IfAr<AreaT, BoundingBox3d> boundingBox3d(const AreaT& area);
+
+//! test whether two areas intersect in 2d.
+template <typename Area1T, typename Area2T>
+IfAr<Area1T, bool> intersects2d(const Area1T& area, const Area2T& otherArea);
+
+//! test whether two areas overlap in 2d (common area < 0).
+//! This is an approximation that ignores the holes of the areas!
+template <typename AreaT>
+IfAr<AreaT, bool> overlaps2d(const AreaT& area, const AreaT& otherArea);
+
+//! test whether two areas overlap in 3d.
+//! This is an approximation that uses the overlap of the outer bound
+template <typename AreaT>
+IfAr<AreaT, bool> overlaps3d(const AreaT& area, const AreaT& otherArea, double heightTolerance);
+
+//! test whether an area and a lanelet overlap in 2d
+//! This is an approximation that uses the overlap of the outer bound
+template <typename AreaT, typename LaneletT>
+IfAr<AreaT, IfLL<LaneletT, bool>> overlaps2d(const AreaT& area, const LaneletT& lanelet);
+
+//! test whether an area and a lanelet overlap in 3d
+//! This is an approximation that uses the overlap of the outer bound
+template <typename AreaT, typename LaneletT>
+IfAr<AreaT, IfLL<LaneletT, bool>> overlaps3d(const AreaT& area, const LaneletT& lanelet, double heightTolerance);
+
+//! Test whether area is left of lanelet
+inline bool leftOf(const ConstLanelet& right, const ConstArea& left);
+
+//! Test whether area is right of lanelet
+inline bool rightOf(const ConstLanelet& left, const ConstArea& area);
+
+//! Test whether area follows lanelet
+inline bool follows(const ConstLanelet& prev, const ConstArea& next);
+
+//! Test whether lanelet follows area
+inline bool follows(const ConstArea& prev, const ConstLanelet& next);
+
+//! Test if two areas are adjacent
+inline bool adjacent(const ConstArea& area1, const ConstArea& area2);
+
+/**
+ * Find line string in area ar that borders Lanelet ll if ar follows ll
+ * @param ll Lanelet
+ * @param ar Area that is following ll
+ * @return LineString3d if it exists
+ */
+inline Optional<ConstLineString3d> determineCommonLinePreceding(const ConstLanelet& ll, const ConstArea& ar);
+
+/**
+ * Find line string in area ar that borders Lanelet ll if ar precedes ll
+ * @param ar Area that is preceding ll
+ * @param ll Lanelet
+ * @return LineString3d if it exists
+ */
+inline Optional<ConstLineString3d> determineCommonLineFollowing(const ConstArea& ar, const ConstLanelet& ll);
+
+/**
+ * Find line string in area ar that borders Lanelet ll if ar precedes or follows ll
+ * @param ar Area that is preceding or following ll
+ * @param ll Lanelet
+ * @return LineString3d if it exists
+ */
+inline Optional<ConstLineString3d> determineCommonLineFollowingOrPreceding(const ConstArea& ar, const ConstLanelet& ll);
+
+/**
+ * Find line string in Lanelet right that borders Area left if ar is left of ll.
+ * @param left Area that borders ll on the left
+ * @param right Lanelet
+ * @return LineString3d in right if it exists
+ */
+inline Optional<ConstLineString3d> determineCommonLineLeft(const ConstLanelet& right, const ConstArea& left);
+
+/**
+ * Find line string in Lanelet left that borders Area right if ar is right of ll.
+ * @param right Area that borders ll on the right
+ * @param left Lanelet
+ * @return LineString3d in left if it exists
+ */
+inline Optional<ConstLineString3d> determineCommonLineRight(const ConstLanelet& left, const ConstArea& right);
+
+/**
+ * Find line string in Lanelet ll that borders Area ar if ar is left or right of ll. Same as
+ * determineCommonLineSideways(ar, ll) but returned line string is guaranteed to be in ll
+ * @param ar Area that borders ll
+ * @param ll Lanelet
+ * @return LineString3d in ll if it exists
+ */
+inline Optional<ConstLineString3d> determineCommonLineSideways(const ConstLanelet& ll, const ConstArea& ar);
+
+/**
+ * Find line string in Area ar that borders Lanelet ll if ar is left or right of ll. Same as
+ * determineCommonLineSideways(ll, ar) but returned line string is guaranteed to be in ar
+ * @param ar Area
+ * @param ll Lanelet that borders ar
+ * @return LineString3d in ar if it exists
+ */
+inline Optional<ConstLineString3d> determineCommonLineSideways(const ConstArea& ar, const ConstLanelet& ll);
+
+/**
+ * Find line string in area ar that borders Lanelet ll anywhere
+ * @param ar Area that is adjacent anywhere to ll
+ * @param ll Lanelet
+ * @return LineString3d if it exists
+ */
+inline Optional<ConstLineString3d> determineCommonLine(const ConstArea& ar, const ConstLanelet& ll);
+
+/**
+ * Find Line String in Area ar1 that is common with Area ar2
+ * @param ar1 Area
+ * @param ar2 Area
+ * @return LineString3d in Area ar1 if it exists. The inverted line string is part of ar2.
+ */
+inline Optional<ConstLineString3d> determineCommonLine(const ConstArea& ar1, const ConstArea& ar2);
+}  // namespace geometry
+}  // namespace lanelet
+
+#include "impl/Area.h"

src/map/lanelet2/include/lanelet2_core/geometry/BoundingBox.h

@@ -0,0 +1,47 @@
+#pragma once
+#include <boost/geometry/geometries/register/box.hpp>
+
+#include "lanelet2_core/geometry/Point.h"
+#include "lanelet2_core/primitives/BoundingBox.h"
+
+// registrations for use with boost::geometry
+BOOST_GEOMETRY_REGISTER_BOX(lanelet::BoundingBox2d, lanelet::BasicPoint2d, min(), max())
+BOOST_GEOMETRY_REGISTER_BOX(lanelet::BoundingBox3d, lanelet::BasicPoint3d, min(), max())
+
+namespace lanelet {
+namespace geometry {
+/**
+ * @brief trivial overload for boundingBoxes
+ * @param b
+ */
+inline BoundingBox2d boundingBox2d(const BoundingBox2d& b) { return b; }
+inline BoundingBox3d boundingBox3d(const BoundingBox2d& b) { return traits::to3D(b); }
+
+/**
+ * @brief trivial overload for boundingBoxes
+ * @param b
+ */
+inline BoundingBox3d boundingBox3d(const BoundingBox3d& b) { return b; }
+inline BoundingBox2d boundingBox2d(const BoundingBox3d& b) { return traits::to2D(b); }
+
+/**
+ * @brief calculates a (very small) 2d bounding box around a point
+ * @param p point to to this for
+ * @return the bounding box
+ */
+inline BoundingBox2d boundingBox2d(const ConstPoint2d& p) { return {p.basicPoint2d(), p.basicPoint2d()}; }
+inline BoundingBox2d boundingBox2d(const ConstPoint3d& p) { return {p.basicPoint2d(), p.basicPoint2d()}; }
+inline BoundingBox2d boundingBox2d(const BasicPoint2d& p) { return {p, p}; }
+inline BoundingBox2d boundingBox2d(const BasicPoint3d& p) { return {traits::to2D(p), traits::to2D(p)}; }
+
+/**
+ * @brief calculates a (very small) 3d bounding box around a point
+ * @param p point to to this for
+ * @return the bounding box
+ */
+inline BoundingBox3d boundingBox3d(const ConstPoint3d& p) { return BoundingBox3d(p.basicPoint(), p.basicPoint()); }
+inline BoundingBox3d boundingBox3d(const ConstPoint2d& p) { return boundingBox3d(utils::to3D(p)); }
+inline BoundingBox3d boundingBox3d(const BasicPoint3d& p) { return {p, p}; }
+inline BoundingBox3d boundingBox3d(const BasicPoint2d& p) { return {traits::to3D(p), traits::to3D(p)}; }
+}  // namespace geometry
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/geometry/GeometryHelper.h

@@ -0,0 +1,189 @@
+#pragma once
+#include <boost/geometry/strategies/cartesian/distance_projected_point.hpp>
+#include <memory>
+
+namespace lanelet {
+namespace helper {
+/**
+ * @brief apply a function for each pair of adjacent elements in a range
+ * @param first first element in range
+ * @param end past-the-end iterator
+ * @param f function/lambda to apply (defines operator())
+ *
+ * This is useful for iterating over a the segments in a LineString.
+ */
+template <typename FwIter, typename Func>
+void forEachPair(FwIter first, FwIter end, Func f) {
+  if (first != end) {
+    FwIter second = first;
+    ++second;
+    for (; second != end; ++first, ++second) {
+      f(*first, *second);
+    }
+  }
+}
+
+/**
+ * @brief Apply a function of all pairs in a sequenc in sorted order until a
+ * predicate returns true
+ * @param first begin of the range to iterate over
+ * @param end end of the range to iterate over
+ * @param f function returning something convertible to bool. If the result is
+ * true, the iteration is stopped
+ * @return Iterator to the first element of the pair that returned true or end
+ *
+ * Works similar to forEachPair but stops if f returns true. Useful for
+ * implementing a search on segments.
+ */
+template <typename FwIter, typename Func>
+FwIter forEachPairUntil(FwIter first, FwIter end, Func f) {
+  if (first != end) {
+    FwIter second = first;
+    ++second;
+    for (; second != end; ++first, ++second) {
+      auto res = f(*first, *second);
+      if (res) {
+        return first;
+      }
+    }
+  }
+  return end;
+}
+
+template <typename InPointT, typename OutPointT>
+std::enable_if_t<std::is_same<InPointT, OutPointT>::value> convertOrAssign(const InPointT& pIn, OutPointT& pOut) {
+  pOut = pIn;
+}
+
+template <typename InPointT, typename OutPointT>
+std::enable_if_t<!std::is_same<InPointT, OutPointT>::value> convertOrAssign(const InPointT& pIn, OutPointT& pOut) {
+  boost::geometry::convert(pIn, pOut);
+}
+
+/**
+ * @brief A strategy for boost::distance.
+ *
+ * Works just like the default strategy for boost::distance on linestrings, but
+ * additionally stores some information on the closest segment.
+ */
+template <typename ProjPoint>
+class ProjectedPoint : public boost::geometry::strategy::distance::projected_point<> {
+ public:
+  using Strategy = boost::geometry::strategy::distance::pythagoras<void>;
+  template <typename Point1T, typename Point2T, typename CalculationResult>
+  auto updateClosestPoint(const Point1T& pSeg1, const Point1T& pSeg2, Point2T& pProj, CalculationResult d) const {
+    using namespace boost::geometry;
+    if (result->dMin < 0 || d < result->dMin) {
+      convert(pProj, result->projectedPoint);
+      convertOrAssign(pSeg1, result->segmentPoint1);
+      convertOrAssign(pSeg2, result->segmentPoint2);
+      result->dMin = d;
+    }
+    return d;
+  }
+  template <typename Point, typename PointOfSegment>
+  inline typename calculation_type<Point, PointOfSegment>::type apply(Point const& p, PointOfSegment const& p1,
+                                                                      PointOfSegment const& p2) const {
+    using namespace boost::geometry;
+    assert_dimension_equal<Point, PointOfSegment>();
+
+    using CalculationResult = typename calculation_type<Point, PointOfSegment>::type;
+
+    // A projected point of points in Integer coordinates must be able to be
+    // represented in FP.
+    using FpVector =  // NOLINT
+        model::point<CalculationResult, dimension<PointOfSegment>::value,
+                     typename coordinate_system<PointOfSegment>::type>;
+
+    FpVector v;
+    FpVector w;
+    FpVector projected;
+
+    convert(p2, v);
+    convert(p, w);
+    convert(p1, projected);
+    subtract_point(v, projected);
+    subtract_point(w, projected);
+
+    Strategy strategy;
+    boost::ignore_unused_variable_warning(strategy);
+
+    auto zero = CalculationResult{};
+    CalculationResult const c1 = dot_product(w, v);
+    if (c1 <= zero) {
+      return updateClosestPoint(p1, p2, p1, strategy.apply(p, p1));
+    }
+    CalculationResult const c2 = dot_product(v, v);
+    if (c2 <= c1) {
+      return updateClosestPoint(p1, p2, p2, strategy.apply(p, p2));
+    }
+
+    // See above, c1 > 0 AND c2 > c1 so: c2 != 0
+    CalculationResult const b = c1 / c2;
+
+    multiply_value(v, b);
+    add_point(projected, v);
+    return updateClosestPoint(p1, p2, projected, strategy.apply(p, projected));
+  }
+  struct Result {
+    ProjPoint projectedPoint, segmentPoint1, segmentPoint2;
+    double dMin{-1.};
+  };
+  mutable std::shared_ptr<Result> result{std::make_shared<Result>()};
+};
+}  // namespace helper
+}  // namespace lanelet
+
+// Helper function definitions for our strategy.
+namespace boost {
+namespace geometry {
+namespace strategy {
+namespace distance {
+namespace services {
+
+template <typename ProjPoint>
+struct tag<lanelet::helper::ProjectedPoint<ProjPoint>> {
+  typedef strategy_tag_distance_point_segment type;  // NOLINT
+};
+
+template <typename ProjPoint, typename P, typename PS>
+struct return_type<lanelet::helper::ProjectedPoint<ProjPoint>, P, PS>
+    : lanelet::helper::ProjectedPoint<ProjPoint>::template calculation_type<P, PS> {};
+
+template <typename ProjPoint>
+struct comparable_type<lanelet::helper::ProjectedPoint<ProjPoint>> {
+  // Define a projected_point strategy with its underlying point-point-strategy
+  // being comparable
+  typedef lanelet::helper::ProjectedPoint<ProjPoint> type;  // NOLINT
+};
+
+template <typename ProjPoint>
+struct get_comparable<lanelet::helper::ProjectedPoint<ProjPoint>> {
+  using comparable_type =  // NOLINT
+      typename comparable_type<lanelet::helper::ProjectedPoint<ProjPoint>>::type;
+
+ public:
+  static inline comparable_type apply(lanelet::helper::ProjectedPoint<ProjPoint> const& p) {  // NOLINT
+    return p;
+  }
+};
+
+template <typename ProjPoint, typename P, typename PS>
+struct result_from_distance<lanelet::helper::ProjectedPoint<ProjPoint>, P, PS> {
+ private:
+  using return_type =  // NOLINT
+      typename return_type<lanelet::helper::ProjectedPoint<ProjPoint>, P, PS>::type;
+
+ public:
+  template <typename T>
+  static inline return_type apply(lanelet::helper::ProjectedPoint<ProjPoint> const&,  // NOLINT
+                                  T const& value) {
+    typename lanelet::helper::ProjectedPoint<ProjPoint>::Strategy s;
+    return result_from_distance<typename lanelet::helper::ProjectedPoint<ProjPoint>::Strategy, P, PS>::apply(s, value);
+  }
+};
+}  // namespace services
+}  // namespace distance
+}  // namespace strategy
+}  // namespace geometry
+}  // namespace boost

src/map/lanelet2/include/lanelet2_core/geometry/Lanelet.h

@@ -0,0 +1,195 @@
+#pragma once
+
+#include "lanelet2_core/primitives/Lanelet.h"
+
+namespace lanelet {
+namespace geometry {
+/**
+ * @brief Checks whether a point is within or at the border of a lanelet
+ * @param lanelet for the check
+ * @param point point to check
+ * @return true if the point is within or at the border, false otherwise
+ */
+template <typename LaneletT>
+IfLL<LaneletT, bool> inside(const LaneletT& lanelet, const BasicPoint2d& point);
+
+/**
+ * @brief approximates length by sampling points along left bound
+ *
+ * avoids to calculate the centerline which might be expensive.
+ */
+template <typename LaneletT>
+double approximatedLength2d(const LaneletT& lanelet);
+
+/**
+ * @brief calculate length of centerline in 2d
+ * @param lanelet lanelet to do this for
+ * @return length in m
+ */
+template <typename LaneletT>
+double length2d(const LaneletT& lanelet);
+
+/**
+ * @brief calculate length of centerline in 3d
+ * @param lanelet lanelet to do this for
+ * @return length in m
+ */
+template <typename LaneletT>
+double length3d(const LaneletT& lanelet);
+
+/**
+ * @brief calculates distance in 2d to the centerline of a lanelet.
+ * @param lanelet lanelet to take the centerline from
+ * @param point the point
+ * @return metric distance
+ *
+ * Has linear complexity on the number of points in the lanelet.
+ */
+template <typename LaneletT>
+double distanceToCenterline2d(const LaneletT& lanelet, const BasicPoint2d& point);
+
+/**
+ * @brief calculates distance in 3d to centerline of a lanelet.
+ * @brief calculates distance in 2d to the centerline of a lanelet.
+ * @param lanelet lanelet to take the centerline from
+ * @param point the point
+ * @return metric distance
+ *
+ * Unlike distance3d, the surface does not have to be planar.
+ * Has linear complexity on the number of points in the lanelet.
+ */
+template <typename LaneletT>
+double distanceToCenterline3d(const LaneletT& lanelet, const BasicPoint3d& point);
+
+/**
+ * @brief calculates an up-right 2d bounding box
+ * @param lanelet lanelet to calculate it from
+ * @return the bounding box.
+ *
+ * Linear on number of points.
+ */
+template <typename LaneletT>
+IfLL<LaneletT, BoundingBox2d> boundingBox2d(const LaneletT& lanelet);
+
+/**
+ * @brief calculates 3d bounding box
+ * @param lanelet lanelet to calculate it from.
+ * @return the bounding box
+ */
+template <typename LaneletT>
+IfLL<LaneletT, BoundingBox3d> boundingBox3d(const LaneletT& lanelet);
+
+/**
+ * @brief test whether two lanelets intersect in 2d.
+ * @param lanelet lanelet to check for
+ * @param otherLanelet other lanelet to check for
+ * @see overlaps2d
+ * @return true if lanelets have intersections.
+ *
+ * This also returns true if the two lanelets only touch each other. Use
+ * overlaps if you do not want this.
+ */
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, bool> intersects2d(const Lanelet1T& lanelet, const Lanelet2T& otherLanelet);
+
+/**
+ * @brief test whether two lanelets overlap in 2d.
+ *
+ * Returns true if the area shared by the two lanelets is >0.
+ */
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, bool> overlaps2d(const Lanelet1T& lanelet, const Lanelet2T& otherLanelet);
+/**
+ * @brief test whether two lanelets intersect in 2d.
+ * @param lanelet lanelet to check for
+ * @param otherLanelet other lanelet to check for
+ * @param heightTolerance distance in z below which lanelets are considered as
+ * intersecting
+ * @todo this is currently only an approximation based on the centerlines
+ * @return true if lanelets have intersections.
+ */
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, bool> intersects3d(const Lanelet1T& lanelet, const Lanelet2T& otherLanelet,
+                                   double heightTolerance = 3.);
+
+/**
+ * @brief test whether two lanelets overlap in 3d.
+ * @todo this is currently only an approximation based on the centerlines
+ *
+ * Returns true if the area shared by the two lanelets is >0.
+ */
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, bool> overlaps3d(const Lanelet1T& lanelet, const Lanelet2T& otherLanelet, double heightTolerance = 3);
+/**
+ * @brief calculates points of intersection between centerlines in 2d.
+ * @param lanelet first lanelet to calculate from
+ * @param otherLanelet second lanelet to calculate from
+ * @param distanceThis optional: if not null will contain distances to travel
+ * along the centerline to each intersection point. Same size as the returned
+ * result.
+ * @param distanceOther optional: same for the other lanelet
+ * @return vector of intersection points, of empty if none.
+ */
+template <typename Lanelet1T, typename Lanelet2T>
+BasicPoints2d intersectCenterlines2d(const Lanelet1T& lanelet, const Lanelet2T& otherLanelet,
+                                     std::vector<double>* distanceThis = nullptr,
+                                     std::vector<double>* distanceOther = nullptr);
+
+/**
+ * @brief checks if a lanelet is direcly left of another by checking if they
+ * share the same boundary
+ *
+ * Be aware that the orientation of the lanelets (see Lanelet::invert()) is
+ * important
+ */
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, IfLL<Lanelet2T, bool>> leftOf(const Lanelet1T& left, const Lanelet2T& right);
+
+/**
+ * @brief checks if a lanelet is direcly right of another by checking if they
+ * share the same boundary
+ *
+ * Be aware that the orientation of the lanelets (see Lanelet::invert()) is
+ * important.
+ */
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, IfLL<Lanelet2T, bool>> rightOf(const Lanelet1T& right, const Lanelet2T& left);
+
+/**
+ * @brief checks if a lanelet is the direct successor by checking if they
+ * share the same start/endpoints
+ *
+ * Be aware that the orientation of the lanelets (see Lanelet::invert()) is
+ * important.
+ */
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, IfLL<Lanelet2T, bool>> follows(const Lanelet1T& prev, const Lanelet2T& next);
+
+/**
+ * @brief find a common line string in ll and other.
+ * @param ll Lanelet
+ * @param other Lanelet
+ * @param allowInverted if true, the orientation of the line strings is ignored
+ * @return line string in ll if it is shared with other
+ */
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, IfLL<Lanelet2T, Optional<ConstLineString3d>>> determineCommonLine(const Lanelet1T& ll,
+                                                                                  const Lanelet2T& other,
+                                                                                  bool allowInverted = false);
+
+/**
+ * @brief calculates the maximum velocity without exceding a maximum lateral
+ * acceleration.
+ * @param lanelet lanelet to calculate this from
+ * @param position position in 2d next to the lanelet
+ * @param maxLateralAcceleration the maximum desired acceleration
+ * @return the maximum velocity. Can be Inf if the lanelet has no curvature
+ * locally.
+ */
+template <typename LaneletT>
+Velocity maxCurveSpeed(const LaneletT& lanelet, const BasicPoint2d& position,
+                       const Acceleration& maxLateralAcceleration = 2.0 * units::MPS2());
+}  // namespace geometry
+}  // namespace lanelet
+
+#include "impl/Lanelet.h"

src/map/lanelet2/include/lanelet2_core/geometry/LaneletMap.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include "lanelet2_core/LaneletMap.h"
+#include "lanelet2_core/geometry/BoundingBox.h"
+#include "lanelet2_core/geometry/Lanelet.h"
+#include "lanelet2_core/geometry/LineString.h"
+#include "lanelet2_core/primitives/BoundingBox.h"
+#include "lanelet2_core/primitives/Lanelet.h"
+
+namespace lanelet {
+namespace geometry {
+/**
+ * @brief Returns all elements that are closer than maxDist to a geometry in 2d
+ * @param layer for the check (a layer of LaneletMap)
+ * @param geometry to check (any 2d geometry)
+ * @param maxDist maximum distance to the input geometry. If zero, only primitives containing the element are returned.
+ * Be aware that rounding errors can affect the result for primitives directly on (or very close to) the boundary.
+ * @return vector of pairs: <actual distance, element> of all elements of a layer which are closer than maxDist to the
+ * geometry. The return type differs depending on if the layer is const or not. Result sorted in ascending distance.
+ * @see findNearest
+ */
+template <typename LayerT, typename GeometryT>
+auto findWithin2d(LayerT& layer, const GeometryT& geometry, double maxDist = 0.)
+    -> std::vector<std::pair<double, traits::LayerPrimitiveType<LayerT>>>;
+
+/**
+ * @brief Returns all elements that are closer than maxDist to a geometry in 3d
+ * @param layer for the check (a layer of LaneletMap)
+ * @param geometry to check (any 3d geometry)
+ * @param maxDist maximum distance to the input geometry. If zero, only primitives containing the element are returned.
+ * Be aware that rounding errors can affect the result for primitives directly on (or very close to) the boundary.
+ * @return vector of pairs: <actual distance, element> of all elements of a layer which are closer than maxDist to the
+ * geometry. The return type differs depending on if the layer is const or not. Result sorted in ascending distance.
+ */
+template <typename LayerT, typename GeometryT>
+auto findWithin3d(LayerT& layer, const GeometryT& geometry, double maxDist = 0.)
+    -> std::vector<std::pair<double, traits::LayerPrimitiveType<LayerT>>>;
+}  // namespace geometry
+}  // namespace lanelet
+
+#include "impl/LaneletMap.h"

src/map/lanelet2/include/lanelet2_core/geometry/LineString.h

@@ -0,0 +1,292 @@
+#pragma once
+#include <boost/geometry/algorithms/intersects.hpp>
+#include <boost/geometry/algorithms/length.hpp>
+#include <boost/geometry/geometries/register/linestring.hpp>
+#include <boost/geometry/geometries/register/segment.hpp>
+
+#include "lanelet2_core/Forward.h"
+#include "lanelet2_core/geometry/BoundingBox.h"
+#include "lanelet2_core/geometry/GeometryHelper.h"
+#include "lanelet2_core/geometry/Point.h"
+#include "lanelet2_core/primitives/CompoundLineString.h"
+#include "lanelet2_core/primitives/LineString.h"
+
+/***********************************************************************
+ *                   BOOST GEOMETRY REGISTRATIONS                      *
+ ***********************************************************************/
+
+BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::BasicLineString3d)
+BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::BasicLineString2d)
+BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::LineString3d)
+BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::LineString2d)
+BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::ConstLineString3d)
+BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::ConstLineString2d)
+BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::ConstHybridLineString3d)
+BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::ConstHybridLineString2d)
+BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::CompoundLineString2d)
+BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::CompoundLineString3d)
+BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::CompoundHybridLineString2d)
+BOOST_GEOMETRY_REGISTER_LINESTRING(lanelet::CompoundHybridLineString3d)
+BOOST_GEOMETRY_REGISTER_SEGMENT_TEMPLATIZED(lanelet::Segment, first, second)
+
+namespace lanelet {
+namespace geometry {
+
+using boost::geometry::intersects;
+using boost::geometry::length;
+using boost::geometry::overlaps;
+using boost::geometry::touches;
+
+template <typename LineStringIterator>
+double rangedLength(LineStringIterator start, LineStringIterator end);
+
+/**
+ * Calculates the length ratios for the lines in the LineString.
+ *
+ * The length ratio of a line is the line's length divided by the LineString's
+ * length.
+ * @param lineString the linestring to do this for.
+ * @return A vector of length ratios. Size is lineString.size()-1
+ *
+ * The function is templated to work on all LineString types, 2d or 3d.
+ * Depending on the type, the result will be computed in 2d or 3d.
+ */
+template <typename LineStringT>
+std::vector<double> lengthRatios(const LineStringT& lineString);
+
+/**
+ * Calculates the accumulated length ratios for the lines in the LineString.
+ *
+ * @see lengthRatios, but summed up along the LineString.
+ * @param lineString the LineString to do this for.
+ * @return A vector of length ratios. Size is lineString.size()-1
+ *
+ * The last element will aways be (near) 1
+ */
+template <typename LineStringT>
+std::vector<double> accumulatedLengthRatios(const LineStringT& lineString);
+
+/**
+ * Calculate the metric signed distance from p to the LineString.
+ * The sign is positive if the point is left of the linestring when projecting
+ * to the xy-plane.
+ *
+ * @param lineString the linestring to check for
+ * @param p the point to check for
+ * @return The metric signed distance in 3d.
+ * @todo not sure if this works as expected
+ *
+ * If the point is before or behind the linestring, this is checked by
+ * extrapolating the first or last segment.
+ */
+template <typename LineString3dT>
+double signedDistance(const LineString3dT& lineString, const BasicPoint3d& p);
+
+/**
+ * Calculate the metric signed distance from p to the LineString.
+ * The sign is positive if the point is left of the linestring when projecting
+ * to the xy-plane.
+ *
+ * @param lineString the linestring to check for
+ * @param p point to check for
+ * @return the metric signed distance in 2d.
+ * @todo not sure if this works as expected
+ *
+ * If the point is before or behind the linestring, this is checked by
+ * extrapolating the first or last segment.
+ */
+template <typename LineString2dT>
+double signedDistance(const LineString2dT& lineString, const BasicPoint2d& p);
+
+/**
+ * Calculate the curvature value given 3 consecutive points.
+ * The curvature value is always positive.
+ *
+ * @param p1, p2, p3 are the points
+ * @return curvature value.
+ *
+ * If any 2 of the 3 points duplicate, return infinity.
+ */
+template <typename Point2dT>
+double curvature2d(const Point2dT& p1, const Point2dT& p2, const Point2dT& p3);
+
+/**
+ *
+ * @brief Transform a point to the coordinates of the linestring
+ *
+ * This computes the Distance along the LineString and distance to the
+ * LineString for the point on the LineString that is closest to the input
+ * point.
+ */
+template <typename LineString2dT>
+ArcCoordinates toArcCoordinates(const LineString2dT& lineString, const BasicPoint2d& point);
+
+/**
+ * Returns the piecewise linearly interpolated point at the given distance.
+ * Negative distances are interpreted backwards from the end.
+ * @param lineString the lineString to iterate. Size must be >0.
+ * @param dist distance along linestring. If negative, the lineString is
+ * iterated in reversed order.
+ * @return The interpolated point (a new point if not perfectly matching)
+ *
+ * This function works in 2d or 3d, depending on the type of the lineString.
+ * If the distance is greater length, the end point is returned (or start point
+ * if <0).
+ */
+template <typename LineStringT>
+traits::BasicPointT<traits::PointType<LineStringT>> interpolatedPointAtDistance(LineStringT lineString, double dist);
+
+/**
+ * @brief returns the cosest point to a position on the linestring
+ * @param lineString the lineString to iterate. Size must be >0.
+ * @param dist distance along linestring. If negative, the lineString is
+ * iterated in reversed order.
+ * @return The closest point.
+ *
+ * This function works in 2d or 3d, depending on the type of the lineString.
+ * If the distance is greater length, the end point is returned (or start point
+ * if <0).
+ */
+template <typename LineStringT>
+traits::PointType<LineStringT> nearestPointAtDistance(LineStringT lineString, double dist);
+
+//! Get the surrounding axis-aligned bounding box in 3d
+template <typename LineString3dT>
+IfLS<LineString3dT, BoundingBox3d> boundingBox3d(const LineString3dT& lineString);
+
+//! Get the surrounding axis-aligned bounding box in 2d
+template <typename LineString2dT>
+IfLS<LineString2dT, BoundingBox2d> boundingBox2d(const LineString2dT& lineString);
+
+/**
+ * @brief Projects the given point in 3d to the LineString.
+ *
+ * If the point is before or behind the lineString, this will be the respective
+ * endpoint of the lineString. If not, this will be some interpolated point on
+ * the linestring that minimizes the distance between pointToProject and the
+ * lineString
+ */
+template <typename LineString3dT, typename = std::enable_if_t<traits::is3D<LineString3dT>()>>
+BasicPoint3d project(const LineString3dT& lineString, const BasicPoint3d& pointToProject);
+
+//! Projects the given point in 2d to the LineString.
+template <typename LineString2dT, typename = std::enable_if_t<traits::is2D<LineString2dT>()>>
+BasicPoint2d project(const LineString2dT& lineString, const BasicPoint2d& pointToProject);
+/**
+ * @brief Computes the projected points on the two linestrings for the shortest
+ * distance
+ *
+ * First element of the pair is located on l1, second on l2
+ */
+template <typename LineString3dT>
+IfLS<LineString3dT, std::pair<BasicPoint3d, BasicPoint3d>> projectedPoint3d(const LineString3dT& l1,
+                                                                            const LineString3dT& l2);
+
+/**
+ * @brief test whether two linestrings intersect in 3d.
+ * @param linestring lanelet to check for
+ * @param otherLinestring other lanelet to check for
+ * @param heightTolerance distance in z below which linestrings are considered
+ * as intersecting (in m)
+ */
+template <typename LineString3dT>
+IfLS<LineString3dT, bool> intersects3d(const LineString3dT& linestring, const LineString3dT& otherLinestring,
+                                       double heightTolerance = 3.);
+
+/**
+ * @brief inverts the two linestrings such that they are parallel
+ * @param left the designated left linestring
+ * @param right the designated right linestring
+ * @return a pair of the left and right linestring (in this order), potentially
+ * inverted.
+ *
+ * Example input:
+ * <<<<<<<<LeftLinestring<<<<<<<
+ *
+ * >>>>>>>>RightLinestring>>>>>>
+ *
+ * Example output. Left was inverted:
+ * >>>>>>>>First>>>>>>>>>>>>>>>>
+ *
+ * >>>>>>>>Second>>>>>>>>>>>>>>>
+ *
+ */
+template <typename LineString1T, typename LineString2T>
+std::pair<LineString1T, LineString2T> align(LineString1T left, LineString2T right);
+
+/**
+ * @brief create a point by moving laterally arcCoords.distance from the linestring point at arcCoords.length
+ * @param lineString line string of which an offset point is generated
+ * @param arcCoords the coordinates the resulting point has (arcLength.distance > 0 creates a point on the left)
+ */
+template <typename LineString2dT>
+BasicPoint2d fromArcCoordinates(const LineString2dT& lineString, const ArcCoordinates& arcCoords);
+
+/**
+ * @brief create a linestring that is offset to the original one. Guarantees no self-intersections and no inversions in
+ * the result.
+ * @param lineString linestring to offset
+ * @param distance offset distance (left = positive)
+ * @throws InvalidInpuError if the linestring has less than 2 points
+ * @throws GeometryError if the linestring is partially inverted after applying offset
+ *
+ * @details The offset is generated by shifting points of the line string and making a new line string from them. The
+ * distance of any points on the generated line string is guaranteed to be between 1 and sqrt(2) times the specified
+ * offset distance. Notice that the reversed case is not true. On angles exceeding 90 degrees, a new point is introduced
+ * to the resulting line string to fulfill this guarantee.
+ */
+template <typename LineString2dT>
+BasicLineString2d offset(const LineString2dT& lineString, double distance);
+
+/**
+ * @brief create a linestring that is offset to the original one.
+ * the result.
+ * @param lineString linestring to offset
+ * @param distance offset distance (left = positive)
+ * @throws InvalidInpuError if the linestring has less than 2 points
+ * @throws GeometryError if the linestring is partially inverted after applying offset
+ *
+ * @details The offset is generated by shifting points of the line string and making a new line string from them. No
+ * exception is thrown when
+ */
+template <typename LineString2dT>
+BasicLineString2d offsetNoThrow(const LineString2dT& lineString, double distance);
+
+/**
+ * @brief find the segment on a 3d line string that is closest to a given point, determined by boost::geometry::distance
+ * @param lineString the line string the distance function is evaluated on
+ * @param pointToProject 3d point that is projected on to the linestring
+ * @returns a new segment that is identical to the closest one on the line string
+ */
+template <typename LineString3dT, typename = std::enable_if_t<traits::is3D<LineString3dT>()>>
+Segment<traits::PointType<LineString3dT>> closestSegment(const LineString3dT& lineString,
+                                                         const BasicPoint3d& pointToProject);
+/**
+ * @brief find the segment on a 2d line string that is closest to a given point, determined by boost::geometry::distance
+ * @param lineString the line string the distance function is evaluated on
+ * @param pointToProject 2d point that is projected on to the linestring
+ * @returns a new segment that is identical to the closest one on the line string
+ */
+template <typename LineString2dT, typename = std::enable_if_t<traits::is2D<LineString2dT>()>>
+Segment<traits::PointType<LineString2dT>> closestSegment(const LineString2dT& lineString,
+                                                         const BasicPoint2d& pointToProject);
+
+/**
+ * @brief find the segment on a 2d line string that is closest to a given point, determined by boost::geometry::distance
+ * @param lineString the line string the distance function is evaluated on
+ * @param pointToProject 2d point that is projected on to the linestring
+ * @returns a new segment that is identical to the closest one on the line string
+ */
+Segment<BasicPoint2d> closestSegment(const BasicLineString2d& lineString, const BasicPoint2d& pointToProject);
+
+/**
+ * @brief find the segment on a 3d line string that is closest to a given point, determined by boost::geometry::distance
+ * @param lineString the line string the distance function is evaluated on
+ * @param pointToProject 3d point that is projected on to the linestring
+ * @returns a new segment that is identical to the closest one on the line string
+ */
+Segment<BasicPoint3d> closestSegment(const BasicLineString3d& lineString, const BasicPoint3d& pointToProject);
+}  // namespace geometry
+}  // namespace lanelet
+
+#include "impl/LineString.h"

src/map/lanelet2/include/lanelet2_core/geometry/Point.h

@@ -0,0 +1,79 @@
+#pragma once
+#include <boost/version.hpp>
+#if BOOST_VERSION < 106300 && BOOST_VERSION >= 106200
+// Boost 1.62 is missing an iostream include...
+#include <iostream>
+#endif
+#include <boost/geometry/algorithms/distance.hpp>
+#include <boost/geometry/geometries/geometries.hpp>
+#include <boost/geometry/geometries/register/point.hpp>
+#include <boost/geometry/strategies/strategies.hpp>
+
+#include "lanelet2_core/primitives/Point.h"
+
+/***********************************************************************
+ *                   BOOST GEOMETRY REGISTRATIONS                      *
+ ***********************************************************************/
+BOOST_GEOMETRY_REGISTER_POINT_3D(lanelet::BasicPoint3d, double, cs::cartesian, x(), y(), z())
+BOOST_GEOMETRY_REGISTER_POINT_2D(lanelet::BasicPoint2d, double, cs::cartesian, x(), y())
+BOOST_GEOMETRY_REGISTER_POINT_2D(Eigen::Vector2d, double, cs::cartesian, x(), y())
+BOOST_GEOMETRY_REGISTER_POINT_2D(lanelet::Point2d, double, cs::cartesian, x(), y())
+BOOST_GEOMETRY_REGISTER_POINT_2D_CONST(lanelet::ConstPoint2d, double, cs::cartesian, x(), y())
+BOOST_GEOMETRY_REGISTER_POINT_3D(lanelet::Point3d, double, cs::cartesian, x(), y(), z())
+BOOST_GEOMETRY_REGISTER_POINT_3D_CONST(lanelet::ConstPoint3d, double, cs::cartesian, x(), y(), z())
+
+namespace boost {
+namespace geometry {
+// Help boost with type deduction for proxies
+template <typename Policy>
+struct robust_point_type<const lanelet::BasicPoint2d, Policy> {
+  using type = lanelet::BasicPoint2d;  // NOLINT
+};
+
+template <>
+struct robust_point_type<const lanelet::BasicPoint2d, detail::no_rescale_policy> {
+  using type = lanelet::BasicPoint2d;  // NOLINT
+};
+
+}  // namespace geometry
+}  // namespace boost
+
+namespace lanelet {
+namespace geometry {
+namespace internal {
+template <typename T, typename Enable = void>
+struct GetGeometry {
+  static inline auto twoD(const T& geometry) { return geometry; }
+  static inline auto threeD(const T& geometry) { return geometry; }
+};
+
+template <typename T>
+struct GetGeometry<T, IfPT<T, void>> {
+  static inline auto twoD(const T& geometry) { return traits::to2D(geometry); }
+  static inline auto threeD(const T& geometry) { return traits::to3D(geometry); }
+};
+
+template <typename T1, typename T2>
+constexpr bool isTrivialDistance() {
+  return traits::noRegelem<T1>() && traits::noRegelem<T2>() &&
+         !(traits::isCategory<T1, traits::LineStringTag>() && traits::isCategory<T2, traits::LineStringTag>() &&
+           traits::is3D<T1>());
+}
+}  // namespace internal
+using boost::geometry::distance;
+
+//! Calculates the distance of two lanelet2 geometries in 2D, converting them if necessary
+template <typename Geometry1T, typename Geometry2T>
+auto distance2d(const Geometry1T& p1, const Geometry2T& p2)
+    -> std::enable_if_t<internal::isTrivialDistance<Geometry1T, Geometry2T>(), double> {
+  return distance(internal::GetGeometry<Geometry1T>::twoD(p1), internal::GetGeometry<Geometry2T>::twoD(p2));
+}
+
+//! Calculates the distance of two lanelet2 geometries in 3D, converting them if necessary
+template <typename Geometry1T, typename Geometry2T>
+auto distance3d(const Geometry1T& p1, const Geometry2T& p2)
+    -> std::enable_if_t<internal::isTrivialDistance<Geometry1T, Geometry2T>(), double> {
+  return distance(internal::GetGeometry<Geometry1T>::threeD(p1), internal::GetGeometry<Geometry2T>::threeD(p2));
+}
+}  // namespace geometry
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/geometry/Polygon.h

@@ -0,0 +1,344 @@
+#pragma once
+#include <boost/geometry/geometries/register/ring.hpp>
+
+#include "lanelet2_core/geometry/LineString.h"
+#include "lanelet2_core/primitives/CompoundPolygon.h"
+#include "lanelet2_core/primitives/Polygon.h"
+
+/***********************************************************************
+ *                   BOOST GEOMETRY REGISTRATIONS                      *
+ ***********************************************************************/
+
+// Register our Polygons
+BOOST_GEOMETRY_REGISTER_RING(lanelet::Polygon3d)
+BOOST_GEOMETRY_REGISTER_RING(lanelet::ConstPolygon3d)
+BOOST_GEOMETRY_REGISTER_RING(lanelet::Polygon2d)
+BOOST_GEOMETRY_REGISTER_RING(lanelet::ConstPolygon2d)
+BOOST_GEOMETRY_REGISTER_RING(lanelet::BasicPolygon2d)
+BOOST_GEOMETRY_REGISTER_RING(lanelet::BasicPolygon3d)
+BOOST_GEOMETRY_REGISTER_RING(lanelet::ConstHybridPolygon2d)
+BOOST_GEOMETRY_REGISTER_RING(lanelet::ConstHybridPolygon3d)
+BOOST_GEOMETRY_REGISTER_RING(lanelet::CompoundPolygon2d)
+BOOST_GEOMETRY_REGISTER_RING(lanelet::CompoundPolygon3d)
+BOOST_GEOMETRY_REGISTER_RING(lanelet::CompoundHybridPolygon2d)
+BOOST_GEOMETRY_REGISTER_RING(lanelet::CompoundHybridPolygon3d)
+
+// traits for boost::geometry. All lanelet polygons are open and clockwise.
+namespace boost {
+namespace geometry {
+namespace traits {
+// traits for lanelet::Polygon
+template <>
+struct point_order<lanelet::Polygon3d> {
+  static const order_selector value = clockwise;
+};
+template <>
+struct closure<lanelet::Polygon3d> {
+  static const closure_selector value = open;
+};
+
+// traits for lanelet::ConstPolygon
+template <>
+struct point_order<lanelet::ConstPolygon3d> {
+  static const order_selector value = clockwise;
+};
+template <>
+struct closure<lanelet::ConstPolygon3d> {
+  static const closure_selector value = open;
+};
+
+// traits for lanelet::Polygon2d
+template <>
+struct point_order<lanelet::Polygon2d> {
+  static const order_selector value = clockwise;
+};
+template <>
+struct closure<lanelet::Polygon2d> {
+  static const closure_selector value = open;
+};
+
+// traits for lanelet::ConstPolygon2d
+template <>
+struct point_order<lanelet::ConstPolygon2d> {
+  static const order_selector value = clockwise;
+};
+template <>
+struct closure<lanelet::ConstPolygon2d> {
+  static const closure_selector value = open;
+};
+
+// traits for lanelet::BasicPolygon2d
+template <>
+struct point_order<lanelet::BasicPolygon2d> {
+  static const order_selector value = clockwise;
+};
+template <>
+struct closure<lanelet::BasicPolygon2d> {
+  static const closure_selector value = open;
+};
+
+// traits for lanelet::BasicPolygon3d
+template <>
+struct point_order<lanelet::BasicPolygon3d> {
+  static const order_selector value = clockwise;
+};
+template <>
+struct closure<lanelet::BasicPolygon3d> {
+  static const closure_selector value = open;
+};
+
+// traits for lanelet::ConstHybridPolygon2d
+template <>
+struct point_order<lanelet::ConstHybridPolygon2d> {
+  static const order_selector value = clockwise;
+};
+template <>
+struct closure<lanelet::ConstHybridPolygon2d> {
+  static const closure_selector value = open;
+};
+
+// traits for lanelet::ConstHybridPolygon
+template <>
+struct point_order<lanelet::ConstHybridPolygon3d> {
+  static const order_selector value = clockwise;
+};
+template <>
+struct closure<lanelet::ConstHybridPolygon3d> {
+  static const closure_selector value = open;
+};
+// traits for lanelet::CompoundHybridPolygon2d
+template <>
+struct point_order<lanelet::CompoundHybridPolygon2d> {
+  static const order_selector value = clockwise;
+};
+template <>
+struct closure<lanelet::CompoundHybridPolygon2d> {
+  static const closure_selector value = open;
+};
+
+// traits for lanelet::CompoundHybridPolygon3d
+template <>
+struct point_order<lanelet::CompoundHybridPolygon3d> {
+  static const order_selector value = clockwise;
+};
+template <>
+struct closure<lanelet::CompoundHybridPolygon3d> {
+  static const closure_selector value = open;
+};
+
+// traits for lanelet::CompoundPolygon2d
+template <>
+struct point_order<lanelet::CompoundPolygon2d> {
+  static const order_selector value = clockwise;
+};
+template <>
+struct closure<lanelet::CompoundPolygon2d> {
+  static const closure_selector value = open;
+};
+
+// traits for lanelet::CompoundPolygon3d
+template <>
+struct point_order<lanelet::CompoundPolygon3d> {
+  static const order_selector value = clockwise;
+};
+template <>
+struct closure<lanelet::CompoundPolygon3d> {
+  static const closure_selector value = open;
+};
+
+// traits for BasicPolygonWithHoles3d to register as a boost::polygon (taken
+// from
+// http://www.boost.org/doc/libs/1_58_0/libs/geometry/doc/html/geometry/examples/example_source_code__adapting_a_legacy_geometry_object_model.html)
+using LLPolygon3d = lanelet::BasicPolygonWithHoles3d;
+template <>
+struct tag<LLPolygon3d> {
+  using type = polygon_tag;  // NOLINT
+};
+template <>
+struct ring_const_type<LLPolygon3d> {
+  using type = const lanelet::BasicPolygon3d&;  // NOLINT
+};
+template <>
+struct ring_mutable_type<LLPolygon3d> {
+  using type = lanelet::BasicPolygon3d&;  // NOLINT
+};
+template <>
+struct interior_const_type<LLPolygon3d> {
+  using type = const lanelet::BasicPolygons3d&;  // NOLINT
+};
+template <>
+struct interior_mutable_type<LLPolygon3d> {
+  using type = lanelet::BasicPolygons3d&;  // NOLINT
+};
+
+template <>
+struct exterior_ring<LLPolygon3d> {
+  static lanelet::BasicPolygon3d& get(LLPolygon3d& p) { return p.outer; }
+  static const lanelet::BasicPolygon3d& get(LLPolygon3d const& p) { return p.outer; }
+};
+
+template <>
+struct interior_rings<LLPolygon3d> {
+  static lanelet::BasicPolygons3d& get(LLPolygon3d& p) { return p.inner; }
+  static const lanelet::BasicPolygons3d& get(LLPolygon3d const& p) { return p.inner; }
+};
+
+// traits for BasicPolygonWithHoles2d to register as a boost::polygon (taken
+// from
+// http://www.boost.org/doc/libs/1_58_0/libs/geometry/doc/html/geometry/examples/example_source_code__adapting_a_legacy_geometry_object_model.html)
+using LLPolygon2d = lanelet::BasicPolygonWithHoles2d;
+template <>
+struct tag<LLPolygon2d> {
+  using type = polygon_tag;  // NOLINT
+};
+template <>
+struct ring_const_type<LLPolygon2d> {
+  using type = const lanelet::BasicPolygon2d&;  // NOLINT
+};
+template <>
+struct ring_mutable_type<LLPolygon2d> {
+  using type = lanelet::BasicPolygon2d&;  // NOLINT
+};
+template <>
+struct interior_const_type<LLPolygon2d> {
+  using type = const lanelet::BasicPolygons2d&;  // NOLINT
+};
+template <>
+struct interior_mutable_type<LLPolygon2d> {
+  using type = lanelet::BasicPolygons2d&;  // NOLINT
+};
+
+template <>
+struct exterior_ring<LLPolygon2d> {
+  static lanelet::BasicPolygon2d& get(LLPolygon2d& p) { return p.outer; }
+  static const lanelet::BasicPolygon2d& get(LLPolygon2d const& p) { return p.outer; }
+};
+
+template <>
+struct interior_rings<LLPolygon2d> {
+  static lanelet::BasicPolygons2d& get(LLPolygon2d& p) { return p.inner; }
+  static const lanelet::BasicPolygons2d& get(LLPolygon2d const& p) { return p.inner; }
+};
+}  // namespace traits
+}  // namespace geometry
+}  // namespace boost
+
+/***********************************************************************
+ *                   ALGORITHMS                                        *
+ ***********************************************************************/
+namespace lanelet {
+namespace geometry {
+using boost::geometry::area;
+using boost::geometry::within;
+
+/**
+ * @brief computes the distance of the outer bounds of two polygons in 3d.
+ *
+ * There is no such implementation in boost::geometry, so we implemented our own
+ * solution using an RTree for more efficient computation.
+ */
+template <typename Polygon3dT>
+IfPoly<Polygon3dT, double> distanceToBorder3d(const Polygon3dT& poly1, const Polygon3dT& poly2);
+
+/**
+ * Get the surrounding bounding box
+ *
+ * @return The enclosing axis aligned bounding box of all points.
+ */
+template <typename Polygon3dT>
+IfPoly<Polygon3dT, BoundingBox3d> boundingBox3d(const Polygon3dT& polygon);
+
+template <typename Polygon2dT>
+IfPoly<Polygon2dT, BoundingBox2d> boundingBox2d(const Polygon2dT& polygon);
+
+template <typename Polygon3dT>
+IfPoly<Polygon3dT, BoundingBox3d> boundingBox3d(const Polygon3dT& polygon) {
+  static_assert(traits::is3D<Polygon3dT>(), "Please call this function with a 3D type!");
+  BoundingBox3d bb;
+  for (const auto& p : polygon) {
+    bb.extend(traits::toBasicPoint(p));
+  }
+  return bb;
+}
+
+template <typename Polygon2dT>
+IfPoly<Polygon2dT, BoundingBox2d> boundingBox2d(const Polygon2dT& polygon) {
+  static_assert(traits::is2D<Polygon2dT>(), "Please call this function with a 2D type!");
+  BoundingBox2d bb;
+  for (const auto& p : polygon) {
+    bb.extend(traits::toBasicPoint(p));
+  }
+  return bb;
+}
+
+template <typename Polygon3dT>
+IfPoly<Polygon3dT, std::pair<BasicPoint3d, BasicPoint3d>> projectedBorderPoint3d(const Polygon3dT& l1,
+                                                                                 const Polygon3dT& l2);
+
+template <typename PolygonT>
+IfPoly<PolygonT, bool> touches2d(const PolygonT& poly1, const PolygonT& poly2);
+
+template <typename Polygon2dT>
+IfPoly<Polygon2dT, bool> overlaps2d(const Polygon2dT& poly1, const Polygon2dT& poly2);
+
+template <typename Polygon3dT>
+IfPoly<Polygon3dT, bool> overlaps3d(const Polygon3dT& poly1, const Polygon3dT& poly2, double heightTolerance);
+
+/**
+ *
+ * @brief Split a concave polygon into convex parts
+ *
+ * Internally computes a Delaunay triangulation which is greedily merged into convex parts. Guaranteed to deliver a
+ * partition with at most 2N+1 parts if N is minimum partition. Since boost only allows integral types, the
+ * triangulation is only millimeter-accurate, even though the resulting points are identical to the input points.
+ *
+ * @tparam Polygon2dT Polygon Type
+ * @param poly Polygon to partition
+ * @return a list of convex polygons yielding the original polygon if merged.
+ */
+template <typename Polygon2dT>
+IfPoly<Polygon2dT, BasicPolygons2d> convexPartition(const Polygon2dT& poly);
+/**
+ *
+ * @brief Split a concave polygon into convex parts
+ *
+ * Internally computes a Delaunay triangulation which is greedily merged into convex parts. Guaranteed to deliver a
+ * partition with at most 2N+1 parts if N is minimum partition. Since boost only allows integral types, the
+ * triangulation is only millimeter-accurate, even though the resulting points are identical to the input points.
+ *
+ * @param poly BasicPolygon2d to partition
+ * @return a list of convex polygons yielding the original polygon if merged.
+ */
+template <>
+IfPoly<BasicPolygon2d, BasicPolygons2d> convexPartition<BasicPolygon2d>(const BasicPolygon2d& poly);
+using IndexedTriangle = std::array<size_t, 3>;
+using IndexedTriangles = std::vector<IndexedTriangle>;
+
+/**
+ *
+ * @brief Split a concave polygon into triangles
+ *
+ * Internally computes a Delaunay triangulation.
+ *
+ * @tparam Polygon2dT Polygon Type
+ * @param poly Polygon to partition
+ * @return a list of indices sets. Each set refers to a triangle in the original polygon.
+ */
+template <typename Polygon2dT>
+IfPoly<Polygon2dT, IndexedTriangles> triangulate(const Polygon2dT& poly);
+/**
+ *
+ * @brief Split a concave polygon into triangles
+ *
+ * Internally computes a Delaunay triangulation.
+ *
+ * @param poly Polygon to partition
+ * @return a list of indices sets. Each set refers to a triangle in the original polygon.
+ */
+template <>
+IfPoly<BasicPolygon2d, IndexedTriangles> triangulate<BasicPolygon2d>(const BasicPolygon2d& poly);
+
+}  // namespace geometry
+}  // namespace lanelet
+
+#include "impl/Polygon.h"

src/map/lanelet2/include/lanelet2_core/geometry/RegulatoryElement.h

@@ -0,0 +1,46 @@
+#pragma once
+#include "lanelet2_core/primitives/BoundingBox.h"
+#include "lanelet2_core/primitives/RegulatoryElement.h"
+
+namespace lanelet {
+namespace geometry {
+/**
+ * @brief compute the 2d bounding box around all RuleParameters contained in
+ * this RegulatoryElement
+ * @see boundingBox3d
+ *
+ * Note that this does not include the lanelet that refers to this regulatry
+ * element, only the the roles in the regulatory element. The bounding box can
+ * also be empty if the RegulatoryElement does not contain any data.
+ */
+BoundingBox2d boundingBox2d(const RegulatoryElement& regElem);
+
+/**
+ * @brief compute bounding box in 2d
+ *
+ * See non-shared-ptr version.
+ */
+inline BoundingBox2d boundingBox2d(const RegulatoryElementConstPtr& regElem) { return boundingBox2d(*regElem); }
+
+/**
+ * @brief compute the 3d bounding box around all RuleParameters contained in
+ * this RegulatoryElement
+ * @see boundingBox2d
+ *
+ * Note that this does not include the lanelet that refers to this regulatry
+ * element, only the the roles in the regulatory element. The bounding box can
+ * also be empty if the regulatory elemnt does not contain any data.
+ */
+BoundingBox3d boundingBox3d(const RegulatoryElement& regElem);
+inline BoundingBox3d boundingBox3d(const RegulatoryElementConstPtr& regElem) { return boundingBox3d(*regElem); }
+
+//! computes the distance between a point and the closest RuleParameter of a
+//! RegulatoryElement
+double distance2d(const RegulatoryElement& regElem, const BasicPoint2d& p);
+
+//! See non-shared-ptr version
+inline double distance2d(const RegulatoryElementConstPtr& regElem1, const BasicPoint2d& p) {
+  return distance2d(*regElem1, p);
+}
+}  // namespace geometry
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/geometry/impl/Area.h

@@ -0,0 +1,146 @@
+#pragma once
+#include <lanelet2_core/geometry/Area.h>
+
+#include "lanelet2_core/geometry/Point.h"
+#include "lanelet2_core/geometry/Polygon.h"
+#include "lanelet2_core/primitives/Area.h"
+#include "lanelet2_core/primitives/Lanelet.h"
+
+namespace lanelet {
+namespace geometry {
+namespace internal {
+template <typename T>
+struct GetGeometry<T, IfAr<T, void>> {
+  static inline auto twoD(const T& geometry) { return geometry.basicPolygonWithHoles2d(); }
+  static inline auto threeD(const T& geometry) { return geometry.basicPolygonWithHoles3d(); }
+};
+}  // namespace internal
+
+template <typename AreaT>
+IfAr<AreaT, bool> inside(const AreaT& area, const BasicPoint2d& point) {
+  return boost::geometry::covered_by(point, area.basicPolygonWithHoles2d());
+}
+
+template <typename AreaT>
+IfAr<AreaT, BoundingBox2d> boundingBox2d(const AreaT& area) {
+  return boundingBox2d(traits::to2D(area.outerBoundPolygon()));
+}
+
+template <typename AreaT>
+IfAr<AreaT, BoundingBox3d> boundingBox3d(const AreaT& area) {
+  return boundingBox3d(area.outerBoundPolygon());
+}
+
+template <typename Area1T, typename Area2T>
+IfAr<Area1T, bool> intersects2d(const Area1T& area, const Area2T& otherArea) {
+  if (area == otherArea) {
+    return true;
+  }
+  return intersects(area.basicPolygonWithHoles2d(), otherArea.basicPolygonWithHoles2d());
+}
+
+template <typename AreaT>
+IfAr<AreaT, bool> overlaps2d(const AreaT& area, const AreaT& otherArea) {
+  return overlaps2d(traits::to2D(area.outerBoundPolygon()), traits::to2D(otherArea.outerBoundPolygon()));
+}
+
+template <typename AreaT>
+IfAr<AreaT, bool> overlaps3d(const AreaT& area, const AreaT& otherArea, double heightTolerance) {
+  return overlaps3d(area.outerBoundPolygon(), otherArea.outerBoundPolygon(), heightTolerance);
+}
+
+template <typename AreaT, typename LaneletT>
+IfAr<AreaT, IfLL<LaneletT, bool>> overlaps2d(const AreaT& area, const LaneletT& lanelet) {
+  return overlaps2d(utils::to2D(area.outerBoundPolygon()), lanelet.polygon2d());
+}
+
+template <typename AreaT, typename LaneletT>
+IfAr<AreaT, IfLL<LaneletT, bool>> overlaps3d(const AreaT& area, const LaneletT& lanelet, double heightTolerance) {
+  return overlaps3d(area.outerBoundPolygon(), lanelet.polygon3d(), heightTolerance);
+}
+
+inline bool leftOf(const ConstLanelet& right, const ConstArea& left) {
+  return utils::anyOf(left.outerBound(), [&right](auto& bound) { return bound.invert() == right.leftBound(); });
+}
+
+inline bool rightOf(const ConstLanelet& left, const ConstArea& area) { return leftOf(left.invert(), area); }
+
+inline bool follows(const ConstLanelet& prev, const ConstArea& next) {
+  auto outer = next.outerBound();
+  return utils::anyOf(outer,
+                      [p1 = prev.leftBound().back(), p2 = prev.rightBound().back()](const ConstLineString3d& ls) {
+                        return (ls.front() == p1 && ls.back() == p2) || (ls.front() == p2 && ls.back() == p1);
+                      });
+}
+
+inline bool follows(const ConstArea& prev, const ConstLanelet& next) { return follows(next.invert(), prev); }
+
+inline bool adjacent(const ConstArea& area1, const ConstArea& area2) {
+  auto outer1 = area1.outerBoundPolygon();
+  auto outer2 = area2.outerBoundPolygon();
+  auto rotatedNext = [&ls = outer2](auto iter) { return iter + 1 == ls.end() ? ls.begin() : iter + 1; };
+  for (auto i = 0u; i < outer1.numSegments(); ++i) {
+    auto segment = outer1.segment(i);
+    auto second = std::find(outer2.begin(), outer2.end(), segment.second);
+    if (second != outer2.end() && *rotatedNext(second) == segment.first) {
+      return true;
+    }
+  }
+  return false;
+}
+
+inline Optional<ConstLineString3d> determineCommonLinePreceding(const ConstLanelet& ll, const ConstArea& ar) {
+  return utils::findIf(ar.outerBound(), [p1 = ll.leftBound().back(), p2 = ll.rightBound().back()](auto& boundLs) {
+    return (boundLs.back() == p1 && boundLs.front() == p2);
+  });
+}
+
+inline Optional<ConstLineString3d> determineCommonLineFollowing(const ConstArea& ar, const ConstLanelet& ll) {
+  return determineCommonLinePreceding(ll.invert(), ar);
+}
+
+inline Optional<ConstLineString3d> determineCommonLineFollowingOrPreceding(const ConstArea& ar,
+                                                                           const ConstLanelet& ll) {
+  return utils::findIf(ar.outerBound(), [p1 = ll.leftBound().front(), p2 = ll.rightBound().front(),
+                                         p3 = ll.leftBound().back(), p4 = ll.rightBound().back()](auto& boundLs) {
+    return ((boundLs.back() == p2 && boundLs.front() == p1) || (boundLs.back() == p3 && boundLs.front() == p4));
+  });
+}
+
+inline Optional<ConstLineString3d> determineCommonLineSideways(const ConstLanelet& ll, const ConstArea& ar) {
+  auto res = determineCommonLineLeft(ll, ar);
+  return res ? res : determineCommonLineRight(ll, ar);
+}
+
+inline Optional<ConstLineString3d> determineCommonLineSideways(const ConstArea& ar, const ConstLanelet& ll) {
+  return utils::findIf(ar.outerBound(), [lb = ll.leftBound(), rb = ll.rightBound()](auto& boundLs) {
+    return boundLs == lb.invert() || boundLs == rb;
+  });
+}
+
+inline Optional<ConstLineString3d> determineCommonLine(const ConstArea& ar1, const ConstArea& ar2) {
+  return utils::findIf(ar1.outerBound(), [&ar2](auto& ar1Bound) {
+    return !!utils::findIf(ar2.outerBound(),
+                           [ar1Bound = ar1Bound.invert()](auto& ar2Bound) { return ar2Bound == ar1Bound; });
+  });
+}
+
+inline Optional<ConstLineString3d> determineCommonLine(const ConstArea& ar, const ConstLanelet& ll) {
+  auto res = determineCommonLineFollowingOrPreceding(ar, ll);
+  return res ? res : determineCommonLineSideways(ar, ll);
+}
+
+inline Optional<ConstLineString3d> determineCommonLineLeft(const ConstLanelet& right, const ConstArea& left) {
+  auto res = determineCommonLineRight(right.invert(), left);
+  if (res) {
+    return res->invert();
+  }
+  return {};
+}
+
+inline Optional<ConstLineString3d> determineCommonLineRight(const ConstLanelet& left, const ConstArea& right) {
+  return utils::findIf(right.outerBound(), [&left](auto& bound) { return bound == left.rightBound(); });
+}
+
+}  // namespace geometry
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/geometry/impl/Lanelet.h

@@ -0,0 +1,199 @@
+#pragma once
+#include <boost/version.hpp>
+#if BOOST_VERSION > 105800
+#include <string.h>  // NOLINT
+
+#include <boost/geometry/algorithms/relate.hpp>
+#else
+#include <boost/geometry/algorithms/detail/relate/relate.hpp>
+#endif
+#include "lanelet2_core/geometry/Polygon.h"
+#include "lanelet2_core/primitives/Lanelet.h"
+#include "lanelet2_core/primitives/LineString.h"
+
+namespace lanelet {
+namespace geometry {
+namespace internal {
+template <typename T>
+struct GetGeometry<T, IfLL<T, void>> {
+  static inline auto twoD(const T& geometry) { return traits::toHybrid(geometry.polygon2d()); }
+  static inline auto threeD(const T& geometry) { return traits::toHybrid(geometry.polygon3d()); }
+};
+}  // namespace internal
+
+template <typename LaneletT>
+IfLL<LaneletT, bool> inside(const LaneletT& lanelet, const BasicPoint2d& point) {
+  return boost::geometry::covered_by(point, lanelet.polygon2d());
+}
+
+template <typename LaneletT>
+double distanceToCenterline2d(const LaneletT& lanelet, const BasicPoint2d& point) {
+  return distance(lanelet.centerline2d(), point);
+}
+
+template <typename LaneletT>
+double distanceToCenterline3d(const LaneletT& lanelet, const BasicPoint3d& point) {
+  return distance(lanelet.centerline3d(), point);
+}
+
+template <typename LaneletT>
+IfLL<LaneletT, BoundingBox2d> boundingBox2d(const LaneletT& lanelet) {
+  BoundingBox2d bb = boundingBox2d(lanelet.leftBound2d());
+  bb.extend(boundingBox2d(lanelet.rightBound2d()));
+  return bb;
+}
+
+template <typename LaneletT>
+IfLL<LaneletT, BoundingBox3d> boundingBox3d(const LaneletT& lanelet) {
+  BoundingBox3d bb = boundingBox3d(lanelet.leftBound3d());
+  bb.extend(boundingBox3d(lanelet.rightBound3d()));
+  return bb;
+}
+
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, bool> intersects2d(const Lanelet1T& lanelet, const Lanelet2T& otherLanelet) {
+  if (lanelet.constData() == otherLanelet.constData()) {
+    return true;
+  }
+  CompoundHybridPolygon2d p1(lanelet.polygon2d());
+  CompoundHybridPolygon2d p2(otherLanelet.polygon2d());
+  return intersects(p1, p2);
+}
+
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, bool> overlaps2d(const Lanelet1T& lanelet, const Lanelet2T& otherLanelet) {
+  if (lanelet.constData() == otherLanelet.constData()) {
+    return true;
+  }
+  // check if lanelets are neighbours (share a border)
+  if (lanelet.rightBound() == otherLanelet.leftBound() || lanelet.leftBound() == otherLanelet.rightBound() ||
+      lanelet.leftBound().invert() == otherLanelet.leftBound() ||
+      lanelet.rightBound().invert() == otherLanelet.rightBound() || follows(lanelet, otherLanelet) ||
+      follows(otherLanelet, lanelet) || follows(lanelet.invert(), otherLanelet) ||
+      follows(otherLanelet.invert(), lanelet)) {
+    return false;
+  }
+  if (!intersects(boundingBox2d(lanelet), boundingBox2d(otherLanelet))) {
+    return false;
+  }
+  CompoundHybridPolygon2d p1(lanelet.polygon2d());
+  CompoundHybridPolygon2d p2(otherLanelet.polygon2d());
+
+#if BOOST_VERSION > 105800
+  using Mask = boost::geometry::de9im::static_mask<'T', '*', '*', '*', '*', '*', '*', '*', '*'>;
+  return boost::geometry::relate(p1, p2, Mask());
+#else
+  // boost 1.58 did not officially support "relate"
+  using Mask = boost::geometry::detail::relate::static_mask<'T', '*', '*', '*', '*', '*', '*', '*', '*'>;
+  return boost::geometry::detail::relate::relate<Mask>(p1, p2);
+#endif
+}
+
+template <typename Lanelet1T, typename Lanelet2T>
+BasicPoints2d intersectCenterlines2d(const Lanelet1T& lanelet, const Lanelet2T& otherLanelet,
+                                     std::vector<double>* distanceThis, std::vector<double>* distanceOther) {
+  //! @todo implement intersect_centerlines
+  BasicPoints2d intersections;
+  const auto centerline = traits::toHybrid(lanelet.centerline2d());
+  const auto otherCenterline = traits::toHybrid(otherLanelet.centerline2d());
+  boost::geometry::intersection(centerline, otherCenterline, intersections);
+  if (distanceThis != nullptr) {
+    std::transform(intersections.begin(), intersections.end(), std::back_inserter(*distanceThis),
+                   [&centerline](const auto& elem) { return toArcCoordinates(centerline, elem).length; });
+  }
+  if (distanceOther != nullptr) {
+    std::transform(intersections.begin(), intersections.end(), std::back_inserter(*distanceOther),
+                   [&centerline](const auto& elem) { return toArcCoordinates(centerline, elem).length; });
+  }
+  return intersections;
+}
+
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, bool> intersects3d(const Lanelet1T& lanelet, const Lanelet2T& otherLanelet, double heightTolerance) {
+  if (intersects2d(lanelet, otherLanelet)) {
+    auto projPts = projectedPoint3d(lanelet.centerline3d(), otherLanelet.centerline3d());
+    return std::abs(projPts.first.z() - projPts.second.z()) < heightTolerance;
+  }
+  return false;
+}
+
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, bool> overlaps3d(const Lanelet1T& lanelet, const Lanelet2T& otherLanelet, double heightTolerance) {
+  if (overlaps2d(lanelet, otherLanelet)) {
+    auto projPts = projectedPoint3d(lanelet.centerline3d(), otherLanelet.centerline3d());
+    return std::abs(projPts.first.z() - projPts.second.z()) < heightTolerance;
+  }
+  return false;
+}
+
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, IfLL<Lanelet2T, bool>> leftOf(const Lanelet1T& left, const Lanelet2T& right) {
+  return left.rightBound() == right.leftBound();
+}
+
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, IfLL<Lanelet2T, bool>> rightOf(const Lanelet1T& right, const Lanelet2T& left) {
+  return leftOf(left, right);
+}
+
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, IfLL<Lanelet2T, bool>> follows(const Lanelet1T& prev, const Lanelet2T& next) {
+  return !prev.leftBound().empty() && !prev.rightBound().empty() && !next.leftBound().empty() &&
+         !next.rightBound().empty() && prev.leftBound().back() == next.leftBound().front() &&
+         prev.rightBound().back() == next.rightBound().front();
+}
+
+template <typename Lanelet1T, typename Lanelet2T>
+IfLL<Lanelet1T, IfLL<Lanelet2T, Optional<ConstLineString3d>>> determineCommonLine(const Lanelet1T& ll,
+                                                                                  const Lanelet2T& other,
+                                                                                  bool allowInverted) {
+  if (leftOf(other, ll)) {
+    return ll.leftBound();
+  }
+  if (rightOf(other, ll)) {
+    return ll.rightBound();
+  }
+  if (allowInverted) {
+    if (leftOf(other.invert(), ll)) {
+      return ll.leftBound();
+    }
+    if (rightOf(other.invert(), ll)) {
+      return ll.rightBound();
+    }
+  }
+  return {};
+}
+
+template <typename LaneletT>
+Velocity maxCurveSpeed(const LaneletT& /*lanelet*/, const BasicPoint2d& /*position*/,
+                       const Acceleration& /*maxLateralAcceleration*/) {
+  //!< @todo Implement maxCurveSpeed
+  throw LaneletError("This is not yet implemented!");
+}
+
+template <typename LaneletT>
+double approximatedLength2d(const LaneletT& lanelet) {
+  double length = 0.;
+  auto line = lanelet.leftBound2d();
+  auto step = std::max(size_t{1}, line.size() / 10);
+  for (auto i1 = size_t{}, i2 = step; i2 < line.size(); i1 += step, i2 += step) {
+    length += distance(line[i1], line[i2]);
+    if (i2 + step >= line.size()) {
+      length += distance(line[i2], line[line.size() - 1]);
+    }
+  }
+  return length;
+}
+
+template <typename LaneletT>
+double length2d(const LaneletT& lanelet) {
+  return double(length(lanelet.centerline2d()));
+}
+
+template <typename LaneletT>
+double length3d(const LaneletT& lanelet) {
+  return double(length(lanelet.centerline3d()));
+}
+
+}  // namespace geometry
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/geometry/impl/LaneletMap.h

@@ -0,0 +1,59 @@
+#pragma once
+#include <boost/geometry/algorithms/buffer.hpp>
+#include <boost/geometry/algorithms/distance.hpp>
+
+#include "lanelet2_core/LaneletMap.h"
+#include "lanelet2_core/primitives/BoundingBox.h"
+#include "lanelet2_core/primitives/Lanelet.h"
+#include "lanelet2_core/utility/Utilities.h"
+
+namespace lanelet {
+namespace geometry {
+
+template <typename LayerT, typename GeometryT>
+auto findWithin2d(LayerT& layer, const GeometryT& geometry, double maxDist)
+    -> std::vector<std::pair<double, traits::LayerPrimitiveType<LayerT>>> {
+  static_assert(std::is_same<traits::TwoD<GeometryT>, GeometryT>::value,
+                "Call this function with a 2d type as geometry!");
+  using RetT = std::vector<std::pair<double, traits::LayerPrimitiveType<LayerT>>>;
+
+  BoundingBox2d boundingBox = boundingBox2d(geometry);
+  if (maxDist > 0.) {
+    boost::geometry::buffer(boundingBox, boundingBox, maxDist);
+  }
+  const auto bboxApproximation = layer.search(boundingBox);
+
+  auto withinVec = utils::createReserved<RetT>(bboxApproximation.size());
+  for (auto const& elem : bboxApproximation) {
+    const double dist = distance2d(geometry, elem);
+    if (dist <= maxDist) {
+      withinVec.push_back(std::make_pair(dist, elem));
+    }
+  }
+  std::sort(withinVec.begin(), withinVec.end(), [](auto& v1, auto& v2) { return v1.first < v2.first; });
+  return withinVec;
+}
+
+template <typename LayerT, typename GeometryT>
+auto findWithin3d(LayerT& layer, const GeometryT& geometry, double maxDist)
+    -> std::vector<std::pair<double, traits::LayerPrimitiveType<LayerT>>> {
+  using RetT = std::vector<std::pair<double, traits::LayerPrimitiveType<LayerT>>>;
+
+  BoundingBox2d boundingBox = boundingBox2d(traits::to2D(geometry));
+  if (maxDist > 0.) {
+    boost::geometry::buffer(boundingBox, boundingBox, maxDist);
+  }
+  const auto bboxApproximation = layer.search(boundingBox);
+
+  auto withinVec = utils::createReserved<RetT>(bboxApproximation.size());
+  for (auto const& elem : bboxApproximation) {
+    const double dist = distance3d(geometry, elem);
+    if (dist <= maxDist) {
+      withinVec.push_back(std::make_pair(dist, elem));
+    }
+  }
+  std::sort(withinVec.begin(), withinVec.end(), [](auto& v1, auto& v2) { return v1.first < v2.first; });
+  return withinVec;
+}
+}  // namespace geometry
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/geometry/impl/LineString.h

@@ -0,0 +1,862 @@
+#pragma once
+#include <boost/geometry/algorithms/intersection.hpp>
+
+#include "lanelet2_core/geometry/GeometryHelper.h"
+#include "lanelet2_core/geometry/Point.h"
+#include "lanelet2_core/primitives/LineString.h"
+#include "lanelet2_core/primitives/Traits.h"
+
+namespace lanelet {
+namespace geometry {
+namespace internal {
+
+struct SelfIntersectionLong {
+  size_t idx;
+  double s;  // coordinate along linestring
+};
+
+struct SelfIntersection2d {
+  SelfIntersectionLong firstSegment;
+  SelfIntersectionLong lastSegment;
+  BasicPoint2d intersectionPoint;
+};
+using SelfIntersections2d = std::vector<SelfIntersection2d>;
+
+struct PointVincinity {
+  const BasicPoint2d preceding;
+  const BasicPoint2d following;
+};
+
+template <typename T>
+struct GetGeometry<T, IfLS<T, void>> {
+  static inline auto twoD(const T& geometry) { return traits::toHybrid(traits::to2D(geometry)); }
+  static inline auto threeD(const T& geometry) { return traits::toHybrid(traits::to3D(geometry)); }
+};
+
+inline auto crossProd(const BasicPoint3d& p1, const BasicPoint3d& p2) { return p1.cross(p2).eval(); }
+inline auto crossProd(const BasicPoint2d& p1, const BasicPoint2d& p2) {
+  return BasicPoint3d(p1.x(), p1.y(), 0.).cross(BasicPoint3d(p2.x(), p2.y(), 0.)).eval();
+}
+// required for Polygon triangulation
+inline auto crossProd(const Eigen::Matrix<double, 2, 1>& p1, const Eigen::Matrix<double, 2, 1>& p2) {
+  return BasicPoint3d(p1.x(), p1.y(), 0.).cross(BasicPoint3d(p2.x(), p2.y(), 0.)).eval();
+}
+
+template <typename LineStringT, typename BasicPointT>
+auto findPoint(const LineStringT& ls, const BasicPointT& p) {
+  return std::find_if(ls.begin(), ls.end(), [&p](const auto& elem) { return boost::geometry::equals(elem, p); });
+}
+
+template <typename PointT>
+bool pointIsLeftOf(const PointT& pSeg1, const PointT& pSeg2, const PointT& p) {
+  return crossProd(PointT(pSeg2 - pSeg1), PointT(p - pSeg1)).z() > 0;
+}
+
+template <typename LineStringT>
+LineStringT invert(const LineStringT& ls) {
+  return ls.invert();
+}
+
+template <>
+inline BasicLineString2d invert(const BasicLineString2d& ls) {
+  return BasicLineString2d{ls.rbegin(), ls.rend()};
+}
+
+template <>
+inline BasicLineString3d invert(const BasicLineString3d& ls) {
+  return BasicLineString3d{ls.rbegin(), ls.rend()};
+}
+
+template <typename LineStringT, typename BasicPointT>
+bool isLeftOf(const LineStringT& ls, const BasicPointT& p, const helper::ProjectedPoint<BasicPointT>& projectedPoint) {
+  BasicPointT pSeg1 = projectedPoint.result->segmentPoint1;
+  BasicPointT pSeg2 = projectedPoint.result->segmentPoint2;
+  BasicPointT projPoint = projectedPoint.result->projectedPoint;
+  bool isLeft = pointIsLeftOf(pSeg1, pSeg2, p);
+  if (pSeg2 == projPoint) {
+    auto nextSegPointIt = std::next(findPoint(ls, pSeg2));
+    if (nextSegPointIt != ls.end()) {
+      // see stackoverflow.com/questions/10583212
+      BasicPointT nextSegPoint;
+      boost::geometry::convert(*nextSegPointIt, nextSegPoint);
+      if (isLeft != pointIsLeftOf(pSeg2, nextSegPoint, p) && isLeft == pointIsLeftOf(pSeg1, pSeg2, nextSegPoint)) {
+        return !isLeft;
+      }
+    }
+  }
+  return isLeft;
+}
+
+template <typename LineStringT, typename PointT>
+std::pair<double, helper::ProjectedPoint<PointT>> signedDistanceImpl(const LineStringT lineString, const PointT& p) {
+  using BasicPoint = PointT;
+  helper::ProjectedPoint<BasicPoint> projectedPoint;
+  const auto d = distance(lineString, p, projectedPoint);
+  auto isLeft = isLeftOf(lineString, p, projectedPoint);
+  return {isLeft ? d : -d, projectedPoint};
+}
+
+std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3d(const ConstHybridLineString3d& l1,
+                                                       const ConstHybridLineString3d& l2);
+
+std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3d(const CompoundHybridLineString3d& l1,
+                                                       const CompoundHybridLineString3d& l2);
+
+std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3d(const ConstHybridLineString3d& l1,
+                                                       const CompoundHybridLineString3d& l2);
+
+std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3d(const CompoundHybridLineString3d& l1,
+                                                       const ConstHybridLineString3d& l2);
+
+std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3d(const ConstHybridLineString3d& l1, const BasicLineString3d& l2);
+
+std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3d(const BasicLineString3d& l1, const ConstHybridLineString3d& l2);
+
+std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3d(const BasicLineString3d& l1, const BasicLineString3d& l2);
+
+template <typename HybridLineStringT>
+BasicPoint2d fromArcCoords(const HybridLineStringT& hLineString, const BasicPoint2d& projStart, const size_t startIdx,
+                           const size_t endIdx, const double distance) {
+  if (hLineString.size() < startIdx) {
+    throw InvalidInputError(std::string("Linestring point out of bounds. Linestring size ") +
+                            std::to_string(hLineString.size()) + ", startIdx " + std::to_string(startIdx));
+  }
+  if (hLineString.size() < endIdx) {
+    throw InvalidInputError(std::string("Linestring point out of bounds. Linestring size ") +
+                            std::to_string(hLineString.size()) + ", endIdx " + std::to_string(endIdx));
+  }
+  if (startIdx == endIdx) {
+    throw InvalidInputError(
+        std::string("Can't determine shift direction from two identical points on linestring. Point index ") +
+        std::to_string(startIdx));
+  }
+  const auto dx(hLineString[endIdx](0) - hLineString[startIdx](0));
+  const auto dy(hLineString[endIdx](1) - hLineString[startIdx](1));
+
+  return projStart + Eigen::Vector2d(-dy, dx).normalized() * distance;
+}
+
+/**
+ * @brief makeVincinity create pair of preceding and following point on a line string
+ * @param lineString line string to evaluate
+ * @param idx index of point to create vincininty around
+ * @return pair of preceding and following point (zero if not applicable)
+ */
+template <typename LineString2dT>
+PointVincinity makeVincinity(const LineString2dT& lineString, const size_t idx) {
+  if (idx == 0) {
+    return PointVincinity{BasicPoint2d::Zero(),
+                          utils::toBasicPoint(lineString[idx + 1]) - utils::toBasicPoint(lineString[idx])};
+  }
+  if (idx + 1 == lineString.size()) {
+    return PointVincinity{utils::toBasicPoint(lineString[idx]) - utils::toBasicPoint(lineString[idx - 1]),
+                          BasicPoint2d::Zero()};
+  }
+  return PointVincinity{utils::toBasicPoint(lineString[idx]) - utils::toBasicPoint(lineString[idx - 1]),
+                        utils::toBasicPoint(lineString[idx + 1]) - utils::toBasicPoint(lineString[idx])};
+}
+
+/**
+ * @brief shiftLateral shift point along the bisectrix
+ * @param lineString original line string
+ * @param idx index of point to shft
+ * @param offset offset distance (left is positive) to shift
+ * @param pv following and preveding point on line string
+ * @return shifted point
+ */
+template <typename LineString2dT>
+BasicPoint2d shiftLateral(const LineString2dT& lineString, const size_t idx, const double offset,
+                          const PointVincinity& pv) {
+  Eigen::Vector2d perpendicular;
+  double realOffset = offset;
+  const auto epsilon{1.e-5};
+  if (idx == 0) {
+    perpendicular = pv.following;
+  } else if (idx + 1 == lineString.size()) {
+    perpendicular = pv.preceding;
+  } else {
+    perpendicular = pv.following.normalized() + pv.preceding.normalized();
+    auto minussin2 = perpendicular.norm() / 2;
+    realOffset = (minussin2 > epsilon) ? offset / minussin2 : 0;
+  }
+
+  Eigen::Vector2d direction(-perpendicular(1), perpendicular(0));
+  return utils::toBasicPoint(lineString[idx]) + direction.normalized() * realOffset;
+}
+
+/**
+ * @return Point and true if convex
+ */
+template <typename LineString2dT>
+bool isConvex(const LineString2dT& lineString, const size_t idx, const bool offsetPositive) {
+  if (idx != 0 && idx + 1 != lineString.size()) {
+    auto isLeft = pointIsLeftOf<BasicPoint2d>(lineString[idx - 1], lineString[idx], lineString[idx + 1]);
+    return offsetPositive ? !isLeft : isLeft;
+  }
+  return true;
+}
+
+enum class Convexity { Concave, Convex, ConvexSharp };
+
+template <typename LineString2dT>
+Convexity getConvexity(const LineString2dT& lineString, const size_t idx, const PointVincinity& pv,
+                       const bool offsetPositive) {
+  if (!isConvex(lineString, idx, offsetPositive)) {
+    return Convexity::Concave;
+  }
+  if (pv.following.dot(pv.preceding) < 0) {
+    return Convexity::ConvexSharp;
+  }
+  return Convexity::Convex;
+}
+
+template <typename LineStringT>
+BasicPoints2d sortAlongS(const LineStringT& ls, const BasicPoints2d& points) {
+  auto idxs = sortAlongSIdxs(ls, points);
+  BasicPoints2d ret;
+  ret.reserve(idxs.size());
+  for (const auto& i : idxs) {
+    ret.emplace_back(points.at(i));
+  }
+  return ret;
+}
+
+/**
+ * @brief create a point by moving distance laterally from the linestring at the point idx. The direction is the
+ * average of the directions orthogonal to the segments neighbouring the point.
+ * @param lineString lineString to operate on
+ * @param idx index of point to shift (negative means counting from back)
+ * @param distance to shift (left = positive)
+ */
+template <typename LineString2dT>
+BasicPoint2d lateralShiftPointAtIndex(const LineString2dT& lineString, const int idx, const double distance) {
+  if (std::abs(idx) + 1 > lineString.size()) {
+    throw InvalidInputError("Index out of bounds");
+  }
+  int startIdx = (idx >= 0) ? std::max(0, idx - 1) : std::max(0, static_cast<int>(lineString.size()) + idx - 1);
+  int endIdx = (idx >= 0)
+                   ? std::min(idx + 1, static_cast<int>(lineString.size()) - 1)
+                   : std::min(static_cast<int>(lineString.size()) + idx + 1, static_cast<int>(lineString.size()) - 1);
+  int pIdx = (idx >= 0) ? idx : static_cast<int>(lineString.size()) + idx;
+  auto hLineString = utils::toHybrid(lineString);
+  return internal::fromArcCoords(hLineString, hLineString[pIdx], startIdx, endIdx, distance);
+}
+
+namespace bgi = boost::geometry::index;
+using IndexedSegment2d = std::pair<BasicSegment2d, size_t>;
+using IndexedSegmentTree = bgi::rtree<IndexedSegment2d, bgi::linear<4>>;
+using SegmentTree = bgi::rtree<BasicSegment2d, bgi::linear<4>>;
+
+struct LineStringsCoordinate {
+  const size_t lineStringIdx;
+  const size_t segmentIdx;
+};
+
+/**
+ * @brief makeIndexedSegmenTree create search tree of segments
+ * @param lineString line string that the segments will be created from
+ * @throws InvalidInputError if the line string size is below 2
+ * @return a search tree
+ */
+template <typename LineString2dT>
+inline SegmentTree makeSegmentTree(const LineString2dT& lineString) {
+  if (lineString.size() < 2) {
+    throw InvalidInputError("Need line string size of at least 2 to make tree");
+  }
+  SegmentTree tree;
+  std::vector<BasicSegment2d> segContainer;
+  segContainer.reserve(lineString.size() - 1);
+  for (size_t i = 0; i < lineString.size() - 1; ++i) {
+    segContainer.emplace_back(
+        BasicSegment2d{utils::toBasicPoint(lineString[i]), utils::toBasicPoint(lineString[i + 1])});
+  }
+  tree.insert(segContainer);
+
+  return tree;
+}
+
+/**
+ * @brief makeIndexedSegmenTree create search tree of segments with indices
+ * @param lineString line string that the segments will be created from
+ * @throws InvalidInputError if the line string size is below 2
+ * @return a search tree
+ */
+inline IndexedSegmentTree makeIndexedSegmenTree(const BasicLineString2d& lineString) {
+  if (lineString.size() < 2) {
+    throw InvalidInputError("Need line string size of at least 2 to make tree");
+  }
+  IndexedSegmentTree tree;
+  std::vector<std::pair<BasicSegment2d, size_t>> segContainer;
+  segContainer.reserve(lineString.size() - 1);
+  for (size_t i = 0; i < lineString.size() - 1; ++i) {
+    segContainer.emplace_back(std::make_pair(BasicSegment2d{lineString.at(i), lineString.at(i + 1)}, i));
+  }
+  tree.insert(segContainer);
+  return tree;
+}
+
+/**
+ * @brief getLowestSAbove find self-intersection nearest to the given start point along a segment
+ * @param selfIntersections list of self-intersections
+ * @param minS start coordinate along the segment
+ * @return index of the search result in selfIntersections
+ */
+inline Optional<size_t> getLowestSAbove(const SelfIntersections2d& selfIntersections, const double minS) {
+  double curMinS = std::numeric_limits<double>::max();
+  Optional<size_t> res;
+  for (size_t i = 0; i < selfIntersections.size(); ++i) {
+    const auto& ci = selfIntersections.at(i);
+    if (ci.firstSegment.s > minS && ci.firstSegment.s < curMinS) {
+      res = i;
+      curMinS = ci.firstSegment.s;
+    }
+  }
+  return res;
+}
+
+/**
+ * @brief getSelfIntersectionsAt find list of intersections based on a segment search tree
+ * @param tree segment search tree
+ * @param segToCheck the segment to be evaluated
+ * @param seg index of the segment to be evaluated
+ * @return a list of intersections involving the given segment
+ */
+inline SelfIntersections2d getSelfIntersectionsAt(const IndexedSegmentTree& tree, const size_t segToCheck,
+                                                  const BasicSegment2d& seg) {
+  SelfIntersections2d curSegIntersections;
+  for (auto it = tree.qbegin(bgi::intersects(seg)); it != tree.qend(); ++it) {
+    const auto& otherSegIdx = it->second;
+    const auto& otherSeg = it->first;
+    if (otherSeg.first != seg.second && otherSeg.second != seg.first && otherSeg.first != seg.first &&
+        otherSegIdx > segToCheck) {
+      BasicPoints2d intersectionPoints;
+      boost::geometry::intersection(seg, otherSeg, intersectionPoints);
+      const auto intersectionPoint = intersectionPoints.front();
+      auto firstS = (intersectionPoint - seg.first).norm();
+      auto lastS = (intersectionPoint - otherSeg.first).norm();
+      curSegIntersections.emplace_back(SelfIntersection2d{SelfIntersectionLong{segToCheck, firstS},
+                                                          SelfIntersectionLong{otherSegIdx, lastS}, intersectionPoint});
+    }
+  }
+
+  return curSegIntersections;
+}
+
+struct PointSearchResult {
+  const BasicPoint2d nextPoint;
+  const size_t nextSegIdx;
+  const double lastS;
+};
+
+/**
+ * @brief findNextPoint find next point to walk on a line strings join operation
+ * @param curSegIntersections list of intersections on the current segment
+ * @param seg segment to evaluate
+ * @param lastS coordinate along the segment to start the search on
+ * @param i segment index
+ * @return coordinate of next point on the walk
+ */
+inline PointSearchResult findNextPoint(const SelfIntersections2d& curSegIntersections, const BasicSegment2d& seg,
+                                       const double lastS, const size_t i) {
+  if (!curSegIntersections.empty()) {
+    auto possibeNextIntersection = getLowestSAbove(curSegIntersections, lastS);
+    if (possibeNextIntersection) {
+      const auto& ni = curSegIntersections.at(*possibeNextIntersection);
+      return PointSearchResult{ni.intersectionPoint, ni.lastSegment.idx, ni.lastSegment.s};
+    }
+  }
+  return PointSearchResult{seg.second, i + 1, 0.};
+}
+
+inline BasicLineString2d removeSelfIntersections(const BasicLineString2d& lineString) {
+  if (lineString.size() <= 3) {
+    return lineString;
+  }
+  auto tree = makeIndexedSegmenTree(lineString);
+  double lastS{0.};
+  BasicLineString2d newLS{lineString.front()};
+  size_t i = 0;
+  while (i < lineString.size() - 2) {
+    BasicSegment2d curSeg{lineString.at(i), lineString.at(i + 1)};
+    auto curSegIntersections = getSelfIntersectionsAt(tree, i, curSeg);
+    auto np = findNextPoint(curSegIntersections, curSeg, lastS, i);
+    newLS.emplace_back(np.nextPoint);
+    i = np.nextSegIdx;
+    lastS = np.lastS;
+  }
+  newLS.push_back(lineString.back());
+  return newLS;
+}
+
+/**
+ * @brief checkForInversion asserts that a shifted line string is not inverted relative to the original line string by
+ * checking the distance of those
+ * @param oldLS the original line string
+ * @param offsetLS the shifted line string
+ * @param distance shifting distance (left is positive)
+ * @param epsilon maximum allowed difference between requested and actual minimum offset
+ */
+template <typename LineString2dT>
+inline void checkForInversion(const LineString2dT& oldLS, const BasicLineString2d& offsetLS, const double distance,
+                              const double epsilon = 1.e-7) {
+  auto tree = internal::makeSegmentTree(oldLS);
+  for (const auto& p : offsetLS) {
+    auto it = tree.qbegin(internal::bgi::nearest(p, 1));
+    auto pd = geometry::distance2d(BasicLineString2d{it->first, it->second}, p);
+    if (pd + epsilon < distance) {
+      throw GeometryError("Possible Lanelet Inversion");
+    }
+  }
+}
+
+/**
+ * @brief shiftPerpendicular shift point perpendicular to either the preceding or following segment
+ * @param lineString original line string
+ * @param idx index of point to shft
+ * @param distance offset distance (left is positive) to shift
+ * @param asLast use preceding segment as reference if true, else use following
+ * @param pv following and preveding point on line string
+ * @return shifted point
+ */
+template <typename LineString2dT>
+inline BasicPoint2d shiftPerpendicular(const LineString2dT& lineString, const size_t idx, const double distance,
+                                       const bool asLast, const PointVincinity& pv) {
+  if (idx == 0 && asLast) {
+    throw GeometryError("Can't shift first point of line string as endpoint of segment");
+  }
+  if (idx + 1 == lineString.size() && !asLast) {
+    throw GeometryError("Can't shift last point of line string as start point of segment");
+  }
+  Eigen::Vector2d perpendicular = asLast ? pv.preceding.normalized() : pv.following.normalized();
+  Eigen::Vector2d norm(-perpendicular(1), perpendicular(0));
+  return utils::toBasicPoint(lineString[idx]) + norm * distance;
+}
+
+/**
+ * @brief shiftConvexSharp shift corner that is convex at a more than 90 degree angle. Inserts a new point to limit
+ * maximum offset
+ * @param lineString original line string
+ * @param idx index of point to shft
+ * @param distance offset distance (left is positive) to shift
+ * @param pv following and preveding point on line string
+ * @return shifted points
+ */
+template <typename LineString2dT>
+inline BasicLineString2d shiftConvexSharp(const LineString2dT& lineString, const size_t idx, const double distance,
+                                          const PointVincinity& pv) {
+  if (idx == 0) {
+    throw GeometryError("Can't shift first point of line string as sharp convex");
+  }
+  if (idx + 1 == lineString.size()) {
+    throw GeometryError("Can't shift last point of line string as sharp convex");
+  }
+  BasicPoint2d firstP = shiftPerpendicular(lineString, idx, distance, true, pv);
+  BasicPoint2d lastP = shiftPerpendicular(lineString, idx, distance, false, pv);
+  auto alpha = M_PI - std::acos(pv.following.dot(pv.preceding) / (pv.preceding.norm() * pv.following.norm()));
+  auto overshoot = distance * std::tan(M_PI_4 - alpha / 4);
+  return {firstP + pv.preceding.normalized() * overshoot, lastP - pv.following.normalized() * overshoot};
+}
+
+/**
+ * @brief shiftPoint in lateral direction
+ * @param lineString original line string
+ * @param distance offset distance (left is positive) to shift
+ * @param idx index of point to shift
+ * @param pv following and preveding point on line string
+ * @return shifted point(s), bool indicating insertion was still convex
+ */
+template <typename LineString2dT>
+inline std::pair<BasicLineString2d, bool> shiftPoint(const LineString2dT& lineString, const double distance,
+                                                     const size_t idx, const PointVincinity& pv) {
+  auto convexity = internal::getConvexity(lineString, idx, pv, distance > 0);
+  if (convexity == Convexity::Concave) {
+    return std::make_pair(BasicLineString2d{shiftPerpendicular(lineString, idx, distance, true, pv)}, false);
+  }
+  if (convexity == Convexity::Convex) {
+    return std::make_pair(BasicLineString2d{shiftLateral(lineString, idx, distance, pv)}, true);
+  }
+  if (convexity == Convexity::ConvexSharp) {
+    return std::make_pair(shiftConvexSharp(lineString, idx, distance, pv), true);
+  }
+  throw InvalidInputError("Unknown Convexity status");
+}
+
+/**
+ * @brief extractConvex get convex parts of line string offset by distance.
+ * @param lineString original line string
+ * @param distance offset distance (left is positive)
+ * @return list of line strings, each one is convex. Guaranteed to intersect at non-convex parts
+ */
+template <typename LineString2dT>
+inline std::vector<BasicLineString2d> extractConvex(const LineString2dT& lineString, const double distance) {
+  std::vector<BasicLineString2d> offsets;
+  size_t curIdx{0};
+  while (curIdx + 1 < lineString.size()) {
+    auto pv = internal::makeVincinity(lineString, curIdx);
+    BasicLineString2d ls{shiftPerpendicular(lineString, curIdx, distance, false, pv)};
+    for (size_t i = curIdx + 1; i < lineString.size(); ++i) {
+      curIdx = i;
+      auto ipv = internal::makeVincinity(lineString, i);
+      auto shiftResult = shiftPoint(lineString, distance, i, ipv);
+      ls.insert(ls.end(), shiftResult.first.begin(), shiftResult.first.end());
+      if (!shiftResult.second) {
+        break;
+      }
+    }
+    offsets.push_back(ls);
+  }
+  return offsets;
+}
+
+using SegmentMap = std::map<size_t, LineStringsCoordinate>;
+
+struct SegmentSearch {
+  IndexedSegmentTree tree;
+  SegmentMap map;
+};
+
+/**
+ * @brief makeTree create search tree of segments from line strings
+ * @param convexSubStrings list of line strings
+ * @return search tree and mapping from segment index of uncut linestring to linestring and segment index of substring
+ */
+
+inline SegmentSearch makeTree(const std::vector<BasicLineString2d>& convexSubStrings) {
+  IndexedSegmentTree tree;
+  SegmentMap segMap;
+  size_t idx{0};
+  for (size_t i = 0; i < convexSubStrings.size(); ++i) {
+    const auto& ss = convexSubStrings.at(i);
+    for (size_t j = 0; j + 1 < ss.size(); ++j) {
+      BasicSegment2d seg{utils::toBasicPoint(ss.at(j)), utils::toBasicPoint(ss.at(j + 1))};
+      tree.insert(std::make_pair(seg, idx));
+      segMap.emplace(idx, LineStringsCoordinate{i, j});
+      ++idx;
+    }
+  }
+  return SegmentSearch{tree, segMap};
+}
+
+/**
+ * @brief joinSubStrings create one line string of several ones by walking along them and joining them at the first
+ * intersection
+ * @param convexSubStrings list of substrings. Walk will start at the first element and continue until the list is
+ * exhausted
+ * @param tree search tree
+ * @param segMap mapping from segment index of uncut linestring to linestring and segment index of substring
+ * @return joined line string
+ */
+inline BasicLineString2d joinSubStrings(const std::vector<BasicLineString2d>& convexSubStrings,
+                                        const IndexedSegmentTree& tree, const SegmentMap& segMap) {
+  if (convexSubStrings.empty()) {
+    return BasicLineString2d();
+  }
+  if (convexSubStrings.size() == 1) {
+    return convexSubStrings.front();
+  }
+  double lastS{0.};
+  BasicLineString2d newLS{convexSubStrings.front().front()};
+  size_t i = 0;
+  size_t idx = 0;
+  while (i < convexSubStrings.size()) {
+    const auto& ls = convexSubStrings.at(i);
+    for (size_t j = 0; j + 1 < ls.size(); ++j) {
+      BasicSegment2d curSeg{ls.at(j), ls.at(j + 1)};
+      auto curSegIntersections = getSelfIntersectionsAt(tree, idx, curSeg);
+      auto np = findNextPoint(curSegIntersections, curSeg, lastS, idx);
+      newLS.emplace_back(np.nextPoint);
+      idx = np.nextSegIdx;
+      if (idx >= segMap.size()) {
+        i = convexSubStrings.size();
+        break;
+      }
+      lastS = np.lastS;
+      if (segMap.at(idx).lineStringIdx != i) {
+        i = segMap.at(idx).lineStringIdx;
+        break;
+      }
+    }
+  }
+  newLS.push_back(convexSubStrings.back().back());
+  return newLS;
+}
+
+}  // namespace internal
+
+template <typename LineStringIterator>
+double rangedLength(LineStringIterator start, LineStringIterator end) {
+  double l = 0.;
+  helper::forEachPair(start, end, [&l](const auto& seg1, const auto& seg2) { l += distance(seg1, seg2); });
+  return l;
+}
+
+template <typename LineStringT>
+std::vector<double> lengthRatios(const LineStringT& lineString) {
+  std::vector<double> lengths;
+  if (lineString.size() <= 1) {
+    return lengths;
+  }
+  if (lineString.size() == 2) {
+    return {1.};
+  }
+  const auto totalLength = length(lineString);
+  lengths.reserve(lineString.size() - 1);
+  helper::forEachPair(lineString.begin(), lineString.end(), [&lengths, &totalLength](const auto& p1, const auto& p2) {
+    lengths.push_back(distance(p1, p2) / totalLength);
+  });
+  return lengths;
+}
+
+template <typename LineStringT>
+std::vector<double> accumulatedLengthRatios(const LineStringT& lineString) {
+  auto lengths = lengthRatios(lineString);
+  helper::forEachPair(lengths.begin(), lengths.end(), [](const auto& l1, auto& l2) { l2 += l1; });
+  return lengths;
+}
+
+template <typename LineStringT>
+traits::BasicPointT<traits::PointType<LineStringT>> interpolatedPointAtDistance(LineStringT lineString, double dist) {
+  assert(!lineString.empty());
+  if (dist < 0) {
+    lineString = internal::invert(lineString);
+    dist = -dist;
+  }
+
+  double currentCumulativeLength = 0.0;
+  for (auto first = lineString.begin(), second = std::next(lineString.begin()); second != lineString.end();
+       ++first, ++second) {
+    auto p1 = traits::toBasicPoint(*first);
+    auto p2 = traits::toBasicPoint(*second);
+    double currentLength = distance(p1, p2);
+    currentCumulativeLength += currentLength;
+    if (currentCumulativeLength >= dist) {
+      double remainingDistance = dist - (currentCumulativeLength - currentLength);
+      if (remainingDistance < 1.e-8) {
+        return p1;
+      }
+      return p1 + remainingDistance / currentLength * (p2 - p1);
+    }
+  }
+  return traits::toBasicPoint(lineString.back());
+}
+
+template <typename LineStringT>
+traits::PointType<LineStringT> nearestPointAtDistance(LineStringT lineString, double dist) {
+  using traits::toBasicPoint;
+  assert(!lineString.empty());
+  if (dist < 0) {
+    lineString = internal::invert(lineString);
+    dist = -dist;
+  }
+  double currentCumulativeLength = 0.0;
+  for (auto first = lineString.begin(), second = std::next(lineString.begin()); second != lineString.end();
+       ++first, ++second) {
+    const auto& p1 = *first;
+    const auto& p2 = *second;
+    double currentLength = distance(p1, p2);
+    currentCumulativeLength += currentLength;
+    if (currentCumulativeLength >= dist) {
+      double remainingDistance = dist - (currentCumulativeLength - currentLength);
+      if (remainingDistance > currentLength / 2.0) {
+        return p2;
+      }
+      return p1;
+    }
+  }
+  return lineString.back();
+}
+
+template <typename LineString3dT>
+double signedDistance(const LineString3dT& lineString, const BasicPoint3d& p) {
+  static_assert(traits::is3D<LineString3dT>(), "Please call this function with a 3D type!");
+  return internal::signedDistanceImpl(lineString, p).first;
+}
+
+template <typename LineString2dT>
+double signedDistance(const LineString2dT& lineString, const BasicPoint2d& p) {
+  static_assert(traits::is2D<LineString2dT>(), "Please call this function with a 2D type!");
+  return internal::signedDistanceImpl(lineString, p).first;
+}
+
+template <typename Point2dT>
+double curvature2d(const Point2dT& p1, const Point2dT& p2, const Point2dT& p3) {
+  // see https://en.wikipedia.org/wiki/Menger_curvature#Definition
+  const double area = 0.5 * ((p2.x() - p1.x()) * (p3.y() - p1.y()) - (p2.y() - p1.y()) * (p3.x() - p1.x()));
+  const double side1 = distance(p1, p2);
+  const double side2 = distance(p1, p3);
+  const double side3 = distance(p2, p3);
+  const double product = side1 * side2 * side3;
+  if (product < 1e-20) {
+    return std::numeric_limits<double>::infinity();
+  }
+  return std::fabs(4 * area / product);
+}
+
+template <typename LineString2dT>
+ArcCoordinates toArcCoordinates(const LineString2dT& lineString, const BasicPoint2d& point) {
+  auto res = internal::signedDistanceImpl(lineString, point);
+  auto dist = res.first;
+  const auto& projectedPoint = res.second;
+  // find first point in segment in linestring
+  double length = 0.;
+  auto accumulateLength = [&length, &point = projectedPoint.result->segmentPoint1](const auto& first,
+                                                                                   const auto& second) {
+    if (boost::geometry::equals(first, point)) {
+      return true;
+    }
+    length += distance(first, second);
+    return false;
+  };
+  helper::forEachPairUntil(lineString.begin(), lineString.end(), accumulateLength);
+  length += distance(projectedPoint.result->segmentPoint1, projectedPoint.result->projectedPoint);
+  return {length, dist};
+}
+
+template <typename LineString3dT>
+IfLS<LineString3dT, BoundingBox3d> boundingBox3d(const LineString3dT& lineString) {
+  static_assert(traits::is3D<LineString3dT>(), "Please call this function with a 3D type!");
+  BoundingBox3d bb;
+  for (const auto& p : lineString) {
+    bb.extend(traits::toBasicPoint(p));
+  }
+  return bb;
+}
+
+template <typename LineString2dT>
+IfLS<LineString2dT, BoundingBox2d> boundingBox2d(const LineString2dT& lineString) {
+  BoundingBox2d bb;
+  for (const auto& p : traits::to2D(lineString)) {
+    bb.extend(traits::toBasicPoint(p));
+  }
+  return bb;
+}
+
+template <typename LineString3dT, typename>
+BasicPoint3d project(const LineString3dT& lineString, const BasicPoint3d& pointToProject) {
+  static_assert(traits::is3D<LineString3dT>(), "Please call this function with a 3D type!");
+  helper::ProjectedPoint<BasicPoint3d> projectedPoint;
+  distance(lineString, pointToProject, projectedPoint);
+  return projectedPoint.result->projectedPoint;
+}
+
+template <typename LineString2dT, typename>
+BasicPoint2d project(const LineString2dT& lineString, const BasicPoint2d& pointToProject) {
+  static_assert(traits::is2D<LineString2dT>(), "Please call this function with a 2D type!");
+  helper::ProjectedPoint<BasicPoint2d> projectedPoint;
+  distance(lineString, pointToProject, projectedPoint);
+  return projectedPoint.result->projectedPoint;
+}
+
+template <typename LineString3dT>
+IfLS<LineString3dT, bool> intersects3d(const LineString3dT& linestring, const LineString3dT& otherLinestring,
+                                       double heightTolerance) {
+  auto ls2d(traits::toHybrid(traits::to2D(linestring)));
+  auto ols2d(traits::toHybrid(traits::to2D(otherLinestring)));
+  BasicPoints2d intersections;
+  boost::geometry::intersection(ls2d, ols2d, intersections);
+  auto distanceSmallerTolerance = [heightTolerance, &linestring, &otherLinestring](const auto& point) {
+    auto pProj1 = project(linestring, BasicPoint3d(point.x(), point.y(), 0));
+    auto pProj2 = project(otherLinestring, BasicPoint3d(point.x(), point.y(), 0));
+    return distance(pProj1, pProj2) < heightTolerance;
+  };
+  return std::any_of(intersections.begin(), intersections.end(), distanceSmallerTolerance);
+}
+
+template <typename LineString3dT>
+IfLS<LineString3dT, std::pair<BasicPoint3d, BasicPoint3d>> projectedPoint3d(const LineString3dT& l1,
+                                                                            const LineString3dT& l2) {
+  static_assert(traits::is3D<LineString3dT>(), "Please call this function with a 3D type!");
+  return internal::projectedPoint3d(traits::toHybrid(l1), traits::toHybrid(l2));
+}
+
+template <typename LineString3d1T, typename LineString3d2T>
+IfLS2<LineString3d1T, LineString3d2T, double> distance3d(const LineString3d1T& l1, const LineString3d2T& l2) {
+  auto projPoint = internal::projectedPoint3d(traits::toHybrid(traits::to3D(l1)), traits::toHybrid(traits::to3D(l2)));
+  return (projPoint.first - projPoint.second).norm();
+}
+
+template <typename LineString1T, typename LineString2T>
+std::pair<LineString1T, LineString2T> align(LineString1T left, LineString2T right) {
+  using traits::toBasicPoint;
+  // degenerated case
+  if ((left.size() <= 1 && right.size() <= 1) || right.empty() || left.empty()) {
+    return {left, right};
+  }
+  auto getMiddlePoint = [](auto& ls) {
+    return ls.size() > 2 ? toBasicPoint(ls[ls.size() / 2]) : (toBasicPoint(ls.front()) + toBasicPoint(ls.back())) * 0.5;
+  };
+  //! @todo this sadly is a bit heuristical...
+  bool rightOfLeft = signedDistance(left, getMiddlePoint(right)) < 0;
+  if (!rightOfLeft && left.size() > 1) {
+    left = left.invert();
+  }
+
+  bool leftOfRight = signedDistance(right, getMiddlePoint(left)) > 0;
+  if (!leftOfRight && right.size() > 1) {
+    right = right.invert();
+  }
+  return {left, right};
+}
+
+template <typename LineString2dT>
+BasicPoint2d fromArcCoordinates(const LineString2dT& lineString, const ArcCoordinates& arcCoords) {
+  if (lineString.size() < 2) {
+    throw InvalidInputError("Can't use arc coordinates on degenerated line string");
+  }
+  auto hLineString = utils::toHybrid(lineString);
+  auto ratios = accumulatedLengthRatios(lineString);
+  const auto llength = length(lineString);
+  size_t startIdx{};
+  size_t endIdx{};
+  for (size_t i = 0; i < ratios.size(); ++i) {
+    if (ratios.at(i) * llength > arcCoords.length) {
+      startIdx = i;
+      endIdx = i + 1;
+      break;
+    }
+  }
+  if (endIdx == 0) {
+    endIdx = lineString.size() - 1;
+    startIdx = endIdx - 1;
+  }
+  return internal::fromArcCoords(hLineString, interpolatedPointAtDistance(utils::to2D(lineString), arcCoords.length),
+                                 startIdx, endIdx, arcCoords.distance);
+}
+
+template <typename LineString2dT>
+BasicLineString2d offsetNoThrow(const LineString2dT& lineString, const double distance) {
+  auto convexSubStrings = internal::extractConvex(lineString, distance);
+  auto segSearch = internal::makeTree(convexSubStrings);
+  auto newLS = internal::joinSubStrings(convexSubStrings, segSearch.tree, segSearch.map);
+  return newLS;
+}
+
+template <typename LineString2dT>
+BasicLineString2d offset(const LineString2dT& lineString, const double distance) {
+  auto newLS = offsetNoThrow(lineString, distance);
+  internal::checkForInversion(lineString, newLS, distance);
+  return newLS;
+}
+
+template <typename LineString3dT, typename>
+Segment<traits::PointType<LineString3dT>> closestSegment(const LineString3dT& lineString,
+                                                         const BasicPoint3d& pointToProject) {
+  static_assert(traits::is3D<LineString3dT>(), "Please call this function with a 3D type!");
+  helper::ProjectedPoint<traits::PointType<LineString3dT>> projectedPoint;
+  distance(utils::toHybrid(lineString), pointToProject, projectedPoint);
+  return Segment<traits::PointType<LineString3dT>>(projectedPoint.result->segmentPoint1,
+                                                   projectedPoint.result->segmentPoint2);
+}
+
+//! Projects the given point in 2d to the LineString.
+template <typename LineString2dT, typename>
+Segment<traits::PointType<LineString2dT>> closestSegment(const LineString2dT& lineString,
+                                                         const BasicPoint2d& pointToProject) {
+  static_assert(traits::is2D<LineString2dT>(), "Please call this function with a 2D type!");
+  helper::ProjectedPoint<traits::PointType<LineString2dT>> projectedPoint;
+  distance(utils::toHybrid(lineString), pointToProject, projectedPoint);
+  return Segment<traits::PointType<LineString2dT>>(projectedPoint.result->segmentPoint1,
+                                                   projectedPoint.result->segmentPoint2);
+}
+}  // namespace geometry
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/geometry/impl/Polygon.h

@@ -0,0 +1,104 @@
+#pragma once
+#include "lanelet2_core/geometry/LineString.h"
+#include "lanelet2_core/geometry/Point.h"
+#include "lanelet2_core/primitives/Polygon.h"
+#if BOOST_VERSION > 105800
+#include <boost/geometry/algorithms/relate.hpp>
+#else
+#include <boost/geometry/algorithms/detail/relate/relate.hpp>
+#endif
+
+namespace lanelet {
+namespace geometry {
+namespace internal {
+template <typename T>
+struct GetGeometry<T, IfPoly<T, void>> {
+  static inline auto twoD(const T& geometry) { return traits::toHybrid(traits::to2D(geometry)); }
+  static inline auto threeD(const T& geometry) { return traits::toHybrid(traits::to3D(geometry)); }
+};
+
+std::pair<BasicPoint3d, BasicPoint3d> projectedBorderPoint3d(const ConstHybridPolygon3d& l1,
+                                                             const ConstHybridPolygon3d& l2);
+
+std::pair<BasicPoint3d, BasicPoint3d> projectedBorderPoint3d(const CompoundHybridPolygon3d& l1,
+                                                             const CompoundHybridPolygon3d& l2);
+BasicPolygons2d convexPartition(const BasicPolygon2d& poly);
+IndexedTriangles triangulate(const BasicPolygon2d& poly);
+}  // namespace internal
+
+template <typename Polygon3dT>
+IfPoly<Polygon3dT, std::pair<BasicPoint3d, BasicPoint3d>> projectedBorderPoint3d(const Polygon3dT& l1,
+                                                                                 const Polygon3dT& l2) {
+  static_assert(traits::is3D<Polygon3dT>(), "Please call this function with a 3D type!");
+  return internal::projectedBorderPoint3d(traits::toHybrid(traits::to3D(l1)), traits::toHybrid(traits::to3D(l2)));
+}
+
+template <typename Polygon3dT>
+IfPoly<Polygon3dT, double> distanceToBorder3d(const Polygon3dT& poly1, const Polygon3dT& poly2) {
+  static_assert(traits::is3D<Polygon3dT>(), "Please call this function with a 3D type!");
+  auto projPoint = internal::projectedBorderPoint3d(traits::toHybrid(poly1), traits::toHybrid(poly2));
+  return (projPoint.first - projPoint.second).norm();
+}
+
+template <typename PolygonT>
+IfPoly<PolygonT, bool> touches2d(const PolygonT& poly1, const PolygonT& poly2) {
+  auto rotatedNext = [&](auto iter) { return iter + 1 == poly2.end() ? poly2.begin() : iter + 1; };
+  for (auto i = 0u; i < poly1.numSegments(); ++i) {
+    auto segment = poly1.segment(i);
+    auto second = std::find(poly2.begin(), poly2.end(), segment.second);
+    if (second != poly2.end() && *rotatedNext(second) == segment.first) {
+      return true;
+    }
+  }
+  return false;
+}
+
+template <typename Polygon2dT>
+IfPoly<Polygon2dT, bool> overlaps2d(const Polygon2dT& poly1, const Polygon2dT& poly2) {
+  static_assert(traits::is2D<Polygon2dT>(), "Please call this function with a 2D type!");
+  if (touches2d(poly1, poly2)) {
+    return false;
+  }
+#if BOOST_VERSION > 105800
+  using Mask = boost::geometry::de9im::static_mask<'T', '*', '*', '*', '*', '*', '*', '*', '*'>;
+  return boost::geometry::relate(utils::toHybrid(poly1), utils::toHybrid(poly2), Mask());
+#else
+  using Mask = boost::geometry::detail::relate::static_mask<'T', '*', '*', '*', '*', '*', '*', '*', '*'>;
+  return boost::geometry::detail::relate::relate<Mask>(utils::toHybrid(poly1), utils::toHybrid(poly2));
+#endif
+}
+
+template <typename Polygon3dT>
+IfPoly<Polygon3dT, bool> overlaps3d(const Polygon3dT& poly1, const Polygon3dT& poly2, double heightTolerance) {
+  static_assert(traits::is3D<Polygon3dT>(), "Please call this function with a 3D type!");
+  if (!overlaps2d(traits::to2D(poly1), traits::to2D(poly2))) {
+    return false;
+  }
+  auto points = projectedBorderPoint3d(poly1, poly2);
+  return std::abs(points.first.z() - points.second.z()) < heightTolerance;
+}
+
+template <typename Polygon2dT>
+IfPoly<Polygon2dT, BasicPolygons2d> convexPartition(const Polygon2dT& poly) {
+  static_assert(traits::is2D<Polygon2dT>(), "Please call this function with a 2D type!");
+  return internal::convexPartition(BasicPolygon2d(poly.basicBegin(), poly.basicEnd()));
+}
+
+template <typename Polygon2dT>
+IfPoly<Polygon2dT, IndexedTriangles> triangulate(const Polygon2dT& poly) {
+  static_assert(traits::is2D<Polygon2dT>(), "Please call this function with a 2D type!");
+  return internal::triangulate(BasicPolygon2d(poly.basicBegin(), poly.basicEnd()));
+}
+
+template <>
+inline IfPoly<BasicPolygon2d, BasicPolygons2d> convexPartition<BasicPolygon2d>(const BasicPolygon2d& poly) {
+  return internal::convexPartition(poly);
+}
+
+template <>
+inline IfPoly<BasicPolygon2d, IndexedTriangles> triangulate<BasicPolygon2d>(const BasicPolygon2d& poly) {
+  return internal::triangulate(poly);
+}
+
+}  // namespace geometry
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/primitives/Area.h

@@ -0,0 +1,374 @@
+// this is for emacs file handling -*- mode: c++; c-basic-offset: 2;
+// indent-tabs-mode: nil -*-
+
+#pragma once
+
+#include <memory>
+#include <utility>
+
+#include "lanelet2_core/Forward.h"
+#include "lanelet2_core/primitives/CompoundPolygon.h"
+#include "lanelet2_core/primitives/Point.h"
+#include "lanelet2_core/primitives/Polygon.h"
+#include "lanelet2_core/primitives/Primitive.h"
+#include "lanelet2_core/utility/Optional.h"
+
+namespace lanelet {
+//! @defgroup AreaPrimitives Area
+//! @ingroup Primitives
+//!
+//! ## General
+//! While \ref LaneletPrimitives represet a road section with directed motion,
+//! Areas represent regions outside of the normal lanes where either no motion
+//! occures (as in green stripes) or undirected motion occurs (e.g. on parking
+//! spaces). Areas represent a polygon that can have holes.
+//!
+//! ## Design
+//! Areas are similar to \ref LaneletPrimitives in that they can have
+//! RegulatoryElements that represent traffic rules.
+//! From a geometrical point of view, they are polygons with an outer contour
+//! and multiple inner contours. Every contour can be composed of multiple \ref
+//! LineStringPrimitives that together form a closed ring.
+//!
+//! ## Geometry
+//! Areas can be used direcly for some geometry calculations (see
+//! lanelet::geometry). They can also be interpreted as a polygon to make use of
+//! all polygon operations that boost::polygon supports.
+
+using InnerBounds = std::vector<LineStrings3d>;
+using ConstInnerBounds = std::vector<ConstLineStrings3d>;
+
+//! @brief Common data management class for all Area-Typed objects.
+//! @ingroup DataObjects
+class AreaData : public PrimitiveData {
+ public:
+  //! Constructs a new, AreaData object
+  AreaData(Id id, LineStrings3d outerBound, std::vector<LineStrings3d> innerBounds = std::vector<LineStrings3d>(),
+           AttributeMap attributes = AttributeMap(), RegulatoryElementPtrs regulatoryElements = RegulatoryElementPtrs())
+      : PrimitiveData(id, std::move(attributes)),
+        outerBound_{std::move(outerBound)},
+        innerBounds_{std::move(innerBounds)},
+        regulatoryElements_{std::move(regulatoryElements)} {
+    resetCache();
+  }
+
+  ConstLineStrings3d outerBound() const {
+    return utils::transform(outerBound_, [](const auto& elem) { return ConstLineString3d(elem); });
+  }
+  ConstInnerBounds innerBounds() const {
+    return utils::transform(innerBounds_, [](const auto& elem) {
+      return utils::transform(elem, [](const auto& ls) { return ConstLineString3d(ls); });
+    });
+  }
+  const LineStrings3d& outerBound() { return outerBound_; }
+  const InnerBounds& innerBounds() { return innerBounds_; }
+
+  const CompoundPolygon3d& outerBoundPolygon() const { return outerBoundPolygon_; }
+
+  const CompoundPolygons3d& innerBoundPolygons() const { return innerBoundPolygons_; }
+
+  RegulatoryElementConstPtrs regulatoryElements() const {
+    return utils::transform(regulatoryElements_, [](const auto& elem) { return RegulatoryElementConstPtr(elem); });
+  }
+  RegulatoryElementPtrs& regulatoryElements() { return regulatoryElements_; }
+
+  template <typename RegElemT>
+  std::vector<std::shared_ptr<const RegElemT>> regulatoryElementsAs() const {
+    return utils::transformSharedPtr<const RegElemT>(regulatoryElements_);
+  }
+  template <typename RegElemT>
+  std::vector<std::shared_ptr<RegElemT>> regulatoryElementsAs() {
+    return utils::transformSharedPtr<RegElemT>(regulatoryElements_);
+  }
+
+  //! sets a new outer bound.
+  void setOuterBound(const LineStrings3d& bound) {
+    outerBound_ = bound;
+    resetCache();
+  }
+
+  void setInnerBounds(const std::vector<LineStrings3d>& bounds) {
+    innerBounds_ = bounds;
+    resetCache();
+  }
+
+  void resetCache() {
+    outerBoundPolygon_ = CompoundPolygon3d(utils::addConst(*this).outerBound());
+    innerBoundPolygons_ = utils::transform(utils::addConst(*this).innerBounds(),
+                                           [](const auto& elem) { return CompoundPolygon3d(elem); });
+  }
+
+ private:
+  LineStrings3d outerBound_;                  //!< linestrings that together form the outer bound
+  std::vector<LineStrings3d> innerBounds_;    //!< vector of ls for inner bounds
+  RegulatoryElementPtrs regulatoryElements_;  //!< regulatory elements that apply
+
+  // caches
+  CompoundPolygon3d outerBoundPolygon_;    //!< represents the outer bounds of the area
+  CompoundPolygons3d innerBoundPolygons_;  //!< represents the inner bounds of the area
+};
+
+//! @brief A const (i.e. immutable) Area.
+//! @ingroup AreaPrimitives
+//! @ingroup ConstPrimitives
+class ConstArea : public ConstPrimitive<AreaData> {
+ public:
+  using ConstType = ConstArea;
+  using MutableType = Area;
+  using TwoDType = ConstArea;
+  using ThreeDType = ConstArea;
+  using Category = traits::AreaTag;
+
+  /**
+   * @brief Constructs an empty or invalid area.
+   * @param id id of this area or InvalId
+   *
+   * This construction is only useful for copying.
+   */
+  explicit ConstArea(Id id = InvalId) : ConstArea(std::make_shared<AreaData>(id, LineStrings3d())) {}
+
+  /**
+   * @brief Constructs a new area
+   * @param id id of the area
+   * @param outerBound outer Boundary of area. Must be in clockwise order.
+   * @param innerBounds inner Boundaries of area. Must be in counter-clockwise
+   * order.
+   * @param attributes attributes of this area
+   * @param regulatoryElements regulatoryElements that apply to this area
+   *
+   * This is the constructor that you most probably want to use.
+   */
+  ConstArea(Id id, const LineStrings3d& outerBound, const InnerBounds& innerBounds = InnerBounds(),
+            const AttributeMap& attributes = AttributeMap(),
+            const RegulatoryElementPtrs& regulatoryElements = RegulatoryElementPtrs())
+      : ConstPrimitive{std::make_shared<AreaData>(id, outerBound, innerBounds, attributes, regulatoryElements)} {}
+
+  //! Constructor to construct from the data of a different Area
+  explicit ConstArea(const std::shared_ptr<const AreaData>& data) : ConstPrimitive<AreaData>(data) {}
+
+  //! Get linestrings that form the outer bound
+  ConstLineStrings3d outerBound() const { return constData()->outerBound(); }
+
+  //! get the linestrings that form the inner bounds
+  ConstInnerBounds innerBounds() const { return constData()->innerBounds(); }
+
+  /**
+   * @brief get the outer bound as polygon
+   * @return the polygon
+   */
+  CompoundPolygon3d outerBoundPolygon() const { return constData()->outerBoundPolygon(); }
+
+  //! get the inner bounds as polygon
+  CompoundPolygons3d innerBoundPolygons() const { return constData()->innerBoundPolygons(); }
+
+  /**
+   * @brief generates a basic polygon in 2d with holes for this area
+   *
+   * The generated polygon only has the points, no ids or attributes. It is
+   * thought for geometry calculations.
+   */
+  BasicPolygonWithHoles3d basicPolygonWithHoles3d() const {
+    return {outerBoundPolygon().basicPolygon(),
+            utils::transform(innerBoundPolygons(), [](const auto& poly) { return poly.basicPolygon(); })};
+  }
+
+  /**
+   * @brief generates a basic polygon in 2d with holes for this area
+   *
+   * The generated polygon only has the points, no ids or attributes. It is
+   * thought for geometry calculations.
+   */
+  BasicPolygonWithHoles2d basicPolygonWithHoles2d() const {
+    return {traits::to2D(outerBoundPolygon()).basicPolygon(),
+            utils::transform(innerBoundPolygons(), [](const auto& poly) { return traits::to2D(poly).basicPolygon(); })};
+  }
+
+  //! get a list of regulatory elements that affect this area
+  RegulatoryElementConstPtrs regulatoryElements() const { return constData()->regulatoryElements(); }
+
+  /**
+   * @brief get the regulatoryElements that could be converted to a type
+   * @return the matching regulatory elements, if any
+   * @tparam RegElemT the type to covert to *without cv qualifiers*
+   *
+   * E.g. regulatoryElementsAs<TrafficLight>() will return all regulatory
+   * elements that are traffic lights.
+   */
+  template <typename RegElemT>
+  std::vector<std::shared_ptr<const RegElemT>> regulatoryElementsAs() const {
+    return constData()->regulatoryElementsAs<RegElemT>();
+  }
+};
+
+//! @brief Famous Area class that represents a basic area as element of the map.
+//! @ingroup AreaPrimitives
+//! @ingroup Primitives
+class Area : public Primitive<ConstArea> {
+ public:
+  using Primitive::Primitive;
+  using TwoDType = Area;
+  using ThreeDType = Area;
+  friend class ConstWeakArea;
+  Area() = default;
+  Area(const AreaDataConstPtr&) = delete;
+  explicit Area(const AreaDataPtr& data) : Primitive{data} {}
+
+  using Primitive::innerBounds;
+  using Primitive::outerBound;
+  using Primitive::regulatoryElements;
+  using Primitive::regulatoryElementsAs;
+
+  //! Get linestrings that form the outer bound
+  const LineStrings3d& outerBound() { return data()->outerBound(); }
+
+  //! Get the linestrings that form the inner bound
+  const InnerBounds& innerBounds() { return data()->innerBounds(); }
+
+  //! Sets a new outer bound and resets the cache
+  void setOuterBound(const LineStrings3d& bound) { data()->setOuterBound(bound); }
+
+  //! sets a new inner bound and resets the cache
+  void setInnerBounds(const std::vector<LineStrings3d>& bounds) { data()->setInnerBounds(bounds); }
+
+  //! return regulatoryElements that affect this area.
+  const RegulatoryElementPtrs& regulatoryElements() { return data()->regulatoryElements(); }
+
+  /**
+   * @brief adds a new regulatory element
+   * @throws nullptr error if regElem is a nullptr
+   */
+  void addRegulatoryElement(RegulatoryElementPtr regElem) {
+    if (regElem == nullptr) {
+      throw NullptrError("regulatory element is a nullptr.");
+    }
+    data()->regulatoryElements().push_back(std::move(regElem));
+  }
+
+  //! removes a regulatory element, returns true on success
+  bool removeRegulatoryElement(const RegulatoryElementPtr& regElem) {
+    auto& regElems = data()->regulatoryElements();
+    auto remove = std::find(regElems.begin(), regElems.end(), regElem);
+    if (remove != regElems.end()) {
+      regElems.erase(remove);
+      return true;
+    }
+    return false;
+  }
+
+  //! get the regulatoryElements that could be converted to RegElemT
+  template <typename RegElemT>
+  std::vector<std::shared_ptr<RegElemT>> regulatoryElementsAs() {
+    return data()->regulatoryElementsAs<RegElemT>();
+  }
+};
+
+/**
+ * @brief used internally by RegulatoryElements to avoid cyclic dependencies.
+ *
+ * It works conceptually similar to a std::weak_ptr. This is the immutable
+ * version of a WeakArea and returns a ConstArea when lock() is called.
+ */
+class ConstWeakArea {
+ public:
+  using ConstType = ConstWeakArea;
+  using MutableType = WeakArea;
+  using TwoDType = ConstWeakArea;
+  using ThreeDType = ConstWeakArea;
+  ConstWeakArea() = default;
+  ConstWeakArea(ConstArea area)  // NOLINT
+      : areaData_(area.constData()) {}
+
+  /**
+   * @brief Obtains the original ConstArea.
+   * @throws NullptrError if the managed lanelet expired.
+   */
+  ConstArea lock() const { return ConstArea(areaData_.lock()); }
+
+  //! tests whether the WeakArea is still valid
+  bool expired() const noexcept { return areaData_.expired(); }
+
+ protected:
+  std::weak_ptr<const AreaData> areaData_;  // NOLINT
+};
+
+/**
+ * @brief used internally by RegulatoryElements to avoid cyclic dependencies.
+ *
+ * It works conceptually similar to a std::weak_ptr. This is the mutable
+ * version of a WeakArea.
+ */
+class WeakArea final : public ConstWeakArea {
+ public:
+  WeakArea() = default;
+  WeakArea(Area llet) : ConstWeakArea(std::move(llet)) {}  // NOLINT
+
+  /**
+   * @brief Obtains the original ConstArea
+   * @throws NullptrError if the managed lanelet expired.
+   */
+  Area lock() const { return Area(std::const_pointer_cast<AreaData>(areaData_.lock())); }
+};
+
+namespace utils {
+/**
+ * @brief returns true if element of a regulatory element has a matching Id
+ * @param ll the element holding other primitives
+ * @param id id to look for
+ * @return true if the primitive has such an element
+ *
+ * This function does not look for the id of the element, only its members
+ * Works for linestrings and polylines. Does NOT check members in regulatory
+ * elements, only ids of regulatory elements itself.
+ * A similar implementation exists for linestrings and regulatory elements.
+ */
+inline bool has(const ConstArea& ll, Id id) { return has(ll.outerBoundPolygon(), id); }
+}  // namespace utils
+
+inline bool operator==(const ConstArea& lhs, const ConstArea& rhs) { return lhs.constData() == rhs.constData(); }
+inline bool operator!=(const ConstArea& lhs, const ConstArea& rhs) { return !(lhs == rhs); }
+inline bool operator==(const ConstWeakArea& lhs, const ConstWeakArea& rhs) {
+  return !lhs.expired() && !rhs.expired() && lhs.lock() == rhs.lock();
+}
+inline bool operator!=(const ConstWeakArea& lhs, const ConstWeakArea& rhs) { return !(lhs == rhs); }
+inline std::ostream& operator<<(std::ostream& stream, const ConstArea& obj) {
+  stream << "[id: ";
+  stream << obj.id();
+  auto obId = obj.outerBoundPolygon().ids();
+  if (!obId.empty()) {
+    stream << " outer: [";
+    std::copy(obId.begin(), obId.end(), std::ostream_iterator<Id>(stream, ","));
+    stream << "]";
+  }
+  auto ibs = obj.innerBoundPolygons();
+  if (!ibs.empty()) {
+    stream << " inner: ";
+    for (const auto& ib : ibs) {
+      stream << "[";
+      auto ibId = ib.ids();
+      std::copy(ibId.begin(), ibId.end(), std::ostream_iterator<Id>(stream, ","));
+      stream << "]";
+    }
+  }
+  stream << "]";
+  return stream;
+}
+
+namespace traits {
+template <typename T>
+constexpr bool isAreaT() {
+  return isCategory<T, traits::AreaTag>();
+}
+}  // namespace traits
+
+template <typename T, typename RetT>
+using IfAr = std::enable_if_t<traits::isAreaT<T>(), RetT>;
+
+}  // namespace lanelet
+
+// Hash function for usage in containers
+namespace std {
+template <>
+struct hash<lanelet::Area> : public lanelet::HashBase<lanelet::Area> {};
+template <>
+struct hash<lanelet::ConstArea> : public lanelet::HashBase<lanelet::ConstArea> {};
+}  // namespace std

src/map/lanelet2/include/lanelet2_core/primitives/BasicRegulatoryElements.h

@@ -0,0 +1,362 @@
+#pragma once
+
+#include <string>
+
+#include "lanelet2_core/Forward.h"
+#include "lanelet2_core/primitives/LineString.h"
+#include "lanelet2_core/primitives/LineStringOrPolygon.h"
+#include "lanelet2_core/primitives/RegulatoryElement.h"
+
+namespace lanelet {
+
+/**
+ * @brief Represents a traffic light restriction on the lanelet
+ * @ingroup RegulatoryElementPrimitives
+ * @ingroup Primitives
+ */
+class TrafficLight : public RegulatoryElement {
+ public:
+  using Ptr = std::shared_ptr<TrafficLight>;
+  static constexpr char RuleName[] = "traffic_light";
+  //! Directly construct a stop line from its required rule parameters.
+  //! Might modify the input data in oder to get correct tags.
+  static Ptr make(Id id, const AttributeMap& attributes, const LineStringsOrPolygons3d& trafficLights,
+                  const Optional<LineString3d>& stopLine = {}) {
+    return Ptr{new TrafficLight(id, attributes, trafficLights, stopLine)};
+  }
+
+  /**
+   * @brief get the stop line for the traffic light
+   * @return the stop line as LineString
+   */
+  Optional<ConstLineString3d> stopLine() const;
+  Optional<LineString3d> stopLine();
+
+  /**
+   * @brief get the relevant traffic lights
+   * @return the traffic lights
+   *
+   * There might be multiple traffic lights but they are required to show the
+   * same signal.
+   */
+  ConstLineStringsOrPolygons3d trafficLights() const;
+  LineStringsOrPolygons3d trafficLights();
+
+  /**
+   * @brief add a new traffic light
+   * @param primitive the traffic light to add
+   *
+   * Traffic lights are represented as linestrings that start at the left edge
+   * of a traffic light and end at the right edge.
+   */
+  void addTrafficLight(const LineStringOrPolygon3d& primitive);
+
+  /**
+   * @brief remove a traffic light
+   * @param primitive the primitive
+   * @return true if the traffic light existed and was removed
+   */
+  bool removeTrafficLight(const LineStringOrPolygon3d& primitive);
+
+  /**
+   * @brief set a new stop line, overwrite the old one
+   * @param stopLine new stop line
+   */
+  void setStopLine(const LineString3d& stopLine);
+
+  //! Deletes the stop line
+  void removeStopLine();
+
+ protected:
+  friend class RegisterRegulatoryElement<TrafficLight>;
+  TrafficLight(Id id, const AttributeMap& attributes, const LineStringsOrPolygons3d& trafficLights,
+               const Optional<LineString3d>& stopLine);
+  explicit TrafficLight(const RegulatoryElementDataPtr& data);
+};
+
+//! Enum to distinguish maneuver types
+enum class ManeuverType {
+  Yield,       //!> Lanelet has right of way
+  RightOfWay,  //!< Lanelet has to yield
+  Unknown      //!< Lanelet ist not part of relation
+};
+
+//! @brief Defines right of way restrictions
+//! @ingroup RegulatoryElementPrimitives
+//! @ingroup Primitives
+class RightOfWay : public RegulatoryElement {
+ public:
+  using Ptr = std::shared_ptr<RightOfWay>;
+  static constexpr char RuleName[] = "right_of_way";
+
+  /**
+   * @brief Create a valid Right of Way object
+   * @param id id for this rule
+   * @param attributes for this rule. Might be extended if necessary
+   * @param rightOfWay the lanelets that have right of way
+   * @param yield the lanelets that have to yield
+   * @param stopLine line where to stop. If there is none, stop at the end of
+   * the lanelet.
+   */
+  static Ptr make(Id id, const AttributeMap& attributes, const Lanelets& rightOfWay, const Lanelets& yield,
+                  const Optional<LineString3d>& stopLine = {}) {
+    return Ptr{new RightOfWay(id, attributes, rightOfWay, yield, stopLine)};
+  }
+
+  //! returns whether a lanelet has to yield or has right of way
+  ManeuverType getManeuver(const ConstLanelet& lanelet) const;
+
+  //! get the lanelets have right of way
+  ConstLanelets rightOfWayLanelets() const;
+  Lanelets rightOfWayLanelets();
+
+  //! get the lanelets that have to yield
+  ConstLanelets yieldLanelets() const;
+  Lanelets yieldLanelets();
+
+  //! get the stop line for the yield lanelets, if present
+  Optional<ConstLineString3d> stopLine() const;
+  Optional<LineString3d> stopLine();
+
+  //! Overwrites the stop line
+  void setStopLine(const LineString3d& stopLine);
+
+  //! Adds a lanelet for RightOfWay
+  void addRightOfWayLanelet(const Lanelet& lanelet);
+
+  //! Add yielding lanelet
+  void addYieldLanelet(const Lanelet& lanelet);
+
+  //! Removes a right of way lanelet and returns true on success
+  bool removeRightOfWayLanelet(const Lanelet& lanelet);
+
+  //! Removes a yielding lanelet and returns true no success
+  bool removeYieldLanelet(const Lanelet& lanelet);
+
+  //! Removes the stop line
+  void removeStopLine();
+
+ protected:
+  friend class RegisterRegulatoryElement<RightOfWay>;
+  RightOfWay(Id id, const AttributeMap& attributes, const Lanelets& rightOfWay, const Lanelets& yield,
+             const Optional<LineString3d>& stopLine = {});
+  explicit RightOfWay(const RegulatoryElementDataPtr& data);
+};
+
+struct LaneletWithStopLine {
+  Lanelet lanelet;
+  Optional<LineString3d> stopLine;
+};
+struct ConstLaneletWithStopLine {
+  ConstLanelet lanelet;
+  Optional<ConstLineString3d> stopLine;
+};
+using LaneletsWithStopLines = std::vector<LaneletWithStopLine>;
+
+//! @brief Defines an all way stop. These are a special form of right of way, where all lanelets have to yield,
+//! depending on the order of arrival and the route through the intersection.
+//! @ingroup RegulatoryElementPrimitives
+//! @ingroup Primitives
+//!
+//! The distance to the intersection is represented either by the distance to the stop line, if present, otherwise the
+//! distance to the end of the lanelet.
+class AllWayStop : public RegulatoryElement {
+ public:
+  using Ptr = std::shared_ptr<AllWayStop>;
+  static constexpr char RuleName[] = "all_way_stop";
+
+  /**
+   * @brief Create a valid all way stop object
+   * @param id id for this rule
+   * @param attributes for this rule. Might be extended if necessary
+   * @param lltsWithStop lanelets with stop line. Either all lanelets have a stop line or none.
+   * @param signs traffic signs that constitute this rule
+   */
+  static Ptr make(Id id, const AttributeMap& attributes, const LaneletsWithStopLines& lltsWithStop,
+                  const LineStringsOrPolygons3d& signs = {}) {
+    return Ptr{new AllWayStop(id, attributes, lltsWithStop, signs)};
+  }
+
+  //! get the lanelets that potentially have to yield
+  ConstLanelets lanelets() const;
+  Lanelets lanelets();
+
+  //! Adds a new lanelet with stop line. This will throw if the other lanelets did not have a stop line and vice versa
+  //! @throws InvalidInputError if stop line is inconsistent
+  void addLanelet(const LaneletWithStopLine& lltWithStop);
+
+  //! Removes a lanelet and the associated stop line, if there is one
+  bool removeLanelet(const Lanelet& llt);
+
+  //! get the stop lines
+  ConstLineStrings3d stopLines() const;
+  LineStrings3d stopLines();
+
+  //! gets the stop line for a lanelet, if there is one
+  Optional<ConstLineString3d> getStopLine(const ConstLanelet& llt) const;
+  Optional<LineString3d> getStopLine(const ConstLanelet& llt);
+
+  //! get list of traffic signs that constitute this AllWayStop if existing
+  ConstLineStringsOrPolygons3d trafficSigns() const;
+  LineStringsOrPolygons3d trafficSigns();
+
+  //! Adds another traffic sign
+  /** Traffic signs are represented as linestrings that start at the left edge
+   * and end at the right edge of a traffic sign.
+   */
+  void addTrafficSign(const LineStringOrPolygon3d& sign);
+
+  //! removes a traffic sign and returns true on success
+  bool removeTrafficSign(const LineStringOrPolygon3d& sign);
+
+ protected:
+  friend class RegisterRegulatoryElement<AllWayStop>;
+  AllWayStop(Id id, const AttributeMap& attributes, const LaneletsWithStopLines& lltsWithStop,
+             const LineStringsOrPolygons3d& signs);
+  explicit AllWayStop(const RegulatoryElementDataPtr& data);
+};
+
+//! Used as input argument to create TrafficSign regElem
+struct TrafficSignsWithType {
+  LineStringsOrPolygons3d trafficSigns;  //!< Lists relevant traffic signs
+  std::string type{""};                  //! Lists their type. If empty, it is assumed that this
+                                         //! is found in the attributes of trafficSigns.
+                                         //! the format is <country-code><ID>, eg de205.
+};
+
+//! @brief Expresses a generic traffic sign rule
+//! @ingroup RegulatoryElementPrimitives
+//! @ingroup Primitives
+class TrafficSign : public RegulatoryElement {
+ public:
+  using Ptr = std::shared_ptr<TrafficSign>;
+  static constexpr char RuleName[] = "traffic_sign";
+
+  /**
+   * @brief Create a valid TrafficSign object
+   *
+   * A traffic sign is usually composed of a set of traffic signs of the same type that mark the beginning of the rule.
+   * It also contains multiple traffic signs (potentially of different types) that mark the end of the rule.
+   * E.g. a 50kph section would contain all 50kph signs of this section as trafficSigns. All signs that stand at the end
+   * of this section (e.g. a 70kph sign and an end of 50kph sign) would be cancellingTrafficSigns.
+   * @param id id of traffic sign rule
+   * @param attributes attributes for it (might be extended if necessary)
+   * @param trafficSigns list of the traffic signs defining the rule
+   * @param cancellingTrafficSigns list of traffic signs where the rule is
+   * cancelled. Can be empty.
+   * @param refLines lines from where the rule becomes valid. Can be empty.
+   * @param cancelLines lines after which a rule becomes invalid. Can be empty.
+   */
+  static Ptr make(Id id, const AttributeMap& attributes, const TrafficSignsWithType& trafficSigns,
+                  const TrafficSignsWithType& cancellingTrafficSigns = {}, const LineStrings3d& refLines = {},
+                  const LineStrings3d& cancelLines = {}) {
+    return Ptr(new TrafficSign(id, attributes, trafficSigns, cancellingTrafficSigns, refLines, cancelLines));
+  }
+
+  //! returns the traffic signs
+  /** There might be multiple but they are all required to show the same symbol.
+   */
+  ConstLineStringsOrPolygons3d trafficSigns() const;
+  LineStringsOrPolygons3d trafficSigns();
+
+  //! get the id/number of the sign(s)
+  /** The id is in the format [country-code][ID], e.g. de205.
+   * The result can be dependant on country
+   */
+  std::string type() const;
+
+  //! gets the line(s) from which a sign becomes valid.
+  /**
+   * There might or might not be such a line. If there is none, the sign is
+   * valid for the whole lanelet
+   */
+  ConstLineStrings3d refLines() const;
+  LineStrings3d refLines();
+
+  //! get list of cancellingTrafficSigns, if existing
+  ConstLineStringsOrPolygons3d cancellingTrafficSigns() const;
+  LineStringsOrPolygons3d cancellingTrafficSigns();
+
+  //! Types of the cancelling traffic signs if they exist
+  std::vector<std::string> cancelTypes() const;
+
+  //! gets the line(s) from which a sign becomes invalid.
+  ConstLineStrings3d cancelLines() const;
+  LineStrings3d cancelLines();
+
+  //! Adds another traffic sign
+  /** Traffic signs are represented as linestrings that start at the left edge
+   * and end at the right edge of a traffic sign.
+   */
+  void addTrafficSign(const LineStringOrPolygon3d& sign);
+
+  //! remove a traffic sign and returns true on success
+  bool removeTrafficSign(const LineStringOrPolygon3d& sign);
+
+  //! Add new cancelling traffic sign
+  void addCancellingTrafficSign(const TrafficSignsWithType& signs);
+
+  //! remove a cancelling traffic sign, returns true on success
+  bool removeCancellingTrafficSign(const LineStringOrPolygon3d& sign);
+
+  //! Add a new reference line
+  void addRefLine(const LineString3d& line);
+
+  //! Remove a reference line. Returns true on success.
+  bool removeRefLine(const LineString3d& line);
+
+  //! Add a new line from where the sign becomes inactive
+  void addCancellingRefLine(const LineString3d& line);
+
+  //! Remove a cancelling line. Returns true on success.
+  bool removeCancellingRefLine(const LineString3d& line);
+
+ protected:
+  TrafficSign(Id id, const AttributeMap& attributes, const TrafficSignsWithType& trafficSigns,
+              const TrafficSignsWithType& cancellingTrafficSigns = {}, const LineStrings3d& refLines = {},
+              const LineStrings3d& cancelLines = {});
+
+  friend class RegisterRegulatoryElement<TrafficSign>;
+  explicit TrafficSign(const RegulatoryElementDataPtr& data);
+};
+
+/**
+ * @brief Represents a speed limit that affects a lanelet
+ * @ingroup RegulatoryElementPrimitives
+ * @ingroup Primitives
+ *
+ * A speed limit is defined by one ore more traffic signs and cancelled by one
+ * or more traffic signs. All lanelets affected by this refer to this traffic
+ * sign.
+ *
+ * As an alternative, the type can also be specified using the sign_type tag of
+ * the regulatory element. However this is not recommended, because will make it
+ * hard to track where the speed limit originates.
+ */
+class SpeedLimit : public TrafficSign {
+ public:
+  using Ptr = std::shared_ptr<SpeedLimit>;
+  static constexpr char RuleName[] = "speed_limit";
+
+  //! Create a speed limit regulatory element. Similar to a traffic sign.
+  static Ptr make(Id id, const AttributeMap& attributes, const TrafficSignsWithType& trafficSigns,
+                  const TrafficSignsWithType& cancellingTrafficSigns = {}, const LineStrings3d& refLines = {},
+                  const LineStrings3d& cancelLines = {}) {
+    return Ptr(new SpeedLimit(id, attributes, trafficSigns, cancellingTrafficSigns, refLines, cancelLines));
+  }
+
+  //! Create a speed limit regulatory element only from a type or speed limit without actual sign
+  static Ptr make(Id id, AttributeMap attributes, const std::string& signType, const LineStrings3d& refLines = {},
+                  const LineStrings3d& cancelLines = {}) {
+    attributes.insert(std::make_pair(AttributeNamesString::SignType, signType));
+    return Ptr(new SpeedLimit(id, attributes, {}, {}, refLines, cancelLines));
+  }
+
+ protected:
+  friend class RegisterRegulatoryElement<SpeedLimit>;
+  SpeedLimit(Id id, const AttributeMap& attributes, const TrafficSignsWithType& trafficSigns,
+             const TrafficSignsWithType& cancellingTrafficSigns = {}, const LineStrings3d& refLines = {},
+             const LineStrings3d& cancelLines = {});
+  explicit SpeedLimit(const RegulatoryElementDataPtr& data);
+};
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/primitives/BoundingBox.h

@@ -0,0 +1,306 @@
+#pragma once
+
+#pragma GCC diagnostic push
+#if defined __GNUC__ && (__GNUC__ >= 6)
+#pragma GCC diagnostic ignored "-Wignored-attributes"
+#pragma GCC diagnostic ignored "-Wint-in-bool-context"
+#endif
+#include <Eigen/Geometry>
+
+#include "lanelet2_core/primitives/Point.h"
+#include "lanelet2_core/primitives/Traits.h"
+#pragma GCC diagnostic pop
+
+namespace lanelet {
+
+/**
+ * @brief Axis-Aligned bounding box in 2d
+ *
+ * Eigens aligned bounding box does not work for us in 2d, because of its memory alignment reqiurements.
+ * This is basically the same implementation, but without the memory alginment. We don't have to do that for the 3d
+ * case, because the 3d case does not use memory alignment in Eigen.
+ */
+class BoundingBox2d {
+ public:
+  enum { AmbientDimAtCompileTime = 2 };
+  using Scalar = double;
+  using ScalarTraits = Eigen::NumTraits<Scalar>;
+  using Index = Eigen::Index;
+  using RealScalar = ScalarTraits::Real;
+  using NonInteger = ScalarTraits::NonInteger;
+  using VectorType = BasicPoint2d;
+
+  /** Define constants to name the corners of a 1D, 2D or 3D axis aligned bounding box */
+  enum CornerType {
+    /** 1D names @{ */
+    Min = 0,
+    Max = 1,
+    /** @} */
+
+    /** Identifier for 2D corner @{ */
+    BottomLeft = 0,
+    BottomRight = 1,
+    TopLeft = 2,
+    TopRight = 3,
+    /** @} */
+
+    /** Identifier for 3D corner  @{ */
+    BottomLeftFloor = 0,
+    BottomRightFloor = 1,
+    TopLeftFloor = 2,
+    TopRightFloor = 3,
+    BottomLeftCeil = 4,
+    BottomRightCeil = 5,
+    TopLeftCeil = 6,
+    TopRightCeil = 7
+    /** @} */
+  };
+
+  /** Default constructor initializing a null box. */
+  inline BoundingBox2d() {
+    if (AmbientDimAtCompileTime != Eigen::Dynamic) {
+      setEmpty();
+    }
+  }
+
+  /** Constructs a null box with \a _dim the dimension of the ambient space. */
+  inline explicit BoundingBox2d(Index dim) : min_(dim), max_(dim) { setEmpty(); }
+
+  /** Constructs a box with extremities \a _min and \a _max.
+   * \warning If either component of \a _min is larger than the same component of \a _max, the constructed box is empty.
+   */
+  template <typename OtherVectorType1, typename OtherVectorType2>
+  inline BoundingBox2d(OtherVectorType1 min, OtherVectorType2 max) : min_(std::move(min)), max_(std::move(max)) {}
+
+  /** Constructs a box containing a single point \a p. */
+  template <typename Derived>
+  inline explicit BoundingBox2d(const Eigen::MatrixBase<Derived>& p) : min_(p), max_(min_) {}
+
+  inline BoundingBox2d(const Eigen::AlignedBox<double, 2>& other) : min_(other.min()), max_(other.max()) {}  // NOLINT
+
+  /** \returns the dimension in which the box holds */
+  inline Index dim() const {
+    return AmbientDimAtCompileTime == Eigen::Dynamic ? min_.size() : Index(AmbientDimAtCompileTime);
+  }
+
+  /** \deprecated use isEmpty() */
+  inline bool isNull() const { return isEmpty(); }
+
+  /** \deprecated use setEmpty() */
+  inline void setNull() { setEmpty(); }
+
+  /** \returns true if the box is empty.
+   * \sa setEmpty */
+  inline bool isEmpty() const { return (min_.array() > max_.array()).any(); }
+
+  /** Makes \c *this an empty box.
+   * \sa isEmpty */
+  inline void setEmpty() {
+    min_.setConstant(ScalarTraits::highest());
+    max_.setConstant(ScalarTraits::lowest());
+  }
+
+  /** \returns the minimal corner */
+  inline const VectorType&(min)() const { return min_; }
+  /** \returns a non const reference to the minimal corner */
+  inline VectorType&(min)() { return min_; }
+  /** \returns the maximal corner */
+  inline const VectorType&(max)() const { return max_; }
+  /** \returns a non const reference to the maximal corner */
+  inline VectorType&(max)() { return max_; }
+
+  /** \returns the center of the box */
+  inline BasicPoint2d center() const { return (min_ + max_) / 2; }
+
+  /** \returns the lengths of the sides of the bounding box.
+   * Note that this function does not get the same
+   * result for integral or floating scalar types: see
+   */
+  inline BasicPoint2d sizes() const { return max_ - min_; }
+
+  /** \returns the volume of the bounding box */
+  inline Scalar volume() const { return sizes().prod(); }
+
+  /** \returns an expression for the bounding box diagonal vector
+   * if the length of the diagonal is needed: diagonal().norm()
+   * will provide it.
+   */
+  inline BasicPoint2d diagonal() const { return sizes(); }
+
+  /** \returns the vertex of the bounding box at the corner defined by
+   * the corner-id corner. It works only for a 1D, 2D or 3D bounding box.
+   * For 1D bounding boxes corners are named by 2 enum constants:
+   * BottomLeft and BottomRight.
+   * For 2D bounding boxes, corners are named by 4 enum constants:
+   * BottomLeft, BottomRight, TopLeft, TopRight.
+   * For 3D bounding boxes, the following names are added:
+   * BottomLeftCeil, BottomRightCeil, TopLeftCeil, TopRightCeil.
+   */
+  inline VectorType corner(CornerType corner) const {
+    VectorType res;
+
+    Index mult = 1;
+    for (Index d = 0; d < dim(); ++d) {
+      if ((mult & corner) != 0) {
+        res[d] = max_[d];
+      } else {
+        res[d] = min_[d];
+      }
+      mult *= 2;
+    }
+    return res;
+  }
+
+  /** \returns a random point inside the bounding box sampled with
+   * a uniform distribution */
+  inline VectorType sample() const { return Eigen::AlignedBox<double, 2>(min_, max_).sample(); }
+
+  /** \returns true if the point \a p is inside the box \c *this. */
+  template <typename Derived>
+  inline bool contains(const Eigen::MatrixBase<Derived>& p) const {
+    return Eigen::AlignedBox<double, 2>(min_, max_).contains(p);
+  }
+
+  /** \returns true if the box \a b is entirely inside the box \c *this. */
+  inline bool contains(const BoundingBox2d& b) const {
+    return (min_.array() <= (b.min)().array()).all() && ((b.max)().array() <= max_.array()).all();
+  }
+
+  /** \returns true if the box \a b is intersecting the box \c *this.
+   * \sa intersection, clamp */
+  inline bool intersects(const BoundingBox2d& b) const {
+    return (min_.array() <= (b.max)().array()).all() && ((b.min)().array() <= max_.array()).all();
+  }
+
+  /** Extends \c *this such that it contains the point \a p and returns a reference to \c *this.
+   * \sa extend(const BoundingBox&) */
+  template <typename Derived>
+  inline BoundingBox2d& extend(const Eigen::MatrixBase<Derived>& p) {
+    auto pN(p.derived().eval());
+    min_ = min_.cwiseMin(pN);
+    max_ = max_.cwiseMax(pN);
+    return *this;
+  }
+
+  /** Extends \c *this such that it contains the box \a b and returns a reference to \c *this.
+   * \sa merged, extend(const MatrixBase&) */
+  inline BoundingBox2d& extend(const BoundingBox2d& b) {
+    min_ = min_.cwiseMin(b.min_);
+    max_ = max_.cwiseMax(b.max_);
+    return *this;
+  }
+
+  /** Clamps \c *this by the box \a b and returns a reference to \c *this.
+   * \note If the boxes don't intersect, the resulting box is empty.
+   * \sa intersection(), intersects() */
+  inline BoundingBox2d& clamp(const BoundingBox2d& b) {
+    min_ = min_.cwiseMax(b.min_);
+    max_ = max_.cwiseMin(b.max_);
+    return *this;
+  }
+
+  /** Returns an BoundingBox2d that is the intersection of \a b and \c *this
+   * \note If the boxes don't intersect, the resulting box is empty.
+   * \sa intersects(), clamp, contains()  */
+  inline BoundingBox2d intersection(const BoundingBox2d& b) const {
+    return {min_.cwiseMax(b.min_), max_.cwiseMin(b.max_)};
+  }
+
+  /** Returns an BoundingBox2d that is the union of \a b and \c *this.
+   * \note Merging with an empty box may result in a box bigger than \c *this.
+   * \sa extend(const BoundingBox2d&) */
+  inline BoundingBox2d merged(const BoundingBox2d& b) const { return {min_.cwiseMin(b.min_), max_.cwiseMax(b.max_)}; }
+
+  /** Translate \c *this by the vector \a t and returns a reference to \c *this. */
+  template <typename Derived>
+  inline BoundingBox2d& translate(const Eigen::MatrixBase<Derived>& aT) {
+    const auto t(aT.derived().eval());
+    min_ += t;
+    max_ += t;
+    return *this;
+  }
+
+  /** \returns the squared distance between the point \a p and the box \c *this,
+   * and zero if \a p is inside the box.
+   * \sa exteriorDistance(const MatrixBase&), squaredExteriorDistance(const BoundingBox2d&)
+   */
+  template <typename Derived>
+  inline Scalar squaredExteriorDistance(const Eigen::MatrixBase<Derived>& p) const {
+    return Eigen::AlignedBox<double, 2>(min_, max_).squaredExteriorDistance(p);
+  }
+
+  /** \returns the squared distance between the boxes \a b and \c *this,
+   * and zero if the boxes intersect.
+   * \sa exteriorDistance(const BoundingBox2d&), squaredExteriorDistance(const MatrixBase&)
+   */
+  inline Scalar squaredExteriorDistance(const BoundingBox2d& b) const {
+    using EigenBox = Eigen::AlignedBox<double, 2>;
+    return EigenBox(min_, max_).squaredExteriorDistance(EigenBox(b.min_, b.max_));
+  }
+
+  /** \returns the distance between the point \a p and the box \c *this,
+   * and zero if \a p is inside the box.
+   * \sa squaredExteriorDistance(const MatrixBase&), exteriorDistance(const BoundingBox2d&)
+   */
+  template <typename Derived>
+  inline NonInteger exteriorDistance(const Eigen::MatrixBase<Derived>& p) const {
+    return std::sqrt(NonInteger(squaredExteriorDistance(p)));
+  }
+
+  /** \returns the distance between the boxes \a b and \c *this,
+   * and zero if the boxes intersect.
+   * \sa squaredExteriorDistance(const BoundingBox2d&), exteriorDistance(const MatrixBase&)
+   */
+  inline NonInteger exteriorDistance(const BoundingBox2d& b) const {
+    using std::sqrt;
+    return sqrt(NonInteger(squaredExteriorDistance(b)));
+  }
+
+  /** Copy constructor with scalar type conversion */
+  template <typename OtherScalarType>
+  inline explicit BoundingBox2d(const Eigen::AlignedBox<OtherScalarType, AmbientDimAtCompileTime>& other) {
+    min_ = (other.min)().template cast<Scalar>();
+    max_ = (other.max)().template cast<Scalar>();
+  }
+
+  /** \returns \c true if \c *this is approximately equal to \a other, within the precision
+   * determined by \a prec.
+   *
+   * \sa MatrixBase::isApprox() */
+  bool isApprox(const BoundingBox2d& other, const RealScalar& prec = ScalarTraits::dummy_precision()) const {
+    return min_.isApprox(other.min_, prec) && max_.isApprox(other.max_, prec);
+  }
+
+ private:
+  VectorType min_, max_;
+};
+
+/**
+ * @brief Convenience type for an axis aligned bounding box in 3d.
+ *
+ * Can be used as Eigen::AlignedBox2d and as boost::geometry::Box.
+ */
+using BoundingBox3d = Eigen::AlignedBox3d;
+
+namespace traits {
+template <>
+struct PrimitiveTraits<BoundingBox2d> {
+  using ConstType = BoundingBox2d;
+  using MutableType = BoundingBox2d;
+  using TwoDType = BoundingBox2d;
+  using ThreeDType = BoundingBox3d;
+  using Category = BoundingBoxTag;
+};
+template <>
+struct PrimitiveTraits<BoundingBox3d> {
+  using ConstType = BoundingBox3d;
+  using MutableType = BoundingBox3d;
+  using TwoDType = BoundingBox2d;
+  using ThreeDType = BoundingBox3d;
+  using Category = BoundingBoxTag;
+};
+
+inline BoundingBox3d to3D(const BoundingBox2d& primitive) { return {to3D(primitive.min()), to3D(primitive.max())}; }
+inline BoundingBox2d to2D(const BoundingBox3d& primitive) { return {to2D(primitive.min()), to2D(primitive.max())}; }
+}  // namespace traits
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/primitives/CompoundLineString.h

@@ -0,0 +1,402 @@
+#pragma once
+#include <boost/noncopyable.hpp>
+#include <utility>
+
+#include "lanelet2_core/primitives/LineString.h"
+#include "lanelet2_core/utility/CompoundIterator.h"
+
+namespace lanelet {
+namespace internal {
+/**
+ * @brief This class selects which iterator CompoundLineStringImpl needs.
+ */
+template <typename PointT>
+struct SelectCompoundLsIterator {
+  using BaseIterator = internal::UniqueCompoundIterator<const ConstLineStrings3d>;
+  using Iterator = internal::TransformIterator<internal::ReverseAndForwardIterator<BaseIterator>, const PointT>;
+};
+
+template <>
+struct SelectCompoundLsIterator<ConstPoint3d> {
+  using BaseIterator = internal::UniqueCompoundIterator<const ConstLineStrings3d>;
+  using Iterator = internal::ReverseAndForwardIterator<BaseIterator>;
+};
+
+template <typename T>
+using SelectCompoundLsIteratorT = typename SelectCompoundLsIterator<T>::Iterator;
+}  // namespace internal
+
+/**
+ * @brief Common data object for all CompoundLineStrings
+ *
+ * This object is only for internal data management and should not be used
+ * directly.
+ *
+ * CompoundLineStrings merely have a shared_ptr to this object and forward all
+ * calls to this object.
+ */
+class CompoundLineStringData : boost::noncopyable {
+  using BaseIter = typename internal::SelectCompoundLsIterator<ConstPoint3d>::BaseIterator;
+  using Reverse = std::reverse_iterator<BaseIter>;
+
+ public:
+  using RFIter = internal::SelectCompoundLsIteratorT<ConstPoint3d>;
+  explicit CompoundLineStringData(ConstLineStrings3d lineStrings) : ls_{std::move(lineStrings)} {}
+  ConstPoints3d points() const { return ConstPoints3d(BaseIter::begin(ls_), BaseIter::end(ls_)); }
+  const ConstLineStrings3d& lineStrings() const { return ls_; }
+  RFIter pointsBegin(bool inverted) const {
+    return inverted ? RFIter(Reverse(BaseIter::end(ls_))) : RFIter(BaseIter::begin(ls_));
+  }
+  RFIter pointsEnd(bool inverted) const {
+    return inverted ? RFIter(Reverse(BaseIter::begin(ls_))) : RFIter(BaseIter::end(ls_));
+  }
+
+ private:
+  const ConstLineStrings3d ls_;  // NOLINT
+};
+
+/**
+ * @brief A collection of lineStrings that act as one line string
+ * @tparam PointT the point type that the linestring returns
+ *
+ * A CompoundLineString has the same interface as a LineString2d/LineString3d,
+ * but internally accesses the points of multiple linestrings. Because it just
+ * provides an interface to the points, the internal data can not be modified.
+ * If adjacent linestrings share the same points, one of them is discarded.
+ *
+ */
+template <typename PointT>
+class CompoundLineStringImpl {
+  using RFIter = CompoundLineStringData::RFIter;
+
+ public:
+  // using declarations
+  using DataType = CompoundLineStringData;
+  using PointType = PointT;
+  using BasicPointType = traits::BasicPointT<PointT>;
+  using ConstPointType = traits::ConstPointT<PointT>;
+  using Category = traits::LineStringTag;
+  static constexpr traits::Dimensions Dimension = traits::PointTraits<PointT>::Dimension;
+  using BasicIterator = internal::TransformIterator<RFIter, const BasicPointType>;
+  using const_iterator =  // NOLINT
+      internal::SelectCompoundLsIteratorT<ConstPointType>;
+  using iterator = const_iterator;  // NOLINT
+  using BasicLineString = internal::SelectBasicLineStringT<BasicPointType>;
+  using SegmentType = Segment<ConstPointType>;
+  /**
+   * @brief Construct from a vector of ConstLineString3d
+   * @param ls objects to construct from. The order will also be the order of
+   * the linestrings.
+   */
+  explicit CompoundLineStringImpl(const ConstLineStrings3d& ls = ConstLineStrings3d())
+      : data_{std::make_shared<CompoundLineStringData>(ls)} {}
+
+  //! Construct from a vector of LineStrings or CompoundLineStrings
+  template <typename OtherT, typename = IfLS<OtherT, void>>
+  explicit CompoundLineStringImpl(const std::vector<OtherT>& lss)
+      : data_{std::make_shared<CompoundLineStringData>(
+            utils::transform(lss, [](const auto& e) { return traits::to3D(traits::toConst(e)); }))} {}
+
+  //! Internal construction from data pointer
+  CompoundLineStringImpl(CompoundLineStringDataConstPtr data, bool inverted)
+      : data_(std::move(data)), inverted_{inverted} {}
+
+  //! construct from other CompoundLineString
+  template <typename OtherT>
+  explicit CompoundLineStringImpl(const CompoundLineStringImpl<OtherT>& other)
+      : data_(other.constData()), inverted_{other.inverted()} {}
+
+  // default the rest (constructor above deletes copy constructors)
+  CompoundLineStringImpl(CompoundLineStringImpl&& rhs) noexcept = default;
+  CompoundLineStringImpl& operator=(CompoundLineStringImpl&& rhs) noexcept = default;
+  CompoundLineStringImpl(const CompoundLineStringImpl& rhs) = default;
+  CompoundLineStringImpl& operator=(const CompoundLineStringImpl& rhs) = default;
+  ~CompoundLineStringImpl() noexcept = default;
+
+  /**
+   * @brief returns whether this is an inverted CompoundLineString
+   *
+   * Inverted linestrings behave just as normal linestrings but provide access
+   * to the points in inverted order.
+   */
+  bool inverted() const noexcept { return inverted_; }
+
+  /**
+   * @brief return the total number of unique points
+   *
+   * Unique means that adjacent points with the same id are not counted.
+   * Note that this O(n) in the number of points, not O(0).
+   */
+  size_t size() const noexcept { return std::distance(begin(), end()); }
+
+  /**
+   * @brief return whether this contains any points
+   *
+   * Note this is O(n) in the number of points.
+   */
+  bool empty() const noexcept { return size() == 0; }
+
+  /**
+   * @brief returns an iterator to the start of the points
+   * @see end, basicBegin
+   *
+   * Dereferencing the iterator will convert the point from the internal
+   * Point3d to PointT. It will return the first point of the first
+   * linestring.
+   */
+  const_iterator begin() const noexcept { return constData()->pointsBegin(inverted()); }
+  /**
+   * @brief returns an iterator to end of the points
+   * @see begin, basicEnd
+   * Dereferencing the iterator will convert the point from the internal
+   * Point3d to PointType
+   */
+  const_iterator end() const noexcept { return constData()->pointsEnd(inverted()); }
+
+  /**
+   * @brief returns the first point of the first linestring
+   * @see back, at
+   * The internal point type is a different one, but because all points
+   * inherit from the internal point type, no slicing is possible. It will
+   * return the first point of the first linestring.
+   */
+  const PointType& front() const noexcept { return *begin(); }
+
+  /**
+   * @brief returns the last point of the last linestring
+   * @see front, at
+   *
+   * The internal point type is a different one, but because all points
+   * inherit from the internal point type, no slicing is possible.
+   */
+  const PointType& back() const noexcept { return *std::prev(end()); }
+
+  /**
+   * @brief returns the point at this position
+   * @see front
+   *
+   * The internal point type is a different one, but because all points
+   * inherit from the internal point type, no slicing is possible.
+   *
+   * Note this is O(n) in the number of points, not O(0)
+   */
+  const PointType& operator[](size_t idx) const {
+    assert(idx < size());
+    return *std::next(begin(), idx);
+  }
+
+  /**
+   * @brief returns the n-th segment. If n equals size() -1, the segment from
+   * back() to front() is returned.
+   *
+   * It is not a good idea to use this in a loop, because this has O(n) for
+   * every iteration. Use helper::forEachSegment instead.
+   */
+  SegmentType segment(size_t idx) const noexcept {
+    assert(idx < size());
+    const auto first = std::next(begin(), idx);
+    const auto second = idx + 1 == size() ? begin() : std::next(first);
+    return SegmentType(*first, *second);
+  }
+
+  //! Returns the number of (geometrically valid) segments.
+  size_t numSegments() const noexcept { return std::max(size_t(1), size()) - 1; }
+
+  /**
+   * @brief returns a normal iterator to the internal point type at begin
+   * @see basicEnd, begin
+   */
+  BasicIterator basicBegin() const { return constData()->pointsBegin(inverted()); }
+
+  /**
+   * @brief returns a normal iterator for the internal point type at end
+   * @see basicBegin, end
+   */
+  BasicIterator basicEnd() const { return constData()->pointsEnd(inverted()); }
+  /**
+   * @brief create a basic linestring from this linestring
+   * @return the basic linestring
+   *
+   * A basic linestring is a simple vector with eigen points. You can do what
+   * you want with these points, because changes to them will not affect
+   * lanelet objects.
+   *
+   * BasicLineString is registered with boost::geometry, therefore you can use
+   * it for geometry calculations.
+   */
+  BasicLineString basicLineString() const { return {basicBegin(), basicEnd()}; }
+
+  /**
+   * @brief returns the internal data on the linestrings managed by this
+   * object
+   */
+  std::shared_ptr<const CompoundLineStringData> constData() const noexcept { return data_; }
+
+  ConstLineStrings3d lineStrings() const {
+    return inverted()
+               ? utils::invert(utils::transform(data_->lineStrings(), [](const auto& ls) { return ls.invert(); }))
+               : data_->lineStrings();
+  }
+
+  /**
+   * @brief returns the ids of all linestrings in order
+   * @return list of ids
+   */
+  Ids ids() const {
+    auto ids = utils::transform(data_->lineStrings(), [](const auto& ls) { return ls.id(); });
+    if (inverted()) {
+      return utils::invert(ids);
+    }
+    return ids;
+  }
+
+ private:
+  CompoundLineStringDataConstPtr data_;
+  bool inverted_{false};
+};
+
+/**
+ * @brief A Compound linestring in 2d (returns Point2d)
+ * @ingroup LineStringPrimitives
+ * @see CompoundLineStringImpl
+ */
+class CompoundLineString2d : public CompoundLineStringImpl<ConstPoint2d> {
+ public:
+  using TwoDType = CompoundLineString2d;
+  using ThreeDType = CompoundLineString3d;
+  using HybridType = CompoundHybridLineString2d;
+  using ConstType = CompoundLineString2d;
+  using MutableType = void;
+  using CompoundLineStringImpl::CompoundLineStringImpl;
+
+  explicit CompoundLineString2d(const CompoundLineStrings2d& other)
+      : CompoundLineString2d(utils::concatenate(other, [](auto& cls) { return cls.lineStrings(); })) {}
+  CompoundLineString2d() = default;  // Not inherited from base class
+
+  //! create a new, inverted linestring from this one
+  CompoundLineString2d invert() const noexcept { return CompoundLineString2d{constData(), !inverted()}; }
+};
+
+/**
+ * @brief A Compound linestring in 3d (returns Point3d)
+ * @ingroup LineStringPrimitives
+ * @see CompoundLineStringImpl
+ */
+class CompoundLineString3d : public CompoundLineStringImpl<ConstPoint3d> {
+ public:
+  using TwoDType = CompoundLineString2d;
+  using ThreeDType = CompoundLineString3d;
+  using HybridType = CompoundHybridLineString3d;
+  using ConstType = CompoundLineString3d;
+  using MutableType = void;
+  using CompoundLineStringImpl::CompoundLineStringImpl;
+  explicit CompoundLineString3d(const CompoundLineStrings3d& other)
+      : CompoundLineString3d(utils::concatenate(other, [](auto& cls) { return cls.lineStrings(); })) {}
+  CompoundLineString3d() = default;  // Not inherited from base class
+
+  //! create a new, inverted linestring from this one
+  CompoundLineString3d invert() const noexcept { return CompoundLineString3d{constData(), !inverted()}; }
+};
+
+/**
+ * @brief A hybrid compound linestring in 2d (returns BasicPoint2d)
+ * @ingroup LineStringPrimitives
+ * @see CompoundLineStringImpl
+ */
+class CompoundHybridLineString2d : public CompoundLineStringImpl<BasicPoint2d> {
+ public:
+  using TwoDType = CompoundHybridLineString2d;
+  using ThreeDType = CompoundHybridLineString3d;
+  using HybridType = CompoundHybridLineString2d;
+  using ConstType = CompoundHybridLineString2d;
+  using MutableType = void;
+  using CompoundLineStringImpl::CompoundLineStringImpl;
+  explicit CompoundHybridLineString2d(const CompoundHybridLineStrings2d& other)
+      : CompoundHybridLineString2d(utils::concatenate(other, [](auto& cls) { return cls.lineStrings(); })) {}
+  CompoundHybridLineString2d() = default;  // Not inherited from base class
+
+  //! create a new, inverted linestring from this one
+  CompoundHybridLineString2d invert() const noexcept { return CompoundHybridLineString2d{constData(), !inverted()}; }
+};
+
+/**
+ * @brief A hybrid compound linestring in 3d (returns BasicPoint3d)
+ * @ingroup LineStringPrimitives
+ * @see CompoundLineStringImpl
+ */
+class CompoundHybridLineString3d : public CompoundLineStringImpl<BasicPoint3d> {
+ public:
+  using TwoDType = CompoundHybridLineString2d;
+  using ThreeDType = CompoundHybridLineString3d;
+  using HybridType = CompoundHybridLineString3d;
+  using ConstType = CompoundHybridLineString3d;
+  using MutableType = void;
+  using CompoundLineStringImpl::CompoundLineStringImpl;
+  explicit CompoundHybridLineString3d(const CompoundHybridLineStrings3d& other)
+      : CompoundHybridLineString3d(utils::concatenate(other, [](auto& cls) { return cls.lineStrings(); })) {}
+  CompoundHybridLineString3d() = default;  // Not inherited from base class
+
+  //! create a new, inverted linestring from this one
+  CompoundHybridLineString3d invert() const noexcept { return CompoundHybridLineString3d{constData(), !inverted()}; }
+};
+
+namespace utils {
+/**
+ * @brief returns true if element of a  primitive has a matching Id
+ * @param ls the element holding other primitives
+ * @param id id to look for
+ * @return true if the primitive has such an element
+ *
+ * This function does not look for the id of the element, only its members
+ * Works for linestrings and polylines.
+ * A similar implementation exists for regulatory elements and lanelets.
+ */
+template <typename PointT>
+bool has(const CompoundLineStringImpl<PointT>& ls, Id id) {
+  return std::any_of(ls.begin(), ls.end(), [&id](const auto& elem) { return elem.id() == id; });
+}
+}  // namespace utils
+
+template <typename Point1T, typename Point2T>
+bool operator==(const CompoundLineStringImpl<Point1T>& lhs, const CompoundLineStringImpl<Point2T>& rhs) {
+  return lhs.lineStrings() == rhs.lineStrings();
+}
+
+template <typename Point1T, typename Point2T>
+bool operator!=(const CompoundLineStringImpl<Point1T>& lhs, const CompoundLineStringImpl<Point2T>& rhs) {
+  return !(rhs == lhs);
+}
+template <typename Point1T, typename Point2T>
+bool operator==(const CompoundLineStringImpl<Point1T>& lhs, const std::vector<Point2T>& rhs) {
+  return lhs.size() == rhs.size() && std::equal(lhs.begin(), lhs.end(), rhs.begin());
+}
+
+template <typename Point1T, typename Point2T>
+bool operator==(const std::vector<Point1T>& lhs, const CompoundLineStringImpl<Point2T>& rhs) {
+  return rhs == lhs;
+}
+
+template <typename Point1T, typename Point2T>
+bool operator!=(const CompoundLineStringImpl<Point1T>& lhs, const std::vector<Point2T>& rhs) {
+  return !(lhs == rhs);
+}
+
+template <typename Point1T, typename Point2T>
+bool operator!=(const std::vector<Point1T>& lhs, const CompoundLineStringImpl<Point2T>& rhs) {
+  return !(lhs == rhs);
+}
+
+template <typename PointT>
+inline std::ostream& operator<<(std::ostream& stream, const CompoundLineStringImpl<PointT>& obj) {
+  auto ids = obj.ids();
+  stream << "[";
+  if (!ids.empty()) {
+    stream << "ids: ";
+    std::copy(ids.begin(), ids.end(), std::ostream_iterator<Id>(stream, " "));
+  }
+  if (obj.inverted()) {
+    stream << ", inverted";
+  }
+  return stream << "]";
+}
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/primitives/CompoundPolygon.h

@@ -0,0 +1,122 @@
+#pragma once
+#include "lanelet2_core/primitives/CompoundLineString.h"
+#include "lanelet2_core/primitives/Polygon.h"
+
+namespace lanelet {
+
+/**
+ * @brief Combines multiple linestrings to one polygon in 2d
+ * @ingroup PolygonPrimitives
+ */
+class CompoundPolygon2d : public CompoundLineStringImpl<ConstPoint2d> {
+ public:
+  using TwoDType = CompoundPolygon2d;
+  using ThreeDType = CompoundPolygon3d;
+  using ConstType = CompoundPolygon2d;
+  using HybridType = CompoundHybridPolygon2d;
+  using MutableType = void;
+  using Category = traits::PolygonTag;
+
+  using CompoundLineStringImpl::CompoundLineStringImpl;
+  explicit CompoundPolygon2d(const CompoundPolygons2d& other)
+      : CompoundPolygon2d(utils::concatenate(other, [](auto& cls) { return cls.lineStrings(); })) {}
+  explicit CompoundPolygon2d(const CompoundLineStrings2d& other)
+      : CompoundPolygon2d(utils::concatenate(other, [](auto& cls) { return cls.lineStrings(); })) {}
+
+  CompoundPolygon2d() = default;
+  explicit CompoundPolygon2d(const CompoundLineString2d& other) : CompoundLineStringImpl(other) {}
+  explicit operator CompoundLineString2d() const { return CompoundLineString2d(constData(), inverted()); }
+  //! Returns the number of (geometrically valid) segments.
+  size_t numSegments() const noexcept { return std::max(size_t(1), size()); }
+
+  BasicLineString basicLineString() = delete;
+  BasicPolygon2d basicPolygon() const { return {basicBegin(), basicEnd()}; }
+};
+
+/**
+ * @brief Combines multiple linestrings to one polygon in 3d
+ * @ingroup PolygonPrimitives
+ */
+class CompoundPolygon3d : public CompoundLineStringImpl<ConstPoint3d> {
+ public:
+  using TwoDType = CompoundPolygon2d;
+  using ThreeDType = CompoundPolygon3d;
+  using ConstType = CompoundPolygon3d;
+  using HybridType = CompoundHybridPolygon3d;
+  using MutableType = void;
+  using CompoundLineStringImpl::CompoundLineStringImpl;
+  using Category = traits::PolygonTag;
+  explicit CompoundPolygon3d(const CompoundLineStrings3d& other)
+      : CompoundPolygon3d(utils::concatenate(other, [](auto& cls) { return cls.lineStrings(); })) {}
+  explicit CompoundPolygon3d(const CompoundPolygons3d& other)
+      : CompoundPolygon3d(utils::concatenate(other, [](auto& cls) { return cls.lineStrings(); })) {}
+
+  CompoundPolygon3d() = default;
+  explicit CompoundPolygon3d(const CompoundLineString3d& other) : CompoundLineStringImpl(other) {}
+  explicit operator CompoundLineString3d() const { return CompoundLineString3d(constData(), inverted()); }
+  //! Returns the number of (geometrically valid) segments.
+  size_t numSegments() const noexcept { return std::max(size_t(1), size()); }
+
+  BasicLineString basicLineString() = delete;
+  BasicPolygon3d basicPolygon() const {
+    return BasicPolygon3d{basicBegin(), basicEnd(), BasicPolygon3d::allocator_type()};
+  }
+};
+
+/**
+ * @brief Combines multiple linestrings to one polygon in 2d that returns
+ * BasicPoint2d
+ * @ingroup PolygonPrimitives
+ */
+class CompoundHybridPolygon2d : public CompoundLineStringImpl<BasicPoint2d> {
+ public:
+  using CompoundLineStringImpl::CompoundLineStringImpl;
+  using TwoDType = CompoundHybridPolygon2d;
+  using ThreeDType = CompoundHybridPolygon3d;
+  using HybridType = CompoundHybridPolygon2d;
+  using ConstType = CompoundHybridPolygon2d;
+  using MutableType = void;
+  using Category = traits::PolygonTag;
+
+  explicit CompoundHybridPolygon2d(const CompoundHybridPolygons2d& other)
+      : CompoundHybridPolygon2d(utils::concatenate(other, [](auto& cls) { return cls.lineStrings(); })) {}
+  explicit CompoundHybridPolygon2d(const CompoundHybridLineStrings2d& other)
+      : CompoundHybridPolygon2d(utils::concatenate(other, [](auto& cls) { return cls.lineStrings(); })) {}
+
+  CompoundHybridPolygon2d() = default;
+  explicit CompoundHybridPolygon2d(const CompoundHybridLineString2d& other) : CompoundLineStringImpl(other) {}
+  explicit operator CompoundHybridLineString2d() const { return CompoundHybridLineString2d(constData(), inverted()); }
+  //! Returns the number of (geometrically valid) segments.
+  size_t numSegments() const noexcept { return std::max(size_t(1), size()); }
+  BasicLineString basicLineString() = delete;
+  BasicPolygon2d basicPolygon() const { return {basicBegin(), basicEnd()}; }
+};
+
+/**
+ * @brief Combines multiple linestrings to one polygon in 3d that returns
+ * BasicPoint3d
+ * @ingroup PolygonPrimitives
+ */
+class CompoundHybridPolygon3d : public CompoundLineStringImpl<BasicPoint3d> {
+ public:
+  using CompoundLineStringImpl::CompoundLineStringImpl;
+  using TwoDType = CompoundHybridPolygon2d;
+  using ThreeDType = CompoundHybridPolygon3d;
+  using ConstType = CompoundHybridPolygon3d;
+  using HybridType = CompoundHybridPolygon3d;
+  using MutableType = void;
+  using Category = traits::PolygonTag;
+
+  explicit CompoundHybridPolygon3d(const CompoundHybridLineStrings3d& other)
+      : CompoundHybridPolygon3d(utils::concatenate(other, [](auto& cls) { return cls.lineStrings(); })) {}
+  explicit CompoundHybridPolygon3d(const CompoundHybridPolygons3d& other)
+      : CompoundHybridPolygon3d(utils::concatenate(other, [](auto& cls) { return cls.lineStrings(); })) {}
+  CompoundHybridPolygon3d() = default;
+  explicit CompoundHybridPolygon3d(const CompoundHybridLineString3d& other) : CompoundLineStringImpl(other) {}
+  explicit operator CompoundHybridLineString3d() const { return CompoundHybridLineString3d(constData(), inverted()); }
+  //! Returns the number of (geometrically valid) segments.
+  size_t numSegments() const noexcept { return std::max(size_t(1), size()); }
+  BasicLineString basicLineString() = delete;
+  BasicPolygon3d basicPolygon() const { return {basicBegin(), basicEnd()}; }
+};
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/primitives/GPSPoint.h

@@ -0,0 +1,14 @@
+#pragma once
+#include "lanelet2_core/Forward.h"
+
+namespace lanelet {
+
+//! A raw GPS point
+class GPSPoint {
+ public:
+  double lat{0.};  //! lat according to WGS84
+  double lon{0.};  //! lon according to WGS84
+  double ele{0.};  //! elevation according to WGS84 (m)
+};
+
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/primitives/Lanelet.h

@@ -0,0 +1,405 @@
+// this is for emacs file handling -*- mode: c++; c-basic-offset: 2;
+// indent-tabs-mode: nil -*-
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <utility>
+
+#include "lanelet2_core/Forward.h"
+#include "lanelet2_core/primitives/LineString.h"
+#include "lanelet2_core/primitives/Primitive.h"
+#include "lanelet2_core/utility/Optional.h"
+
+namespace lanelet {
+enum class LaneletType { OneWay, Bidirectional };
+
+/**
+ * @brief Common data management class for all Lanelet-Typed objects.
+ * @ingroup DataObjects
+ */
+class LaneletData : public PrimitiveData {
+ public:
+  /**
+   * @brief Constructs a new, valid LaneletData object
+   * @see ConstLanelet::ConstLanelet
+   */
+  LaneletData(Id id, LineString3d leftBound, LineString3d rightBound, const AttributeMap& attributes = AttributeMap(),
+              RegulatoryElementPtrs regulatoryElements = RegulatoryElementPtrs())
+      : PrimitiveData(id, attributes),
+        leftBound_{std::move(leftBound)},
+        rightBound_{std::move(rightBound)},
+        regulatoryElements_{std::move(regulatoryElements)} {}
+
+  const ConstLineString3d& leftBound() const { return leftBound_; }
+  const LineString3d& leftBound() { return leftBound_; }
+
+  const ConstLineString3d& rightBound() const { return rightBound_; }
+  const LineString3d& rightBound() { return rightBound_; }
+
+  RegulatoryElementConstPtrs regulatoryElements() const {
+    return utils::transform(regulatoryElements_, [](const auto& elem) { return RegulatoryElementConstPtr(elem); });
+  }
+  RegulatoryElementPtrs& regulatoryElements() { return regulatoryElements_; }
+
+  template <typename RegElemT>
+  std::vector<std::shared_ptr<const RegElemT>> regulatoryElementsAs() const {
+    return utils::transformSharedPtr<const RegElemT>(regulatoryElements_);
+  }
+  template <typename RegElemT>
+  std::vector<std::shared_ptr<RegElemT>> regulatoryElementsAs() {
+    return utils::transformSharedPtr<RegElemT>(regulatoryElements_);
+  }
+
+  //! Sets a new left bound. Resets all cached data of this object
+  void setLeftBound(const LineString3d& bound);
+
+  //! Sets a new right bound. Resets all cached data of this object
+  void setRightBound(const LineString3d& bound);
+
+  //! Sets a new right bound. Resets all cached data of this object
+  void setCenterline(const LineString3d& centerline);
+
+  //! Returns whether the centerline has been overridden by setCenterline
+  bool hasCustomCenterline() const;
+
+  //! call this to indicate that the objects data has been modified.
+  void resetCache() const;
+
+  //! Returns centerline by computing it, if necessary. Result is cached.
+  ConstLineString3d centerline() const;
+
+  //! Get the bounding polygon of this lanelet. Result is cached.
+  CompoundPolygon3d polygon() const;
+
+ private:
+  LineString3d leftBound_;                    //!< represents the left bound
+  LineString3d rightBound_;                   //!< represents the right bound
+  RegulatoryElementPtrs regulatoryElements_;  //!< regulatory elements
+
+  // Cached data
+  mutable std::shared_ptr<ConstLineString3d> centerline_;
+};
+
+/**
+ * @defgroup LaneletPrimitives Lanelets
+ * @ingroup Primitives
+ *
+ * ## General
+ * Lanelets represent an atomic section of a lane.
+ *
+ * A lanelet consists of two bounds, attributes and regulatory elements.
+ * The orientation of the bounds is important and must be consistent as it
+ * indicates the driving direction. The attributes can define simple constaints
+ * for a lanelet and regulatory elements impose traffic regulations (right of
+ * way, speed limits).
+ *
+ * ## Centerline
+ * You can get the centerline of a Lanelet. This computation is a bit costly but
+ * this is necessary to guarantee that the centerline never violates the bounds
+ * of its polygon. To speed up subsequend calls, the result is cached. However,
+ * if points are modified, the Lanelet does not get notified and the cached
+ * centerline will be wrong. To solve that, manually reset the cache of the
+ * affected lanelets when changing point coordinates.
+ *
+ * You can override the centerline by setting your own. If you do that, you take
+ * resposability of the validity of the centerline. If the bounds of lanelet
+ * with such a centerline is updated, the centerline will stay unchanged.
+ *
+ * ## Geometry
+ * Lanelets can directly be used in some geometry calculations. see
+ * lanelet::geometry. If this does not satisfy your needs, you can try the
+ * Lanelet::polygon2d or Lanelet::polygon3d instead.
+ *
+ * ## Regulatory Elements
+ * Regulatory Elements represent traffic rules that affect a lanelet. Instead of
+ * trying to interpret the traffic rules by hand, use the lanelet2_traffic_rules
+ * package. It is designed to yield senseful results for all commmon traffic
+ * rules. If they do not fit your needs, you can extend them easily.
+ *
+ * ## Inverting
+ * A lanelet implicitly stands for a certain driving direction. To get the
+ * opposite direction, Lanelets can be inverted in O(0). An inverted Lanelet
+ * shares its data with all normal lanelets, it just returns its bounds with
+ * left and right flipped.
+ */
+
+//! @brief An immutable lanelet.
+//! @ingroup LaneletPrimitives
+//! @ingroup ConstPrimitives
+class ConstLanelet : public ConstPrimitive<LaneletData> {
+ public:
+  using ConstType = ConstLanelet;
+  using MutableType = Lanelet;
+  using TwoDType = ConstLanelet;
+  using ThreeDType = ConstLanelet;
+  using Category = traits::LaneletTag;
+
+  //! Constructs an empty or invalid lanelet.
+  explicit ConstLanelet(Id id = InvalId)
+      : ConstLanelet(std::make_shared<LaneletData>(id, LineString3d(), LineString3d()), false) {}
+
+  //! Constructs a lanelet from id, attributes, regulatoryElements and bounds.
+  ConstLanelet(Id id, const LineString3d& leftBound, const LineString3d& rightBound,
+               const AttributeMap& attributes = AttributeMap(),
+               const RegulatoryElementPtrs& regulatoryElements = RegulatoryElementPtrs())
+      : ConstPrimitive{std::make_shared<LaneletData>(id, leftBound, rightBound, attributes, regulatoryElements)} {}
+
+  //! Construct from the data of a different Lanelet
+  explicit ConstLanelet(const std::shared_ptr<const LaneletData>& data, bool inverted = false)
+      : ConstPrimitive<lanelet::LaneletData>(data), inverted_{inverted} {}
+
+  //! returns if this is an inverted lanelet
+  bool inverted() const { return inverted_; }
+
+  //! returns the inverted lanelet
+  ConstLanelet invert() const { return ConstLanelet{constData(), !inverted()}; }
+
+  //! get the left bound.
+  ConstLineString3d leftBound() const { return leftBound3d(); }
+  //! get the left bound in 2d. To be used over leftBound where geometric calculations are required.
+  ConstLineString2d leftBound2d() const { return utils::to2D(leftBound3d()); }
+  //! get the left bound in 3d. To be used over leftBound where geometric calculations are required.
+  ConstLineString3d leftBound3d() const {
+    return inverted() ? constData()->rightBound().invert() : constData()->leftBound();
+  }
+
+  //! get the right bound.
+  ConstLineString3d rightBound() const { return rightBound3d(); }
+  //! get the right bound in 2d. To be used over rightBound where geometric calculations are required.
+  ConstLineString2d rightBound2d() const { return utils::to2D(rightBound3d()); }
+  //! get the right bound in 3d. To be used over rightBound where geometric calculations are required.
+  ConstLineString3d rightBound3d() const {
+    return inverted() ? constData()->leftBound().invert() : constData()->rightBound();
+  }
+
+  //! get a list of regulatory elements that affect this lanelet
+  RegulatoryElementConstPtrs regulatoryElements() const { return constData()->regulatoryElements(); }
+
+  //! get the regulatoryElements that could be converted to RegElemT
+  template <typename RegElemT>
+  std::vector<std::shared_ptr<const RegElemT>> regulatoryElementsAs() const {
+    return constData()->regulatoryElementsAs<RegElemT>();
+  }
+
+  //! get the (cached) centerline of this lanelet.
+  ConstLineString3d centerline() const { return centerline3d(); }
+
+  //! get the (cached) centerline of this lanelet in 2d. To be used in context of geometric calculations.
+  ConstLineString2d centerline2d() const { return utils::to2D(centerline3d()); }
+
+  //! get the (cached) centerline of this lanelet in 3d. To be used in context of geometric calculations.
+  ConstLineString3d centerline3d() const {
+    return inverted() ? constData()->centerline().invert() : constData()->centerline();
+  }
+
+  //! Returns whether the centerline has been overridden by setCenterline
+  bool hasCustomCenterline() const { return constData()->hasCustomCenterline(); }
+
+  //! returns the surface covered by this lanelet as 3-dimensional polygon.
+  CompoundPolygon3d polygon3d() const;
+
+  //! returns the surface covered by this lanelet as 2-dimensional polygon.
+  CompoundPolygon2d polygon2d() const;
+
+  /**
+   * @brief resets the internal cache of the centerline
+   *
+   * this can be necessary if an element of the linestring was modified
+   * somewhere else.
+   */
+  void resetCache() const { constData()->resetCache(); }
+
+ private:
+  bool inverted_{false};  //!< indicates if this lanelet is inverted
+};
+
+//! @brief The famous (mutable) lanelet class
+//! @ingroup LaneletPrimitives
+//! @ingroup Primitives
+class Lanelet : public Primitive<ConstLanelet> {
+ public:
+  using Primitive::Primitive;
+  using TwoDType = Lanelet;
+  using ThreeDType = Lanelet;
+  friend class ConstWeakLanelet;
+  Lanelet() = default;
+  Lanelet(const std::shared_ptr<const LaneletData>&, bool inverted) = delete;
+  Lanelet(const LaneletDataPtr& data, bool inverted) : Primitive{data, inverted} {}
+
+  //! non-const version to invert a lanelet
+  Lanelet invert() const { return Lanelet{data(), !inverted()}; }
+
+  using Primitive::leftBound;
+  using Primitive::leftBound2d;
+  using Primitive::leftBound3d;
+  //! Get the left bound
+  LineString3d leftBound() { return leftBound3d(); }
+
+  //! get the left bound in 2d. To be used over leftBound where geometric calculations are required.
+  LineString2d leftBound2d() { return utils::to2D(leftBound3d()); }
+
+  //! get the left bound in 3d. To be used over leftBound where geometric calculations are required.
+  LineString3d leftBound3d() { return inverted() ? data()->rightBound().invert() : data()->leftBound(); }
+
+  using Primitive::rightBound;
+  using Primitive::rightBound2d;
+  using Primitive::rightBound3d;
+  //! Get the right bound
+  LineString3d rightBound() { return rightBound3d(); }
+
+  //! get the right bound in 2d. To be used over rightBound where geometric calculations are required.
+  LineString2d rightBound2d() { return utils::to2D(rightBound3d()); }
+
+  //! get the right bound in 3d. To be used over rightBound where geometric calculations are required.
+  LineString3d rightBound3d() { return inverted() ? data()->leftBound().invert() : data()->rightBound(); }
+
+  //! get the right bound.
+  ConstLineString3d rightBound() const { return rightBound3d(); }
+
+  //! Sets a new left bound and resets the cached centerline
+  void setLeftBound(const LineString3d& bound) {
+    inverted() ? data()->setRightBound(bound.invert()) : data()->setLeftBound(bound);
+  }
+
+  //! Sets a new right bound and resets the cached centerline
+  void setRightBound(const LineString3d& bound) {
+    inverted() ? data()->setLeftBound(bound.invert()) : data()->setRightBound(bound);
+  }
+
+  //! Forcefully set a new centerline. Must have a valid Id.
+  //!
+  //! This overrides the default behaviour where the centerline is automatically
+  //! calculated by the library. The centerline will not be updated if the
+  //! bounds of the lanelet are modified.
+  //!
+  //! There is no check whether the centerline is actually valid.
+  void setCenterline(const LineString3d& centerline) {
+    inverted() ? data()->setCenterline(centerline.invert()) : data()->setCenterline(centerline);
+  }
+
+  //! Return regulatoryElements that affect this lanelet.
+  const RegulatoryElementPtrs& regulatoryElements() { return data()->regulatoryElements(); }
+
+  //! Add a new regulatory element
+  void addRegulatoryElement(RegulatoryElementPtr regElem) {
+    if (regElem == nullptr) {
+      throw NullptrError("regulatory element is a nullptr.");
+    }
+    data()->regulatoryElements().push_back(std::move(regElem));
+  }
+
+  //! Removes a regulatory element, returns true on success
+  bool removeRegulatoryElement(const RegulatoryElementPtr& regElem) {
+    auto& regElems = data()->regulatoryElements();
+    auto remove = std::find(regElems.begin(), regElems.end(), regElem);
+    if (remove != regElems.end()) {
+      regElems.erase(remove);
+      return true;
+    }
+    return false;
+  }
+
+  using Primitive::regulatoryElements;
+  using Primitive::regulatoryElementsAs;
+
+  //! Get the regulatoryElements that could be converted to RegElemT
+  template <typename RegElemT>
+  std::vector<std::shared_ptr<RegElemT>> regulatoryElementsAs() {
+    return data()->regulatoryElementsAs<RegElemT>();
+  }
+};
+
+//! Used internally by RegulatoryElements to avoid cyclic dependencies.
+//! Conceptually similar to a std::weak_ptr.
+class ConstWeakLanelet {
+ public:
+  using ConstType = ConstWeakLanelet;
+  using MutableType = WeakLanelet;
+  using TwoDType = ConstWeakLanelet;
+  using ThreeDType = ConstWeakLanelet;
+  ConstWeakLanelet() = default;
+  ConstWeakLanelet(const ConstLanelet& llet)  // NOLINT
+      : laneletData_{llet.constData()}, inverted_{llet.inverted()} {}
+
+  /**
+   * @brief Obtains the original ConstLanelet.
+   * @throws NullptrError if the managed lanelet expired.
+   */
+  ConstLanelet lock() const { return ConstLanelet(laneletData_.lock(), inverted_); }
+
+  //! tests whether the WeakLanelet is still valid
+  bool expired() const noexcept { return laneletData_.expired(); }
+
+ protected:
+  std::weak_ptr<const LaneletData> laneletData_;  // NOLINT
+  bool inverted_{false};                          // NOLINT
+};
+
+//! Used internally by RegulatoryElements to avoid cyclic dependencies.
+//! Conceptually similar to a std::weak_ptr.
+class WeakLanelet final : public ConstWeakLanelet {
+ public:
+  WeakLanelet() = default;
+  WeakLanelet(const Lanelet& llet) : ConstWeakLanelet(llet) {}  // NOLINT
+
+  /**
+   * @brief Obtains the original Lanelet.
+   * @throws NullptrError if the managed lanelet expired.
+   */
+  Lanelet lock() const { return Lanelet(std::const_pointer_cast<LaneletData>(laneletData_.lock()), inverted_); }
+};
+
+namespace utils {
+/**
+ * @brief returns true if element of a regulatory element has a matching Id
+ * @param ll the element holding other primitives
+ * @param id id to look for
+ * @return true if the primitive has such an element
+ *
+ * This function does not look for the id of the element, only its members
+ * Works for linestrings and polylines. Does NOT check members in regulatory
+ * elements, only ids of regulatory elements itself.
+ * A similar implementation exists for linestrings and regulatory elements.
+ */
+bool has(const ConstLanelet& ll, Id id);
+}  // namespace utils
+
+inline bool operator==(const ConstLanelet& lhs, const ConstLanelet& rhs) {
+  return lhs.constData() == rhs.constData() && lhs.inverted() == rhs.inverted();
+}
+inline bool operator!=(const ConstLanelet& lhs, const ConstLanelet& rhs) { return !(lhs == rhs); }
+inline bool operator==(const ConstWeakLanelet& lhs, const ConstWeakLanelet& rhs) {
+  return !lhs.expired() && !rhs.expired() && lhs.lock() == rhs.lock();
+}
+inline bool operator!=(const ConstWeakLanelet& lhs, const ConstWeakLanelet& rhs) { return !(lhs == rhs); }
+std::ostream& operator<<(std::ostream& stream, const ConstLanelet& obj);
+
+namespace traits {
+template <typename T>
+constexpr bool isLaneletT() {
+  return isCategory<T, traits::LaneletTag>();
+}
+}  // namespace traits
+
+template <typename T, typename RetT>
+using IfLL = std::enable_if_t<traits::isLaneletT<T>(), RetT>;
+
+}  // namespace lanelet
+
+// Hash function for usage in containers
+namespace std {
+template <>
+struct hash<lanelet::Lanelet> : public lanelet::HashBase<lanelet::Lanelet> {};
+template <>
+struct hash<lanelet::ConstLanelet> : public lanelet::HashBase<lanelet::ConstLanelet> {};
+
+template <typename T, typename U>
+struct hash<std::pair<U, T>> {
+  size_t operator()(const std::pair<U, T> x) const noexcept {
+    return std::hash<U>()(x.first) ^ std::hash<T>()(x.second);
+  }
+};
+
+}  // namespace std

src/map/lanelet2/include/lanelet2_core/primitives/LaneletOrArea.h

@@ -0,0 +1,144 @@
+#pragma once
+#include <boost/variant.hpp>
+
+#include "lanelet2_core/primitives/Area.h"
+#include "lanelet2_core/primitives/Lanelet.h"
+
+namespace lanelet {
+
+//! An object that can either refer to a lanelet or an area
+class ConstLaneletOrArea {
+ public:
+  ConstLaneletOrArea() = default;
+  ConstLaneletOrArea(ConstLaneletOrArea&& rhs) = default;
+  ConstLaneletOrArea& operator=(ConstLaneletOrArea&& rhs) = default;
+  ConstLaneletOrArea(const ConstLaneletOrArea& rhs) = default;
+  ConstLaneletOrArea& operator=(const ConstLaneletOrArea& rhs) = default;
+  ~ConstLaneletOrArea() noexcept = default;
+
+  ConstLaneletOrArea(ConstLanelet lanelet)  // NOLINT
+      : laneletOrArea_{std::move(lanelet)} {}
+  ConstLaneletOrArea(ConstArea area)  // NOLINT
+      : laneletOrArea_{std::move(area)} {}
+  ConstLaneletOrArea& operator=(ConstLanelet lanelet) {
+    laneletOrArea_ = std::move(lanelet);
+    return *this;
+  }
+  ConstLaneletOrArea& operator=(ConstArea area) {
+    laneletOrArea_ = std::move(area);
+    return *this;
+  }
+
+  //! true if this object holds an area
+  bool isArea() const { return laneletOrArea_.which() == 1; }
+
+  //! true if this objct holds a lanelet
+  bool isLanelet() const { return laneletOrArea_.which() == 0; }
+
+  //! convert to lanelet (type is not checked)
+  explicit operator const ConstLanelet&() const { return boost::get<ConstLanelet>(laneletOrArea_); }
+
+  //! convert to area (type is not checked)
+  explicit operator const ConstArea&() const { return boost::get<ConstArea>(laneletOrArea_); }
+
+  //! apply a generic visitor
+  template <typename VisitorT>
+  decltype(auto) applyVisitor(VisitorT visitor) const {
+    return boost::apply_visitor(visitor, laneletOrArea_);
+  }
+
+  //! get the id of the lanelet or area
+  Id id() const {
+    return applyVisitor([](auto& elem) { return elem.id(); });
+  }
+
+  //! get the attributes of the lanelet or area
+  const AttributeMap& attributes() const {
+    return applyVisitor([](auto& elem) -> const AttributeMap& { return elem.attributes(); });
+  }
+
+  RegulatoryElementConstPtrs regulatoryElements() const {
+    return applyVisitor([](auto& elem) { return elem.regulatoryElements(); });
+  }
+
+  template <typename T>
+  std::vector<std::shared_ptr<const T>> regulatoryElementsAs() const {
+    return applyVisitor([](auto& elem) { return elem.template regulatoryElementsAs<T>(); });
+  }
+
+  //! return the managed lanelet
+  Optional<ConstLanelet> lanelet() const {
+    const auto* ll = boost::get<ConstLanelet>(&laneletOrArea_);
+    if (ll != nullptr) {
+      return *ll;
+    }
+    return {};
+  }
+
+  //! get the managed area
+  Optional<ConstArea> area() const {
+    const auto* ar = boost::get<ConstArea>(&laneletOrArea_);
+    if (ar != nullptr) {
+      return *ar;
+    }
+    return {};
+  }
+  //! compares this lanelet or area
+  bool equals(const ConstLaneletOrArea& other) const { return laneletOrArea_ == other.laneletOrArea_; }
+
+  //! returns the outer bound if it is an area or the polygon made of the lanelet bounds if it's a lanelet
+  CompoundPolygon3d boundingPolygon() const {
+    if (isArea()) {
+      return area()->outerBoundPolygon();
+    }
+    return lanelet()->polygon3d();
+  }
+
+ private:
+  boost::variant<ConstLanelet, ConstArea> laneletOrArea_;
+};
+
+inline bool operator==(const ConstLaneletOrArea& lhs, const ConstLaneletOrArea& rhs) { return lhs.equals(rhs); }
+inline bool operator!=(const ConstLaneletOrArea& lhs, const ConstLaneletOrArea& rhs) { return !(lhs == rhs); }
+
+inline std::ostream& operator<<(std::ostream& stream, const ConstLaneletOrArea& obj) {
+  if (obj.isArea()) {
+    stream << *obj.area();
+  }
+  if (obj.isLanelet()) {
+    stream << *obj.lanelet();
+  }
+  return stream;
+}
+
+namespace utils {
+inline ConstLanelets getAllLanelets(const ConstLaneletOrAreas& lars) {
+  ConstLanelets lanelets;
+  lanelets.reserve(lars.size());
+  for (const auto& lar : lars) {
+    if (lar.isLanelet()) {
+      lanelets.push_back(static_cast<const ConstLanelet&>(lar));
+    }
+  };
+  return lanelets;
+}
+inline ConstAreas getAllAreas(const ConstLaneletOrAreas& lars) {
+  ConstAreas lanelets;
+  lanelets.reserve(lars.size());
+  for (const auto& lar : lars) {
+    if (lar.isArea()) {
+      lanelets.push_back(static_cast<const ConstArea&>(lar));
+    }
+  };
+  return lanelets;
+}
+}  // namespace utils
+
+}  // namespace lanelet
+
+// Hash function for usage in containers
+namespace std {
+template <>
+struct hash<lanelet::ConstLaneletOrArea> : public lanelet::HashBase<lanelet::ConstLaneletOrArea> {};
+
+}  // namespace std

src/map/lanelet2/include/lanelet2_core/primitives/LaneletSequence.h

@@ -0,0 +1,276 @@
+#pragma once
+#include <utility>
+
+#include "lanelet2_core/primitives/CompoundLineString.h"
+#include "lanelet2_core/primitives/CompoundPolygon.h"
+#include "lanelet2_core/primitives/Lanelet.h"
+
+namespace lanelet {
+
+/**
+ * @brief Common data management class for LaneletSequences
+ *
+ * This object is only for internal data management and should not be used
+ * directly.
+ *
+ * LaneletSequence merely has a shared_ptr to this object and forwards all
+ * calls to this object.
+ *
+ * Because calculating a polygon and a centerline is expensive, the results are
+ * cached by this object.
+ */
+class LaneletSequenceData : public boost::noncopyable {
+ public:
+  using iterator =  // NOLINT
+      internal::ReverseAndForwardIterator<ConstLanelets::const_iterator>;
+  /**
+   * @brief Constructs a new, valid LaneletData object
+   * @see ConstLanelet::ConstLanelet
+   */
+  explicit LaneletSequenceData(ConstLanelets lanelets);
+
+  const CompoundLineString3d& leftBound() const { return leftBound3d(); }
+  const CompoundLineString3d& leftBound3d() const { return leftBound_; }
+  CompoundLineString2d leftBound2d() const { return utils::to2D(leftBound_); }
+
+  const CompoundLineString3d& rightBound() const { return rightBound3d(); }
+  const CompoundLineString3d& rightBound3d() const { return rightBound_; }
+  CompoundLineString2d rightBound2d() const { return utils::to2D(rightBound3d()); }
+  const ConstLanelets& lanelets() const { return lanelets_; }
+
+  /**
+   * @brief computes the centerline. Result is cached
+   * @return centerline object. Can not be modified.
+   * @see ConstLanelet::centerline
+   */
+  CompoundLineString3d centerline() const;
+
+  /**
+   * @brief Get the bounding polygon around all lanelets. Result is cached.
+   * @return a polygon object. Can not be modified.
+   * @see ConstLanelet::polygon
+   */
+  CompoundPolygon3d polygon() const;
+
+  iterator begin(bool inverted) const noexcept {
+    return inverted ? iterator(lanelets_.rbegin()) : iterator(lanelets_.begin());
+  }
+
+  iterator end(bool inverted) const noexcept {
+    return inverted ? iterator(lanelets_.rend()) : iterator(lanelets_.end());
+  }
+
+ private:
+  const ConstLanelets lanelets_;  // NOLINT
+  //!< Cached data
+  const CompoundLineString3d leftBound_;                      // NOLINT
+  const CompoundLineString3d rightBound_;                     // NOLINT
+  mutable std::shared_ptr<CompoundLineString3d> centerline_;  //!< combined centerline
+  mutable std::shared_ptr<CompoundPolygon3d> polygon_;        //!< combined polygon
+};
+
+/**
+ * @brief A collection of Lanelets
+ * @see ConstLanelet
+ *
+ * A LaneletSequence is a collection of Lanelets that behaves like
+ * like one single Lanelet. Because of that, it can be passed to most functions
+ * expecting a normal lanelet.
+ *
+ * The contents of a LaneletSequence can not be modified. Modifications have to
+ * be done to the Lanelets directly.
+ *
+ * If
+ */
+class LaneletSequence {
+ public:
+  using DataType = LaneletSequenceData;
+  using TwoDType = void;
+  using ThreeDType = void;
+  using HybridType = void;
+  using ConstType = LaneletSequence;
+  using MutableType = void;
+  using iterator = LaneletSequenceData::iterator;  // NOLINT
+
+  /**
+   * @brief Construct from a vector of ConstLineString3d
+   * @param ls objects to construct from. The order will also be the order
+   * of the linestrings.
+   */
+  LaneletSequence(ConstLanelets ls = ConstLanelets())  // NOLINT
+      : data_{std::make_shared<LaneletSequenceData>(std::move(ls))} {}
+
+  /**
+   * @brief Create a compound lanelet from other compound lanelets
+   * @param ls vector of compound lanelets
+   */
+  explicit LaneletSequence(const LaneletSequences& ls)
+      : LaneletSequence(utils::concatenate(ls, [](const auto& e) { return e.lanelets(); })) {}
+
+  LaneletSequence(LaneletSequenceDataConstPtr data, bool inverted) : data_{std::move(data)}, inverted_{inverted} {}
+
+  /**
+   * @brief returns out if this is an inverted lanelets.
+   * @return true if inverted
+   *
+   * Inverted LaneletSequences behave differently from normal lanelets in that
+   * they internally interpret their inverted left bound as right bound and vice
+   * versa.
+   */
+  bool inverted() const { return inverted_; }
+
+  //! Returns wether this holds any lanelets
+  bool empty() const noexcept { return data_->lanelets().empty(); }
+
+  //! Returns number of lanelets
+  size_t size() const noexcept { return data_->lanelets().size(); }
+
+  /**
+   * @brief returns the respective inverted LaneletSequence
+   *
+   * Both lanelets (this and the inverted one) share the same data.
+   * Inversion only requires copying a shared ptr.
+   */
+  LaneletSequence invert() const { return {constData(), !inverted()}; }
+
+  //! get the combined left bounds of all lanelets
+  CompoundLineString3d leftBound() const { return leftBound3d(); }
+  //! get the left bound in 2d. To be used over leftBound where geometric calculations are required.
+  CompoundLineString2d leftBound2d() const { return utils::to2D(leftBound3d()); }
+  CompoundLineString3d leftBound3d() const {
+    return inverted() ? constData()->rightBound().invert() : constData()->leftBound();
+  }
+
+  //! get the combined right bounds of all lanelets
+  CompoundLineString3d rightBound() const { return rightBound3d(); }
+  //! get the right bound in 2d. To be used over rightBound where geometric calculations are required.
+  CompoundLineString2d rightBound2d() const { return utils::to2D(rightBound3d()); }
+  //! get the combined right bounds of all lanelets
+  CompoundLineString3d rightBound3d() const {
+    return inverted() ? constData()->leftBound().invert() : constData()->rightBound();
+  }
+
+  //! get a list of all regulatory elements that affect one of the lanelets
+  RegulatoryElementConstPtrs regulatoryElements() const;
+
+  /**
+   * @brief get all regulatory elements of type RegElemT
+   * @return list of pointers to regulatory elements in the LaneletSequence
+   */
+  template <typename RegElemT>
+  std::vector<std::shared_ptr<const RegElemT>> regulatoryElementsAs() const {
+    return utils::concatenate(lanelets(),
+                              [](const auto& elem) { return elem.template regulatoryElementsAs<RegElemT>(); });
+  }
+
+  /**
+   * @brief get the centerline of this lanelet
+   *
+   * Note the computation of the centerline is expensive, but as the result is
+   * cached, repeated calls to this functions are cheap.
+   *
+   * The returned centerline is only guarateed to be within the the lanelet, but
+   * not always perfecly in the middle of it. However, if your lanelet is more
+   * or
+   * less monotonic, this is negligible.
+   *
+   * The start (and similarly the end point) will always be direcly in the
+   * middle between the first left and right points.
+   */
+  CompoundLineString3d centerline() const { return centerline3d(); }
+  CompoundLineString3d centerline3d() const {
+    return inverted() ? constData()->centerline().invert() : constData()->centerline();
+  }
+  CompoundLineString2d centerline2d() const { return utils::to2D(centerline3d()); }
+
+  /**
+   * @brief returns the surface covered by the lanelets as 3-dimensional
+   * polygon.
+   * The result of the computation is cached, therefore foo(polygon().begin(),
+   * polygon.end()) provides valid iterators.
+   *
+   * Note that many geometry functions are only available for a 2d-polygon.
+   */
+  CompoundPolygon3d polygon3d() const { return constData()->polygon(); }
+
+  /**
+   * @brief returns the surface covered by the lanelets as 2-dimensional
+   * polygon.
+   */
+  CompoundPolygon2d polygon2d() const { return CompoundPolygon2d(polygon3d()); }
+
+  /**
+   * @brief returns the lanelets that are part of this object
+   * @return vector of lanelets
+   */
+  ConstLanelets lanelets() const { return inverted() ? utils::invert(data_->lanelets()) : data_->lanelets(); }
+
+  //! Returns iterator to the first lanelet (or to the last, if inverted)
+  iterator begin() const noexcept { return data_->begin(inverted()); }
+
+  //! Returns iterator to the last lanelet (or to the first, if inverted)
+  iterator end() const noexcept { return data_->end(inverted()); }
+
+  //! Access an individual lanelet
+  const ConstLanelet& operator[](size_t idx) const {
+    assert(idx < data_->lanelets().size());
+    return *std::next(begin(), idx);
+  }
+
+  /**
+   * @brief returns the ids of all lanelets in order
+   * @return list of ids
+   */
+  Ids ids() const {
+    return utils::transform(lanelets(), [](const auto& ls) { return ls.id(); });
+  }
+
+  /**
+   * @brief returns the internal data on the linestrings managed by this object
+   */
+  LaneletSequenceDataConstPtr constData() const { return data_; }
+
+  /**
+   * @brief call this function if one of the contained Lanelets was modified to
+   * give this
+   * object the opportunity to update its cache.
+   */
+  void resetCache() { data_ = std::make_shared<LaneletSequenceData>(data_->lanelets()); }
+
+ private:
+  std::shared_ptr<const LaneletSequenceData> data_;  //!< internal data
+  bool inverted_{false};                             //!< Flag that indicates this lanelet is inverted
+};
+
+namespace utils {
+/**
+ * @brief returns true if element of a  primitive has a matching Id
+ * @param ll the element holding other primitives
+ * @param id id to look for
+ * @return true if the primitive has such an element
+ *
+ * This function does not look for the id of the element, only its members
+ * Works for linestrings and polylines.
+ * A similar implementation exists for regulatory elements and lanelets.
+ */
+inline bool has(const LaneletSequence& ll, Id id) { return has(ll.leftBound(), id) || has(ll.rightBound(), id); }
+}  // namespace utils
+
+inline bool operator==(const LaneletSequence& lhs, const LaneletSequence& rhs) {
+  return lhs.lanelets() == rhs.lanelets();
+}
+inline bool operator!=(const LaneletSequence& lhs, const LaneletSequence& rhs) { return !(rhs == lhs); }
+
+inline std::ostream& operator<<(std::ostream& stream, const LaneletSequence& obj) {
+  auto ids = obj.ids();
+  stream << "[";
+  if (!ids.empty()) {
+    stream << "ids: ";
+    std::copy(ids.begin(), ids.end(), std::ostream_iterator<Id>(stream, " "));
+  }
+  if (obj.inverted()) {
+    stream << ", inverted";
+  }
+  return stream << "]";
+}
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/primitives/LineString.h

@@ -0,0 +1,801 @@
+#pragma once
+#include <utility>
+
+#include "lanelet2_core/Forward.h"
+#include "lanelet2_core/primitives/BoundingBox.h"
+#include "lanelet2_core/primitives/Point.h"
+#include "lanelet2_core/primitives/Primitive.h"
+#include "lanelet2_core/utility/ReverseAndForwardIterator.h"
+#include "lanelet2_core/utility/TransformIterator.h"
+#include "lanelet2_core/utility/Utilities.h"
+
+namespace lanelet {
+
+using BasicLineString2d = BasicPoints2d;
+using BasicLineString3d = BasicPoints3d;
+
+template <typename PointT>
+using Segment = std::pair<PointT, PointT>;
+
+using Segment2d = Segment<Point2d>;
+using ConstSegment2d = Segment<ConstPoint2d>;
+using Segment3d = Segment<Point3d>;
+using ConstSegment3d = Segment<ConstPoint3d>;
+using BasicSegment2d = Segment<BasicPoint2d>;
+using BasicSegment3d = Segment<BasicPoint3d>;
+
+namespace traits {
+template <>
+struct PrimitiveTraits<BasicLineString2d> {
+  using ConstType = BasicLineString2d;
+  using MutableType = BasicLineString2d;
+  using TwoDType = BasicLineString2d;
+  using ThreeDType = BasicLineString3d;
+  using Category = LineStringTag;
+};
+template <>
+struct LineStringTraits<BasicLineString2d> : public PrimitiveTraits<BasicLineString2d> {
+  using PointType = BasicPoint2d;
+  using HybridType = BasicLineString2d;
+};
+template <>
+struct PrimitiveTraits<BasicLineString3d> {
+  using ConstType = BasicLineString3d;
+  using MutableType = BasicLineString3d;
+  using TwoDType = BasicLineString2d;
+  using ThreeDType = BasicLineString3d;
+  using Category = LineStringTag;
+};
+template <>
+struct LineStringTraits<BasicLineString3d> : public PrimitiveTraits<BasicLineString3d> {
+  using PointType = BasicPoint3d;
+  using HybridType = BasicLineString3d;
+};
+template <typename PointT>
+struct PrimitiveTraits<Segment<PointT>> {
+  using ConstType = Segment<ConstPrimitiveType<PointT>>;
+  using MutableType = Segment<MutablePrimitiveType<PointT>>;
+  using TwoDType = Segment<TwoD<PointT>>;
+  using ThreeDType = Segment<ThreeD<PointT>>;
+  using Category = LineStringTag;
+};
+template <typename PointT>
+struct LineStringTraits<Segment<PointT>> : public PrimitiveTraits<Segment<PointT>> {
+  using PointType = PointT;
+  using HybridType = Segment<traits::BasicPointT<PointT>>;
+};
+template <>
+inline BasicLineString2d to2D<BasicLineString3d>(const BasicLineString3d& primitive) {
+  BasicLineString2d ls2d(primitive.size());
+  std::transform(primitive.begin(), primitive.end(), ls2d.begin(), utils::to2D<BasicPoint3d>);
+  return ls2d;
+}
+}  // namespace traits
+
+namespace internal {
+template <typename PointT>
+struct SelectLsIterator {
+  using Iterator = internal::TransformIterator<internal::ReverseAndForwardIterator<Points3d::iterator>, PointT>;
+};
+template <typename PointT>
+struct SelectLsIterator<const PointT> {
+  using Iterator =
+      internal::TransformIterator<internal::ReverseAndForwardIterator<Points3d::const_iterator>, const PointT>;
+};
+template <>
+struct SelectLsIterator<Point3d> {
+  using Iterator = internal::ReverseAndForwardIterator<Points3d::iterator>;
+};
+template <>
+struct SelectLsIterator<const Point3d> {
+  using Iterator = internal::ReverseAndForwardIterator<Points3d::const_iterator>;
+};
+
+template <typename T>
+using SelectLsIteratorT = typename SelectLsIterator<T>::Iterator;
+
+template <typename PointT>
+struct SelectBasicLineString {};
+
+template <>
+struct SelectBasicLineString<BasicPoint2d> {
+  using Type = BasicLineString2d;
+};
+
+template <>
+struct SelectBasicLineString<BasicPoint3d> {
+  using Type = BasicLineString3d;
+};
+
+template <typename T>
+using SelectBasicLineStringT = typename SelectBasicLineString<T>::Type;
+
+inline Points3d::const_iterator pointIter(internal::ReverseAndForwardIterator<Points3d::iterator> it) {
+  return Points3d::iterator(it);
+}
+template <typename T>
+struct SelectInsertIterator {
+  using Type = internal::TransformIterator<T, Point3d>;
+};
+template <>
+struct SelectInsertIterator<typename SelectLsIterator<Point2d>::Iterator> {
+  using Type = internal::ReverseAndForwardIterator<Points3d::iterator>;
+};
+
+}  // namespace internal
+
+//! Common data management class for all LineString primitives.
+//! @ingroup DataObjects
+class LineStringData : public PrimitiveData {
+ public:
+  explicit LineStringData(Id id) : PrimitiveData(id) {}
+  LineStringData(Id id, Points3d points, AttributeMap attributes)
+      : PrimitiveData(id, std::move(attributes)), points_(std::move(points)) {}
+  // NOLINTNEXTLINE
+  using iterator = internal::ReverseAndForwardIterator<Points3d::iterator>;
+  // NOLINTNEXTLINE
+  using const_iterator = internal::ReverseAndForwardIterator<Points3d::const_iterator>;
+
+  /**
+   * @brief returns a reference to the points
+   *
+   * This ignores the "inverted" property
+   */
+  Points3d& points() { return points_; }
+
+  template <typename IteratorT = const_iterator>
+  IteratorT begin(bool inverted) const noexcept {
+    return inverted ? IteratorT(const_iterator(points_.rbegin())) : IteratorT(const_iterator(points_.begin()));
+  }
+
+  template <typename IteratorT = iterator>
+  IteratorT begin(bool inverted) noexcept {
+    return inverted ? IteratorT(iterator(points_.rbegin())) : IteratorT(iterator(points_.begin()));
+  }
+
+  template <typename IteratorT = const_iterator>
+  IteratorT end(bool inverted) const noexcept {
+    return inverted ? IteratorT(const_iterator(points_.rend())) : IteratorT(const_iterator(points_.end()));
+  }
+  template <typename IteratorT = iterator>
+  IteratorT end(bool inverted) noexcept {
+    return inverted ? IteratorT(iterator(points_.rend())) : IteratorT(iterator(points_.end()));
+  }
+  auto size() const noexcept { return points_.size(); }
+  auto empty() const noexcept { return points_.empty(); }
+  const ConstPoint3d& at(bool inverted, size_t idx) const noexcept {
+    return inverted ? points_[points_.size() - 1 - idx] : points_[idx];
+  }
+  const ConstPoint3d& front(bool inverted) const noexcept { return inverted ? points_.back() : points_.front(); }
+  const ConstPoint3d& back(bool inverted) const noexcept { return inverted ? points_.front() : points_.back(); }
+
+ private:
+  Points3d points_;
+};  // class LineStringData
+
+//! @defgroup LineStringPrimitives LineString
+//! @ingroup Primitives
+//!
+//! ## General
+//! LineStrings are a series of line segments defined by an ordered list of points.
+//! A LineString consists at least of one point and must not contain the same point
+//! multiple times in succession.
+//!
+//! LineStrings exist in const/non-const, 2d/3d and in normal/hybrid versions.
+//! They all have the same interface but differ in the point types they provide.
+//! While the normal versions return Point2d or Point3d as points,
+//! the hybrid versions only return Eigen points when iterating.
+//!
+//! ## Design
+//! The interface of linestrings is designed to look as similar to an
+//! std::vector as possible. Therefore it is possible to pass Linestrings to
+//! most stl-algorithms. Ranged for-loops are possible as well.
+//!
+//! ## Inverting
+//! Linestrings can be inverted without cost overhead. When you iterate over
+//! their points, you iterate them in reversed order. When you modify inverted
+//! LineStrings, the non-inverted copies will be affected as well.
+//!
+//! ## Compound versions
+//! The CompoundLineString3d/CompoundLineString2d/... classes allow to combine
+//! multiple line strings so that they behave like one. They have the same
+//! interface like a single normal line strings, but internally access the data
+//! of multiple. The can also be passed to most geometry functions.
+//!
+//! If two adjacent linestrings in a compound linestring share the same point,
+//! one of them is left out. CompoundLineStrings are immutable.
+
+//! Implementation template class that is common to all LineStrings.
+template <typename PointT>
+class ConstLineStringImpl : public ConstPrimitive<LineStringData> {
+ public:
+  // using declarations
+  using BasicPointType = traits::BasicPointT<PointT>;
+  using ConstPointType = traits::ConstPointT<PointT>;
+  using PointType = ConstPointType;  //! linestring will return this point type
+  using MutablePointType = traits::MutablePointT<PointT>;
+  using Category = traits::LineStringTag;
+
+  // NOLINTNEXTLINE
+  using const_iterator = internal::SelectLsIteratorT<const ConstPointType>;
+  using iterator = const_iterator;                             // NOLINT
+  using value_type = ConstPointType;                           // NOLINT
+  using size_type = size_t;                                    // NOLINT
+  using difference_type = typename iterator::difference_type;  // NOLINT
+
+  using BasicIterator = internal::TransformIterator<const_iterator, const BasicPointType>;
+  using BasicLineString = internal::SelectBasicLineStringT<BasicPointType>;
+  using SegmentType = Segment<PointT>;
+  using ConstSegmentType = traits::ConstPrimitiveType<SegmentType>;
+  static constexpr traits::Dimensions Dimension = traits::PointTraits<PointT>::Dimension;
+
+  //! Constructs a LineString or similar from an Id and a list of points
+  explicit ConstLineStringImpl(Id id = InvalId, Points3d points = Points3d(),
+                               const AttributeMap& attributes = AttributeMap())
+      : ConstPrimitive{std::make_shared<LineStringData>(id, std::move(points), attributes)} {}
+
+  //! Constructs a linestring from the data object of another linestring
+  explicit ConstLineStringImpl(const std::shared_ptr<const LineStringData>& data, bool inverted = false)
+      : ConstPrimitive(data), inverted_{inverted} {}
+
+  //! construct from other ConstLineStrings
+  template <typename OtherT>
+  explicit ConstLineStringImpl(const ConstLineStringImpl<OtherT>& other)
+      : ConstPrimitive(other.constData()), inverted_{other.inverted()} {}
+
+  //! Returns whether this is an inverted linestring
+  bool inverted() const noexcept { return inverted_; }
+
+  //! Return the number of points in this linestring
+  size_t size() const noexcept { return constData()->size(); }
+
+  //! return if there are any points in this object
+  bool empty() const noexcept { return constData()->empty(); }
+
+  //! Returns an iterator to the start of the points
+  /**
+   * @see end, basicBegin
+   * Dereferencing the iterator will convert the point from the internal
+   * Point3d to PointType
+   */
+  const_iterator begin() const noexcept { return constData()->template begin<const_iterator>(inverted()); }
+  //! Returns an iterator to end of the points
+  /**
+   * @see begin, basicEnd
+   * Dereferencing the iterator will convert the point from the internal
+   * Point3d to PointType
+   */
+  const_iterator end() const noexcept { return constData()->template end<const_iterator>(inverted()); }
+
+  //! returns the first point (if it exist)
+  /**
+   * @see back, at
+   * The internal point type is a different one, but because all points inherit
+   * from the internal point type, no slicing is possible.
+   */
+  const ConstPointType& front() const noexcept { return constData()->front(inverted()); }
+
+  /**
+   * @brief returns the last point (if it exist)
+   * @see front, operator[]
+   *
+   * The internal point type is a different one, but because all points inherit
+   * from the internal point type, no slicing is possible.
+   */
+  const ConstPointType& back() const noexcept { return constData()->back(inverted()); }
+
+  /**
+   * @brief returns the point at this position
+   * @see front, operator[]
+   *
+   * The internal point type is a different one, but because all points inherit
+   * from the internal point type, no slicing is possible.
+   */
+  const ConstPointType& operator[](size_t idx) const noexcept {
+    assert(idx < size());
+    return constData()->at(inverted(), idx);
+  }
+
+  /**
+   * @brief returns a normal iterator to the internal point type at begin
+   * @see basicEnd, begin
+   */
+  BasicIterator basicBegin() const noexcept { return constData()->template begin<BasicIterator>(inverted_); }
+
+  /**
+   * @brief returns a normal iterator for the internal point type at end
+   * @see basicBegin, end
+   */
+  BasicIterator basicEnd() const noexcept { return constData()->template end<BasicIterator>(inverted_); }
+
+  /**
+   * @brief returns the n-th segment. If n equals size() -1, the segment from
+   * back() to front() is returned.
+   */
+  ConstSegmentType segment(size_t idx) const noexcept {
+    assert(idx < size());
+    auto first = begin() + idx;
+    auto second = idx + 1 == size() ? begin() : first + 1;
+    return ConstSegmentType(*first, *second);
+  }
+
+  //! Returns the number of (geometrically valid) segments.
+  size_t numSegments() const noexcept { return size() <= 1 ? 0 : size() - 1; }
+
+  /**
+   * @brief Create a basic linestring (ie a vector of Eigen Points)
+   *
+   * A basic linestring is a simple vector with eigen points. You can do what
+   * you want with these points, because changes to them will not affect lanelet
+   * objects.
+   *
+   * BasicLineString is registered with boost::geometry, therefore you can use
+   * it for geometry calculations.
+   */
+  BasicLineString basicLineString() const noexcept { return {basicBegin(), basicEnd()}; }
+
+  template <typename LineStringT>
+  class CompoundLineString;
+
+ protected:
+  // This class is only an implementation class and should never be instanciated
+  ConstLineStringImpl(ConstLineStringImpl&& rhs) noexcept = default;
+  ConstLineStringImpl& operator=(ConstLineStringImpl&& rhs) noexcept = default;
+  ConstLineStringImpl(const ConstLineStringImpl& rhs) = default;
+  ConstLineStringImpl& operator=(const ConstLineStringImpl& rhs) = default;
+  ~ConstLineStringImpl() noexcept = default;
+
+ private:
+  bool inverted_{false};
+};  // class ConstLineStringImpl
+
+//! Implementation template class that is common to all non-const types
+template <typename ConstLineStringT>
+class LineStringImpl : public Primitive<ConstLineStringT> {
+ protected:
+  using Base = Primitive<ConstLineStringT>;
+  using Base::data;
+
+ public:
+  using PointType = typename Base::MutablePointType;
+  using iterator = internal::SelectLsIteratorT<PointType>;  // NOLINT
+  using Base::Base;
+  using Base::inverted;
+  using Base::size;
+  using SegmentType = typename Base::SegmentType;
+  LineStringImpl() = default;  // Not inherited from base class
+
+  //! Construct from other (mutable!) LineStrings
+  template <typename OtherT>
+  explicit LineStringImpl(const LineStringImpl<OtherT>& other) : Base(other) {}
+
+  explicit LineStringImpl(const std::shared_ptr<const LineStringData>&, bool inverted) = delete;
+
+  //! Construct from linestring data
+  explicit LineStringImpl(const std::shared_ptr<LineStringData>& data, bool inverted) : Base(data, inverted) {}
+
+  //! Copy assign from a normal vector. The Id of this object is unchanged.
+  LineStringImpl& operator=(std::vector<PointType> rhs) {
+    if (inverted()) {
+      points() = utils::transform(rhs.rbegin(), rhs.rend(), [](const auto& v) { return Point3d(v); });
+    } else {
+      points() = utils::transform(rhs.begin(), rhs.end(), [](const auto& v) { return Point3d(v); });
+    }
+    return *this;
+  }
+
+  //! Move assign from a normal vector. Id and attributes stay unchanged.
+  LineStringImpl& operator=(std::vector<Point3d>&& rhs) {
+    if (inverted()) {
+      utils::invert(rhs);
+    }
+    points() = std::move(rhs);
+    return *this;
+  }
+
+  /**
+   * @brief Inserts an element at a specific position
+   *
+   * Just as you would expect from an std::vector...
+   * If you plan to insert more elements than one, use the ranged version below.
+   * That saves you from some unnecessary cache regenerations.
+   */
+  iterator insert(iterator position, const PointType& point) {
+    using Iter = LineStringData::iterator;
+    auto fwIter = points().insert(internal::pointIter(position), Point3d(point));
+    return inverted() ? Iter::reversed(fwIter + 1) : Iter(fwIter);
+  }
+
+  //! Inserts an range of elements at a specific position
+  template <typename InIter>
+  iterator insert(iterator position, InIter start, InIter end) {
+    using Viter = typename internal::SelectInsertIterator<InIter>::Type;
+    using RIter = LineStringData::iterator;
+    if (inverted()) {
+      using RViter = std::reverse_iterator<Viter>;
+      auto fwIter = points().insert(internal::pointIter(position), RViter(end), RViter(start));
+      return RIter(Points3d::reverse_iterator(fwIter + 1));
+    }
+    auto fwIter = points().insert(internal::pointIter(position), Viter(start), Viter(end));
+    return RIter(fwIter);
+  }
+
+  //! inserts a new element at the end
+  void push_back(const PointType& point) {  // NOLINT
+    inverted() ? void(points().insert(points().begin(), Point3d(point))) : points().push_back(Point3d(point));
+  }
+
+  //! Removes point, returns iterator to the next point
+  iterator erase(iterator position) {  // NOLINT
+    if (inverted()) {
+      position += 1;
+    }
+    using Iter = LineStringData::iterator;
+    auto it = points().erase(internal::pointIter(position));
+    return inverted() ? Iter::reversed(it) : Iter(it);
+  }
+
+  //! removes element from the end
+  void pop_back() {  // NOLINT
+    inverted() ? void(points().erase(points().begin())) : points().pop_back();
+  }
+
+  //! request a change in capacity
+  void reserve(size_t num) { points().reserve(num); }
+
+  //! Changes the number of points contained either by deleting them or by
+  //! inserting new (invalId points).
+  void resize(size_t num) {
+    if (inverted()) {
+      std::reverse(begin(), end());
+    }
+    points().resize(num);
+    if (inverted()) {
+      std::reverse(begin(), end());
+    }
+  }
+
+  //! Remove all points, making the linestring invalid until new points are
+  //! inserted.
+  void clear() { points().clear(); }
+
+  // inherit the const versions
+  using Base::back;
+  using Base::begin;
+  using Base::end;
+  using Base::front;
+  using Base::operator[];
+  using Base::segment;
+
+  //! mutable iterator for the elements of this vector.
+  iterator begin() { return inverted() ? iterator(points().rbegin()) : iterator(points().begin()); }
+
+  //! mutable iterator to the end of the elements of this vector.
+  iterator end() { return inverted() ? iterator(points().rend()) : iterator(points().end()); }
+
+  //! get a refernce to the first element (make sure it exists)
+  PointType& front() { return inverted() ? points().back() : points().front(); }
+
+  //! get a reference to the last element if it exists
+  PointType& back() { return inverted() ? points().front() : points().back(); }
+
+  //! access element at specific position
+  PointType& operator[](size_t idx) {
+    assert(idx < size());
+    return points()[inverted() ? points().size() - 1 - idx : idx];
+  }
+
+  /**
+   * @brief returns the n-th segment. If n equals size() -1, the segment from
+   * back() to front() is returned.
+   */
+  SegmentType segment(size_t idx) noexcept {
+    assert(idx < size());
+    const auto snd = idx + 1;
+    return SegmentType(operator[](idx), operator[](snd == size() ? 0 : snd));
+  }
+
+ protected:
+  // This class is only an implementation class and should never be instanciated
+  LineStringImpl(LineStringImpl&& rhs) noexcept = default;
+  LineStringImpl& operator=(LineStringImpl&& rhs) noexcept = default;
+  LineStringImpl(const LineStringImpl& rhs) = default;
+  LineStringImpl& operator=(const LineStringImpl& rhs) = default;
+  ~LineStringImpl() noexcept = default;
+  Points3d& points() { return data()->points(); }
+};  // class LineStringImpl
+
+//! @brief A normal 3d linestring with immutable data
+//! @ingroup LineStringPrimitives
+//! @ingroup ConstPrimitives
+class ConstLineString3d : public ConstLineStringImpl<Point3d> {
+ public:
+  using ConstLineStringImpl::ConstLineStringImpl;
+  using ConstType = ConstLineString3d;
+  using MutableType = LineString3d;
+  using TwoDType = ConstLineString2d;
+  using ThreeDType = ConstLineString3d;
+  using HybridType = ConstHybridLineString3d;
+  ConstLineString3d() = default;  // Not inherited from base class
+
+  //! create a new, inverted linestring from this one
+  ConstLineString3d invert() const noexcept { return ConstLineString3d{constData(), !inverted()}; }
+};
+
+//! @brief A normal 3d linestring with mutable data
+//! @ingroup LineStringPrimitives
+//! @ingroup Primitives
+class LineString3d : public LineStringImpl<ConstLineString3d> {
+ public:
+  using LineStringImpl::LineStringImpl;
+  using TwoDType = LineString2d;
+  using ThreeDType = LineString3d;
+  LineString3d() = default;  // Not inherited from base class
+
+  //! create a new, inverted linestring from this one
+  LineString3d invert() const noexcept { return LineString3d{data(), !inverted()}; }
+};
+
+//! @brief A normal 2d linestring with immutable data
+//! @ingroup LineStringPrimitives
+//! @ingroup ConstPrimitives
+class ConstLineString2d : public ConstLineStringImpl<Point2d> {
+ public:
+  using ConstLineStringImpl::ConstLineStringImpl;
+  using ConstType = ConstLineString2d;
+  using MutableType = LineString2d;
+  using TwoDType = ConstLineString2d;
+  using ThreeDType = ConstLineString3d;
+  using HybridType = ConstHybridLineString2d;
+  ConstLineString2d() = default;  // Not inherited from base class
+
+  //! create a new, inverted linestring from this one
+  ConstLineString2d invert() const noexcept { return ConstLineString2d{constData(), !inverted()}; }
+};
+
+//! @brief A normal 2d linestring with mutable data
+//! @ingroup LineStringPrimitives
+//! @ingroup Primitives
+class LineString2d : public LineStringImpl<ConstLineString2d> {
+ public:
+  using LineStringImpl::LineStringImpl;
+  using TwoDType = LineString2d;
+  using ThreeDType = LineString3d;
+  LineString2d() = default;  // Not inherited from base class
+
+  //! create a new, inverted linestring from this one
+  LineString2d invert() const noexcept { return LineString2d{data(), !inverted()}; }
+};
+
+//! @brief A Linestring that returns BasicPoint3d instead of Point3d
+//!
+//! For usage with boost::geometry. Has no mutable version.
+//! @ingroup LineStringPrimitives
+//! @ingroup ConstPrimitives
+class ConstHybridLineString3d : public ConstLineString3d {
+ public:
+  using PointType = BasicPointType;
+  using const_iterator = BasicIterator;  // NOLINT
+  using iterator = BasicIterator;        // NOLINT
+  using ConstType = ConstHybridLineString3d;
+  using TwoDType = ConstHybridLineString2d;
+  using ThreeDType = ConstHybridLineString3d;
+  using SegmentType = Segment<BasicPointType>;
+
+  using ConstLineString3d::ConstLineString3d;
+  ConstHybridLineString3d() = default;  // Not inherited from base class
+  explicit ConstHybridLineString3d(const ConstLineString3d& ls) : ConstLineString3d(ls) {}
+  explicit ConstHybridLineString3d(const LineString3d& ls) : ConstLineString3d(ls) {}
+
+  //! Returns an inverted linestring, O(0)
+  ConstHybridLineString3d invert() const noexcept { return ConstHybridLineString3d{constData(), !inverted()}; }
+
+  //! BasicPoint3d Iterator to begin
+  BasicIterator begin() const noexcept { return basicBegin(); }
+
+  //! BasicPoint3d Iterator to past-the-end
+  BasicIterator end() const noexcept { return basicEnd(); }
+
+  //! Get first BasicPoint3d
+  const BasicPointType& front() const noexcept { return ConstLineString3d::front().basicPoint(); }
+
+  //! Get last BasicPoint3d
+  const BasicPointType& back() const noexcept { return ConstLineString3d::back().basicPoint(); }
+
+  //! access BasicPoint3d at specific position
+  const BasicPointType& operator[](size_t idx) const noexcept {
+    return ConstLineString3d::operator[](idx).basicPoint();
+  }
+
+  /**
+   * @brief returns the n-th segment. If n equals size() -1, the segment from
+   * back() to front() is returned.
+   */
+  SegmentType segment(size_t idx) const noexcept {
+    assert(idx < size());
+    const auto snd = idx + 1;
+    return {operator[](idx), operator[](snd == size() ? 0 : snd)};
+  }
+};
+
+//! @brief A Linestring that returns BasicPoint2d instead of Point2d
+//!
+//! For usage with boost::geometry. Has no mutable version.
+//! @ingroup LineStringPrimitives
+//! @ingroup ConstPrimitives
+class ConstHybridLineString2d : public ConstLineString2d {
+ public:
+  using PointType = BasicPointType;
+  using const_iterator = BasicIterator;  // NOLINT
+  using iterator = BasicIterator;        // NOLINT
+  using ConstType = ConstHybridLineString2d;
+  using TwoDType = ConstHybridLineString2d;
+  using ThreeDType = ConstHybridLineString3d;
+  using SegmentType = Segment<BasicPointType>;
+
+  using ConstLineString2d::ConstLineString2d;
+  ConstHybridLineString2d() = default;  // Not inherited from base class
+  explicit ConstHybridLineString2d(const ConstLineString2d& ls) : ConstLineString2d(ls) {}
+  explicit ConstHybridLineString2d(const LineString2d& ls) : ConstLineString2d(ls) {}
+
+  //! Returns an inverted linestring, O(0)
+  ConstHybridLineString2d invert() const noexcept { return ConstHybridLineString2d{constData(), !inverted()}; }
+
+  //! BasicPoint2d Iterator to begin
+  BasicIterator begin() const noexcept { return basicBegin(); }
+
+  //! BasicPoint2d Iterator to past-the-end
+  BasicIterator end() const noexcept { return basicEnd(); }
+
+  //! Get first BasicPoint2d
+  const BasicPointType& front() const noexcept { return ConstLineString2d::front().basicPoint(); }
+
+  //! Get last BasicPoint2d
+  const BasicPointType& back() const noexcept { return ConstLineString2d::back().basicPoint(); }
+
+  //! access element at specific position
+  const BasicPointType& operator[](size_t idx) const noexcept {
+    return ConstLineString2d::operator[](idx).basicPoint();
+  }
+
+  /**
+   * @brief returns the n-th segment. If n equals size() -1, the segment from
+   * back() to front() is returned.
+   */
+  SegmentType segment(size_t idx) const noexcept {
+    assert(idx < size());
+    const auto snd = idx + 1;
+    return {operator[](idx), operator[](snd == size() ? 0 : snd)};
+  }
+};
+
+inline std::ostream& operator<<(std::ostream& stream, const ConstLineString2d& obj) {
+  stream << "[id: " << obj.id();
+  if (obj.inverted()) {
+    stream << ", inverted";
+  }
+  stream << " point ids: ";
+  for (auto it = obj.begin(); it != obj.end(); ++it) {
+    stream << it->id();
+    if (std::next(it) != obj.end()) {
+      stream << ", ";
+    }
+  }
+  return stream << "]";
+}
+
+inline std::ostream& operator<<(std::ostream& stream, const ConstLineString3d& obj) {
+  stream << "[id: " << obj.id();
+  if (obj.inverted()) {
+    stream << ", inverted";
+  }
+  stream << " point ids: ";
+  for (auto it = obj.begin(); it != obj.end(); ++it) {
+    stream << it->id();
+    if (std::next(it) != obj.end()) {
+      stream << ", ";
+    }
+  }
+  return stream << "]";
+}
+
+// comparison
+template <typename LhsPointT, typename RhsPointT>
+bool operator==(const ConstLineStringImpl<LhsPointT>& lhs, const ConstLineStringImpl<RhsPointT>& rhs) {
+  return lhs.constData() == rhs.constData() && lhs.inverted() == rhs.inverted();
+}
+template <typename LhsPointT, typename RhsPointT>
+bool operator!=(const ConstLineStringImpl<LhsPointT>& lhs, const ConstLineStringImpl<RhsPointT>& rhs) {
+  return !(lhs == rhs);
+}
+
+template <typename PointT>
+bool operator==(const ConstLineStringImpl<PointT>& lhs, const std::vector<PointT>& rhs) {
+  return lhs.size() == rhs.size() && std::equal(lhs.begin(), lhs.end(), rhs.begin());
+}
+
+template <typename PointT>
+bool operator==(const std::vector<PointT>& lhs, const ConstLineStringImpl<PointT>& rhs) {
+  return rhs == lhs;
+}
+
+template <typename PointT>
+bool operator!=(const ConstLineStringImpl<PointT>& lhs, const std::vector<PointT>& rhs) {
+  return !(lhs == rhs);
+}
+
+template <typename PointT>
+bool operator!=(const std::vector<PointT>& lhs, const ConstLineStringImpl<PointT>& rhs) {
+  return !(lhs == rhs);
+}
+
+namespace traits {
+template <typename T>
+constexpr bool isLinestringT() {
+  return isCategory<T, traits::LineStringTag>();
+}
+
+namespace detail {
+template <typename T, typename Enable = void>
+struct HybridType {};
+
+template <typename T>
+struct HybridType<T, std::enable_if_t<traits::isLinestringT<T>(), void>> {
+  using Type = typename LineStringTraits<T>::HybridType;
+};
+template <typename T>
+struct HybridType<T, std::enable_if_t<!traits::isLinestringT<T>(), void>> {
+  using Type = typename LineStringTraits<T>::HybridType;
+};
+}  // namespace detail
+template <typename LineStringT>
+using HybridT = typename detail::HybridType<LineStringT>::Type;
+
+template <typename LineStringT>
+constexpr auto toHybrid(const LineStringT ls) {
+  return HybridT<LineStringT>(ls);
+}
+}  // namespace traits
+
+template <typename T, typename RetT>
+using IfLS = std::enable_if_t<traits::isLinestringT<T>(), RetT>;
+
+template <typename T1, typename T2, typename RetT>
+using IfLS2 = std::enable_if_t<traits::isLinestringT<T1>() && traits::isLinestringT<T2>(), RetT>;
+
+namespace utils {
+using traits::toHybrid;
+
+/**
+ * @brief returns true if element of a lanelet primitive has a matching Id
+ * @param ls the element holding other primitives
+ * @param id id to look for
+ * @return true if the primitive has such an element
+ *
+ * This function does not look for the id of the element, only its members
+ * Works for linestrings and polylines.
+ * A similar implementation exists for regulatory elements and lanelets.
+ */
+template <typename PointT>
+bool has(const ConstLineStringImpl<PointT>& ls, Id id) {
+  return std::any_of(ls.begin(), ls.end(), [&id](const auto& elem) { return elem.id() == id; });
+}
+}  // namespace utils
+
+}  // namespace lanelet
+
+// Hash function for usage in containers
+namespace std {
+template <>
+struct hash<lanelet::LineString3d> : public lanelet::HashBase<lanelet::LineString3d> {};
+template <>
+struct hash<lanelet::ConstLineString3d> : public lanelet::HashBase<lanelet::ConstLineString3d> {};
+template <>
+struct hash<lanelet::LineString2d> : public lanelet::HashBase<lanelet::LineString2d> {};
+template <>
+struct hash<lanelet::ConstLineString2d> : public lanelet::HashBase<lanelet::ConstLineString2d> {};
+template <>
+struct hash<lanelet::ConstHybridLineString2d> : public lanelet::HashBase<lanelet::ConstHybridLineString2d> {};
+template <>
+struct hash<lanelet::ConstHybridLineString3d> : public lanelet::HashBase<lanelet::ConstHybridLineString3d> {};
+}  // namespace std

src/map/lanelet2/include/lanelet2_core/primitives/LineStringOrPolygon.h

@@ -0,0 +1,143 @@
+#pragma once
+#include <boost/variant.hpp>
+
+#include "lanelet2_core/primitives/LineString.h"
+#include "lanelet2_core/primitives/Polygon.h"
+#include "lanelet2_core/primitives/RegulatoryElement.h"
+
+namespace lanelet {
+//! Base class for objects that can either refer to linestrings or polygons
+template <typename LineStringT, typename PolygonT>
+class LineStringOrPolygonBase {
+ public:
+  LineStringOrPolygonBase() = default;
+  LineStringOrPolygonBase(LineStringOrPolygonBase&& rhs) = default;             // NOLINT
+  LineStringOrPolygonBase& operator=(LineStringOrPolygonBase&& rhs) = default;  // NOLINT
+  LineStringOrPolygonBase(const LineStringOrPolygonBase& rhs) = default;
+  LineStringOrPolygonBase& operator=(const LineStringOrPolygonBase& rhs) = default;
+  ~LineStringOrPolygonBase() noexcept = default;
+
+  LineStringOrPolygonBase(LineStringT linestring)  // NOLINT
+      : lsOrPoly_{std::move(linestring)} {}
+  LineStringOrPolygonBase(PolygonT polygon)  // NOLINT
+      : lsOrPoly_{std::move(polygon)} {}
+  LineStringOrPolygonBase& operator=(LineStringT linestring) {
+    lsOrPoly_ = std::move(linestring);
+    return *this;
+  }
+  LineStringOrPolygonBase& operator=(PolygonT poly) {
+    lsOrPoly_ = std::move(poly);
+    return *this;
+  }
+
+  //! true if this object holds an polygon
+  bool isPolygon() const { return lsOrPoly_.which() == 1; }
+
+  //! true if this objct holds a lineString
+  bool isLineString() const { return lsOrPoly_.which() == 0; }
+
+  //! convert to linestring (type is not checked)
+  explicit operator const LineStringT&() const { return boost::get<LineStringT>(lsOrPoly_); }
+
+  //! convert to polygon (type is not checked)
+  explicit operator const PolygonT&() const { return boost::get<PolygonT>(lsOrPoly_); }
+
+  //! apply a generic visitor
+  template <typename VisitorT>
+  decltype(auto) applyVisitor(VisitorT visitor) const {
+    return boost::apply_visitor(visitor, lsOrPoly_);
+  }
+
+  //! get the id of the linestring or polygon
+  Id id() const {
+    return applyVisitor([](auto& elem) { return elem.id(); });
+  }
+
+  //! get the attributes of the linestring or polygon
+  const AttributeMap& attributes() const {
+    return applyVisitor([](auto& elem) -> const AttributeMap& { return elem.attributes(); });
+  }
+
+  //! return the managed linestring
+  Optional<LineStringT> lineString() const {
+    const auto* ls = boost::get<LineStringT>(&lsOrPoly_);
+    if (ls != nullptr) {
+      return *ls;
+    }
+    return {};
+  }
+
+  //! get the managed polygon
+  Optional<PolygonT> polygon() const {
+    const auto* poly = boost::get<PolygonT>(&lsOrPoly_);
+    if (poly != nullptr) {
+      return *poly;
+    }
+    return {};
+  }
+
+  bool equals(const LineStringOrPolygonBase& other) const { return lsOrPoly_ == other.lsOrPoly_; }
+
+ protected:
+  boost::variant<LineStringT, PolygonT> lsOrPoly_;  // NOLINT
+};
+
+//! This class holds either a LineString3d or a Polygon3d
+class LineStringOrPolygon3d : public LineStringOrPolygonBase<LineString3d, Polygon3d> {
+ public:
+  using Base = LineStringOrPolygonBase<LineString3d, Polygon3d>;
+  using Base::LineStringOrPolygonBase;
+  operator RuleParameter() const { return asRuleParameter(); }  // NOLINT
+  RuleParameter asRuleParameter() const {
+    return applyVisitor([](auto& prim) { return RuleParameter(prim); });
+  }
+
+  using Base::applyVisitor;
+  //! apply a generic visitor
+  template <typename VisitorT>
+  decltype(auto) applyVisitor(VisitorT visitor) {
+    return boost::apply_visitor(visitor, lsOrPoly_);
+  }
+};
+
+//! This class holds either a ConstLineString3d or a ConstPolygon3d
+class ConstLineStringOrPolygon3d : public LineStringOrPolygonBase<ConstLineString3d, ConstPolygon3d> {
+ public:
+  using Base = LineStringOrPolygonBase<ConstLineString3d, ConstPolygon3d>;
+  using Base::LineStringOrPolygonBase;
+  ConstLineStringOrPolygon3d(const LineStringOrPolygon3d& lsOrPoly) {  // NOLINT
+    if (lsOrPoly.isLineString()) {
+      *this = *lsOrPoly.lineString();
+    }
+    if (lsOrPoly.isPolygon()) {
+      *this = *lsOrPoly.polygon();
+    }
+  }
+  operator ConstRuleParameter() const { return asRuleParameter(); }  // NOLINT
+  ConstRuleParameter asRuleParameter() const {
+    return applyVisitor([](auto& prim) { return ConstRuleParameter(prim); });
+  }
+};
+
+inline bool operator==(const LineStringOrPolygon3d& lhs, const LineStringOrPolygon3d& rhs) { return lhs.equals(rhs); }
+inline bool operator!=(const LineStringOrPolygon3d& lhs, const LineStringOrPolygon3d& rhs) { return !(lhs == rhs); }
+
+inline bool operator==(const ConstLineStringOrPolygon3d& lhs, const ConstLineStringOrPolygon3d& rhs) {
+  return lhs.equals(rhs);
+}
+inline bool operator!=(const ConstLineStringOrPolygon3d& lhs, const ConstLineStringOrPolygon3d& rhs) {
+  return !(lhs == rhs);
+}
+
+using LineStringsOrPolygons3d = std::vector<LineStringOrPolygon3d>;
+using ConstLineStringsOrPolygons3d = std::vector<ConstLineStringOrPolygon3d>;
+
+inline std::ostream& operator<<(std::ostream& stream, const LineStringOrPolygon3d& obj) {
+  obj.applyVisitor([&stream](auto& prim) { stream << prim; });
+  return stream;
+}
+inline std::ostream& operator<<(std::ostream& stream, const ConstLineStringOrPolygon3d& obj) {
+  obj.applyVisitor([&stream](auto& prim) { stream << prim; });
+  return stream;
+}
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/primitives/Point.h

@@ -0,0 +1,349 @@
+#pragma once
+#pragma GCC diagnostic push
+#if defined __GNUC__ && (__GNUC__ >= 6)
+#pragma GCC diagnostic ignored "-Wignored-attributes"
+#pragma GCC diagnostic ignored "-Wint-in-bool-context"
+#endif
+#include <Eigen/Core>
+#pragma GCC diagnostic pop
+#include <utility>
+
+#include "lanelet2_core/Forward.h"
+#include "lanelet2_core/primitives/Primitive.h"
+#include "lanelet2_core/primitives/Traits.h"
+#include "lanelet2_core/utility/TransformIterator.h"
+#include "lanelet2_core/utility/Utilities.h"
+
+namespace lanelet {
+
+using BasicPoint3d = Eigen::Vector3d;                                //!< a simple 3d-point
+using BasicPoint2d = Eigen::Matrix<double, 2, 1, Eigen::DontAlign>;  //!< a simple 2d-point
+using BasicPoints2d = std::vector<Eigen::Vector2d,
+                                  Eigen::aligned_allocator<Eigen::Vector2d>>;  //!< multiple simple 2d-points
+using BasicPoints3d = std::vector<BasicPoint3d>;                               //!< multiple simple 3d-points
+
+namespace internal {
+template <>
+class Converter<const BasicPoint2d> {
+ public:
+  template <typename InType>
+  const BasicPoint2d& convert(const InType& t) const {
+    return t.basicPoint2d();
+  }
+};
+}  // namespace internal
+
+namespace traits {
+//! used for templates that require info about this point
+template <>
+struct PrimitiveTraits<BasicPoint3d> {
+  using ConstType = BasicPoint3d;
+  using MutableType = BasicPoint3d;
+  using TwoDType = BasicPoint2d;
+  using ThreeDType = BasicPoint3d;
+  using Category = PointTag;
+};
+template <>
+struct PrimitiveTraits<BasicPoint2d> {
+  using ConstType = BasicPoint2d;
+  using MutableType = BasicPoint2d;
+  using TwoDType = BasicPoint2d;
+  using ThreeDType = BasicPoint3d;
+  using Category = PointTag;
+};
+template <>
+struct PrimitiveTraits<Eigen::Vector2d> {
+  using ConstType = BasicPoint2d;
+  using MutableType = BasicPoint2d;
+  using TwoDType = BasicPoint2d;
+  using ThreeDType = BasicPoint3d;
+  using Category = PointTag;
+};
+template <>
+struct PointTraits<BasicPoint3d> : PrimitiveTraits<BasicPoint3d> {
+  using BasicPoint = BasicPoint3d;
+  using ConstPoint = typename PrimitiveTraits<BasicPoint3d>::ConstType;
+  using MutablePoint = typename PrimitiveTraits<BasicPoint3d>::MutableType;
+  static constexpr bool IsPrimitive = false;
+  static constexpr Dimensions Dimension = Dimensions::Three;
+};
+template <>
+struct PointTraits<BasicPoint2d> : PrimitiveTraits<BasicPoint2d> {
+  using BasicPoint = BasicPoint2d;
+  using ConstPoint = typename PrimitiveTraits<BasicPoint2d>::ConstType;
+  using MutablePoint = typename PrimitiveTraits<BasicPoint2d>::MutableType;
+  static constexpr bool IsPrimitive = false;
+  static constexpr Dimensions Dimension = Dimensions::Three;
+};
+template <>
+struct PointTraits<Eigen::Vector2d> : PrimitiveTraits<Eigen::Vector2d> {
+  using BasicPoint = BasicPoint2d;
+  using ConstPoint = typename PrimitiveTraits<BasicPoint2d>::ConstType;
+  using MutablePoint = typename PrimitiveTraits<BasicPoint2d>::MutableType;
+  static constexpr bool IsPrimitive = false;
+  static constexpr Dimensions Dimension = Dimensions::Three;
+};
+
+template <>
+inline BasicPoint3d to3D(const BasicPoint2d& primitive) {
+  return {primitive.x(), primitive.y(), 0};
+}
+template <>
+inline BasicPoint2d to2D(const BasicPoint3d& primitive) {
+  return {primitive.x(), primitive.y()};
+}
+}  // namespace traits
+
+//! Describes a position in linestring coordinates
+struct ArcCoordinates {
+  double length{0.};    //!< length along linestring
+  double distance{0.};  //!< signed distance to linestring (left=positive)
+};
+
+//! Common data management class for all Point primitives.
+//! @ingroup DataObjects
+class PointData : public PrimitiveData {  // NOLINT
+ public:
+  PointData(Id id, BasicPoint3d point, const AttributeMap& attributes = AttributeMap())
+      : PrimitiveData(id, attributes), point(point), point2d_{point.x(), point.y()} {}
+  PointData(const PointData&) = delete;
+  PointData& operator=(const LineStringData&) = delete;
+  PointData(PointData&&) = default;
+  PointData& operator=(PointData&&) = default;
+  ~PointData() = default;
+  const BasicPoint2d& point2d() const {
+    if (BOOST_UNLIKELY(point.head<2>() != point2d_)) {
+      point2d_ = point.head<2>();
+    }
+    return point2d_;
+  }
+
+  BasicPoint3d point;  // NOLINT(cppcoreguidelines-non-private-member-variables-in-classes)
+
+ private:
+  mutable BasicPoint2d point2d_;
+};
+
+//! @defgroup PointPrimitives Point
+//! @ingroup Primitives
+//!
+//! # Design
+//! Points are the most basic data structure of lanelet2. They are
+//! characterized by their coordinates and, as all primitives, an ID and
+//! attributes.
+//!
+//! The exist in a 2D and 3D and in a mutable/immutable version.
+//!
+//! # Geometry
+//! Points can be used for geometry calculations in boost::geometry. If you need
+//! more basic operations, you can convert them to their respective basic point
+//! type, e.g. BasicPoint2d, which is an Eigen object that supports all usual
+//! math operations and is mutable.
+//!
+//! All coordinates are in *meters*.
+
+/**
+ * @brief An immutable 2d point
+ * @ingroup PointPrimitives
+ * @ingroup ConstPrimitives
+ */
+class ConstPoint2d : public ConstPrimitive<PointData> {
+ public:
+  using BasicPoint = BasicPoint2d;  //!< returned basic point type
+  using ConstType = ConstPoint2d;
+  using MutableType = Point2d;
+  using TwoDType = ConstPoint2d;
+  using ThreeDType = ConstPoint3d;
+  using Category = traits::PointTag;
+  static constexpr traits::Dimensions Dimension = traits::Dimensions::Two;
+
+  using ConstPrimitive::ConstPrimitive;
+
+  //! @brief Construct from an id and a point
+  ConstPoint2d(Id id, const BasicPoint3d& point, const AttributeMap& attributes = AttributeMap())
+      : ConstPrimitive<PointData>{std::make_shared<PointData>(id, point, attributes)} {}  // NOLINT
+
+  //! @brief Construct from an id and coordinates
+  /**
+   * The z coordinate is required because this point can be converted to a 3d
+   * point, where z matters.
+   */
+  explicit ConstPoint2d(Id id = InvalId, double x = 0., double y = 0., double z = 0.,
+                        const AttributeMap& attributes = AttributeMap())
+      : ConstPrimitive<PointData>{std::make_shared<PointData>(  // NOLINT
+            id, BasicPoint3d(x, y, z), attributes)} {}
+
+  //! A ConstPoint 2d is implicitly convertible to a normal 2d point
+  operator BasicPoint2d() const noexcept {  // NOLINT
+    return point2d();
+  }
+
+  //! Get an Eigen point for faster calculations
+  const BasicPoint& basicPoint() const noexcept { return point2d(); }
+
+  //! Get a 2d Eigen point. Does not make sense for this particular class, but
+  //! for its children.
+  const BasicPoint2d& basicPoint2d() const noexcept { return point2d(); }
+
+  //! gets x coordinate
+  double x() const noexcept { return point().x(); }
+
+  //! gets y coordinate
+  double y() const noexcept { return point().y(); }
+
+ protected:
+  const BasicPoint3d& point() const { return constData()->point; }
+  const BasicPoint2d& point2d() const { return constData()->point2d(); }
+};
+
+//! @brief An immutable 3d point.
+//! @ingroup PointPrimitives
+//! @ingroup ConstPrimitives
+class ConstPoint3d : public ConstPoint2d {
+ public:
+  using BasicPoint = BasicPoint3d;
+  using ConstType = ConstPoint3d;
+  using MutableType = Point3d;
+  static constexpr traits::Dimensions Dimension = traits::Dimensions::Three;
+
+  using ConstPoint2d::ConstPoint2d;
+  ConstPoint3d() = default;
+  explicit ConstPoint3d(const ConstPoint2d& other) : ConstPoint2d(other) {}
+
+  operator BasicPoint2d() const noexcept = delete;  // NOLINT
+
+  //! Implicit cast to a basic 3d point
+  operator const BasicPoint3d&() const noexcept {  // NOLINT
+    return point();
+  }
+
+  //! get a basic 3d point
+  const BasicPoint& basicPoint() const noexcept { return point(); }
+
+  //! get the z coordinate
+  double z() const noexcept { return point().z(); }
+};
+
+//! @brief A mutable 2d point.
+//! @ingroup PointPrimitives
+//! @ingroup Primitives
+class Point2d : public Primitive<ConstPoint3d> {
+ public:
+  using BasicPoint = BasicPoint2d;
+  using ConstType = ConstPoint2d;
+  using MutableType = Point2d;
+  using TwoDType = Point2d;
+  using ThreeDType = Point3d;
+  static constexpr traits::Dimensions Dimension = traits::Dimensions::Two;
+
+  using Primitive::Primitive;
+  using Primitive::x;
+  using Primitive::y;
+  Point2d() = default;
+  operator const BasicPoint3d&() const = delete;
+
+  //! Cast to a basic 2d point
+  operator BasicPoint2d() const noexcept {  // NOLINT
+    return point().head<2>();
+  }
+
+  //! get a mutable reference to the 2d point data
+  auto basicPoint() noexcept { return point().head<2>(); }
+
+  //! get a basic 2d point
+  const BasicPoint2d& basicPoint() const noexcept { return point2d(); }
+
+  //! gets a mutable reference to the x coordinate
+  double& x() noexcept { return point().x(); }
+
+  //! gets a mutable reference to the y coordinate
+  double& y() noexcept { return point().y(); }
+  double z() const noexcept = delete;
+
+ protected:
+  using Primitive::point;
+  BasicPoint3d& point() { return data()->point; }
+};
+
+//! @brief A mutable 3d point.
+//! @ingroup PointPrimitives
+//! @ingroup Primitives
+class Point3d : public Point2d {
+ public:
+  using BasicPoint = BasicPoint3d;
+  using ConstType = ConstPoint3d;
+  using MutableType = Point3d;
+  Point3d() = default;
+  static constexpr traits::Dimensions Dimension = traits::Dimensions::Three;
+
+  using Point2d::Point2d;
+
+  //! constructs a 3D from a 2D point. No data is lost in that process.
+  explicit Point3d(const Point2d& other) : Point2d(other) {}
+
+  using Primitive::x;
+  using Primitive::y;
+  using Primitive::z;
+
+  operator BasicPoint2d() const noexcept = delete;
+
+  //! Implicit cast to a mutable BasicPoint3d
+  operator BasicPoint3d&() noexcept { return point(); }  // NOLINT
+
+  //! Implicit cast to an immutable basicPoint3d
+  operator const BasicPoint3d&() const noexcept { return point(); }  // NOLINT
+
+  //! Get a mutable reference to the internal 3d point
+  BasicPoint& basicPoint() noexcept { return point(); }
+
+  //! Get an immutable reference to the internal 3d point
+  const BasicPoint& basicPoint() const noexcept { return point(); }
+
+  //! gets a mutable reference to the x coordinate
+  double& x() noexcept { return point().x(); }
+
+  //! gets a mutable reference to the y coordinate
+  double& y() noexcept { return point().y(); }
+
+  //! gets a mutable reference to the z coordinate
+  double& z() noexcept { return point().z(); }
+};
+
+inline std::ostream& operator<<(std::ostream& stream, const ConstPoint2d& obj) {
+  return stream << "[id: " << obj.id() << " x: " << obj.x() << " y: " << obj.y() << "]";
+}
+
+inline std::ostream& operator<<(std::ostream& stream, const ConstPoint3d& obj) {
+  return stream << "[id: " << obj.id() << " x: " << obj.x() << " y: " << obj.y() << " z: " << obj.z() << "]";
+}
+
+inline std::ostream& operator<<(std::ostream& stream, const Point2d& obj) {
+  return stream << "[id: " << obj.id() << " x: " << obj.x() << " y: " << obj.y() << "]";
+}
+
+inline std::ostream& operator<<(std::ostream& stream, const Point3d& obj) {
+  return stream << "[id: " << obj.id() << " x: " << obj.x() << " y: " << obj.y() << " z: " << obj.z() << "]";
+}
+
+namespace traits {
+template <typename T>
+constexpr bool isPointT() {
+  return isCategory<T, traits::PointTag>();
+}
+}  // namespace traits
+template <typename T, typename RetT>
+using IfPT = std::enable_if_t<traits::isPointT<T>(), RetT>;
+
+}  // namespace lanelet
+
+// Hash function for usage in containers
+namespace std {
+template <>
+struct hash<lanelet::Point3d> : public lanelet::HashBase<lanelet::Point3d> {};
+template <>
+struct hash<lanelet::ConstPoint3d> : public lanelet::HashBase<lanelet::ConstPoint3d> {};
+template <>
+struct hash<lanelet::Point2d> : public lanelet::HashBase<lanelet::Point2d> {};
+template <>
+struct hash<lanelet::ConstPoint2d> : public lanelet::HashBase<lanelet::ConstPoint2d> {};
+}  // namespace std

src/map/lanelet2/include/lanelet2_core/primitives/Polygon.h

@@ -0,0 +1,411 @@
+#pragma once
+
+#include "lanelet2_core/Forward.h"
+#include "lanelet2_core/primitives/LineString.h"
+
+namespace lanelet {
+
+/**
+ * @brief Primitive 2d polygon with basic points
+ * @ingroup PolygonPrimitives
+ *
+ *
+ * This just a normal vector with normal points. We inherit from it, because we
+ * need to create a new type that will be recognized as polygon by
+ * boost::geometry (and not as linestring).
+ */
+class BasicPolygon2d : public BasicLineString2d {
+ public:
+  explicit BasicPolygon2d(const BasicLineString2d& other) : BasicLineString2d(other) {}
+  using TwoDType = BasicPolygon2d;
+  using ThreeDType = BasicPolygon3d;
+  using HybridType = BasicPolygon2d;
+  using PointType = BasicPoint2d;
+  using BasicLineString2d::BasicLineString2d;
+  BasicPolygon2d() = default;
+};
+
+/**
+ * @brief Primitive 3d polygon with basic points
+ * @ingroup PolygonPrimitives
+ *
+ *
+ * This just a normal vector with normal points. We inherit from it, because we
+ * need to create a new type that will be recognized as polygon by
+ * boost::geometry (and not as linestring).
+ */
+class BasicPolygon3d : public BasicLineString3d {
+ public:
+  explicit BasicPolygon3d(const BasicLineString3d& other) : BasicLineString3d(other) {}
+  using TwoDType = BasicPolygon2d;
+  using ThreeDType = BasicPolygon3d;
+  using HybridType = BasicPolygon3d;
+  using PointType = BasicPoint3d;
+  using BasicLineString3d::BasicLineString3d;
+  BasicPolygon3d() = default;
+};
+
+//! @defgroup PolygonPrimitives Polygon
+//! @ingroup Primitives
+//!
+//! ## General
+//! Polygons are very similar to LineStringPrimitives. The only difference is
+//! their interpretation: LineStrings are a one-dimensional structure, Polygons
+//! are two-dimensional. They are assumed to be in clockwise order. An implicit
+//! edge is assumend between the last point and the first point of the polygon.
+//!
+//! These polygons are second-class citisens of lanelet2. Most of the time, you
+//! will use Area instead. Their only role is for an annotation of a region in
+//! the map.
+//!
+//! ## Design
+//! The design of a Polygon is completely identical to LineStrings. They even
+//! share the same data structures and are directily convertible. The only
+//! difference is that polygons can not be inverted. This would not make sense.
+//!
+//! As LineStrings and Polygons share the same data object, converting between
+//! the two is as cheap as copying a shared_ptr.
+//!
+//! ## Compound versions
+//! Similar to the CompoundLineString3d / CompoundLineString2d / ... classes,
+//! CompoundPolygon2d / CompoundPolygon3d exists that allows to combine multiple
+//! LineStrings3d to one closed polygon. They act like a normal polygon but
+//! internally access the data of multiple line strings.
+
+/**
+ * @brief An immutable *clockwise* oriented, *open* (ie start point != end
+ * point) polygon in 2d
+ * @ingroup PolygonPrimitives
+ * @ingroup ConstPrimitives
+ */
+class ConstPolygon2d : public ConstLineStringImpl<Point2d> {
+ public:
+  using ConstLineStringImpl::ConstLineStringImpl;
+  using ConstType = ConstPolygon2d;
+  using MutableType = Polygon2d;
+  using TwoDType = ConstPolygon2d;
+  using ThreeDType = ConstPolygon3d;
+  using Category = traits::PolygonTag;
+  using HybridType = ConstHybridPolygon2d;
+
+  /**
+   * @brief Conversion from/to ConstLineString2d
+   *
+   * This does not make sure that the polygon is valid and not
+   * self-intersecting! If you want to ensure this, use boost::polygon's
+   * is_valid.
+   */
+  explicit ConstPolygon2d(const ConstLineString2d& other) : ConstLineStringImpl(other) {}
+  explicit operator ConstLineString2d() const { return ConstLineString2d(constData(), inverted()); }
+
+  ConstPolygon2d() = default;
+
+  //! create a simple 2d polygon from this (just a vector)
+  BasicPolygon2d basicPolygon() const { return {basicBegin(), basicEnd()}; }
+
+  //! implicit conversion to a basic polygon in 2d
+  operator BasicPolygon2d() const {  // NOLINT
+    return {basicBegin(), basicEnd()};
+  }
+
+  //! Returns the number of (geometrically valid) segments.
+  size_t numSegments() const noexcept { return size() < 2 ? 0 : size(); }
+};
+
+/**
+ * @brief An immutable *clockwise* oriented, *open* (ie start point != end
+ * point) polygon in 3d
+ * @ingroup PolygonPrimitives
+ * @ingroup ConstPrimitives
+ */
+class ConstPolygon3d : public ConstLineStringImpl<Point3d> {
+ public:
+  using ConstLineStringImpl::ConstLineStringImpl;
+  using ConstType = ConstPolygon3d;
+  using MutableType = Polygon3d;
+  using TwoDType = ConstPolygon2d;
+  using ThreeDType = ConstPolygon3d;
+  using Category = traits::PolygonTag;
+  using HybridType = ConstHybridPolygon3d;
+
+  ConstPolygon3d() = default;
+
+  //! Conversion from ConstLineString3d. Does not ensure validity of the
+  //! polygon!
+  explicit ConstPolygon3d(const ConstLineString3d& other) : ConstLineStringImpl(other) {}
+  explicit operator ConstLineString3d() const { return ConstLineString3d(constData(), inverted()); }
+
+  //! create a simple 3d polygon from this (just a vector)
+  BasicPolygon3d basicPolygon() const { return {basicBegin(), basicEnd()}; }
+
+  //! implicit conversion to a basic polygon
+  operator BasicPolygon3d() const {  // NOLINT
+    return {basicBegin(), basicEnd()};
+  }
+
+  //! Returns the number of (geometrically valid) segments.
+  size_t numSegments() const noexcept { return size() < 2 ? 0 : size(); }
+};
+
+/**
+ * @brief A mutable *clockwise* oriented, *open* (ie start point != end point)
+ * polygon in 3d
+ * @ingroup PolygonPrimitives
+ * @ingroup Primitives
+ */
+class Polygon3d : public LineStringImpl<ConstPolygon3d> {
+ public:
+  using LineStringImpl::LineStringImpl;
+  using ConstType = ConstPolygon3d;
+  using MutableType = Polygon3d;
+  using TwoDType = Polygon2d;
+  using ThreeDType = Polygon3d;
+
+  Polygon3d() = default;  // gcc5 needs this
+
+  //! Conversion from LineString3d. Does not ensure validity of the polygon!
+  explicit Polygon3d(const LineString3d& other) : LineStringImpl(other) {}
+  explicit operator LineString3d() const { return LineString3d(data(), inverted()); }
+
+  friend class Polygon2d;
+
+  //! create a simple 3d polygon from this (just a vector)
+  BasicPolygon3d basicPolygon() const { return {basicBegin(), basicEnd()}; }
+
+  //! implicit conversion to a basic polygon
+  operator BasicPolygon3d() const {  // NOLINT
+    return {basicBegin(), basicEnd()};
+  }
+};
+
+/**
+ * @brief An mutable *clockwise* oriented, *open* (ie start point != end
+ * point) polygon in 2d
+ * @ingroup PolygonPrimitives
+ * @ingroup Primitives
+ */
+class Polygon2d : public LineStringImpl<ConstPolygon2d> {
+ public:
+  using LineStringImpl::LineStringImpl;
+  using ConstType = ConstPolygon2d;
+  using MutableType = Polygon2d;
+  using TwoDType = Polygon2d;
+  using ThreeDType = Polygon3d;
+
+  Polygon2d() = default;
+
+  //! Conversion from LineString2d. Does not ensure validity of the polygon!
+  explicit Polygon2d(const LineString2d& other) : LineStringImpl(other) {}
+  explicit operator LineString2d() const { return LineString2d(data(), inverted()); }
+
+  /**
+   * @brief create a simple 2d polygon from this (just a vector)
+   */
+  inline BasicPolygon2d basicPolygon() const {
+    BasicPolygon2d::allocator_type alloc;  // gcc produces undef refs otherwise
+    return BasicPolygon2d(basicBegin(), basicEnd(), alloc);
+  }
+  inline operator BasicPolygon2d() const {  // NOLINT
+    return BasicPolygon2d(basicBegin(), basicEnd());
+  }
+  friend class Polygon3d;
+};
+
+/**
+ * @brief Polygon with access to primitive points
+ * @ingroup PolygonPrimitives
+ * @ingroup ConstPrimitives
+ *
+ *
+ * This polygon tries to behave as close to a BasicPolygon3d as possible
+ * while still keeping all properties (id, attributes) of a ConstPolygon. This
+ * is important as some functions of boost::geometry do not like our ConstPoint
+ * type.
+ *
+ * As with all lanelet primitives, conversion from other primitives is cheap.
+ */
+class ConstHybridPolygon3d : public ConstPolygon3d {
+ public:
+  using const_iterator = BasicIterator;  // NOLINT
+  using iterator = BasicIterator;        // NOLINT
+
+  using ConstPolygon3d::ConstPolygon3d;
+  using ConstType = ConstHybridPolygon3d;
+  using TwoDType = ConstHybridPolygon2d;
+  using ThreeDType = ConstHybridPolygon3d;
+  using PointType = BasicPoint3d;
+
+  ConstHybridPolygon3d() = default;
+
+  //! Conversion from ConstPolygon3d
+  explicit ConstHybridPolygon3d(const ConstPolygon3d& poly) : ConstPolygon3d(poly) {}
+
+  //! BasicPoint3d Iterator to begin
+  BasicIterator begin() const noexcept { return basicBegin(); }
+
+  //! BasicPoint3d Iterator to past-the-end
+  BasicIterator end() const noexcept { return basicEnd(); }
+
+  //! Get first BasicPoint3d
+  const BasicPointType& front() const noexcept { return ConstPolygon3d::front().basicPoint(); }
+
+  //! Get last BasicPoint3d
+  const BasicPointType& back() const noexcept { return ConstPolygon3d::back().basicPoint(); }
+
+  //! access BasicPoint3d at specific position
+  const BasicPointType& operator[](size_t idx) const noexcept { return ConstPolygon3d::operator[](idx).basicPoint(); }
+};
+
+/**
+ * @brief Polygon with access to primitive points
+ * @ingroup PolygonPrimitives
+ * @ingroup ConstPrimitives
+ *
+ *
+ * This polygon tries to behave as close to a BasicPolygon2d as possible
+ * while still keeping all properties (id, attributes) of a ConstPolygon2d. This
+ * is important as some functions of boost::geometry do not like our
+ * ConstPoint2d type.
+ *
+ * As with all lanelet primitives, conversion from other primitives is cheap.
+ */
+class ConstHybridPolygon2d : public ConstPolygon2d {
+ public:
+  using const_iterator = BasicIterator;  // NOLINT
+  using iterator = BasicIterator;        // NOLINT
+
+  using ConstPolygon2d::ConstPolygon2d;
+  using ConstType = ConstHybridPolygon2d;
+  using TwoDType = ConstHybridPolygon2d;
+  using ThreeDType = ConstHybridPolygon3d;
+  using PointType = BasicPoint2d;
+  using HybridType = ConstHybridPolygon2d;
+
+  ConstHybridPolygon2d() = default;
+
+  //! Conversion from ConstPolygon2d
+  explicit ConstHybridPolygon2d(const ConstPolygon2d& poly) : ConstPolygon2d(poly) {}
+
+  //! BasicPoint2d Iterator to begin
+  BasicIterator begin() const noexcept { return basicBegin(); }
+
+  //! BasicPoint2d Iterator to past-the-end
+  BasicIterator end() const noexcept { return basicEnd(); }
+
+  //! Get first BasicPoint2d
+  BasicPointType front() const noexcept { return ConstPolygon2d::front().basicPoint(); }
+
+  //! Get last BasicPoint2d
+  BasicPointType back() const noexcept { return ConstPolygon2d::back().basicPoint(); }
+
+  //! access element at specific position
+  BasicPointType operator[](size_t idx) const noexcept { return ConstPolygon2d::operator[](idx).basicPoint(); }
+};
+
+/**
+ * @brief A (basic) 2d polygon with holes inside
+ * @ingroup PolygonPrimitives
+ *
+ * This class is thought for geometry calculations,
+ * it has no properties of a normal lanelet primitive.
+ */
+class BasicPolygonWithHoles3d {
+ public:
+  BasicPolygon3d outer;
+  BasicPolygons3d inner;
+};
+
+/**
+ * @brief A (basic) 2d polygon with holes inside
+ * @ingroup PolygonPrimitives
+ *
+ * This class is thought for geometry calculations,
+ * it has no properties of a normal lanelet primitive.
+ */
+class BasicPolygonWithHoles2d {
+ public:
+  BasicPolygon2d outer;
+  BasicPolygons2d inner;
+};
+
+inline std::ostream& operator<<(std::ostream& stream, const ConstPolygon2d& obj) {
+  return stream << ConstLineString2d(obj);
+}
+
+inline std::ostream& operator<<(std::ostream& stream, const ConstPolygon3d& obj) {
+  return stream << ConstLineString2d(obj);
+}
+
+namespace internal {
+
+template <>
+class Converter<const ConstPolygon3d> {
+ public:
+  template <typename T>
+  const ConstPolygon3d& convert(const T& t) const {
+    val_ = t;
+    return val_;
+  }
+
+ private:
+  mutable ConstPolygon3d val_;
+};
+template <>
+class Converter<const ConstPolygon2d> {
+ public:
+  template <typename T>
+  const ConstPolygon2d& convert(const T& t) const {
+    val_ = t;
+    return val_;
+  }
+
+ private:
+  mutable ConstPolygon2d val_;
+};
+}  // namespace internal
+
+namespace traits {
+template <typename T>
+constexpr bool isPolygonT() {
+  return isCategory<T, traits::PolygonTag>();
+}
+
+template <>
+struct PrimitiveTraits<BasicPolygon2d> {
+  using ConstType = BasicPolygon2d;
+  using MutableType = BasicPolygon2d;
+  using TwoDType = BasicPolygon2d;
+  using ThreeDType = BasicPolygon3d;
+  using Category = PolygonTag;
+};
+template <>
+struct PrimitiveTraits<BasicPolygon3d> {
+  using ConstType = BasicPolygon3d;
+  using MutableType = BasicPolygon3d;
+  using TwoDType = BasicPolygon2d;
+  using ThreeDType = BasicPolygon3d;
+  using Category = PolygonTag;
+};
+template <>
+inline BasicPolygon2d to2D<BasicPolygon3d>(const BasicPolygon3d& primitive) {
+  BasicPolygon2d p2d(primitive.size());
+  std::transform(primitive.begin(), primitive.end(), p2d.begin(), utils::to2D<BasicPoint3d>);
+  return p2d;
+}
+}  // namespace traits
+template <typename T, typename RetT>
+using IfPoly = std::enable_if_t<traits::isPolygonT<T>(), RetT>;
+
+}  // namespace lanelet
+
+// Hash function for usage in containers
+namespace std {
+template <>
+struct hash<lanelet::Polygon3d> : public lanelet::HashBase<lanelet::Polygon3d> {};
+template <>
+struct hash<lanelet::ConstPolygon3d> : public lanelet::HashBase<lanelet::ConstPolygon3d> {};
+template <>
+struct hash<lanelet::Polygon2d> : public lanelet::HashBase<lanelet::Polygon2d> {};
+template <>
+struct hash<lanelet::ConstPolygon2d> : public lanelet::HashBase<lanelet::ConstPolygon2d> {};
+}  // namespace std

src/map/lanelet2/include/lanelet2_core/primitives/Primitive.h

@@ -0,0 +1,344 @@
+// this is for emacs file handling -*- mode: c++; c-basic-offset: 2;
+// indent-tabs-mode: nil -*-
+
+#pragma once
+#include <limits>
+#include <utility>
+
+#include "lanelet2_core/Attribute.h"
+#include "lanelet2_core/Exceptions.h"
+#include "lanelet2_core/primitives/Traits.h"
+
+namespace lanelet {
+//! @defgroup DataObjects lanelet data management
+//! All the data of lanelets are managed by these objects. Usually you should
+//! not get in contact with them, as the lanelet Primitives and ConstPrimitives
+//! hide them.
+//!
+//! All data classes represent a unique part of the map and are therefore
+//! noncopyable.
+
+/**
+ * @brief Common data class for all lanelet primitives
+ * @ingroup DataObjects
+ *
+ * This class provides the data that all lanelet primitives have in common: id
+ * and attributes. It is inherited by all other data classes.
+ *
+ * This is only an implementation class and only meant to be derived from, never
+ * instanciated.
+ */
+class PrimitiveData {
+ public:
+  PrimitiveData() noexcept = default;
+  PrimitiveData(PrimitiveData&& rhs) noexcept = default;
+  PrimitiveData& operator=(PrimitiveData&& rhs) noexcept = default;
+  PrimitiveData(const PrimitiveData& rhs) = default;
+  PrimitiveData& operator=(const PrimitiveData& rhs) = default;
+
+  /**
+   * @brief Constructs a PrimitiveData object
+   */
+  explicit PrimitiveData(Id id, AttributeMap attributes = AttributeMap()) : id{id}, attributes{std::move(attributes)} {}
+
+  Id id{InvalId};           //!< Id of this primitive (unique across one map)
+  AttributeMap attributes;  //!< attributes of this primitive
+ protected:
+  ~PrimitiveData() = default;
+};  // class PrimitiveData
+
+//! @defgroup ConstPrimitives Immutable lanelet primitives
+//! All lanelet primitives are defined in a mutable and an immutable version
+//! (see also @ref Primitives). All primitives can be converted to their const
+//! version but not vice versa. Each primitive represents one of lanelet2's
+//! basic types.
+//!
+//! All ConstPrimitives have in common that they usually only have one data
+//! member: a shared pointer to the actual data of the primitve. Copying them is
+//! therefore extremely cheap. This also ensures that if a primitive is
+//! modified, all existing copies will also be affected.
+//!
+//! They have in common that they have a unique ID and attributes in the form of
+//! key-value pairs. They all derive from the ConstPrimitive template class.
+
+/**
+ * @brief Basic Primitive class for all primitives of lanelet2
+ * @tparam Data Type of the data object that this ConstPrimitive holds
+ * @ingroup ConstPrimitive
+ */
+template <typename Data>
+class ConstPrimitive {
+ public:
+  using DataType = Data;
+  static constexpr bool IsConst = true;
+  /**
+   * @brief Construct from a pointer to the data
+   * @param data internal data for this primitive. Must not be null.
+   */
+  explicit ConstPrimitive(const std::shared_ptr<const Data>& data) : constData_{data} {
+    if (!data) {
+      throw lanelet::NullptrError("Nullptr passed to constructor!");
+    }
+  }
+
+  // Comparison
+  bool operator==(const ConstPrimitive& rhs) const { return this->constData_ == rhs.constData_; }
+  bool operator!=(const ConstPrimitive& rhs) const { return !(*this == rhs); }
+
+  //! get the attributes of this primitive
+  const AttributeMap& attributes() const { return constData_->attributes; }
+
+  //! get the unique id of this primitive
+  /**
+   * Keep in mind that the Id can also be InvalId, if the element is a temporary
+   * structure that is not a part of the map.
+   */
+  Id id() const noexcept { return constData_->id; }
+
+  //! check whether this primitive has a specific attribute
+  bool hasAttribute(const std::string& name) const noexcept { return attributes().find(name) != attributes().end(); }
+
+  //! check for an attribute (enum version)
+  /**
+   * The enum version provides a much more efficient access to the data. It is
+   * basically the difference between access via a vector and a std::map lookup.
+   * However, this kind of access only works for the most common attribte names.
+   */
+  bool hasAttribute(AttributeName name) const noexcept { return attributes().find(name) != attributes().end(); }
+
+  /**
+   * @brief retrieve an attribute
+   * @throws NoSuchAttributeError if it does not exist
+   */
+  const Attribute& attribute(const std::string& name) const {
+    try {
+      return attributes().at(name);
+    } catch (std::out_of_range& err) {
+      throw NoSuchAttributeError(err.what());
+    }
+  }
+
+  /**
+   * @brief retrieve an attribute (enum version)
+   * @throws NoSuchAttributeError if it does not exist
+   */
+  const Attribute& attribute(AttributeName name) const {
+    try {
+      return attributes().at(name);
+    } catch (std::out_of_range& err) {
+      throw NoSuchAttributeError(err.what());
+    }
+  }
+
+  /**
+   * @brief retrieve an attribute (string version)
+   * @param name name of the attribute
+   * @param defaultVal value returned if not existing
+   * @tparam T return type (must be one of Attribute::as<T>())
+   *
+   * T can also be an Optional so that you get an empty optional if the value
+   * did not exist or could not be converted.
+   */
+  template <typename T>
+  T attributeOr(const std::string& name, T defaultVal) const noexcept {
+    auto elem = attributes().find(name);
+    if (elem == attributes().end()) {
+      return defaultVal;
+    }
+    auto val = elem->second.template as<T>();
+    if (!!val) {
+      return *val;
+    }
+    return defaultVal;
+  }
+
+  /**
+   * @brief retrieve an attribute (enum version)
+   * @param name name of the attribute
+   * @param defaultVal value returned if not existing
+   * @tparam T return type (must be one of Attribute::as<T>())
+   *
+   * T can also be an Optional so that you get an empty optional if the value
+   * did not exist or could not be converted.
+   */
+  template <typename T>
+  T attributeOr(AttributeName name, T defaultVal) const {
+    auto elem = attributes().find(name);
+    if (elem == attributes().end()) {
+      return defaultVal;
+    }
+    auto val = elem->second.template as<T>();
+    if (!!val) {
+      return *val;
+    }
+    return defaultVal;
+  }
+
+  //! get the internal data of this primitive
+  const std::shared_ptr<const Data>& constData() const { return constData_; }
+
+ protected:
+  // This class is only an implementation class and should never be instanciated
+  ConstPrimitive(ConstPrimitive&& rhs) noexcept = default;
+  ConstPrimitive& operator=(ConstPrimitive&& rhs) noexcept = default;
+  ConstPrimitive(const ConstPrimitive& rhs) = default;
+  ConstPrimitive& operator=(const ConstPrimitive& rhs) = default;
+  ~ConstPrimitive() noexcept = default;
+
+ private:
+  std::shared_ptr<const Data> constData_;  //!< the data this primitive holds
+
+};  // class ConstPrimitive
+
+//! @defgroup Primitives Lanelet Primitives
+//!
+//! All primitives are first-class citizens of lanelet2. Unlike \ref
+//! ConstPrimitives they provide functionality to not only view the data but
+//! also modify it.
+//!
+//! ## Design concepts
+//! All primitives are split in a const and a non-const version that both have a
+//! std::shared_ptr to the actual data. This means that a primitive itself is
+//! very lightweight. It also means that all copys of a primitive share the same
+//! data and are therefore
+//!
+//! ## Geometry
+//! All primitives are registered with boost::geometry (see also the
+//! lanelet::geometry). This means that they can be passed directly to
+//! boost::geometry functions.
+//!
+//! With some exceptions: Some of boost::geometry's functions require objects to
+//! contain mutable data. This is not possible for lanelet primitives. If you
+//! see errors complaining about a missing "set" function, you hit one of those
+//! functions. For these functions, you have to pass *hybrid* primitives (see
+//! ConstHybridLineString3d).
+//!
+//! All primitives exist in a 2d and a 3d version and can be converted cheaply
+//! and without data loss in both directions (see also lanelet::traits::to2D and
+//! lanelet::traits::to3d). This works thanks to the fact that they still
+//! reference the same PrimitiveData object.
+//!
+//! ## Modification
+//! Be aware that some parts of lanelet2 cache data about others, e.g. Lanelet
+//! and LaneletMap. Modification is not propagated upwards in the lanelet
+//! hierarchy. E.g. If you modify the coordinates of a Point, the centerline of
+//! a lanelet relying on this point will not be updated. You have to reset these
+//! caches by yourself.
+//!
+//! If you intend to never modify parts of a LaneletMap, better work with
+//! \ref ConstPrimitives. All primitives can be converted to a const version.
+//! See also lanelet::traits::toConst.
+//!
+//! ## Thread safety
+//! No thread safety is guaranteed when primitives are modified. You have to
+//! ensure thread safety yourself. Concurrent reads are always thread safe.
+
+/**
+ * @brief Base class for all mutable Primitives of lanelet2.
+ * @tparam DerivedConstPrimitive the ConstPrimitive class that this class
+ * derives from
+ * @ingroup Primitive
+ */
+template <typename DerivedConstPrimitive>
+class Primitive : public DerivedConstPrimitive {
+ public:
+  Primitive() = default;  // not inherited
+  using DataType = typename DerivedConstPrimitive::DataType;
+  using DerivedConstPrimitive::DerivedConstPrimitive;
+  static constexpr bool IsConst = false;
+
+  // needs a non-const shared_ptr to construct
+  Primitive(const std::shared_ptr<const DataType>&) = delete;
+
+  //! Construct a new primitive from shared_ptr to its data
+  explicit Primitive(const std::shared_ptr<DataType>& data) : DerivedConstPrimitive(data) {
+    if (!data) {
+      throw lanelet::NullptrError("Nullptr passed to constructor!");
+    }
+  }
+
+  //! Construct from another primitive. Only works if both share the same
+  template <typename OtherT>
+  explicit Primitive(const Primitive<OtherT>& rhs) : DerivedConstPrimitive(rhs) {}
+
+  /**
+   * @brief set a new id for this primitive
+   *
+   * This is the best way to corrupt a map, because all primitives are
+   * identified by their id. Make sure you know what you are doing!
+   */
+  void setId(Id id) noexcept { data()->id = id; }
+
+  //! @brief set or overwrite an attribute
+  void setAttribute(const std::string& name, const Attribute& attribute) { attributes()[name] = attribute; }
+
+  //! set or overwrite an attribute (enum version)
+  void setAttribute(AttributeName name, const Attribute& attribute) { attributes()[name] = attribute; }
+
+  // need this for copy construction to work
+  template <typename>
+  friend class Primitive;
+
+  using DerivedConstPrimitive::attributes;
+  //! get the attributes in a mutable way
+  AttributeMap& attributes() noexcept { return data()->attributes; }
+
+ protected:
+  std::shared_ptr<DataType> data() const {
+    // const_pointer_cast is ok. Non-const primitives cannot be created without
+    // a non-const pointer.
+    return std::const_pointer_cast<DataType>(this->constData());
+  }
+
+  // This class is only an implementation class and should never be instanciated
+  Primitive(Primitive&& rhs) noexcept;
+  Primitive& operator=(Primitive&& rhs) noexcept;
+  Primitive(const Primitive& rhs) noexcept;
+  Primitive& operator=(const Primitive& rhs) noexcept;
+  ~Primitive() noexcept = default;  // NOLINT
+};
+
+namespace traits {
+
+namespace internal {
+template <typename PrimitiveT>
+class IsPrimitiveHelper : public std::false_type {};
+template <typename PrimitiveT>
+class IsPrimitiveHelper<ConstPrimitive<PrimitiveT>> : public std::true_type {};
+
+template <typename DataT>
+bool isLaneletPrimitiveHelper(ConstPrimitive<DataT>* /*unused*/, int /*unused*/) {
+  return true;
+}
+
+template <typename NotPrimitive>
+bool isLaneletPrimitiveHelper(NotPrimitive* /*unused*/, long /*unused*/) {  // NOLINT
+  return false;
+}
+}  // namespace internal
+
+template <typename PrimitiveT>
+constexpr bool isLaneletPrimitive() {
+  PrimitiveT* v = nullptr;
+  return IsLaneletPrimitiveHelper(v, 0);
+}
+}  // namespace traits
+
+template <typename PrimitiveT>
+struct HashBase {
+  size_t operator()(const PrimitiveT& x) const noexcept { return std::hash<Id>()(x.id()); }
+};
+
+template <typename DerivedConstPrimitive>
+Primitive<DerivedConstPrimitive>::Primitive(Primitive&& rhs) noexcept = default;
+
+template <typename DerivedConstPrimitive>
+Primitive<DerivedConstPrimitive>& Primitive<DerivedConstPrimitive>::operator=(Primitive&& rhs) noexcept = default;
+
+template <typename DerivedConstPrimitive>
+Primitive<DerivedConstPrimitive>::Primitive(const Primitive& rhs) noexcept = default;
+
+template <typename DerivedConstPrimitive>
+Primitive<DerivedConstPrimitive>& Primitive<DerivedConstPrimitive>::operator=(const Primitive& rhs) noexcept = default;
+
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/primitives/RegulatoryElement.h

@@ -0,0 +1,516 @@
+#pragma once
+
+#include <boost/variant.hpp>
+#include <utility>
+
+#include "lanelet2_core/Forward.h"
+#include "lanelet2_core/primitives/Area.h"
+#include "lanelet2_core/primitives/Lanelet.h"
+#include "lanelet2_core/primitives/LineString.h"
+#include "lanelet2_core/primitives/Point.h"
+#include "lanelet2_core/primitives/Polygon.h"
+#include "lanelet2_core/primitives/Primitive.h"
+#include "lanelet2_core/utility/HybridMap.h"
+
+namespace lanelet {
+//! @defgroup RegulatoryElementPrimitives Regulatory Element
+//! @ingroup Primitives
+//!
+//! ## General
+//! Regulatory Elements define any kind of traffic rule that affects a Lanelet
+//! or an Area. Traffic rules are usually composed by the thing that defined the
+//! rule (usually a traffic sign), and other things that are relevant for the
+//! rule, e.g. a StopLine. These things are called RuleParameter.
+//!
+//! ## Design
+//! Every regulatoryElement has a number of RuleParameters that are implemented
+//! as boost::variants. A RuleParameter can be any lanelet primitive. For
+//! techical reasons (to avoid cyclic shared_ptr issues), Area and Lanelet is
+//! stored using WeakPtrs. If Lanelets go out of scope, this weak ptr will
+//! become invalid. Usually this will not be an issue because LaneletMap still
+//! holds the Lanelets.
+//!
+//! RegulatoryElements are the only primitives that should be referenced by a
+//! shared_ptr because they make use of dynamic inheritance.
+//!
+//! ### RoleNames
+//! For faster access, roles can be queried using the RoleName enum (this works
+//! similarly to Attribute). Instead of a std::map lookup, this is only a
+//! std::vector operation.
+//!
+//! ## Adding new RegulatoryElements
+//! All RegulatoryElements inherit from a common, abstract, immutable base
+//! class: The RegulatoryElement. New regulatory elements can be added by
+//! inhering from it, implementing whatever needs to be implemented and
+//! registering it with the RegulatoryElementFactory.
+//! Concrete RegulatoryElements are identified by their subtype attribute.
+//!
+//! ## Generic rules
+//! For corner cases, a GenericRegulatoryElement is offered which is mutable and
+//! can be used to model any yet unknown traffic rule.
+//! Traffic rules should be interpreted by extending the lanele2_traffic_rules
+//! package, preferably for all countries that have this rule.
+
+//! Typical role names within lanelet (for faster lookup)
+enum class RoleName {
+  Refers,      //!< The primitive(s) that are the origin of this rule (ie signs)
+  RefLine,     //!< The referring line where the rule becomes active
+  RightOfWay,  //!< A lanelet that has right of way in a relation
+  Yield,       //!< A lanelet that has to yield
+  Cancels,     //!< primitive(s) that invalidate a rule (eg end of speed zone)
+  CancelLine,  //!< The line from which a rule is invalidated
+};
+
+//! Lists which role strings are mapped to which enum value
+struct RoleNameString {
+  static constexpr const char Refers[] = "refers";
+  static constexpr const char RefLine[] = "ref_line";
+  static constexpr const char Yield[] = "yield";
+  static constexpr const char RightOfWay[] = "right_of_way";
+  static constexpr const char Cancels[] = "cancels";
+  static constexpr const char CancelLine[] = "cancel_line";
+
+  // roles not included in fast lookup
+  static constexpr const char Left[] = "left";
+  static constexpr const char Right[] = "right";
+  static constexpr const char Centerline[] = "centerline";
+  static constexpr const char Inner[] = "inner";
+  static constexpr const char Outer[] = "outer";
+  static constexpr const char Lanelet[] = "lanelet";
+  static constexpr const char RegulatoryElement[] = "regulatory_element";
+
+  using RoleNamesItem = std::pair<const char*, const RoleName>;
+  static constexpr RoleNamesItem Map[]{{Refers, RoleName::Refers},   {RefLine, RoleName::RefLine},
+                                       {Yield, RoleName::Yield},     {RightOfWay, RoleName::RightOfWay},
+                                       {Cancels, RoleName::Cancels}, {CancelLine, RoleName::CancelLine}};
+};
+
+//! We call every element of a rule a "parameter"
+//! A parameter can be any primitive in lanelet2
+using RuleParameter = boost::variant<Point3d, LineString3d, Polygon3d, WeakLanelet, WeakArea>;
+
+//! Const-version of the parameters
+using ConstRuleParameter =
+    boost::variant<ConstPoint3d, ConstLineString3d, ConstPolygon3d, ConstWeakLanelet, ConstWeakArea>;
+
+namespace traits {
+template <>
+struct PrimitiveTraits<RuleParameter> {
+  using DataType = void;
+  using ConstType = ConstRuleParameter;
+  using MutableType = RuleParameter;
+  using TwoDType = void;
+  using ThreeDType = void;
+  using Category = void;
+};
+template <>
+struct PrimitiveTraits<ConstRuleParameter> {
+  using DataType = void;
+  using ConstType = ConstRuleParameter;
+  using MutableType = RuleParameter;
+  using TwoDType = void;
+  using ThreeDType = void;
+  using Category = void;
+};
+
+template <>
+ConstRuleParameter toConst<RuleParameter>(const RuleParameter& primitive);
+}  // namespace traits
+
+//! Multiple parameters can have the same role in a rule (eg traffic_lights)
+using RuleParameters = std::vector<RuleParameter>;
+
+//! Const version for a range of rule parameters
+using ConstRuleParameters = std::vector<ConstRuleParameter>;
+
+//! Rules are stored in a map internally
+using RuleParameterMap = HybridMap<RuleParameters, decltype(RoleNameString::Map)&, RoleNameString::Map>;
+
+//! Rules are stored in a map internally (const version)
+using ConstRuleParameterMap = HybridMap<ConstRuleParameters, decltype(RoleNameString::Map)&, RoleNameString::Map>;
+
+//! @brief Data container for all RegulatoryElement types
+//! @ingroup DataObjects
+class RegulatoryElementData : public PrimitiveData {
+ public:
+  explicit RegulatoryElementData(Id id, RuleParameterMap parameters = RuleParameterMap(),
+                                 const AttributeMap& attributes = AttributeMap())
+      : PrimitiveData(id, attributes), parameters{std::move(parameters)} {}
+  RuleParameterMap parameters;
+};
+
+/**
+ * @brief You can inherit from this visitor to perform an operation on each
+ * parameter of a regulatory element
+ * @see RegulatoryElement::applyVisitor
+ */
+class RuleParameterVisitor : public boost::static_visitor<void> {  // NOLINT
+ public:
+  virtual void operator()(const ConstPoint3d& /*unused*/) {}
+  virtual void operator()(const ConstLineString3d& /*unused*/) {}
+  virtual void operator()(const ConstPolygon3d& /*unused*/) {}
+  virtual void operator()(const ConstWeakLanelet& /*unused*/) {}
+  virtual void operator()(const ConstWeakArea& /*unused*/) {}
+  virtual ~RuleParameterVisitor() = default;
+  std::string role;  //!< applyVisitor will set the current role here
+};
+
+namespace internal {
+class MutableParameterVisitor : public boost::static_visitor<void> {  // NOLINT
+ public:
+  virtual void operator()(const Point3d /*unused*/&) = 0;
+  virtual void operator()(const LineString3d& /*unused*/) = 0;
+  virtual void operator()(const Polygon3d& /*unused*/) = 0;
+  virtual void operator()(const WeakLanelet& /*unused*/) = 0;
+  virtual void operator()(const WeakArea& /*unused*/) = 0;
+  virtual ~MutableParameterVisitor() = default;
+  std::string role;  //!< applyVisitor will set the current role here
+};
+}  // namespace internal
+
+//! @brief A general rule or limitation for a lanelet (abstract base class)
+//! @ingroup RegulatoryElementPrimitives
+//! @ingroup ConstPrimitives
+class RegulatoryElement  // NOLINT
+    : public ConstPrimitive<RegulatoryElementData>,
+      private boost::noncopyable {
+ public:
+  using ConstType = RegulatoryElement;
+  using MutableType = GenericRegulatoryElement;
+  using TwoDType = RegulatoryElement;
+  using ThreeDType = RegulatoryElement;
+  using Category = traits::RegulatoryElementTag;
+  using const_iterator = RuleParameterMap::const_iterator;  // NOLINT
+  using iterator = RuleParameterMap::iterator;              // NOLINT
+  static constexpr char RuleName[] = "basic_regulatory_element";
+
+  virtual ~RegulatoryElement();
+
+  /*
+   * @brief set a new id for this primitive
+   *
+   * This is the best way to corrupt a map, because all primitives are
+   * identified by their id. Make sure you know what you are doing!
+   */
+  void setId(Id id) noexcept { data()->id = id; }
+
+  //! Returns all parameters as const object (coversion overhead for const)
+  ConstRuleParameterMap getParameters() const;
+
+  //! Returns a vector of all RuleParameters that could be converted to T.
+  template <typename T>
+  std::vector<T> getParameters(const std::string& role) const {
+    static_assert(traits::isConst<T>(), "You must pass a const primitive type");
+    auto it = constData()->parameters.find(role);
+    if (it == constData()->parameters.end()) {
+      return {};
+    }
+    return utils::getVariant<T>(it->second);
+  }
+
+  //! Returns a vector of all RuleParameters that could be converted to T (enum
+  //! version).
+  template <typename T>
+  std::vector<T> getParameters(RoleName role) const {
+    static_assert(traits::isConst<T>(), "You must pass a const primitive type");
+    auto it = constData()->parameters.find(role);
+    if (it == constData()->parameters.end()) {
+      return {};
+    }
+    return utils::getVariant<T>(it->second);
+  }
+
+  //! returns all the roles this regulatory element has
+  std::vector<std::string> roles() const {
+    return utils::transform(parameters(), [](const auto& elem) { return elem.first; });
+  }
+
+  //! Finds a parameter by its id, independent of the role
+  template <typename T>
+  Optional<T> find(Id id) const;
+
+  //! returns true if this object contains no parameters
+  bool empty() const { return constData()->parameters.empty(); }
+
+  //! get the number of roles in this regulatoryElement
+  size_t size() const { return constData()->parameters.size(); }
+
+  //! applies a visitor to every parameter in the regulatory element
+  void applyVisitor(RuleParameterVisitor& visitor) const;
+
+ protected:
+  const_iterator begin() const { return constData()->parameters.begin(); }
+  const_iterator end() const { return constData()->parameters.end(); }
+  void applyVisitor(internal::MutableParameterVisitor& visitor) const;
+  const RuleParameterMap& parameters() const { return constData()->parameters; }
+  RuleParameterMap& parameters() { return std::const_pointer_cast<RegulatoryElementData>(constData())->parameters; }
+  RegulatoryElementDataPtr data() { return std::const_pointer_cast<RegulatoryElementData>(constData()); }
+  template <typename T>
+  std::vector<T> getParameters(RoleName role) {
+    auto it = data()->parameters.find(role);
+    if (it == data()->parameters.end()) {
+      return {};
+    }
+    return utils::getVariant<T>(it->second);
+  }
+
+  friend class RegulatoryElementFactory;
+  friend class LaneletMap;  // Needs access to add all parameters of a regElem
+  explicit RegulatoryElement(Id id = InvalId, const RuleParameterMap& members = RuleParameterMap(),
+                             const AttributeMap& attributes = AttributeMap())
+      : ConstPrimitive(std::make_shared<RegulatoryElementData>(id, members, attributes)) {}
+  explicit RegulatoryElement(const RegulatoryElementDataPtr& data) : ConstPrimitive(data) {}
+};
+
+/**
+ * @brief A GenericRegulatoryElement can hold any parameters.
+ * @ingroup RegulatoryElementPrimitives
+ * @ingroup Primitives
+ */
+class GenericRegulatoryElement final : public Primitive<RegulatoryElement> {
+ public:
+  static constexpr char RuleName[] = "regulatory_element";
+  explicit GenericRegulatoryElement(const RegulatoryElementDataPtr& data)
+      : Primitive<lanelet::RegulatoryElement>(data) {}
+
+  //! Construct generically from id, parameters and attributes
+  explicit GenericRegulatoryElement(Id id = InvalId, const RuleParameterMap& parameters = RuleParameterMap(),
+                                    const AttributeMap& attributes = AttributeMap())
+      : Primitive<lanelet::RegulatoryElement>(id, parameters, attributes) {}
+
+  //! Add a (mutable) primitive to the regulatory element
+  template <typename PrimitiveT>
+  void addParameter(const std::string& role, const PrimitiveT& primitive);
+
+  //! Add a (mutable) primitive (RoleName version)
+  template <typename PrimitiveT>
+  void addParameter(RoleName role, const PrimitiveT& primitive);
+
+  using Primitive<RegulatoryElement>::getParameters;
+
+  //! getter for all parameters of a regulatory element.
+  RuleParameterMap& parameters() noexcept { return data()->parameters; }
+};
+
+namespace traits {
+template <>
+inline Id getId<RegulatoryElementPtr>(const RegulatoryElementPtr& prim) {
+  return prim->id();
+}
+
+template <>
+inline Id getId<RegulatoryElementConstPtr>(const RegulatoryElementConstPtr& prim) {
+  return prim->id();
+}
+
+//! Extracts the id of a rule parameter
+template <>
+Id getId<RuleParameter>(const RuleParameter& prim);
+
+template <>
+Id getId<ConstRuleParameter>(const ConstRuleParameter& prim);
+}  // namespace traits
+
+namespace utils {
+/**
+ * @brief returns true if element of a regulatory element has a matching Id
+ * @param regElem the element holding other primitives
+ * @param id id to look for
+ * @return true if the primitive has such an element
+ *
+ * This function does not look for the id of the element, only its members
+ * Works for linestrings and polylines.
+ * A similar implementation exists for linestrings and lanelets.
+ */
+bool has(const RegulatoryElement& regElem, Id id);
+inline bool has(const RegulatoryElementConstPtr& ls, Id id) { return has(*ls, id); }
+}  // namespace utils
+
+/**
+ * @brief Creates regulatory elements based on their type
+ */
+class RegulatoryElementFactory {
+ public:
+  using FactoryFcn = std::function<RegulatoryElementPtr(const RegulatoryElementDataPtr&)>;
+  void registerStrategy(const std::string& strategy, const FactoryFcn& factoryFunction) {
+    registry_[strategy] = factoryFunction;
+  }
+
+  /**
+   * @brief create a regulatory element based on the name of the rule
+   * @param ruleName RuleName of the regulatory element to create. If empty, a
+   * GenericRegulatoryElement will be returned.
+   * @param data the data with which to create the RegulatoryElement
+   * @throws InvalidInputError if the object could not be created (e.g. if
+   * requisites for the specific type are not met or ruleName does not exist)
+   * @return a matching instance of the regulatory element.
+   *
+   * The factory will make sure that the subtype tag of the returned object
+   * matches the ruleName as is required by liblanelet.
+   */
+  static RegulatoryElementPtr create(std::string ruleName, const RegulatoryElementDataPtr& data);
+
+  static RegulatoryElementPtr create(const std::string& ruleName, Id id, const RuleParameterMap& map,
+                                     const AttributeMap& attributes = AttributeMap()) {
+    return create(ruleName, std::make_shared<RegulatoryElementData>(id, map, attributes));
+  }
+
+  //! returns regulatory element names that this factory can handle
+  static std::vector<std::string> availableRules();
+
+  static RegulatoryElementFactory& instance();
+
+ private:
+  RegulatoryElementFactory() = default;
+  std::map<std::string, FactoryFcn> registry_;
+};
+
+/**
+ * @brief template class for registering new RegulatoryElements.
+ *
+ * The parser will determine which rule to pick based on the "rule" tag of a
+ * RegulatoryElement. If no rule matches, a GenericRegulatoryElement will be
+ * created.
+ *
+ * To register a class, put
+ * RegisterRegulatoryElement<MyClass> regMyClass;
+ * somewhere in your cpp file.
+ *
+ * Your class is required to have a constructor that takes a
+ * RegulatoryElementDataPtr as argument. The constructor of this class is
+ * allowed to throw when the data passed to it is invalid.
+ */
+template <class T>
+class RegisterRegulatoryElement {
+ public:
+  RegisterRegulatoryElement() {
+    static_assert(!utils::strequal(T::RuleName, "basic_regulatory_element"),
+                  "You did not provide a RuleName for your regulatoryElement!");
+    RegulatoryElementFactory::instance().registerStrategy(
+        T::RuleName, [](const RegulatoryElementDataPtr& data) -> RegulatoryElementPtr {
+          return std::shared_ptr<T>(new T(data));  // have to use new because of friendship
+        });
+  }
+};
+
+template <typename PrimitiveT>
+void GenericRegulatoryElement::addParameter(const std::string& role, const PrimitiveT& primitive) {
+  parameters()[role].push_back(primitive);
+}
+
+template <typename PrimitiveT>
+void GenericRegulatoryElement::addParameter(RoleName role, const PrimitiveT& primitive) {
+  parameters()[role].push_back(primitive);
+}
+
+template <typename T>
+Optional<T> RegulatoryElement::find(Id id) const {
+  static_assert(traits::isConst<T>(), "You must pass a const primitive type!");
+  using MutableT = traits::MutablePrimitiveType<T>;
+  for (const auto& params : parameters()) {
+    for (const auto& elem : params.second) {
+      auto telem = boost::get<MutableT>(&elem);
+      if (telem && telem->id() == id) {
+        return *telem;
+      }
+    }
+  }
+  return {};
+}
+
+template <>
+inline Optional<ConstRuleParameter> RegulatoryElement::find(Id id) const {
+  for (const auto& params : parameters()) {
+    for (const auto& elem : params.second) {
+      if (utils::getId(elem) == id) {
+        return traits::toConst(elem);
+      }
+    }
+  }
+  return {};
+}
+
+template <>
+inline boost::optional<ConstLanelet> RegulatoryElement::find<ConstLanelet>(Id id) const {
+  for (const auto& params : parameters()) {
+    for (const auto& elem : params.second) {
+      const auto* telem = boost::get<WeakLanelet>(&elem);
+      if (telem != nullptr && !telem->expired() && telem->lock().id() == id) {
+        return telem->lock();
+      }
+    }
+  }
+  return {};
+}
+
+template <>
+inline boost::optional<ConstArea> RegulatoryElement::find<ConstArea>(Id id) const {
+  for (const auto& params : parameters()) {
+    for (const auto& elem : params.second) {
+      const auto* telem = boost::get<WeakArea>(&elem);
+      if (telem != nullptr && !telem->expired() && telem->lock().id() == id) {
+        return telem->lock();
+      }
+    }
+  }
+  return {};
+}
+
+std::ostream& operator<<(std::ostream& stream, const RegulatoryElement& obj);
+
+template <>
+inline std::vector<ConstLanelet> RegulatoryElement::getParameters(const std::string& role) const {
+  auto it = parameters().find(role);
+  if (it == parameters().end()) {
+    return {};
+  }
+  return utils::strong(utils::getVariant<ConstWeakLanelet>(it->second));
+}
+template <>
+inline std::vector<ConstLanelet> RegulatoryElement::getParameters(RoleName role) const {
+  auto it = parameters().find(role);
+  if (it == parameters().end()) {
+    return {};
+  }
+  return utils::strong(utils::getVariant<ConstWeakLanelet>(it->second));
+}
+
+template <>
+inline std::vector<ConstArea> RegulatoryElement::getParameters(const std::string& role) const {
+  auto it = parameters().find(role);
+  if (it == parameters().end()) {
+    return {};
+  }
+  return utils::strong(utils::getVariant<ConstWeakArea>(it->second));
+}
+template <>
+inline std::vector<ConstArea> RegulatoryElement::getParameters(RoleName role) const {
+  auto it = parameters().find(role);
+  if (it == parameters().end()) {
+    return {};
+  }
+  return utils::strong(utils::getVariant<ConstWeakArea>(it->second));
+}
+
+namespace traits {
+template <typename T>
+constexpr bool isRegulatoryElementT() {
+  using DataT = typename PrimitiveTraits<T>::DataType;
+  return std::is_same<DataT, RegulatoryElementData>::value;
+}
+template <>
+struct Owned<RegulatoryElementPtr> {
+  using Type = RuleParameter;
+};
+}  // namespace traits
+
+template <typename T, typename RetT>
+using IfRE = std::enable_if_t<traits::isRegulatoryElementT<T>(), RetT>;
+
+}  // namespace lanelet
+
+// Hash function for usage in containers
+namespace std {
+template <>
+struct hash<lanelet::RegulatoryElement> : public lanelet::HashBase<lanelet::RegulatoryElement> {};
+}  // namespace std

src/map/lanelet2/include/lanelet2_core/primitives/Traits.h

@@ -0,0 +1,207 @@
+#pragma once
+#include "lanelet2_core/Forward.h"
+
+namespace lanelet {
+namespace traits {
+struct PointTag {};              //! Identifies PointPrimitives
+struct BoundingBoxTag {};        //! Identifies bounding boxes
+struct LineStringTag {};         //! Identifies LineStringPrimitives
+struct PolygonTag {};            //! Identifies PolygonPrimitives
+struct LaneletTag {};            //! Identifies LaneletPrimitives
+struct AreaTag {};               //! Identifies AreaPrimitives
+struct RegulatoryElementTag {};  //! Identifies RegulatoryElementPrimitives
+
+/**
+ *  @brief Trait class for querying information on a lanelet primitive
+ *
+ * All lanelet primitives are compatible with this trait class.
+ */
+template <typename T>
+struct PrimitiveTraits {
+  using DataType = typename T::DataType;
+  using ConstType = typename T::ConstType;
+  using MutableType = typename T::MutableType;
+  using TwoDType = typename T::TwoDType;
+  using ThreeDType = typename T::ThreeDType;
+  using Category = typename T::Category;
+};
+
+// specialization for RegulatoryElementPtr
+template <>
+struct PrimitiveTraits<RegulatoryElementPtr> {
+  using DataType = RegulatoryElementData;
+  using ConstType = RegulatoryElementConstPtr;
+  using MutableType = RegulatoryElementPtr;
+  using TwoDType = RegulatoryElementPtr;
+  using ThreeDType = RegulatoryElementPtr;
+  using Category = RegulatoryElementTag;
+};
+template <>
+struct PrimitiveTraits<RegulatoryElementConstPtr> {
+  using DataType = RegulatoryElementData;
+  using ConstType = RegulatoryElementConstPtr;
+  using MutableType = RegulatoryElementPtr;
+  using TwoDType = RegulatoryElementPtr;
+  using ThreeDType = RegulatoryElementPtr;
+  using Category = RegulatoryElementTag;
+};
+
+//! Can be used to determine which primitive type is managed by a primitive
+template <typename PrimitiveT>
+struct Owned {
+  using Type = PrimitiveT;
+};
+
+template <>
+struct Owned<LineString3d> {
+  using Type = Point3d;
+};
+
+template <>
+struct Owned<Polygon3d> {
+  using Type = Point3d;
+};
+
+template <>
+struct Owned<Lanelet> {
+  using Type = LineString3d;
+};
+
+template <>
+struct Owned<Area> {
+  using Type = LineString3d;
+};
+
+template <typename PrimitiveT>
+using OwnedT = typename Owned<PrimitiveT>::Type;
+
+//! Utility for determinig the matching const type for a primitive
+template <typename PrimitiveT>
+using ConstPrimitiveType = typename PrimitiveTraits<PrimitiveT>::ConstType;
+
+//! Utility for determinig the matching mutable type for a primitive
+template <typename PrimitiveT>
+using MutablePrimitiveType = typename PrimitiveTraits<PrimitiveT>::MutableType;
+
+//! Utility for determinig the matching two dimensional type for a primitive
+template <typename PrimitiveT>
+using TwoD = typename PrimitiveTraits<PrimitiveT>::TwoDType;
+
+//! f Utility for determinig the matching three dimensional type for a primitive
+template <typename PrimitiveT>
+using ThreeD = typename PrimitiveTraits<PrimitiveT>::ThreeDType;
+
+//! Converts a primitive to its matching threeD type
+template <typename PrimitiveT>
+auto to3D(const PrimitiveT& primitive) -> ThreeD<PrimitiveT> {
+  return ThreeD<PrimitiveT>(primitive);
+}
+
+//! Converts a primitive to its matching twoD type
+template <typename PrimitiveT>
+auto to2D(const PrimitiveT& primitive) -> TwoD<PrimitiveT> {
+  return TwoD<PrimitiveT>(primitive);
+}
+
+//! Converts a primitive to its matching const type
+template <typename PrimitiveT>
+ConstPrimitiveType<PrimitiveT> toConst(const PrimitiveT& primitive) {
+  return ConstPrimitiveType<PrimitiveT>(primitive);
+}
+
+template <typename PrimitiveT>
+constexpr bool isConst() {
+  return std::is_same<PrimitiveT, ConstPrimitiveType<PrimitiveT>>::value;
+}
+
+//! Checks (at compile time) whether a primitive is two dimensional
+template <typename PrimitiveT>
+constexpr bool is2D() {
+  return std::is_same<std::decay_t<PrimitiveT>, TwoD<PrimitiveT>>::value;
+}
+
+//! Checks (at compile time) whether a primitive is three dimensional
+template <typename PrimitiveT>
+constexpr bool is3D() {
+  return std::is_same<std::decay_t<PrimitiveT>, ThreeD<PrimitiveT>>::value;
+}
+
+enum class Dimensions { Two = 2, Three = 3 };
+
+//! Specialization of traits for points
+template <typename PointT>
+struct PointTraits : PrimitiveTraits<PointT> {
+  using BasicPoint = typename PointT::BasicPoint;
+  using ConstPoint = typename PrimitiveTraits<PointT>::ConstType;
+  using MutablePoint = typename PrimitiveTraits<PointT>::MutableType;
+  static constexpr bool IsPrimitive = true;
+  static constexpr Dimensions Dimension = PointT::Dimension;
+};
+
+//! Specialization of traits for linestrings
+template <typename LineStringT>
+struct LineStringTraits : PrimitiveTraits<LineStringT> {
+  using PointType = typename LineStringT::PointType;
+  using HybridType = typename LineStringT::HybridType;
+};
+
+//! Specialization of traits for polygons
+template <typename PolygonT>
+struct PolygonTraits : PrimitiveTraits<PolygonT> {
+  using PointType = typename PolygonT::PointType;
+  using HybridType = typename PolygonT::HybridType;
+};
+
+template <typename PointT>
+using BasicPointT = typename PointTraits<PointT>::BasicPoint;
+
+template <typename PointT>
+using ConstPointT = typename PointTraits<PointT>::ConstPoint;
+
+template <typename PointT>
+using MutablePointT = typename PointTraits<PointT>::MutablePoint;
+
+template <typename PointT>
+auto toBasicPoint(const PointT& point) -> std::enable_if_t<PointTraits<PointT>::IsPrimitive, BasicPointT<PointT>> {
+  return point.basicPoint();
+}
+template <typename PointT>
+auto toBasicPoint(const PointT& point)
+    -> std::enable_if_t<!PointTraits<PointT>::IsPrimitive && std::is_same<PointT, BasicPointT<PointT>>::value, PointT> {
+  return point;
+}
+template <typename PointT>
+auto toBasicPoint(const PointT& point)
+    -> std::enable_if_t<!PointTraits<PointT>::IsPrimitive && !std::is_same<PointT, BasicPointT<PointT>>::value,
+                        BasicPointT<PointT>> {
+  return BasicPointT<PointT>(point);
+}
+
+template <typename PrimT, typename TagT>
+constexpr bool isCategory() {
+  return std::is_same<typename PrimitiveTraits<std::decay_t<PrimT>>::Category, TagT>::value;
+}
+
+template <typename T>
+constexpr bool noRegelem() {
+  return !traits::isCategory<T, traits::RegulatoryElementTag>();
+}
+
+template <typename LineStringT>
+using PointType = typename LineStringTraits<LineStringT>::PointType;
+
+template <typename PrimT>
+Id getId(const PrimT& prim) {
+  return prim.id();
+}
+}  // namespace traits
+
+namespace utils {
+// make trait based helper functions available in utils namespace
+using traits::getId;
+using traits::to2D;
+using traits::to3D;
+using traits::toBasicPoint;
+using traits::toConst;
+}  // namespace utils
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/utility/CompoundIterator.h

@@ -0,0 +1,140 @@
+#pragma once
+#include <algorithm>
+#include <boost/iterator/iterator_adaptor.hpp>
+#include <iostream>
+
+namespace lanelet {
+namespace internal {
+
+/**
+ * @brief This iterator iterates over a container of containers as if it was one
+ * single container. Succeeding elements that are equal are ignored.
+ *
+ * This container is used by the compound types of lanelet2, e.g.
+ * CompoundLineString2d.
+ *
+ * The iterator is only bidirectional, i.e. it allows no random access in O(0).
+ */
+template <typename ContainerT>
+class UniqueCompoundIterator
+    : public boost::iterator_facade<UniqueCompoundIterator<ContainerT>,
+                                    std::remove_reference_t<decltype(*ContainerT().begin()->begin())>,
+                                    std::random_access_iterator_tag> {
+  using Base = boost::iterator_facade<UniqueCompoundIterator<ContainerT>,
+                                      std::remove_reference_t<decltype(*ContainerT().begin()->begin())>,
+                                      std::random_access_iterator_tag>;
+
+ public:
+  using ItOuter = std::decay_t<decltype(ContainerT().begin())>;
+  using ItInner = std::decay_t<decltype(ItOuter()->begin())>;
+  UniqueCompoundIterator() = default;
+  static UniqueCompoundIterator begin(ContainerT& c) {
+    auto itBegin = firstNonempty(c);
+    if (itBegin != c.end()) {
+      return UniqueCompoundIterator(c, itBegin, itBegin->begin());
+    }
+    return UniqueCompoundIterator(c, itBegin, ItInner());
+  }
+  static UniqueCompoundIterator end(ContainerT& c) { return UniqueCompoundIterator(c, c.end(), {}); }
+
+ private:
+  friend class boost::iterator_core_access;
+  UniqueCompoundIterator(ContainerT& c, ItOuter itOuter, ItInner itInner)
+      : c_{&c}, itOuter_{itOuter}, itInner_{itInner} {}
+  static ItOuter firstNonempty(ContainerT& c) {
+    return std::find_if(c.begin(), c.end(), [](auto& c) { return !c.empty(); });
+  }
+
+  void increment() {
+    auto& old = *(*this);
+    auto endIt = end(*c_);
+    do {
+      incrementOne();
+    } while ((*this) != endIt && old == *(*this));
+  }
+
+  void decrement() {
+    decrementOne();
+    const auto startIt = begin(*c_);
+    if (*this == startIt) {
+      return;
+    }
+    auto next = *this;
+    do {
+      *this = next;
+      next.decrementOne();
+      if (*next != *(*this)) {
+        return;
+      }
+    } while (next != startIt);
+    *this = next;
+  }
+
+  void incrementOne() {
+    if (itOuter_->end() == std::next(itInner_)) {
+      do {
+        ++itOuter_;
+      } while (itOuter_ != c_->end() && itOuter_->empty());
+      if (itOuter_ == c_->end()) {
+        itInner_ = ItInner();
+      } else {
+        itInner_ = itOuter_->begin();
+      }
+    } else {
+      ++itInner_;
+    }
+  }
+
+  void decrementOne() {
+    if (itOuter_ == c_->end() || itInner_ == itOuter_->begin()) {
+      const auto begin = firstNonempty(*c_);
+      do {
+        --itOuter_;
+      } while (itOuter_ != begin && itOuter_->empty());
+      itInner_ = std::prev(itOuter_->end());
+    } else {
+      --itInner_;
+    }
+  }
+
+  void advance(typename Base::difference_type d) {
+    if (d > 0) {
+      for (; d != 0; d--) {
+        increment();
+      }
+    } else {
+      for (; d != 0; d++) {
+        decrement();
+      }
+    }
+  }
+
+  typename Base::difference_type distance_to(  // NOLINT
+      UniqueCompoundIterator other) const {
+    typename Base::difference_type d{};
+    auto cp = *this;
+    if (other.itOuter_ > itOuter_ || (other.itOuter_ == itOuter_ && other.itInner_ > itInner_)) {
+      while (!cp.equal(other)) {
+        cp.increment();
+        d++;
+      }
+    } else {
+      while (!cp.equal(other)) {
+        other.increment();
+        d--;
+      }
+    }
+    return d;
+  }
+
+  bool equal(UniqueCompoundIterator other) const { return itOuter_ == other.itOuter_ && itInner_ == other.itInner_; }
+
+  decltype(auto) dereference() const { return *itInner_; }
+
+  ContainerT* c_{nullptr};
+  ItOuter itOuter_;
+  ItInner itInner_;
+};
+
+}  // namespace internal
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/utility/HybridMap.h

@@ -0,0 +1,242 @@
+#pragma once
+#include <algorithm>
+#include <map>
+#include <ostream>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace lanelet {
+
+namespace detail {
+template <typename ArrayT, ArrayT Arr>
+class ArrayView {
+ public:
+  static constexpr auto begin() { return std::begin(Arr); }
+  static constexpr auto end() { return std::end(Arr); }
+
+  static constexpr auto findValue(const decltype(Arr[0].second)& val) {
+    auto it = begin();
+    for (; it != end(); ++it) {
+      if (it->second == val) {
+        break;
+      }
+    }
+    return it;
+  }
+  static constexpr auto findKey(const decltype(Arr[0].first)& val) {
+    auto it = begin();
+    for (; it != end(); ++it) {
+      if (strcmp(it->first, val) == 0) {
+        break;
+      }
+    }
+    return it;
+  }
+};
+
+template <typename Iterator, typename Map>
+std::vector<Iterator> copyIterators(const std::vector<Iterator>& oldV, const Map& oldM, Map& newM) {
+  std::vector<Iterator> newV(oldV.size(), newM.end());
+  for (auto i = 0u; i < oldV.size(); ++i) {
+    if (oldV[i] != oldM.end()) {
+      newV[i] = newM.find(oldV[i]->first);
+    }
+  }
+  return newV;
+}
+
+template <typename Iterator, typename Vector>
+void replaceIterator(Vector& v, const Iterator& replace, const Iterator& by) {
+  for (auto& elem : v) {
+    if (elem == replace) {
+      elem = by;
+    }
+  }
+}
+}  // namespace detail
+
+/**
+ * @brief A hybrid map is just like a normal map with keys as string, but
+ * elements can also be accessed using an enum for the keys. This is much faster
+ * than using strings for the lookup.
+ * @tparam Enum an enum with *continuous* values. The last element must be named
+ * "End".
+ */
+template <typename ValueT, typename PairArrayT, PairArrayT PairArray>
+class HybridMap {
+  using Array = detail::ArrayView<PairArrayT, PairArray>;
+
+ public:
+  using Map = std::map<std::string, ValueT>;
+  using Vec = std::vector<typename Map::iterator>;
+  using Enum = std::decay_t<decltype(PairArray[0].second)>;
+
+  using key_type = typename Map::key_type;                // NOLINT
+  using mapped_type = typename Map::mapped_type;          // NOLINT
+  using iterator = typename Map::iterator;                // NOLINT
+  using const_iterator = typename Map::const_iterator;    // NOLINT
+  using value_type = typename Map::value_type;            // NOLINT
+  using difference_type = typename Map::difference_type;  // NOLINT
+  using size_type = typename Map::size_type;              // NOLINT
+  HybridMap() noexcept = default;
+  HybridMap(HybridMap&& rhs) noexcept : m_(std::move(rhs.m_)), v_{std::move(rhs.v_)} {
+    // move invalidates no iterators except end
+    detail::replaceIterator(v_, rhs.m_.end(), m_.end());
+  }
+  HybridMap& operator=(HybridMap&& rhs) noexcept {
+    m_ = std::move(rhs.m_);
+    v_ = std::move(rhs.v_);
+    // move invalidates no iterators except end
+    detail::replaceIterator(v_, rhs.m_.end(), m_.end());
+    return *this;
+  }
+  HybridMap(const HybridMap& rhs) : m_{rhs.m_}, v_{detail::copyIterators(rhs.v_, rhs.m_, m_)} {}
+  HybridMap& operator=(const HybridMap& rhs) {
+    m_ = rhs.m_;
+    v_ = detail::copyIterators(rhs.v_, rhs.m_, m_);
+    return *this;
+  }
+  HybridMap(const std::initializer_list<std::pair<const std::string, ValueT>>& list) {
+    for (const auto& item : list) {
+      insert(item);
+    }
+  }
+  template <typename InputIterator>
+  HybridMap(InputIterator begin, InputIterator end) {
+    for (; begin != end; ++begin) {
+      insert(*begin);
+    }
+  }
+
+  ~HybridMap() noexcept = default;
+
+  iterator find(const key_type& k) { return m_.find(k); }
+  iterator find(Enum k) {
+    const auto pos = static_cast<size_t>(k);
+    return v_.size() < pos + 1 ? m_.end() : v_[pos];
+  }
+  const_iterator find(const key_type& k) const { return m_.find(k); }
+  const_iterator find(Enum k) const {
+    const auto pos = static_cast<size_t>(k);
+    return v_.size() < pos + 1 ? m_.end() : v_[pos];
+  }
+
+  iterator begin() { return m_.begin(); }
+  iterator end() { return m_.end(); }
+  const_iterator begin() const { return m_.begin(); }
+  const_iterator end() const { return m_.end(); }
+
+  std::pair<iterator, bool> insert(const value_type& v) {
+    auto it = m_.insert(v);
+    if (it.second) {
+      updateV(it.first);
+    }
+    return it;
+  }
+
+  iterator insert(const_iterator hint, const value_type& v) {
+    auto it = m_.insert(hint, v);
+    updateV(it);
+    return it;
+  }
+
+  iterator erase(const_iterator pos) {
+    auto itV = std::find(v_.begin(), v_.end(), pos);
+    if (itV != v_.end()) {
+      *itV = m_.end();
+    }
+    return m_.erase(pos);
+  }
+
+  size_type erase(const std::string& k) {
+    auto it = find(k);
+    if (it == end()) {
+      return 0;
+    }
+    erase(it);
+    return 1;
+  }
+
+  ValueT& operator[](const key_type& k) {
+    auto it = find(k);
+    if (it == m_.end()) {
+      it = insert({k, mapped_type()}).first;
+    }
+    return it->second;
+  }
+  ValueT& operator[](const Enum& k) {
+    auto it = find(k);
+    auto end = m_.end();
+    if (it == end) {
+      auto attr = Array::findValue(k);
+      it = insert({attr->first, mapped_type()}).first;
+    }
+    return it->second;
+  }
+
+  const ValueT& at(const key_type& k) const {
+    auto it = find(k);
+    if (it == m_.end()) {
+      throw std::out_of_range(std::string("Could not find ") + k);
+    }
+    return it->second;
+  }
+  const ValueT& at(const Enum& k) const {
+    auto it = find(k);
+    if (it == m_.end()) {
+      throw std::out_of_range(std::string("Could not find ") + std::to_string(static_cast<int>(k)));
+    }
+    return it->second;
+  }
+
+  ValueT& at(const key_type& k) {
+    auto it = find(k);
+    if (it == m_.end()) {
+      throw std::out_of_range(std::string("Could not find ") + k);
+    }
+    return it->second;
+  }
+  ValueT& at(const Enum& k) {
+    auto it = find(k);
+    if (it == m_.end()) {
+      throw std::out_of_range(std::string("Could not find ") + std::to_string(static_cast<int>(k)));
+    }
+    return it->second;
+  }
+
+  void clear() { m_.clear(), v_.clear(); }
+
+  bool empty() const { return m_.empty(); }
+  size_t size() const { return m_.size(); }
+
+  auto key_comp() const { return m_.key_comp(); }    // NOLINT
+  auto value_comp() const { return m_.key_comp(); }  // NOLINT
+
+  bool operator==(const HybridMap& other) const { return this->m_ == other.m_; }
+  bool operator!=(const HybridMap& other) const { return !(*this == other); }
+
+ private:
+  void updateV(iterator it) {
+    auto attr = Array::findKey(it->first.c_str());
+    if (attr != std::end(PairArray)) {  // NOLINT
+      const auto pos = static_cast<size_t>(attr->second);
+      if (v_.size() < pos + 1) {
+        v_.resize(pos + 1, m_.end());
+      }
+      v_[pos] = it;
+    }
+  }
+  Map m_;
+  Vec v_;
+};
+
+template <typename Value, typename Enum, const std::pair<const char*, const Enum> Lookup[]>
+std::ostream& operator<<(std::ostream& stream, HybridMap<Value, Enum, Lookup> map) {
+  stream << "{";
+  for (const auto& elem : map) {
+    stream << "{" << elem.first << ": " << elem.second << "}";
+  }
+  return stream << "}";
+}
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/utility/Optional.h

@@ -0,0 +1,9 @@
+#pragma once
+#include <boost/optional.hpp>
+
+namespace lanelet {
+// To be replaced by std::optional once c++17 is established
+template <typename T>
+using Optional = boost::optional<T>;
+
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/utility/ReverseAndForwardIterator.h

@@ -0,0 +1,48 @@
+#pragma once
+#include <boost/iterator/iterator_adaptor.hpp>
+#include <cassert>
+
+namespace lanelet {
+namespace internal {
+
+template <typename IteratorT>
+class ReverseAndForwardIterator : public boost::iterator_adaptor<ReverseAndForwardIterator<IteratorT>, IteratorT> {
+  using Base = boost::iterator_adaptor<ReverseAndForwardIterator<IteratorT>, IteratorT>;
+  friend class boost::iterator_core_access;
+
+ public:
+  using Iterator = IteratorT;
+  using ReverseIterator = std::reverse_iterator<IteratorT>;
+
+  static ReverseAndForwardIterator reversed(Iterator iter) {
+    ReverseAndForwardIterator riter(iter);
+    riter.forward_ = false;
+    return riter;
+  }
+  ReverseAndForwardIterator() = default;
+  ReverseAndForwardIterator(Iterator iter) : Base(iter) {}  // NOLINT
+  ReverseAndForwardIterator(ReverseIterator iter)           // NOLINT
+      : Base(iter.base()), forward_{false} {}
+  operator Iterator() const { return this->base(); }  // NOLINT
+
+  bool reversed() const { return !forward_; }
+  bool forward() const { return forward_; }
+
+ private:
+  void increment() { forward() ? ++iter() : --iter(); }
+  void decrement() { forward() ? --iter() : ++iter(); }
+  void advance(typename Base::difference_type d) { forward() ? iter() += d : iter() -= d; }
+  typename Base::reference dereference() const { return forward_ ? *this->base() : *ReverseIterator(this->base()); }
+
+  template <class OtherIterator>
+  typename Base::difference_type distance_to(  // NOLINT
+      const ReverseAndForwardIterator<OtherIterator>& y) const {
+    assert(forward() == y.forward());
+    auto d = std::distance(this->base(), y.base());
+    return forward() ? d : -d;
+  }
+  inline IteratorT& iter() { return this->base_reference(); }
+  bool forward_{true};
+};
+}  // namespace internal
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/utility/TransformIterator.h

@@ -0,0 +1,70 @@
+#pragma once
+#include <boost/iterator/iterator_adaptor.hpp>
+#include <memory>
+
+namespace lanelet {
+namespace internal {
+template <typename RetType>
+class Converter {
+ public:
+  template <typename InType>
+  RetType& convert(InType& t) const {
+    return static_cast<RetType&>(t);
+  }
+};
+
+template <typename T>
+class Converter<std::shared_ptr<const T>> {
+ public:
+  std::shared_ptr<const T>& convert(std::shared_ptr<T>& t) const {
+    val_ = t;
+    return val_;
+  }
+
+ private:
+  //! @todo make this thread-safe
+  mutable std::shared_ptr<const T> val_;
+};
+
+template <typename T>
+class Converter<const std::shared_ptr<const T>> {
+ public:
+  const std::shared_ptr<const T>& convert(const std::shared_ptr<T>& t) const {
+    val_ = t;
+    return val_;
+  }
+
+ private:
+  mutable std::shared_ptr<const T> val_;
+};
+
+template <typename RetType>
+class PairConverter {
+ public:
+  template <typename PairType>
+  RetType& convert(PairType& p) const {
+    return c_.convert(p.second);
+  }
+
+ private:
+  Converter<RetType> c_;
+};
+
+template <typename Iterator, typename RetType, typename Converter = Converter<RetType>>
+class TransformIterator
+    : public boost::iterator_adaptor<TransformIterator<Iterator, RetType, Converter>, Iterator, RetType> {
+  using Base = boost::iterator_adaptor<TransformIterator<Iterator, RetType, Converter>, Iterator, RetType>;
+  friend class boost::iterator_core_access;
+
+ public:
+  // Init and convert
+  TransformIterator() = default;
+  TransformIterator(Iterator iter) : Base{iter} {}    // NOLINT
+  operator Iterator() const { return this->base(); }  // NOLINT
+
+ private:
+  typename Base::reference dereference() const { return converter_.convert(*this->base()); }
+  Converter converter_;
+};
+}  // namespace internal
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/utility/Units.h

@@ -0,0 +1,57 @@
+#pragma once
+#include <boost/units/base_units/metric/hour.hpp>
+#include <boost/units/base_units/us/mile.hpp>
+#include <boost/units/make_scaled_unit.hpp>
+
+#include "lanelet2_core/Forward.h"
+
+namespace lanelet {
+namespace units {
+using Mile = boost::units::us::mile_base_unit::unit_type;
+using Meter = boost::units::si::meter_base_unit::unit_type;
+using Exp3 = boost::units::scale<10, boost::units::static_rational<3> >;
+using NoUnit = boost::units::si::dimensionless::unit_type;
+using Kilo = boost::units::make_scaled_unit<NoUnit, Exp3>::type;
+using Km = boost::units::multiply_typeof_helper<Kilo, Meter>::type;
+using Second = boost::units::si::second_base_unit::unit_type;
+using Hour = boost::units::metric::hour_base_unit::unit_type;
+using MPH = boost::units::divide_typeof_helper<Mile, Hour>::type;
+using KmH = boost::units::divide_typeof_helper<Km, Hour>::type;
+
+using MeterQuantity = boost::units::quantity<Meter>;
+using SecondQuantity = boost::units::quantity<Second>;
+using KmHQuantity = boost::units::quantity<KmH>;
+using MPHQuantity = boost::units::quantity<MPH>;
+using Distance = MeterQuantity;
+using Time = SecondQuantity;
+
+namespace literals {
+inline Distance operator"" _m(long double d) { return Distance(d * Meter()); }
+inline Distance operator"" _m(unsigned long long int d) {  // NOLINT
+  return Distance(d * Meter());
+}
+
+inline Time operator"" _s(long double s) { return Time(s * Second()); }
+inline Time operator"" _s(unsigned long long int s) {  // NOLINT
+  return Time(s * Second());
+}
+
+inline Velocity operator"" _mps(long double d) { return Velocity{d * MPS()}; }
+inline Velocity operator"" _mps(unsigned long long int d) {  // NOLINT
+  return Velocity{d * MPS()};
+}
+inline Velocity operator"" _kmh(long double d) { return Velocity{d * KmH()}; }
+inline Velocity operator"" _kmh(unsigned long long int d) {  // NOLINT
+  return Velocity{d * KmH()};
+}
+inline Velocity operator"" _mph(long double d) { return Velocity{d * MPH()}; }
+inline Velocity operator"" _mph(unsigned long long int d) {  // NOLINT
+  return Velocity{d * MPH()};
+}
+inline Acceleration operator"" _mps2(long double d) { return Acceleration{d * MPS2()}; }
+inline Acceleration operator"" _mps2(unsigned long long int d) {  // NOLINT
+  return Acceleration{d * MPS2()};
+}
+}  // namespace literals
+}  // namespace units
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_core/utility/Utilities.h

@@ -0,0 +1,371 @@
+#pragma once
+
+#include <algorithm>
+#include <boost/iterator/iterator_adaptor.hpp>
+#include <boost/type_traits/remove_cv.hpp>
+#include <boost/variant/get.hpp>
+#include <memory>
+#include <numeric>
+#include <type_traits>
+#include <vector>
+
+#include "lanelet2_core/Forward.h"
+#include "lanelet2_core/utility/Optional.h"
+
+namespace lanelet {
+
+namespace utils {
+/**
+ * @brief Wrapper around std::accumulate for summing up the result of Func over
+ * a range.
+ */
+template <typename Range, typename Func>
+auto sum(Range&& r, Func f);
+
+namespace detail {
+template <bool Move>
+struct MoveIf {};
+
+template <>
+struct MoveIf<true> {
+  template <typename Iter>
+  auto operator()(Iter it) {
+    return std::make_move_iterator(it);
+  }
+};
+
+template <>
+struct MoveIf<false> {
+  template <typename Iter>
+  auto operator()(Iter it) {
+    return it;
+  }
+};
+
+template <typename ContainerT>
+struct ReserveIfNecessary {
+  void operator()(ContainerT& /*c*/, size_t /*size*/) const {}
+};
+
+template <typename T>
+struct ReserveIfNecessary<std::vector<T>> {
+  void operator()(std::vector<T>& c, size_t size) const { c.reserve(size); }
+};
+
+template <typename VectorT>
+inline VectorT createReserved(size_t size) {
+  VectorT vector;
+  ReserveIfNecessary<VectorT>()(vector, size);
+  return vector;
+}
+
+template <typename VectorT, typename ContainerT>
+VectorT concatenate(ContainerT&& c) {
+  VectorT conced;
+  const auto size = sum(c, [](auto& elem) { return elem.size(); });
+  ReserveIfNecessary<VectorT>()(conced, size);
+  MoveIf<std::is_rvalue_reference<decltype(c)>::value> move;
+  for (auto& elem : c) {
+    conced.insert(conced.end(), move(elem.begin()), move(elem.end()));
+  }
+  return conced;
+}
+
+template <typename VectorT, typename ContainerT, typename Func>
+VectorT concatenate(ContainerT&& c, Func f) {
+  VectorT conced;
+  MoveIf<true> move;
+  for (auto& elem : c) {
+    auto value = f(elem);
+    conced.insert(conced.end(), move(value.begin()), move(value.end()));
+  }
+  return conced;
+}
+
+template <typename ContainerT, typename Func>
+auto concatenateRange(ContainerT&& c, Func f) {
+  auto size = sum(c, [f](auto&& elem) {
+    auto range = f(elem);
+    return std::distance(range.first, range.second);
+  });
+  using RetT = typename decltype(f(*c.begin()).first)::value_type;
+  std::vector<RetT> ret;
+  ret.reserve(size);
+  MoveIf<std::is_rvalue_reference<decltype(c)>::value> move;
+  for (auto&& elem : c) {
+    auto range = f(elem);
+    ret.insert(ret.end(), move(range.first), move(range.second));
+  }
+  return ret;
+}
+
+template <typename ContainerT, typename Func, typename AllocatorT>
+auto concatenateRange(ContainerT&& c, Func f, const AllocatorT& alloc) {
+  auto size = sum(c, [f](auto&& elem) {
+    auto range = f(elem);
+    return std::distance(range.first, range.second);
+  });
+  using RetT = typename decltype(f(*c.begin()).first)::value_type;
+  std::vector<RetT, AllocatorT> ret(alloc);
+  ret.reserve(size);
+  MoveIf<std::is_rvalue_reference<decltype(c)>::value> move;
+  for (auto&& elem : c) {
+    auto range = f(elem);
+    ret.insert(ret.end(), move(range.first), move(range.second));
+  }
+  return ret;
+}
+
+template <typename Container, typename Func>
+auto transform(Container&& c, Func f) {
+  using RetT = std::decay_t<decltype(f(*c.begin()))>;
+  std::vector<RetT> transformed;
+  transformed.reserve(c.size());
+  std::transform(c.begin(), c.end(), std::back_inserter(transformed), f);
+  return transformed;
+}
+}  // namespace detail
+
+using detail::createReserved;
+
+//! Compares ids of primitives. Can be used for some stl algorithms
+template <typename PrimitiveT>
+constexpr bool idLess(const PrimitiveT& p1, const PrimitiveT& p2) {
+  return p1.id() < p2.id();
+}
+
+//! Compares ids of primitives. Can be used for some sorted stl containers
+template <typename T>
+struct IdLess {
+  constexpr bool operator()(const T& a, const T& b) { return idLess(a, b); }
+};
+
+template <typename T1, typename T2>
+inline auto orderedPair(T1& first, T2& second) {
+  return (first.id() < second.id()) ? std::make_pair(first, second) : std::make_pair(second, first);
+}
+
+template <class T>
+const T& addConst(T& t) {
+  return t;
+}
+
+template <class T>
+std::shared_ptr<const T> addConst(std::shared_ptr<T> t) {
+  return std::const_pointer_cast<const T>(t);
+}
+
+template <class T>
+std::shared_ptr<T> removeConst(std::shared_ptr<const T> t) {
+  return std::const_pointer_cast<T>(t);
+}
+
+template <class T>
+T& removeConst(const T& t) {
+  return const_cast<T&>(t);  // NOLINT
+}
+
+template <typename Container>
+Container invert(const Container& cont) {
+  Container cOut;
+  std::reverse_copy(cont.begin(), cont.end(), std::back_inserter(cOut));
+  return cOut;
+}
+
+template <typename Iterator, typename Func>
+auto transform(Iterator begin, Iterator end, const Func f) {
+  using ValueT = decltype(*begin);
+  using RetT = std::decay_t<decltype(f(*begin))>;
+  std::vector<RetT> transformed;
+  transformed.reserve(std::distance(begin, end));
+  std::transform(begin, end, std::back_inserter(transformed), [&f](ValueT elem) { return f(elem); });
+  return transformed;
+}
+
+template <typename ContainerT, typename ValueT>
+auto find(ContainerT&& c, const ValueT& val) {
+  using RetT = Optional<std::decay_t<decltype(*std::begin(c))>>;
+  auto it = std::find(std::begin(c), std::end(c), val);
+  if (it != std::end(c)) {
+    return RetT(*it);
+  }
+  return RetT();
+}
+
+template <typename ContainerT, typename Func>
+auto findIf(ContainerT&& c, Func f) {
+  using RetT = Optional<std::decay_t<decltype(*std::begin(c))>>;
+  auto it = std::find_if(std::begin(c), std::end(c), f);
+  if (it != std::end(c)) {
+    return RetT(*it);
+  }
+  return RetT();
+}
+
+template <typename Container, typename Func>
+auto transform(Container&& c, Func f) {
+  return detail::transform(std::forward<Container>(c), f);
+}
+
+template <typename T, typename Func>
+auto transform(std::initializer_list<T>&& c, Func f) {
+  return detail::transform(std::move(c), f);
+}
+
+template <typename Container, typename Predicate>
+bool anyOf(const Container& c, Predicate&& p) {
+  return std::any_of(c.begin(), c.end(), p);
+}
+
+template <typename Container, typename Value>
+bool contains(const Container& c, const Value& v) {
+  return std::find(c.begin(), c.end(), v) != c.end();
+}
+
+template <typename Container, typename Func>
+void forEach(Container&& c, Func&& f) {
+  std::for_each(c.begin(), c.end(), std::forward<Func>(f));
+}
+
+template <typename OutT, typename InT>
+auto transformSharedPtr(const std::vector<std::shared_ptr<InT>>& v) {
+  std::vector<std::shared_ptr<OutT>> transformed;
+  transformed.reserve(v.size());
+  for (const auto& elem : v) {
+    auto cast = std::dynamic_pointer_cast<OutT>(elem);
+    if (cast) {
+      transformed.push_back(std::move(cast));
+    }
+  }
+  return transformed;
+}
+
+/**
+ * @brief Wrapper around std::accumulate for summing up the result of Func over
+ * a range.
+ */
+template <typename Range, typename Func>
+auto sum(Range&& r, Func f) {
+  using ValT = decltype(f(*std::begin(r)));
+  return std::accumulate(std::begin(r), std::end(r), ValT(), [&f](auto v, auto&& elem) { return v + f(elem); });
+}
+
+/**
+ * @brief overload assuming that c is a container of containers.
+ * The return type will be the type of the inner container.
+ *
+ * For containers holding heavy objects consider moving the container into this.
+ */
+template <typename ContainerT>
+auto concatenate(ContainerT&& c) {
+  using VectorT = std::decay_t<decltype(*std::begin(c))>;
+  return detail::concatenate<VectorT>(std::forward<ContainerT>(c));
+}
+
+template <typename T>
+auto concatenate(std::initializer_list<T>&& c) {
+  using VectorT = std::decay_t<decltype(*std::begin(c))>;
+  return detail::concatenate<VectorT>(std::move(c));
+}
+
+/**
+ * @brief create one big vector from a vector and a function that returns a
+ * vector for each element in this vector
+ */
+template <typename ContainerT, typename Func>
+auto concatenate(ContainerT&& c, Func f) {
+  using VectorT = std::decay_t<decltype(f(*c.begin()))>;
+  return detail::concatenate<VectorT>(std::forward<ContainerT>(c), f);
+}
+
+template <typename T, typename Func>
+auto concatenate(std::initializer_list<T>&& c, Func f) {
+  using VectorT = std::decay_t<decltype(f(*c.begin()))>;
+  return detail::concatenate<VectorT>(std::move(c), f);
+}
+
+/**
+ * @brief Similar to concatenate but expects Func to return a pair of begin and
+ * end of the range to concatenated.
+ *
+ * Significantly more efficient than the above but requires that the range is
+ * already present in memory. A use case is a vector of vectors.
+ */
+template <typename ContainerT, typename Func>
+auto concatenateRange(ContainerT&& c, Func f) {
+  return detail::concatenateRange(std::forward<ContainerT>(c), f);
+}
+
+template <typename T, typename Func>
+auto concatenateRange(std::initializer_list<T>&& c, Func f) {
+  return detail::concatenateRange(std::move(c), f);
+}
+
+/// Overload for an allocator (e.g. for Eigen::aligned_allocator)
+template <typename ContainerT, typename Func, typename AllocatorT>
+auto concatenateRange(ContainerT&& c, Func f, const AllocatorT& alloc) {
+  return detail::concatenateRange(std::forward<ContainerT>(c), f, alloc);
+}
+
+template <typename T, typename Func, typename AllocatorT>
+auto concatenateRange(std::initializer_list<T>&& c, Func f, const AllocatorT& alloc) {
+  return detail::concatenateRange(std::move(c), f, alloc);
+}
+
+template <typename T, typename Variant>
+std::vector<T> getVariant(const std::vector<Variant>& v) {
+  std::vector<T> s;
+  s.reserve(v.size());
+  for (const auto& e : v) {
+    const auto* t = boost::get<typename T::MutableType>(&e);
+    if (t) {
+      s.push_back(*t);
+    }
+  }
+  return s;
+}
+
+/**
+ * @brief compares two c-strings at compile time. Do not use this for run time.
+ * @return true if strings match.
+ */
+constexpr bool strequal(char const* lhs, char const* rhs) {
+  while (*lhs != 0 && *rhs != 0) {
+    if (*lhs++ != *rhs++) {  // NOLINT
+      return false;
+    }
+  }
+  return *lhs == 0 && *rhs == 0;
+}
+
+/**
+ * @brief transforms a vector of weak_ptrs to a vector of shared_ptrs
+ *
+ * Expired elements are discarded.
+ */
+template <typename WeakT>
+auto strong(std::vector<WeakT> v) {
+  std::vector<decltype(v.front().lock())> s;
+  s.reserve(v.size());
+  for (const auto& e : v) {
+    if (!e.expired()) {
+      s.push_back(e.lock());
+    }
+  }
+  return s;
+}
+
+//! Tests if a double is close to zero within a small margin
+inline bool nearZero(double d) { return std::abs(d) < 1e-8; }
+
+}  // namespace utils
+
+namespace internal {
+// See
+// https://stackoverflow.com/questions/765148/how-to-remove-constness-of-const-iterator
+template <typename Container, typename ConstIterator>
+typename Container::iterator removeConst(Container& c, ConstIterator it) {
+  return c.erase(it, it);
+}
+}  // namespace internal
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_io/Configuration.h

@@ -0,0 +1,10 @@
+#pragma once
+#include <lanelet2_core/Attribute.h>
+
+#include <map>
+#include <string>
+namespace lanelet {
+namespace io {
+using Configuration = std::map<std::string, Attribute>;
+}  // namespace io
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_io/Exceptions.h

@@ -0,0 +1,75 @@
+#pragma once
+#include <lanelet2_core/Exceptions.h>
+
+#include <stdexcept>
+
+namespace lanelet {
+/**
+ * @brief Generic error for all errors in this module
+ *
+ * Threse are derived from multi error, because often multiple issues occur at
+ * the same time when reading or writing a map.
+ */
+class IOError : public LaneletMultiError {
+  using LaneletMultiError::LaneletMultiError;
+};
+
+/**
+ * @brief Error for not existent filepaths
+ */
+class FileNotFoundError : public IOError {
+  using IOError::IOError;
+};
+
+/**
+ * @brief Error for an unsupported extension
+ */
+class UnsupportedExtensionError : public IOError {
+  using IOError::IOError;
+};
+
+/**
+ * @brief Error thrown if an unsupported handler (parser/writer) has been
+ * specified
+ */
+class UnsupportedIOHandlerError : public IOError {
+  using IOError::IOError;
+};
+
+/**
+ * @brief Error thrown if some error occured during the parsing of the file
+ */
+class ParseError : public IOError {
+  using IOError::IOError;
+};
+
+/**
+ * @brief Error thown if some error occurd during writing of a map
+ */
+class WriteError : public IOError {
+  using IOError::IOError;
+};
+
+/**
+ * @brief Thrown by the projector classes if projection from lat/lon to x/y
+ * fails
+ */
+class ForwardProjectionError : public ParseError {
+  using ParseError::ParseError;
+};
+
+/**
+ * @brief Thrown by the projector classes if projection from x/y to lat/lon
+ * fails
+ */
+class ReverseProjectionError : public WriteError {
+  using WriteError::WriteError;
+};
+
+/**
+ * @brief Thrown when a user attempts to load a map with georeferenced data without providing an origin.
+ */
+class DefaultProjectionNotAllowedError : public IOError {
+  using IOError::IOError;
+};
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_io/Io.h

@@ -0,0 +1,171 @@
+#pragma once
+#include <lanelet2_core/LaneletMap.h>
+
+#include <string>
+#include <vector>
+
+#include "lanelet2_io/Configuration.h"
+#include "lanelet2_io/Projection.h"
+
+namespace lanelet {
+using ErrorMessages = std::vector<std::string>;
+/**
+ * @brief Loads a lanelet map from a file.
+ * @param filename name to load from. The extension decides how the file will be parsed.
+ * @param origin origin for the lat/lon -> x/y conversion. This *must not* be the default origin if the specified format
+ * requires an origin (e.g. you must always provide an origin if you load .osm data).
+ * @param errors if this points to a valid object, errors occured during parsing the map will be passed via this
+ * parameter. If this is nullptr, an exception will be thrown if errors have occurred.
+ * @param params optional params for loading. It depends on the parser that is used which parameters are required. If no
+ * params are passed, default values will be used
+ * @return Loaded map. Pointer is aways valid. Otherwise, an exception will be thrown
+ * @throws lanelet2::IOError if the file did not exist, could not be parsed or extension is not supported. If errors is
+ * not null, the loader will instead try to recover and only throw on unrecoverable errors (ie if the map did not
+ * exist). However, the loaded map will be incomplete if errors occurred.
+ * The loader will also throw if you did not provide a valid origin for a map that requires an origin for projections.
+ *
+ * It can not be stressed enough that it is important to provide a valid origin to write/load maps with georeferenced
+ * (lat/lon) data. If no origin is specified, the loaded map will most likely be not correctly located and deformed.
+ */
+std::unique_ptr<LaneletMap> load(const std::string& filename, const Origin& origin = Origin::defaultOrigin(),
+                                 ErrorMessages* errors = nullptr,
+                                 const io::Configuration& params = io::Configuration());
+
+/**
+ * @brief Loads a lanelet map from a file.
+ * @param filename name to load from. The extension decides how the file will be parsed.
+ * @param projector projection object for the transformations used
+ * @param errors if not null, errors will be reported here instead of throwing
+ * @param params optional params for loading. It depends on the parser that is used which parameters are required. If no
+ * params are passed, default values will be used
+ * @return Loaded map. Pointer is aways valid. Otherwise, an exception will be thrown
+ * @throws lanelet2::IOError if the file did not exist, could not be parsed or extension is not supported. If errors is
+ * not null, the loader will instead try to recover and only throw on unrecoverable errors (ie if the map did not
+ * exist). However, the loaded map will be incomplete if errors occurred.
+ */
+std::unique_ptr<LaneletMap> load(const std::string& filename, const Projector& projector,
+                                 ErrorMessages* errors = nullptr,
+                                 const io::Configuration& params = io::Configuration());
+
+/**
+ * @brief Loads a lanelet map from a file.
+ * @param filename name to load from
+ * @param parserName name of the parser to use. Available parsers can be queried with supportedParsers()
+ * @param origin origin for the lat/lon -> x/y conversion. This *must not* be the default origin if the specified format
+ * requires an origin (e.g. you must always provide an origin if you load .osm data).
+ * @param errors if this points to a valid object, errors occured during parsing the map will be passed via this
+ * parameter. If this is nullptr, an exception will be thrown if errors have occurred.
+ * @param params optional params for loading. It depends on the parser that is used which parameters are required. If no
+ * params are passed, default values will be used
+ * @return Loaded map. Pointer is aways valid. Otherwise, an exception will be thrown
+ * @throws lanelet2::IOError if the file did not exist, could not be parsed, the default origin was provided for a map
+ * that requires osm conversion or extension is not supported
+ */
+std::unique_ptr<LaneletMap> load(const std::string& filename, const std::string& parserName,
+                                 const Origin& origin = Origin::defaultOrigin(), ErrorMessages* errors = nullptr,
+                                 const io::Configuration& params = io::Configuration());
+
+/**
+ * @brief Loads a lanelet map from file.
+ * @param filename name to load from
+ * @param parserName name of the parser to use. Available parsers can be queried with supportedParsers()
+ * @param errors if this points to a valid object, errors occured during parsing the map will be passed via this
+ * parameter. If this is nullptr, an exception will be thrown if errors have occurred.
+ * @param projector projection object for the transformations
+ * @param params optional params for loading. It depends on the parser that is used which parameters are required. If no
+ * params are passed, default values will be used
+ * @return Loaded map. Pointer is aways valid. Otherwise, an exception will be thrown
+ * @throws lanelet2::IOError if the file did not exist, could not be parsed or extension is not supported
+ */
+std::unique_ptr<LaneletMap> load(const std::string& filename, const std::string& parserName, const Projector& projector,
+                                 ErrorMessages* errors = nullptr,
+                                 const io::Configuration& params = io::Configuration());
+
+/**
+ * @brief returns the names of the currently registered parsers (parsers from plugins included)
+ * @return list of names
+ */
+std::vector<std::string> supportedParsers();
+
+/**
+ * @brief returns the names of the currently supported extensions for parsing (including the dot)
+ * @return list of extensions
+ * @addtogroup load
+ */
+std::vector<std::string> supportedParserExtensions();
+
+/**
+ * @brief writes a map to a file
+ * @param filename file to write to (parent folders must exist!)
+ * @param map map to be written
+ * @param origin origin for x,y -> lat/lon conversion. You will receive a warning if you try to write to a format that
+ * requires lat/lon conversion and passed the defaultOrigin.
+ * @param errors if this points to a valid object, errors occured during write will be passed via this parameter. If
+ * this is nullptr, an exception will be thrown if errors have occurred.
+ * @param params extra parameters for the writer (if required). If empty, default parameters will be used.
+ * @throws lanelet2::IOError if the file could not be created or writing failed. If errors is not null, the writer will
+ * instead try to recover and only throw on unrecoverable errors (ie if the file could not be created). However, the
+ * written map will be incomplete if errors occurred.
+ *
+ * It can not be stressed enough that it is important to provide a valid origin to write/load maps with georeferenced
+ * (lat/lon) data. If no origin is specified, the written map will most likely be not correctly located and deformed.
+ */
+void write(const std::string& filename, const lanelet::LaneletMap& map, const Origin& origin = Origin::defaultOrigin(),
+           ErrorMessages* errors = nullptr, const io::Configuration& params = io::Configuration());
+
+/**
+ * @brief writes a map to a file
+ * @param filename file to write to (parent folders must exist!). The extension is used to deduce the format.
+ * @param map map to be written
+ * @param projector projector object for x,y -> lat/lon conversion. You will receive a warning if you try to write to a
+ * format that requires lat/lon conversion and passed the defaultOrigin.
+ * @param errors if this points to a valid object, errors occured during write will be passed via this parameter. If
+ * this is nullptr, an exception will be thrown if errors have occurred.
+ * @param params extra parameters for the writer (if required). If empty, default parameters will be used.
+ * @throws lanelet2::IOError if the file could not be created or writing failed.
+ */
+void write(const std::string& filename, const LaneletMap& map, const Projector& projector,
+           ErrorMessages* errors = nullptr, const io::Configuration& params = io::Configuration());
+
+/**
+ * @brief writes a map to a file
+ * @param filename file to write to (parent folders must exist!). The extension is used to deduce the format.
+ * @param map map to be written
+ * @param writerName name of the writer format to use. Available writers can be queried with supportedWriters().
+ * @param errors if this points to a valid object, errors occured during write will be passed via this parameter. If
+ * this is nullptr, an exception will be thrown if errors have occurred.
+ * @param origin origin definition for x,y -> lat/lon conversion
+ * @param params extra parameters for the writer (if required). If empty, default parameters will be used.
+ * @throws lanelet2::IOError if the file could not be created or writing failed.
+ */
+void write(const std::string& filename, const lanelet::LaneletMap& map, const std::string& writerName,
+           const Origin& origin = Origin::defaultOrigin(), ErrorMessages* errors = nullptr,
+           const io::Configuration& params = io::Configuration());
+
+/**
+ * @brief writes a map to a file
+ * @param filename file to write to (parent folders must exist!). The extension is used to deduce the format.
+ * @param map map to be written
+ * @param writerName name of the writer format to use. Available writers can be queried with supportedWriters().
+ * @param projector projector object for x,y -> lat/lon conversion
+ * @param errors if this points to a valid object, errors occured during write will be passed via this parameter. If
+ * this is nullptr, an exception will be thrown if errors have occurred.
+ * @param params extra parameters for the writer (if required). If empty, default parameters will be used.
+ * @throws lanelet2::IOError if the file could not be created or writing failed.
+ */
+void write(const std::string& filename, const LaneletMap& map, const std::string& writerName,
+           const Projector& projector, ErrorMessages* errors = nullptr,
+           const io::Configuration& params = io::Configuration());
+
+/**
+ * @brief returns the names of the currently registered writing (writers from plugins included)
+ * @return list of names
+ */
+std::vector<std::string> supportedWriters();
+
+/**
+ * @brief returns the names of the currently supported extensions for writing (including the dot)
+ * @return list of extensions
+ */
+std::vector<std::string> supportedWriterExtensions();
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_io/Projection.h

@@ -0,0 +1,79 @@
+#pragma once
+#include <lanelet2_core/primitives/GPSPoint.h>
+#include <lanelet2_core/primitives/Point.h>
+
+#include <memory>
+#include <vector>
+
+namespace lanelet {
+//!< When transforming between global and local coordinates, this offset will be applied to the lat/lon coordinates.
+//! The object is also used to determine a senseful local coordinate system (e.g. choosing the correct UTM tile).
+struct Origin {
+  explicit Origin(const GPSPoint& position) : position{position}, isDefault{false} {}
+  Origin() = default;
+
+  static Origin defaultOrigin() { return {}; }
+  GPSPoint position;     //! The position of the origin
+  bool isDefault{true};  //! The Parser/Writer check for this if appropriate to make sure they are using a valid origin
+};
+
+//! Base class for implementing projection models. To use your own projection,
+//! implement this class and and pass it to the write/load functions.
+class Projector {  // NOLINT
+ public:
+  using Ptr = std::shared_ptr<Projector>;
+  explicit Projector(Origin origin = Origin::defaultOrigin()) : origin_{origin} {}
+  Projector(Projector&& rhs) noexcept = default;
+  Projector& operator=(Projector&& rhs) noexcept = default;
+  Projector(const Projector& rhs) = default;
+  Projector& operator=(const Projector& rhs) = default;
+  virtual ~Projector() noexcept = default;
+
+  //! @brief Project a point from lat/lon coordinates to a local coordinate system
+  //! @throws ForwardProjectionError if projection is impossible
+  virtual BasicPoint3d forward(const GPSPoint& p) const = 0;
+
+  //! @brief Project a point from local coordinates to global lat/lon coordinates
+  //! @throws ReverseProjectionError if projection is impossible
+  virtual GPSPoint reverse(const BasicPoint3d& p) const = 0;
+
+  //! Obtain the internal origin
+  const Origin& origin() const { return origin_; }
+
+ private:
+  Origin origin_;
+};
+
+namespace projection {
+/**
+ * @brief implements a simple spherical mercator projection.
+ *
+ * Will be too unprecise for most calculations, but its fast and needs no external dependencies. If you want more, check
+ * out lanelet2_projection! This is the same projection that was used by lanelet1.
+ */
+class SphericalMercatorProjector : public Projector {
+ public:
+  using Projector::Projector;
+  BasicPoint3d forward(const GPSPoint& p) const override {
+    const auto scale = std::cos(origin().position.lat * M_PI / 180.0);
+    const double x{scale * p.lon * M_PI * EarthRadius / 180.0};
+    const double y{scale * EarthRadius * std::log(std::tan((90.0 + p.lat) * M_PI / 360.0))};
+    return {x, y, p.ele};
+  }
+
+  GPSPoint reverse(const BasicPoint3d& p) const override {
+    const double scale = std::cos(origin().position.lat * M_PI / 180.0);
+    const double lon = p.x() * 180.0 / (M_PI * EarthRadius * scale);
+    const double lat = 360.0 * std::atan(std::exp(p.y() / (EarthRadius * scale))) / M_PI - 90.0;
+    return {lat, lon, p.z()};
+  }
+
+ private:
+  static constexpr double EarthRadius{6378137.0};
+};
+}  // namespace projection
+
+using DefaultProjector = projection::SphericalMercatorProjector;
+
+inline DefaultProjector defaultProjection(Origin origin = Origin::defaultOrigin()) { return DefaultProjector(origin); }
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_io/io_handlers/BinHandler.h

@@ -0,0 +1,32 @@
+#pragma once
+#include "lanelet2_io/io_handlers/Parser.h"
+#include "lanelet2_io/io_handlers/Writer.h"
+
+namespace lanelet {
+namespace io_handlers {
+/**
+ * @brief Writer class for binary files (using boost serialization)
+ */
+class BinWriter : public Writer {
+ public:
+  using Writer::Writer;
+
+  void write(const std::string& filename, const LaneletMap& laneletMap, ErrorMessages& /*errors*/) const override;
+
+  static constexpr const char* extension() { return ".bin"; }
+
+  static constexpr const char* name() { return "bin_handler"; }
+};
+
+class BinParser : public Parser {
+ public:
+  using Parser::Parser;
+
+  std::unique_ptr<LaneletMap> parse(const std::string& filename, ErrorMessages& /*errors*/) const override;
+
+  static constexpr const char* extension() { return ".bin"; }
+
+  static constexpr const char* name() { return "bin_handler"; }
+};
+}  // namespace io_handlers
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_io/io_handlers/Factory.h

@@ -0,0 +1,164 @@
+#pragma once
+#include <lanelet2_core/utility/Utilities.h>
+
+#include <cstring>
+#include <functional>
+#include <map>
+#include <string>
+
+#include "lanelet2_io/Configuration.h"
+#include "lanelet2_io/Projection.h"
+#include "lanelet2_io/io_handlers/Parser.h"
+#include "lanelet2_io/io_handlers/Writer.h"
+
+namespace lanelet {
+namespace io_handlers {
+/**
+ * @brief Factory class for all suppored lanelet map parsers
+ */
+class ParserFactory {  // NOLINT
+ public:
+  using ParserCreationFcn = std::function<Parser*(const Projector&, const io::Configuration&)>;
+
+  static ParserFactory& instance();
+
+  /**
+   * @brief creates a parser that matches the given name.
+   * @param parserName name of the parser (one of availableParsers())
+   * @param projector projection object passed to the parser
+   * @param config config object passed to the parser
+   * @return created parser
+   * @throws a lanelet2::UnsupportedIOHandlerError if handler is not registered
+   */
+  static Parser::Ptr create(const std::string& parserName, const Projector& projector,
+                            const io::Configuration& config = io::Configuration());
+
+  /**
+   * @brief creates a matching parser for the given file extension
+   * @param extension extension to look for (including the dot!)
+   * @param projector projection object passed to the parser
+   * @param config config object passed to the parser
+   * @return created parser
+   * @throws a lanelet2::UnsupportedExtensionError if extension is not
+   * registered
+   */
+  static Parser::Ptr createFromExtension(const std::string& extension, const Projector& projector,
+                                         const io::Configuration& config = io::Configuration());
+
+  /**
+   * @brief returns all available parsers as vector
+   * @return vector of parser names
+   */
+  static std::vector<std::string> availableParsers();
+
+  /**
+   * @brief returns all available extensions as vector
+   * @return vector of extensions (including the dot)
+   */
+  static std::vector<std::string> availableExtensions();
+
+  template <typename T>
+  friend class RegisterParser;
+
+ private:
+  void registerParser(const std::string& strategy, const std::string& extension,
+                      const ParserCreationFcn& factoryFunction);
+
+  ParserFactory() = default;
+  std::map<std::string, ParserCreationFcn> registry_;
+  std::map<std::string, ParserCreationFcn> extensionRegistry_;
+};
+
+/**
+ * @brief Factory class for all supported lanelet map writers
+ */
+class WriterFactory {  // NOLINT
+ public:
+  using WriterCreationFcn = std::function<Writer*(const Projector&, const io::Configuration&)>;
+
+  static WriterFactory& instance();
+
+  /**
+   * @brief creates a writer that matches the given name.
+   * @param writerName name of the writer (one of availableParsers())
+   * @param projector projection object passed to the writer
+   * @param config config object passed to the writer
+   * @return created writer
+   * @throws a lanelet2::UnsupportedIOHandlerError if handler is not registered
+   */
+  static Writer::Ptr create(const std::string& writerName, const Projector& projector,
+                            const io::Configuration& config = io::Configuration());
+
+  /**
+   * @brief creates a matching writer for the given file extension
+   * @param extension extension to look for (including the dot!)
+   * @param projector projection object passed to the writer
+   * @param config config object passed to the writer
+   * @return created writer
+   * @throws a lanelet2::UnsupportedExtensionError if extension is not
+   * registered
+   */
+  static Writer::Ptr createFromExtension(const std::string& extension, const Projector& projector,
+                                         const io::Configuration& config = io::Configuration());
+
+  /**
+   * @brief returns all available writers as vector
+   * @return vector of writer names
+   */
+  static std::vector<std::string> availableWriters();
+
+  /**
+   * @brief returns all available extensions as vector
+   * @return vector of extensions (including the dot)
+   */
+  static std::vector<std::string> availableExtensions();
+
+  template <typename T>
+  friend class RegisterWriter;
+
+ private:
+  void registerWriter(const std::string& strategy, const std::string& extension,
+                      const WriterCreationFcn& factoryFunction);
+
+  WriterFactory() = default;
+  std::map<std::string, WriterCreationFcn> registry_;
+  std::map<std::string, WriterCreationFcn> extensionRegistry_;
+};
+
+/**
+ * @brief Registration object for a writer. Needs to be instanciated as static
+ * object once to register a writer.
+ * Registration might look like this:
+ *   static RegisterWriter<Mywriter> register;
+ */
+template <class T>
+class RegisterWriter {
+ public:
+  RegisterWriter() {
+    static_assert(!utils::strequal(T::name(), ""), "You did not overload the name() function!");
+    WriterFactory::instance().registerWriter(
+        T::name(), T::extension(), [](const Projector& projector, const io::Configuration& config) -> Writer* {
+          return new T(projector, config);
+        });
+  }
+};
+
+/**
+ * @brief Registration object for a parser. Needs to be instanciated as static
+ * object once to register a parser.
+ * Registration might look like this:
+ *   static RegisterParser<Myparser> register;
+ */
+template <class T>
+class RegisterParser {
+ public:
+  RegisterParser() {
+    static_assert(!utils::strequal(T::name(), ""), "You did not overload the name() function!");
+    ParserFactory::instance().registerParser(
+        T::name(), T::extension(), [](const Projector& projector, const io::Configuration& config) -> Parser* {
+          return new T(projector, config);
+        });
+  }
+};
+}  // namespace io_handlers
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_io/io_handlers/IoHandler.h

@@ -0,0 +1,57 @@
+#pragma once
+#include <memory>
+
+#include "lanelet2_io/Configuration.h"
+#include "lanelet2_io/Projection.h"
+
+namespace lanelet {
+using ErrorMessages = std::vector<std::string>;
+namespace io_handlers {
+/**
+ * @brief Base class for all handlers (writers and parsers)
+ */
+class IOHandler {  // NOLINT
+ public:
+  using Ptr = std::shared_ptr<IOHandler>;
+  explicit IOHandler(const Projector& projector, const io::Configuration& config = io::Configuration())
+      : projector_{&projector}, config_{&config} {}
+  virtual ~IOHandler() = default;
+
+  /**
+   * @brief returns the extension supported by this parser
+   * @return extension (including the dot)
+   * Extension can be empty if extension
+   */
+  static constexpr const char* extension() { return ""; }
+
+  /**
+   * @brief returns the name of this handler. Must not be empty for child
+   * classes
+   * @return name
+   */
+  static constexpr const char* name() { return ""; }
+
+  const Projector& projector() const {
+    if (projector_->origin().isDefault) {
+      handleDefaultProjector();
+    }
+    return *projector_;
+  }
+
+  io::Configuration config() { return *config_; }
+
+ protected:
+  IOHandler() = default;  // workaround for gcc5 bug
+
+ private:
+  const Projector* projector_{};       //!< projection object for lat/lon
+                                       //!< from/to x/y conversions
+  const io::Configuration* config_{};  //!< config object for additional parameters.
+                                       //! Parser should always use default parameters
+                                       //! if parameters are missing
+
+  //! using a default projector is not allowed. The implementations define how this issue is handled.
+  virtual void handleDefaultProjector() const = 0;
+};
+}  // namespace io_handlers
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_io/io_handlers/OsmFile.h

@@ -0,0 +1,109 @@
+#pragma once
+#include <lanelet2_core/Forward.h>
+#include <lanelet2_core/primitives/GPSPoint.h>
+
+#include <deque>
+#include <map>
+#include <pugixml.hpp>
+#include <string>
+#include <utility>
+
+namespace lanelet {
+namespace osm {
+
+struct Primitive;
+using Attributes = std::map<std::string, std::string>;
+using Role = std::pair<std::string, Primitive*>;
+using Roles = std::deque<Role>;  // need iterator validity on push_back
+using Errors = std::vector<std::string>;
+
+//! Common abstract base class for all osm primitives. Provides id and attributes.
+struct Primitive {
+  Primitive() = default;
+  Primitive(Primitive&& rhs) noexcept = default;  // NOLINT
+  Primitive& operator=(Primitive&& rhs) noexcept = default;
+  Primitive(const Primitive& rhs) = delete;
+  Primitive& operator=(const Primitive& rhs) = delete;
+  virtual ~Primitive() = default;
+  Primitive(Id id, Attributes attributes) : id{id}, attributes{std::move(attributes)} {}
+  virtual std::string type() = 0;
+
+  Id id{0};
+  Attributes attributes;
+};
+
+//! Osm node object
+struct Node : public Primitive {
+  Node() = default;
+  Node(Id id, Attributes attributes, GPSPoint point) : Primitive{id, std::move(attributes)}, point{point} {}
+  std::string type() override { return "node"; }
+  GPSPoint point;
+};
+
+//! Osm way object
+struct Way : public Primitive {
+  Way() = default;
+  Way(Id id, Attributes attributes, std::vector<Node*> nodes)
+      : Primitive{id, std::move(attributes)}, nodes{std::move(nodes)} {}
+  std::string type() override { return "way"; }
+  std::vector<Node*> nodes;
+};
+
+//! Osm relation object
+struct Relation : public Primitive {
+  Relation() = default;
+  Relation(Id id, Attributes attributes, Roles roles = Roles())
+      : Primitive{id, std::move(attributes)}, members{std::move(roles)} {}
+  std::string type() override { return "relation"; }
+  Roles members;
+};
+
+using Nodes = std::map<Id, Node>;
+using Ways = std::map<Id, Way>;
+using Relations = std::map<Id, Relation>;
+
+/**
+ * @brief Intermediate representation of an osm file.
+ *
+ * Tries its best to cover the osm file specification.
+ */
+struct File {
+  File() noexcept = default;
+  File(File&& rhs) noexcept = default;  // NOLINT
+  File& operator=(File&& rhs) noexcept = default;
+  File(const File& rhs) = delete;
+  File& operator=(const File& rhs) = delete;
+  ~File() noexcept = default;
+
+  Nodes nodes;
+  Ways ways;
+  Relations relations;
+};
+
+inline auto findRole(const Roles& roles, const std::string& roleName) {
+  return std::find_if(roles.begin(), roles.end(), [&](auto& role) { return roleName == role.first; });
+}
+
+template <typename Func>
+inline auto forEachMember(const Roles& roles, const std::string& roleName, Func&& f) {
+  std::for_each(roles.begin(), roles.end(), [&](auto& role) {
+    if (roleName == role.first) {
+      f(role);
+    };
+  });
+}
+
+//! Reads an xml document into an osm file representation and optionally reports
+//! parser errors.
+File read(pugi::xml_document& node, lanelet::osm::Errors* errors = nullptr);
+
+//! Creates an xml representation from an osm file representation. This is
+//! guaranteed to work without errors.
+std::unique_ptr<pugi::xml_document> write(const File& file);
+
+inline bool operator==(const Node& lhs, const Node& rhs) { return lhs.id == rhs.id; }
+bool operator==(const Way& lhs, const Way& rhs);
+bool operator==(const Relation& lhs, const Relation& rhs);
+bool operator==(const File& lhs, const File& rhs);
+}  // namespace osm
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_io/io_handlers/OsmHandler.h

@@ -0,0 +1,41 @@
+#pragma once
+#include "lanelet2_io/io_handlers/OsmFile.h"
+#include "lanelet2_io/io_handlers/Parser.h"
+#include "lanelet2_io/io_handlers/Writer.h"
+
+namespace lanelet {
+namespace io_handlers {
+/**
+ * @brief Writer class for osm files
+ */
+class OsmWriter : public Writer {
+ public:
+  using Writer::Writer;
+
+  void write(const std::string& filename, const LaneletMap& laneletMap, ErrorMessages& errors) const override;
+
+  std::unique_ptr<osm::File> toOsmFile(const LaneletMap& laneletMap, ErrorMessages& errors) const;
+
+  static constexpr const char* extension() { return ".osm"; }
+
+  static constexpr const char* name() { return "osm_handler"; }
+};
+/**
+
+ * @brief Parser class for osm files
+ */
+class OsmParser : public Parser {
+ public:
+  using Parser::Parser;
+
+  std::unique_ptr<LaneletMap> parse(const std::string& filename, ErrorMessages& errors) const override;
+
+  std::unique_ptr<LaneletMap> fromOsmFile(const osm::File& file, ErrorMessages& errors) const;
+
+  static constexpr const char* extension() { return ".osm"; }
+
+  static constexpr const char* name() { return "osm_handler"; }
+};
+
+}  // namespace io_handlers
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_io/io_handlers/Parser.h

@@ -0,0 +1,36 @@
+#pragma once
+#include <lanelet2_core/LaneletMap.h>
+
+#include <memory>
+
+#include "lanelet2_io/Exceptions.h"
+#include "lanelet2_io/Projection.h"
+#include "lanelet2_io/io_handlers/IoHandler.h"
+
+namespace lanelet {
+namespace io_handlers {
+/**
+ * @brief Base object for parsers.
+ * To create a new parser, you have to do the following steps:
+ * 1. Inherit from this parser
+ * 2. Overload the parse()-function, the name()-function (from IOHandler) and
+ * optionally the extension()-function (from
+ * IOHandler)
+ * 3. Inherit the constructors (using Parser::Parser)
+ * 4. register your parser using the RegisterParser object
+ */
+class Parser : public IOHandler {
+ public:
+  using IOHandler::IOHandler;
+  Parser() = default;
+  using Ptr = std::shared_ptr<Parser>;
+  virtual std::unique_ptr<LaneletMap> parse(const std::string& filename, ErrorMessages& errors) const = 0;
+
+ private:
+  //! loading a map with a default origin throws an error
+  void handleDefaultProjector() const final {
+    throw lanelet::IOError("You must pass an origin when loading a map with georeferenced (lat/lon) data!");
+  }
+};
+}  // namespace io_handlers
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_io/io_handlers/Serialize.h

@@ -0,0 +1,502 @@
+#pragma once
+#include <lanelet2_core/LaneletMap.h>
+
+#include <boost/serialization/map.hpp>
+#include <boost/serialization/serialization.hpp>
+#include <boost/serialization/shared_ptr.hpp>
+#include <boost/serialization/split_free.hpp>
+#include <boost/serialization/variant.hpp>
+#include <boost/serialization/vector.hpp>
+#include <map>
+#include <set>
+
+namespace lanelet {
+namespace impl {
+// these methods are there because serializing vectors on boost 1.54 is done through a proxy object.
+// this does not work here, because we need the real, final address for RegelemDeserialization.
+// In Boost 1.60 and up, this is fixed and a simple ar<<regelems would be enough.
+template <typename Archive, typename RegelemsT>
+void saveRegelems(Archive& ar, RegelemsT regelems) {
+  auto size = regelems.size();
+  ar << size;
+  for (auto& regelem : regelems) {
+    auto ncRegelem = std::const_pointer_cast<RegulatoryElement>(regelem);
+    ar << ncRegelem;
+  }
+}
+template <typename Archive, typename RegelemsT>
+void loadRegelems(Archive& ar, RegelemsT& regelems) {
+  size_t size{};
+  ar >> size;
+  regelems.resize(size);
+  for (auto i = 0u; i < size; ++i) {
+    ar >> regelems[i];
+  }
+}
+}  // namespace impl
+}  // namespace lanelet
+
+namespace boost {
+namespace serialization {
+template <typename Archive>
+void load(Archive& ar, lanelet::Attribute& p, unsigned int /*version*/) {
+  std::string val;
+  ar& val;
+  p = std::move(val);
+}
+
+template <typename Archive>
+void save(Archive& ar, const lanelet::Attribute& p, unsigned int /*version*/) {
+  ar& p.value();
+}
+
+template <typename Archive>
+void load(Archive& ar, lanelet::AttributeMap& p, unsigned int /*version*/) {
+  boost::serialization::load_map_collection(ar, p);
+}
+
+template <typename Archive>
+void save(Archive& ar, const lanelet::AttributeMap& p, unsigned int /*version*/) {
+  boost::serialization::stl::save_collection(ar, p);
+}
+
+template <typename Archive>
+void load(Archive& ar, lanelet::RuleParameterMap& p, unsigned int /*version*/) {
+  boost::serialization::load_map_collection(ar, p);
+}
+
+template <typename Archive>
+void save(Archive& ar, const lanelet::RuleParameterMap& p, unsigned int /*version*/) {
+  boost::serialization::stl::save_collection(ar, p);
+}
+
+//! lanelet +data
+template <typename Archive>
+void serialize(Archive& /*ar*/, lanelet::LaneletData& /*p*/, unsigned int /*version*/) {}
+template <class Archive>
+// NOLINTNEXTLINE
+inline void save_construct_data(Archive& ar, const lanelet::LaneletData* llt, unsigned int /*version*/) {
+  auto lltnc = const_cast<lanelet::LaneletData*>(llt);  // NOLINT
+  ar << lltnc->id << lltnc->attributes << lltnc->leftBound() << lltnc->rightBound();
+  lanelet::impl::saveRegelems(ar, llt->regulatoryElements());
+  auto hasCenterline = llt->hasCustomCenterline();
+  ar << hasCenterline;
+  if (hasCenterline) {
+    auto centerline = llt->centerline();
+    lanelet::LineString3d center(std::const_pointer_cast<lanelet::LineStringData>(centerline.constData()),
+                                 centerline.inverted());
+    ar << center;
+  }
+}
+
+template <class Archive>
+// NOLINTNEXTLINE
+inline void load_construct_data(Archive& ar, lanelet::LaneletData* llt, unsigned int /*version*/
+) {
+  using namespace lanelet;
+  Id id = 0;
+  AttributeMap attrs;
+  LineString3d left;
+  LineString3d right;
+  ar >> id >> attrs >> left >> right;
+  new (llt) LaneletData(id, left, right, attrs);
+  lanelet::impl::loadRegelems(ar, llt->regulatoryElements());  // must happen directly to the correct memory location
+  bool hasCenterline = false;
+  ar >> hasCenterline;
+  if (hasCenterline) {
+    LineString3d centerline;
+    ar >> centerline;
+    llt->setCenterline(centerline);
+  }
+}
+
+template <typename Archive>
+void save(Archive& ar, const lanelet::Lanelet& l, unsigned int /*version*/) {
+  auto inv = l.inverted();
+  auto ptr = lanelet::utils::removeConst(l.constData());
+  ar << inv << ptr;
+}
+template <typename Archive>
+void load(Archive& ar, lanelet::Lanelet& l, unsigned int /*version*/) {
+  std::shared_ptr<lanelet::LaneletData> ptr;
+  bool inv = false;
+  ar >> inv >> ptr;
+  l = lanelet::Lanelet(ptr, inv);
+}
+template <typename Archive>
+void save(Archive& ar, const lanelet::ConstLanelet& l, unsigned int /*version*/) {
+  auto inv = l.inverted();
+  auto ptr = lanelet::utils::removeConst(l.constData());
+  ar << inv << ptr;
+}
+
+template <typename Archive>
+void load(Archive& ar, lanelet::ConstLanelet& l, unsigned int /*version*/) {
+  std::shared_ptr<lanelet::LaneletData> ptr;
+  bool inv = false;
+  ar >> inv >> ptr;
+  l = lanelet::Lanelet(ptr, inv);
+}
+
+template <typename Archive>
+void save(Archive& ar, const lanelet::WeakLanelet& l, unsigned int /*version*/) {
+  if (l.expired()) {
+    throw lanelet::LaneletError("Can not serialize expired weak pointer!");
+  }
+  auto sp = l.lock();
+  ar& sp;
+}
+template <typename Archive>
+void load(Archive& ar, lanelet::WeakLanelet& l, unsigned int /*version*/) {
+  lanelet::Lanelet lanelet;
+  ar& lanelet;
+  l = lanelet;
+}
+
+//! linestring + data
+template <typename Archive>
+void serialize(Archive& /*ar*/, lanelet::LineStringData& /*l*/, unsigned int /*version*/) {}
+template <class Archive>
+// NOLINTNEXTLINE
+inline void save_construct_data(Archive& ar, const lanelet::LineStringData* l, unsigned int /*version*/) {
+  auto lnc = const_cast<lanelet::LineStringData*>(l);  // NOLINT
+  ar << lnc->id << lnc->attributes << lnc->points();
+}
+
+template <class Archive>
+// NOLINTNEXTLINE
+inline void load_construct_data(Archive& ar, lanelet::LineStringData* l, unsigned int /*version*/) {
+  using namespace lanelet;
+  Id id = 0;
+  AttributeMap attr;
+  Points3d points;
+  ar >> id >> attr >> points;
+  new (l) LineStringData(id, points, attr);
+}
+
+template <typename Archive>
+void save(Archive& ar, const lanelet::LineString3d& l, unsigned int /*version*/) {
+  auto inv = l.inverted();
+  auto ptr = lanelet::utils::removeConst(l.constData());
+  ar << inv << ptr;
+}
+template <typename Archive>
+void load(Archive& ar, lanelet::LineString3d& l, unsigned int /*version*/) {
+  std::shared_ptr<lanelet::LineStringData> ptr;
+  bool inv{};
+  ar >> inv;
+  ar >> ptr;
+  l = lanelet::LineString3d(ptr, inv);
+}
+template <typename Archive>
+void save(Archive& ar, const lanelet::Polygon3d& l, unsigned int /*version*/) {
+  auto inv = l.inverted();
+  auto ptr = lanelet::utils::removeConst(l.constData());
+  ar << inv << ptr;
+}
+template <typename Archive>
+void load(Archive& ar, lanelet::Polygon3d& l, unsigned int /*version*/) {
+  std::shared_ptr<lanelet::LineStringData> ptr;
+  bool inv{};
+  ar >> inv;
+  ar >> ptr;
+  l = lanelet::Polygon3d(ptr, inv);
+}
+template <typename Archive>
+void save(Archive& ar, const lanelet::ConstLineString3d& l, unsigned int /*version*/) {
+  auto inv = l.inverted();
+  auto ptr = lanelet::utils::removeConst(l.constData());
+  ar << inv;
+  ar << ptr;
+}
+template <typename Archive>
+void load(Archive& ar, lanelet::ConstLineString3d& l, unsigned int /*version*/) {
+  std::shared_ptr<lanelet::LineStringData> ptr;
+  bool inv{};
+  ar >> inv;
+  ar >> ptr;
+  l = lanelet::LineString3d(ptr, inv);
+}
+
+template <typename Archive>
+void serialize(Archive& ar, lanelet::LineString2d& ls, unsigned int /*version*/) {
+  auto ls3d = lanelet::traits::to3D(ls);
+  ar& ls3d;
+  ls = lanelet::traits::to2D(ls3d);
+}
+
+//! point + data
+template <typename Archive>
+void serialize(Archive& /*ar*/, lanelet::PointData& /*l*/, unsigned int /*version*/) {}
+template <class Archive>
+// NOLINTNEXTLINE
+inline void save_construct_data(Archive& ar, const lanelet::PointData* p, unsigned int /*version*/) {
+  ar << p->id << p->attributes << p->point.x() << p->point.y() << p->point.z();
+}
+
+template <class Archive>
+// NOLINTNEXTLINE
+inline void load_construct_data(Archive& ar, lanelet::PointData* p, unsigned int /*version*/) {
+  using namespace lanelet;
+  Id id = 0;
+  AttributeMap attrs;
+  BasicPoint3d pt;
+  ar >> id >> attrs >> pt.x() >> pt.y() >> pt.z();
+  new (p) PointData(id, pt, attrs);
+}
+
+template <typename Archive>
+void save(Archive& ar, const lanelet::Point3d& p, unsigned int /*version*/) {
+  auto ptr = lanelet::utils::removeConst(p.constData());
+  ar << ptr;
+}
+template <typename Archive>
+void load(Archive& ar, lanelet::Point3d& p, unsigned int /*version*/) {
+  std::shared_ptr<lanelet::PointData> ptr;
+  ar >> ptr;
+  p = lanelet::Point3d(ptr);
+}
+template <typename Archive>
+void save(Archive& ar, const lanelet::ConstPoint3d& p, unsigned int /*version*/) {
+  ar << lanelet::utils::removeConst(p.constData());
+}
+template <typename Archive>
+void load(Archive& ar, lanelet::ConstPoint3d& p, unsigned int /*version*/) {
+  std::shared_ptr<lanelet::PointData> ptr;
+  ar >> ptr;
+  p = lanelet::Point3d(ptr);
+}
+
+//! area
+template <typename Archive>
+void serialize(Archive& /*ar*/, lanelet::AreaData& /*a*/, unsigned int /*version*/) {}
+template <class Archive>
+// NOLINTNEXTLINE
+inline void save_construct_data(Archive& ar, const lanelet::AreaData* a, unsigned int /*version*/) {
+  auto anc = const_cast<lanelet::AreaData*>(a);  // NOLINT
+  ar << anc->id;
+  ar << anc->attributes;
+  ar << anc->innerBounds();
+  ar << anc->outerBound();
+  lanelet::impl::saveRegelems(ar, anc->regulatoryElements());
+}
+
+template <class Archive>
+// NOLINTNEXTLINE
+inline void load_construct_data(Archive& ar, lanelet::AreaData* a, unsigned int /*version*/) {
+  using namespace lanelet;
+  Id id = 0;
+  AttributeMap attrs;
+  InnerBounds inner;
+  LineStrings3d outer;
+  ar >> id >> attrs >> inner >> outer;
+  new (a) AreaData(id, outer, inner, attrs);
+  lanelet::impl::loadRegelems(ar, a->regulatoryElements());
+}
+
+template <typename Archive>
+void save(Archive& ar, const lanelet::Area& a, unsigned int /*version*/) {
+  auto ptr = lanelet::utils::removeConst(a.constData());
+  ar << ptr;
+}
+template <typename Archive>
+void load(Archive& ar, lanelet::Area& a, unsigned int /*version*/) {
+  std::shared_ptr<lanelet::AreaData> ptr;
+  ar >> ptr;
+  a = lanelet::Area(ptr);
+}
+template <typename Archive>
+void save(Archive& ar, const lanelet::ConstArea& a, unsigned int /*version*/) {
+  auto ptr = lanelet::utils::removeConst(a.constData());
+  ar << ptr;
+}
+template <typename Archive>
+void load(Archive& ar, lanelet::ConstArea& a, unsigned int /*version*/) {
+  std::shared_ptr<lanelet::AreaData> ptr;
+  ar >> ptr;
+  a = lanelet::Area(ptr);
+}
+template <typename Archive>
+void save(Archive& ar, const lanelet::WeakArea& a, unsigned int /*version*/) {
+  if (a.expired()) {
+    throw lanelet::LaneletError("Can not serialize expired weak pointer!");
+  }
+  auto sp = a.lock();
+  ar& sp;
+}
+template <typename Archive>
+void load(Archive& ar, lanelet::WeakArea& a, unsigned int /*version*/) {
+  lanelet::Area area;
+  ar& area;
+  a = area;
+}
+
+//! Regulatory element
+// Regluatory elements can not be directly serialized, because we need the factory function to create regelems of the
+// correct type (ie child of RegulatoryElement)
+class RegelemSerialization {
+ public:
+  bool currentlySerializing(const lanelet::RegulatoryElementPtr& regelem) {
+    if (serializing_.find(regelem->id()) == serializing_.end()) {
+      serializing_.insert(regelem->id());
+      return false;
+    }
+    return true;
+  }  // true if this is already serialized
+ private:
+  std::set<lanelet::Id> serializing_;
+};
+class RegelemDeserialization {
+  struct DeserialInfo {
+    lanelet::RegulatoryElementPtr deserialResult;
+    std::vector<lanelet::RegulatoryElementPtr*> toDeserialize;
+  };
+
+ public:
+  bool currentlyDeserializing(lanelet::Id id, lanelet::RegulatoryElementPtr& regelem) {
+    auto res = deserial_.find(id);
+    if (res == deserial_.end()) {
+      deserial_.emplace(id, DeserialInfo{});
+      return false;
+    }
+    if (!res->second.deserialResult) {
+      // wait until done
+      res->second.toDeserialize.push_back(&regelem);
+    } else {
+      regelem = res->second.deserialResult;
+    }
+
+    return true;
+  }
+  void deserializationDone(lanelet::RegulatoryElementPtr& regelem) {
+    auto& entry = deserial_.find(regelem->id())->second;
+    for (auto& elem : entry.toDeserialize) {
+      *elem = regelem;
+    }
+    entry.deserialResult = regelem;
+  }
+
+ private:
+  std::map<lanelet::Id, DeserialInfo> deserial_;
+};
+
+template <typename Archive>
+void serialize(Archive& /*ar*/, lanelet::RegulatoryElementData& /*r*/, unsigned int /*version*/) {}
+template <class Archive>
+// NOLINTNEXTLINE
+inline void save_construct_data(Archive& ar, const lanelet::RegulatoryElementData* r, unsigned int /*version*/) {
+  ar << r->id << r->attributes << r->parameters;
+}
+
+template <class Archive>
+// NOLINTNEXTLINE
+inline void load_construct_data(Archive& ar, lanelet::RegulatoryElementData* r, unsigned int /*version*/) {
+  using namespace lanelet;
+  Id id = 0;
+  AttributeMap attr;
+  RuleParameterMap params;
+  ar >> id >> attr >> params;
+  new (r) RegulatoryElementData(id, params, attr);
+}
+
+template <typename Archive>
+void save(Archive& ar, const lanelet::RegulatoryElementPtr& r, unsigned int /*version*/) {
+  auto id = r->id();
+  ar << id;
+  auto& helper = ar.template get_helper<RegelemSerialization>(&ar);
+  if (helper.currentlySerializing(r)) {
+    return;
+  }
+  auto ptr = lanelet::utils::removeConst(r->constData());
+  ar << ptr;
+}
+
+template <typename Archive>
+void load(Archive& ar, lanelet::RegulatoryElementPtr& r, unsigned int /*version*/) {
+  auto& helper = ar.template get_helper<RegelemDeserialization>(&ar);
+  lanelet::Id id = 0;
+  ar >> id;
+  if (helper.currentlyDeserializing(id, r)) {
+    return;
+  }
+  std::shared_ptr<lanelet::RegulatoryElementData> ptr;
+  ar >> ptr;
+  auto typeAttr = ptr->attributes.find(lanelet::AttributeName::Subtype);
+  auto regeltype = typeAttr == ptr->attributes.end() ? "" : typeAttr->second.value();
+  r = lanelet::RegulatoryElementFactory::create(regeltype, ptr);
+  helper.deserializationDone(r);  // will update all other instances using this regelem
+}
+
+template <typename Archive>
+void save(Archive& ar, const lanelet::RegulatoryElementConstPtr& r, unsigned int /*version*/) {
+  save(ar, const_pointer_cast<lanelet::RegulatoryElement>(r), 0);
+}
+
+template <typename Archive>
+void load(Archive& ar, lanelet::RegulatoryElementConstPtr& r, unsigned int /*version*/) {
+  load(ar, const_pointer_cast<lanelet::RegulatoryElement>(r), 0);
+}
+
+template <typename Archive>
+void save(Archive& ar, const lanelet::LaneletMap& m, unsigned int /*version*/) {
+  auto& mnc = const_cast<lanelet::LaneletMap&>(m);  // NOLINT
+  auto storeLayer = [&ar](auto& layer) {
+    size_t size = layer.size();
+    ar << size;
+    for (auto& pt : layer) {
+      ar << pt;
+    }
+  };
+  storeLayer(mnc.pointLayer);
+  storeLayer(mnc.lineStringLayer);
+  storeLayer(mnc.polygonLayer);
+  storeLayer(mnc.areaLayer);
+  storeLayer(mnc.laneletLayer);
+  storeLayer(mnc.regulatoryElementLayer);
+}
+
+template <typename Archive>
+void load(Archive& ar, lanelet::LaneletMap& m, unsigned int /*version*/) {
+  auto loadLayer = [&ar](auto& layerMap) {
+    using Prim = typename std::decay_t<decltype(layerMap)>::mapped_type;
+    size_t size = 0;
+    ar >> size;
+    for (auto i = 0u; i < size; ++i) {
+      Prim p;
+      ar >> p;
+      layerMap.insert(std::pair<const lanelet::Id, Prim>(lanelet::utils::getId(p), p));
+    }
+  };
+  lanelet::PointLayer::Map pMap;
+  lanelet::LineStringLayer::Map lsMap;
+  lanelet::PolygonLayer::Map polyMap;
+  lanelet::AreaLayer::Map arMap;
+  lanelet::LaneletLayer::Map lltMap;
+  lanelet::RegulatoryElementLayer::Map reMap;
+  loadLayer(pMap);
+  loadLayer(lsMap);
+  loadLayer(polyMap);
+  loadLayer(arMap);
+  loadLayer(lltMap);
+  loadLayer(reMap);
+  m = lanelet::LaneletMap(lltMap, arMap, reMap, polyMap, lsMap, pMap);
+}
+
+}  // namespace serialization
+}  // namespace boost
+
+BOOST_SERIALIZATION_SPLIT_FREE(lanelet::AttributeMap)
+BOOST_SERIALIZATION_SPLIT_FREE(lanelet::Attribute)
+BOOST_SERIALIZATION_SPLIT_FREE(lanelet::WeakArea)
+BOOST_SERIALIZATION_SPLIT_FREE(lanelet::Area)
+BOOST_SERIALIZATION_SPLIT_FREE(lanelet::WeakLanelet)
+BOOST_SERIALIZATION_SPLIT_FREE(lanelet::Lanelet)
+BOOST_SERIALIZATION_SPLIT_FREE(lanelet::Point3d)
+BOOST_SERIALIZATION_SPLIT_FREE(lanelet::ConstPoint3d)  // NOLINT
+BOOST_SERIALIZATION_SPLIT_FREE(lanelet::LineString3d)
+BOOST_SERIALIZATION_SPLIT_FREE(lanelet::ConstLineString3d)  // NOLINT
+BOOST_SERIALIZATION_SPLIT_FREE(lanelet::Polygon3d)
+BOOST_SERIALIZATION_SPLIT_FREE(lanelet::RuleParameterMap)
+BOOST_SERIALIZATION_SPLIT_FREE(lanelet::RegulatoryElementPtr)
+BOOST_SERIALIZATION_SPLIT_FREE(lanelet::RegulatoryElementConstPtr)
+BOOST_SERIALIZATION_SPLIT_FREE(lanelet::LaneletMap)

src/map/lanelet2/include/lanelet2_io/io_handlers/Writer.h

@@ -0,0 +1,39 @@
+#pragma once
+#include <lanelet2_core/LaneletMap.h>
+
+#include <iostream>
+#include <memory>
+
+#include "lanelet2_io/Projection.h"
+#include "lanelet2_io/io_handlers/IoHandler.h"
+
+namespace lanelet {
+namespace io_handlers {
+/**
+ * @brief Base object for writers.
+ * To create a new writer, you have to do the following steps:
+ * 1. Inherit from this writer
+ * 2. Overload the parse()-function, the name()-function (from IOHandler) and
+ * optionally the extension()-function (from
+ * IOHandler)
+ * 3. Inherit the constructors (using Writer::Writer)
+ * 4. register your writer using the Registerwriter object
+ */
+class Writer : public IOHandler {
+ public:
+  Writer() = default;
+  using IOHandler::IOHandler;
+  using Ptr = std::shared_ptr<Writer>;
+
+  virtual void write(const std::string& filename, const LaneletMap& laneletMap, ErrorMessages& errors) const = 0;
+
+  // IOHandler interface
+ private:
+  void handleDefaultProjector() const final {
+    std::cout << "Default origin should not be used when writing into a format that uses georeferenced lat/lon "
+                 "coordinates. Will continue to write the map, but the data will be dislocated and deformed"
+              << std::endl;
+  }
+};
+}  // namespace io_handlers
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_matching/Exceptions.h

@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2019
+ * FZI Forschungszentrum Informatik, Karlsruhe, Germany (www.fzi.de)
+ * KIT, Institute of Measurement and Control, Karlsruhe, Germany (www.mrt.kit.edu)
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#pragma once
+
+#include <lanelet2_core/Exceptions.h>
+#include <lanelet2_core/Forward.h>
+
+#include <stdexcept>
+
+namespace lanelet {
+
+/**
+ * @brief Thrown when matching is not possible.
+ */
+class MatchingError : public LaneletError {
+  using LaneletError::LaneletError;
+};
+
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_matching/LaneletMatching.h

@@ -0,0 +1,98 @@
+/*
+ * Copyright (c) 2019
+ * FZI Forschungszentrum Informatik, Karlsruhe, Germany (www.fzi.de)
+ * KIT, Institute of Measurement and Control, Karlsruhe, Germany (www.mrt.kit.edu)
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#pragma once
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+
+#include "Exceptions.h"
+#include "Types.h"
+#include "Utilities.h"
+
+namespace lanelet {
+namespace matching {
+
+/**
+ * @brief get deterministic lanelet matches of an object with a maximum distance of maxDist, sorted ascending by
+ * distance
+ */
+std::vector<LaneletMatch> getDeterministicMatches(LaneletMap& map, const Object2d& obj, double maxDist);
+std::vector<ConstLaneletMatch> getDeterministicMatches(const LaneletMap& map, const Object2d& obj, double maxDist);
+
+/**
+ * @brief get probabilistic lanelet matches of an object with a maximum deterministic euler distance of maxDist, sorted
+ * ascending by Mahalanobis distance
+ * @throws MatchingError if the orientationCovarianceRadians or the determinant of the position covariance is zero
+ *
+ * for the distance computation see D. Petrich, T. Dang, D. Kasper, G. Breuel and C. Stiller,
+ * "Map-based long term motion prediction for vehicles in traffic environments,"
+ * 16th International IEEE Conference on Intelligent Transportation Systems (ITSC 2013),
+ * The Hague, 2013, pp. 2166-2172. doi: 10.1109/ITSC.2013.6728549
+ * https://ieeexplore.ieee.org/document/6728549
+ */
+std::vector<LaneletMatchProbabilistic> getProbabilisticMatches(LaneletMap& map, const ObjectWithCovariance2d& obj,
+                                                               double maxDist);
+std::vector<ConstLaneletMatchProbabilistic> getProbabilisticMatches(const LaneletMap& map,
+                                                                    const ObjectWithCovariance2d& obj, double maxDist);
+
+/**
+ * @brief Determine whether an object is within a maximum distance to any primitive of the layer
+ */
+template <typename LayerT>
+bool isCloseTo(const LayerT& map, const Object2d& obj, double maxDist) {
+  auto closePrimitives = utils::findWithin(map, obj, maxDist);
+  return !closePrimitives.empty();
+}
+
+/**
+ * @brief Determine whether an object is (at least partially) within any primitive of the layer
+ */
+template <typename LayerT>
+bool isWithin(const LayerT& map, const Object2d& obj) {
+  return isCloseTo(map, obj, 0.);
+}
+
+/**
+ * @brief Remove non traffic rule compliant probabilistic lanelet matches
+ */
+template <typename MatchVectorT>
+MatchVectorT removeNonRuleCompliantMatches(const MatchVectorT& matches,
+                                           const lanelet::traffic_rules::TrafficRulesPtr& trafficRulesPtr) {
+  MatchVectorT compliantMatches = matches;
+  compliantMatches.erase(
+      std::remove_if(compliantMatches.begin(), compliantMatches.end(),
+                     [&trafficRulesPtr](auto& match) { return !trafficRulesPtr->canPass(match.lanelet); }),
+      compliantMatches.end());
+  return compliantMatches;
+}
+
+}  // namespace matching
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_matching/Types.h

@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2019
+ * FZI Forschungszentrum Informatik, Karlsruhe, Germany (www.fzi.de)
+ * KIT, Institute of Measurement and Control, Karlsruhe, Germany (www.mrt.kit.edu)
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#pragma once
+
+#include <lanelet2_core/Forward.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_core/primitives/Point.h>
+#include <lanelet2_core/primitives/Polygon.h>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace lanelet {
+namespace matching {
+
+using Pose2d = Eigen::Transform<double, 2, Eigen::Isometry, Eigen::DontAlign>;  //!< a 2d pose
+using PositionCovariance2d = Eigen::Matrix<double, 2, 2, Eigen::DontAlign>;     //!< a covariance of a 2d position
+using Hull2d = BasicPolygon2d;                                                  //!< a hull of 2d-points, as
+                                                                                //! objects are usually closed rings,
+                                                                                //! closing the ring by
+                                                                                //! appending the first point
+                                                                                //! of the polygon as last
+                                                                                //! point again is suggested
+
+struct Object2d {
+  Id objectId{InvalId};             //!< Id as convenience for the user, not used by this library
+  Pose2d pose{Pose2d::Identity()};  //!< Pose of the object in map coordinates
+  Hull2d absoluteHull;  //!< Hull of the object in map coordinates, position is used for matching when hull is empty
+};
+
+struct ObjectWithCovariance2d : Object2d {
+  PositionCovariance2d positionCovariance{PositionCovariance2d::Zero()};
+  double vonMisesKappa{0.};  //!< kappa as defined in https://en.wikipedia.org/wiki/Von_Mises_distribution
+};
+
+struct LaneletMatch {
+  Lanelet lanelet;
+  double distance{0.};  //!< euclidean distance to lanelet
+};
+
+struct ConstLaneletMatch {
+  ConstLanelet lanelet;
+  double distance{0.};  //!< euclidean distance to lanelet
+};
+
+struct LaneletMatchProbabilistic : LaneletMatch {
+  double mahalanobisDistSq{0.};  //!< squared Mahalanobis distance to closest point on centerline of lanelet
+};
+
+struct ConstLaneletMatchProbabilistic : ConstLaneletMatch {
+  double mahalanobisDistSq{0.};  //!< squared Mahalanobis distance to closest point on centerline of lanelet
+};
+
+}  // namespace matching
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_matching/Utilities.h

@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) 2019
+ * FZI Forschungszentrum Informatik, Karlsruhe, Germany (www.fzi.de)
+ * KIT, Institute of Measurement and Control, Karlsruhe, Germany (www.mrt.kit.edu)
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holder nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#pragma once
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/LaneletMap.h>
+#include <lanelet2_core/geometry/Polygon.h>
+
+#include "Exceptions.h"
+#include "Types.h"
+
+namespace lanelet {
+namespace matching {
+namespace utils {
+
+/**
+ * @brief Find all primitives as close as or closer than maxDist to an object
+ */
+template <typename LayerT>
+auto findWithin(LayerT& map, const Object2d& obj, double maxDist)
+    -> std::vector<std::pair<double, traits::LayerPrimitiveType<LayerT>>> {
+  if (obj.absoluteHull.empty()) {
+    return lanelet::geometry::findWithin2d(map, lanelet::BasicPoint2d{obj.pose.translation()}, maxDist);
+  }
+  return lanelet::geometry::findWithin2d(map, lanelet::BasicPolygon2d{obj.absoluteHull}, maxDist);
+}
+
+/**
+ * @brief Compute squared mahalanobis distance based on pose and covariance, hull is not used
+ * @throws MatchingError if the orientationCovarianceRadians or the determinant of the position covariance is zero
+ *
+ * see D. Petrich, T. Dang, D. Kasper, G. Breuel and C. Stiller,
+ * "Map-based long term motion prediction for vehicles in traffic environments,"
+ * 16th International IEEE Conference on Intelligent Transportation Systems (ITSC 2013),
+ * The Hague, 2013, pp. 2166-2172. doi: 10.1109/ITSC.2013.6728549
+ * https://ieeexplore.ieee.org/document/6728549
+ *
+ * uses approximation orientationCovariance = 1./obj.vonMisesKappa
+ */
+double getMahalanobisDistSq(const ConstLanelet& lanelet, const ObjectWithCovariance2d& obj);
+
+}  // namespace utils
+}  // namespace matching
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_projection/Mercator.h

@@ -0,0 +1,49 @@
+#pragma once
+#include <lanelet2_io/Projection.h>
+
+namespace lanelet {
+namespace projection {
+
+//! Implements the usual elliptical mercator projection
+//! This is the same transformation used by liblanelet(1).
+class Mercator : public Projector {
+ public:
+  explicit Mercator(const Origin& origin = Origin::defaultOrigin())
+      : Projector(origin), offset_{rawForward(origin.position)} {}
+
+  BasicPoint3d forward(const GPSPoint& pGps) const override { return rawForward(pGps) - offset_; }
+  GPSPoint reverse(const BasicPoint3d& p) const override { return rawReverse(p + offset_); }
+
+ private:
+  static BasicPoint3d rawForward(const GPSPoint& p) {
+    double lat = std::min(89.5, std::max(p.lat, -89.5));
+    double phi = lat * M_PI / 180.0;
+    double con = Eccent * std::sin(phi);
+    con = std::pow((1.0 - con) / (1.0 + con), 0.5 * Eccent);
+    double ts = std::tan(0.5 * (M_PI * 0.5 - phi)) / con;
+    const double y = -RMajor * std::log(ts);
+    const double x = RMajor * p.lon * M_PI / 180.;
+    return {x, y, p.ele};
+  }
+  static GPSPoint rawReverse(const BasicPoint3d& p) {
+    double ts = std::exp(-p.y() / RMajor);
+    double phi = M_PI / 2 - 2 * std::atan(ts);
+    double dphi = 1.0;
+    for (int i = 0; fabs(dphi) > 0.000000001 && i < 15; i++) {
+      double con = Eccent * sin(phi);
+      dphi = M_PI / 2 - 2 * atan(ts * pow((1.0 - con) / (1.0 + con), 0.5 * Eccent)) - phi;
+      phi += dphi;
+    }
+    const double lat = 180 / M_PI * phi;
+    const double lon = 180 / M_PI * p.x() / RMajor;
+    return {lat, lon, p.z()};
+  }
+
+  BasicPoint3d offset_{0, 0, 0};
+  static constexpr double RMajor{6378137.0};
+  static constexpr double RMinor{6356752.3142};
+  static constexpr double Eccent{0.081819190928906924};  //=std::sqrt(1.0 - RMinor*RMinor/RMajor/RMajor)
+};
+
+}  // namespace projection
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_projection/UTM.h

@@ -0,0 +1,22 @@
+#pragma once
+#include <lanelet2_io/Exceptions.h>
+#include <lanelet2_io/Projection.h>
+
+namespace lanelet {
+namespace projection {
+class UtmProjector : public Projector {
+ public:
+  explicit UtmProjector(Origin origin, bool useOffset = true, bool throwInPaddingArea = false);
+
+  BasicPoint3d forward(const GPSPoint& gps) const override;
+
+  GPSPoint reverse(const BasicPoint3d& utm) const override;
+
+ private:
+  int zone_{};
+  bool isInNorthernHemisphere_{true}, useOffset_{}, throwInPaddingArea_{};
+  double xOffset_{}, yOffset_{};
+};
+
+}  // namespace projection
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_routing/Exceptions.h

@@ -0,0 +1,23 @@
+#pragma once
+#include <lanelet2_core/Exceptions.h>
+#include <lanelet2_core/Forward.h>
+
+#include <stdexcept>
+
+namespace lanelet {
+/**
+ * @brief Thrown when an export to the provided file(name) cannot be done.
+ */
+class ExportError : public LaneletError {
+  using LaneletError::LaneletError;
+};
+
+/**
+ * @brief Thrown when an there's an error in the routing graph.
+ * It will feature further information.
+ */
+class RoutingGraphError : public LaneletError {
+  using LaneletError::LaneletError;
+};
+
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_routing/Forward.h

@@ -0,0 +1,126 @@
+#pragma once
+
+#include <lanelet2_core/Forward.h>
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace lanelet {
+namespace routing {
+namespace internal {
+
+using IdPair = std::pair<Id, Id>;
+
+template <typename BaseGraphT>
+class Graph;
+
+class RoutingGraphGraph;
+class RouteGraph;
+}  // namespace internal
+
+using LaneId = uint16_t;
+class RoutingGraph;
+using RoutingGraphPtr = std::shared_ptr<RoutingGraph>;
+using RoutingGraphUPtr = std::unique_ptr<RoutingGraph>;
+using RoutingGraphConstPtr = std::shared_ptr<const RoutingGraph>;
+
+class RoutingGraphContainer;
+using RoutingGraphContainerUPtr = std::unique_ptr<RoutingGraphContainer>;
+
+class Route;
+struct LaneletRelation;
+using LaneletRelations = std::vector<LaneletRelation>;
+
+using RouteUPtr = std::unique_ptr<Route>;
+using Routes = std::vector<Route>;
+class RoutingCost;
+using RoutingCostPtr = std::shared_ptr<RoutingCost>;
+using RoutingCostUPtr = std::unique_ptr<RoutingCost>;
+using RoutingCosts = std::vector<RoutingCost>;
+using RoutingCostUPtrs = std::vector<RoutingCostUPtr>;
+using RoutingCostPtrs = std::vector<RoutingCostPtr>;
+using RoutingCostId = uint16_t;
+
+class LaneletPath;
+using LaneletPaths = std::vector<LaneletPath>;
+class LaneletOrAreaPath;
+using LaneletOrAreaPaths = std::vector<LaneletOrAreaPath>;
+
+//! This enum expresses the types of relations lanelet2 distiguishes internally. Between two lanelets a and b (in this
+//! order), exactly one of these relation exists.
+//!
+//! @note The relation between b and a is different than between a and b. There is also no obvious
+//! symmetry. When a is left of b, b can be either right or adjacent right to b.
+enum class RelationType : uint8_t {
+  None = 0,                 //!< No relation
+  Successor = 0b1,          //!< A (the only) direct, reachable successor. Not merging and not diverging.
+  Left = 0b10,              //!< (the only) directly adjacent, reachable left neighbour
+  Right = 0b100,            //!< (the only) directly adjacent, reachable right neighbour
+  AdjacentLeft = 0b1000,    //!< directly adjacent, unreachable left neighbor
+  AdjacentRight = 0b10000,  //!< directly adjacent, unreachable right neighbor
+  Conflicting = 0b100000,   //!< Unreachable but with overlapping shape
+  Area = 0b1000000          //!< Adjacent to a reachable area
+};
+
+using RelationUnderlyingType = std::underlying_type_t<RelationType>;
+
+constexpr RelationType allRelations() { return static_cast<RelationType>(0b1111111); }
+static_assert(allRelations() > RelationType::Area, "allRelations is wrong!");
+
+constexpr RelationType operator~(RelationType r) { return RelationType(~RelationUnderlyingType(r)); }
+constexpr RelationType operator&(RelationType r1, RelationType r2) {
+  return RelationType(RelationUnderlyingType(r1) & RelationUnderlyingType(r2));
+}
+constexpr RelationType operator&=(RelationType& r1, RelationType r2) { return r1 = r1 & r2; }
+constexpr RelationType operator|(RelationType r1, RelationType r2) {
+  return RelationType(std::underlying_type_t<RelationType>(r1) | RelationUnderlyingType(r2));
+}
+constexpr RelationType operator|=(RelationType& r1, RelationType r2) { return r1 = r1 | r2; }
+
+// Used for graph export
+inline std::string relationToString(RelationType type) {
+  switch (type) {
+    case RelationType::None:
+      return "None";
+    case RelationType::Successor:
+      return "Successor";
+    case RelationType::Left:
+      return "Left";
+    case RelationType::Right:
+      return "Right";
+    case RelationType::AdjacentLeft:
+      return "AdjacentLeft";
+    case RelationType::AdjacentRight:
+      return "AdjacentRight";
+    case RelationType::Conflicting:
+      return "Conflicting";
+    case RelationType::Area:
+      return "Area";
+  }
+  return "";  // some compilers need that
+}
+
+inline std::string relationToColor(RelationType type) {
+  switch (type) {
+    case RelationType::None:
+      return "";
+    case RelationType::Successor:
+      return "green";
+    case RelationType::Left:
+      return "blue";
+    case RelationType::Right:
+      return "magenta";
+    case RelationType::Conflicting:
+      return "red";
+    case RelationType::AdjacentLeft:
+    case RelationType::AdjacentRight:
+      return "black";
+    case RelationType::Area:
+      return "yellow";
+  }
+  return "";  // some compilers need that
+}
+}  // namespace routing
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_routing/LaneletPath.h

@@ -0,0 +1,85 @@
+#pragma once
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_core/primitives/LaneletOrArea.h>
+#include <lanelet2_core/primitives/LaneletSequence.h>
+#include <lanelet2_core/utility/Optional.h>
+
+#include "lanelet2_routing/Forward.h"
+
+namespace lanelet {
+namespace routing {
+//! @brief A lanelet path represents a set of lanelets that can be reached in order by either driving straight or doing
+//! lane changes.
+class LaneletPath {
+ public:
+  using iterator = ConstLanelets::iterator;              // NOLINT
+  using const_iterator = ConstLanelets::const_iterator;  // NOLINT
+
+  explicit LaneletPath(ConstLanelets lanelets = {}) : lanelets_{std::move(lanelets)} {}
+
+  iterator begin() { return lanelets_.begin(); }
+  const_iterator begin() const { return lanelets_.begin(); }
+  iterator end() { return lanelets_.end(); }
+  const_iterator end() const { return lanelets_.end(); }
+  size_t size() const { return lanelets_.size(); }
+  bool empty() const { return lanelets_.empty(); }
+  const ConstLanelet& front() const { return lanelets_.front(); }
+  const ConstLanelet& back() const { return lanelets_.back(); }
+  const ConstLanelet& operator[](size_t idx) const {
+    assert(idx < lanelets_.size());
+    return lanelets_[idx];
+  }
+
+  bool operator!=(const LaneletPath& other) const { return !(*this == other); }
+
+  bool operator==(const LaneletPath& other) const { return lanelets_ == other.lanelets_; }
+
+  //! Returns all succeeding lanelets from the current position that can be reached without changing lanes
+  LaneletSequence getRemainingLane(const_iterator laneletPosition) const;
+
+  LaneletSequence getRemainingLane(const ConstLanelet& llt) const {
+    return getRemainingLane(std::find(lanelets_.begin(), lanelets_.end(), llt));
+  }
+
+ private:
+  ConstLanelets lanelets_;
+};
+
+//! Similar to LaneletPath, but can also contain areas.
+class LaneletOrAreaPath {
+ public:
+  using iterator = ConstLaneletOrAreas::iterator;              // NOLINT
+  using const_iterator = ConstLaneletOrAreas::const_iterator;  // NOLINT
+
+  explicit LaneletOrAreaPath(ConstLaneletOrAreas lltsOrArea = {}) : laneletsOrAreas_{std::move(lltsOrArea)} {}
+
+  iterator begin() { return laneletsOrAreas_.begin(); }
+  const_iterator begin() const { return laneletsOrAreas_.begin(); }
+  iterator end() { return laneletsOrAreas_.end(); }
+  const_iterator end() const { return laneletsOrAreas_.end(); }
+  size_t size() const { return laneletsOrAreas_.size(); }
+  bool empty() const { return laneletsOrAreas_.empty(); }
+  const ConstLaneletOrArea& front() const { return laneletsOrAreas_.front(); }
+  const ConstLaneletOrArea& back() const { return laneletsOrAreas_.back(); }
+  const ConstLaneletOrArea& operator[](size_t idx) const {
+    assert(idx < laneletsOrAreas_.size());
+    return laneletsOrAreas_[idx];
+  }
+
+  bool operator!=(const LaneletOrAreaPath& other) const { return !(*this == other); }
+
+  bool operator==(const LaneletOrAreaPath& other) const { return laneletsOrAreas_ == other.laneletsOrAreas_; }
+
+ private:
+  ConstLaneletOrAreas laneletsOrAreas_;
+};
+
+/**
+ * finds the Polygon containing all Lanelets and Areas in Path. All points on the polygon will be identical to points
+ * of primitives in path.
+ * @param path Path to merge
+ * @return Polygon containing all Lanelets and Areas in Path
+ */
+BasicPolygon3d getEnclosingPolygon3d(const LaneletOrAreaPath& path);
+}  // namespace routing
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_routing/Route.h

@@ -0,0 +1,207 @@
+#pragma once
+#include <lanelet2_core/Forward.h>
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/primitives/LaneletSequence.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "lanelet2_routing/Forward.h"
+#include "lanelet2_routing/LaneletPath.h"
+#include "lanelet2_routing/Types.h"
+
+namespace lanelet {
+namespace routing {
+
+/** @brief The famous route object that marks a route from A to B.
+ *
+ * - A Route includes all Lanelets that are adjacent to the Lanelets that represent the shortest path and lead
+ * towards the goal. Any Lanelet that is close, but not directly attached, e.g. the other way around a channeling island
+ * is not included. Any adjacent Lanelet which does not lead to the goal is also not included.
+ * - A Route does not have any relations to Lanelets that are not part of the Route (the only exception are
+ * conflicting Lanelets)
+ * - A Route is usually obtained from a RoutingGraph via "RoutingGraph::getRoute".
+ * - A Route is self-sustained in terms of that there is no relation to the RoutingGraph anymore
+ * - RelationTypes within a Route refer to relations of the Elements that are part of the Route. A lanelet can have
+ * fewer successors than in the routing graph if some are not part of the route
+ * - The route is divided into lanes with individual Ids. The exact value of the id for a lane is random. Ids are not
+ * continuous.
+ * - A new lane begins at every lanelet that has not exactly one predecessor or that is part of a diverging situation.
+ * Simple routes often just have one lane leading across multiple intersections.
+ * - It is recommended to check a couple of basic things with the "checkValidity" function once the Route is created
+ * - Routes can also be circular (e.g. if start and end lanelet are the same). The "last" lanelet of the route will then
+ * have the first lanelet of the route as successor.
+ * */
+class Route {
+ public:
+  using Errors = std::vector<std::string>;
+  Route();
+  Route(const Route& other) = delete;
+  Route& operator=(const Route& other) = delete;
+  Route& operator=(Route&& other) noexcept;
+  Route(Route&& other) noexcept;
+  ~Route() noexcept;
+
+  //! Constructs a route. Not supposed to be called directly. Use RoutingGraph to obtain routes.
+  Route(LaneletPath shortestPath, std::unique_ptr<internal::RouteGraph> graph,
+        LaneletSubmapConstPtr laneletSubmap) noexcept;
+
+  /** @brief Returns the shortest path that was the base of this route */
+  inline const LaneletPath& shortestPath() const noexcept { return shortestPath_; }
+
+  /**
+   * @brief Obtains the remaining shortest path to the destination. If the route is circular, the result will always
+   * have the same length and end before the lanelet passed as input argument
+   * @param ll a lanelet on the shortest path
+   * @return shortest path where lanelet is the first element. Nothing if the lanelet is not on the shortest path.
+   */
+  LaneletPath remainingShortestPath(const ConstLanelet& ll) const;
+
+  /** @brief Returns the complete lane a Lanelet belongs to. Circular lanes will always have 'll' as the first element.
+   *  @param ll Lanelet to get lane for
+   *  @return All lanelets of that route. Nothing if Lanelet is not part of the route.
+   *
+   * If
+   */
+  LaneletSequence fullLane(const ConstLanelet& ll) const;
+
+  /** @brief Returns that remaining lane a Lanelet belongs to
+   *  @param ll Lanelet to get remaining lane for
+   *  @return All remaining lanelets of that lane. Nothing if Lanelet is not part of the route.*/
+  LaneletSequence remainingLane(const ConstLanelet& ll) const;
+
+  /** @brief Get the 2d length of the shortest path of this route */
+  double length2d() const;
+
+  /** @brief Returns the number of individual lanes
+   *  @return Number of lanes */
+  size_t numLanes() const;
+
+  /** @brief A LaneletSubmap with all lanelets that are part of the route.
+   *
+   *  Can be used to do spatial lookups like 'which Lanelets of the route are close to my position'. It does not contain
+   * anything beyond that, no points, linestrings, etc.
+   */
+  inline LaneletSubmapConstPtr laneletSubmap() const noexcept { return laneletSubmap_; }
+
+  /** @brief A LaneletMap with all lanelets that are part of the route and those referenced by regelems.
+   *  @return A laneletMap with all lanelets of the route, excluding regulatory elements.
+   *  Can be used to do spatial lookups like 'which Lanelets of the route are close to my position'.
+   *  Note that not all lanelets in the map automatically belong to the route. They can also belong to one of the
+   * regulatory elements of the lanelet. Use Route::contains for that.
+   *
+   * Due to this behaviour that the map can contain lanelets that are not on the route, this function is deprecated.
+   * laneletSubmap() only returns lanelets on the map. To get the old behaviour use laneletSubmap()->laneletMap().
+   */
+  [[deprecated("Use laneletSubmap() to obtain a view on the elements within this route")]] inline LaneletMapConstPtr
+  laneletMap() const noexcept {
+    return laneletSubmap_->laneletMap();
+  }
+
+  /** @brief Get a laneletMap that represents the Lanelets of the Route and their relationship.
+   *  @return The laneletMap
+   *  Note that this laneletMap won't contain any Lanelets but rather just points and line string that represent
+   * the lanelets and their relationship. */
+  LaneletMapPtr getDebugLaneletMap() const;
+
+  /** @brief Number of Lanelets in the route
+   *  @return Number of lanelets.
+   */
+  size_t size() const;
+
+  /** @brief Provides information of the following lanelets within the Route
+   *  @param lanelet Lanelet to get information about.
+   * @return Relations to following lanelets in the route. Nothing if 'lanelet' is not part of the route. The relation
+   * will always be "succesor"
+   */
+  LaneletRelations followingRelations(const ConstLanelet& lanelet) const;
+
+  //! Similar to followingRelations but directly provides the following lanelets within the Route
+  ConstLanelets following(const ConstLanelet& lanelet) const;
+
+  /** @brief Provides information of the previous lanelets within the Route
+   *  @param lanelet Lanelet to get information about.
+   * @return Relations to previous lanelets in the route. Nothing if 'lanelet' is not part of the route. The type
+   * will always be "successor".
+   */
+  LaneletRelations previousRelations(const ConstLanelet& lanelet) const;
+
+  //! Similar to followingRelations but directly provides the following lanelets within the Route
+  ConstLanelets previous(const ConstLanelet& lanelet) const;
+
+  /** @brief Provides information of the lanelet left of a given lanelet within the Route
+   *  @note The returned lanelet can include a lanelet that can be switched or not-switched to. The relation will be
+   * "left" and "adjacent_left" accordingly.
+   *  @param lanelet Lanelet to get information about.
+   * @return Relation to lanelet left of the input lanelet in the route. Nothing if 'lanelet' is not part of the
+   * route.*/
+  Optional<LaneletRelation> leftRelation(const ConstLanelet& lanelet) const;
+
+  /** @brief Provides information of the *all* lanelets left of a given lanelet within the Route.
+   *  @note The returned lanelets include lanelets that can be switched or not-switched to. The relations will be
+   * "left" and "adjacent_left" accordingly.
+   *  @param lanelet Lanelet to get information about.
+   * @return Relations to lanelets left of the input lanelet in the route. Nothing if 'lanelet' is not part of the
+   * route.*/
+  LaneletRelations leftRelations(const ConstLanelet& lanelet) const;
+
+  /** @brief Provides information of the lanelet right of a given lanelet within the Route
+   *  @note The returned lanelet can include a lanelet that can be switched or not-switched to. The relation will be
+   * "right" and "adjacent_right" accordingly.
+   *  @param lanelet Lanelet to get information about.
+   * @return Relation to lanelet right of the input lanelet in the route. Nothing if 'lanelet' is not part of the
+   * route.*/
+  Optional<LaneletRelation> rightRelation(const ConstLanelet& lanelet) const;
+
+  /** @brief Provides information of the *all* lanelets right of a given lanelet within the Route.
+   *  @note The returned lanelets include lanelets that can be switched or not-switched to. The relations will be
+   * "right" and "adjacent_right" accordingly.
+   *  @param lanelet Lanelet to get information about.
+   * @return Relations to lanelets right of the input lanelet in the route. Nothing if 'lanelet' is not part of the
+   * route.*/
+  LaneletRelations rightRelations(const ConstLanelet& lanelet) const;
+
+  //! Can be used to search the route object with a custom function that is called for the successors of lanelet.
+  //! This function works similar to RoutingGraph::forEachSuccessor. Which costs and whether lane changes are used to
+  //! determine the shortest path depends on the cost id that was used to create this route object.
+  void forEachSuccessor(const ConstLanelet& lanelet, const LaneletVisitFunction& f) const;
+
+  //! Similar to forEachSuccessor but goes backwards in the routing graph instead of forwards.
+  void forEachPredecessor(const ConstLanelet& lanelet, const LaneletVisitFunction& f) const;
+
+  /** @brief Information about conflicting lanelets of a lanelet within the route
+   *  @param lanelet Lanelet to find conflicting lanelets to
+   *  @return Vector of conflicting lanelets. Empty vector if input lanelet is not part of the route.
+   *  @see conflictingInMap */
+  ConstLanelets conflictingInRoute(const ConstLanelet& lanelet) const;
+
+  /** @brief Information about conflicting lanelets of a lanelet within all passable lanelets in the laneletMap
+   *  @param lanelet Lanelet to find conflicting lanelets to
+   *  @return Vector of conflicting lanelets. Empty vector if input lanelet is not part of the route. Lanelets that are
+   * also conflicting in route are returned as well.
+   *  @see conflictingInRoute
+   */
+  ConstLaneletOrAreas conflictingInMap(const ConstLanelet& lanelet) const;
+
+  /**
+   * @brief Provides all lanelets in the map that conflict with any lanelet in the route.
+   */
+  ConstLaneletOrAreas allConflictingInMap() const;
+
+  //! Checks if a specific lanelet is part of the route
+  bool contains(const ConstLanelet& lanelet) const;
+
+  /** @brief Perform some sanity checks on the route
+   *  @param throwOnError Throw or not-throw an exception if errors are found
+   *  @throw Throws if errors are encountered and throwOnError is true
+   *  @return Vector of errors if no-throw is chosen and errors are found */
+  Errors checkValidity(bool throwOnError = false) const;
+
+ private:
+  std::unique_ptr<internal::RouteGraph> graph_;  ///< The internal graph
+  LaneletPath shortestPath_;                     ///< The underlying shortest path used to create the route
+  LaneletSubmapConstPtr laneletSubmap_;          ///< LaneletSubmap with all lanelets that are part of the route
+};
+}  // namespace routing
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_routing/RoutingCost.h

@@ -0,0 +1,126 @@
+#pragma once
+
+#include <lanelet2_core/Exceptions.h>
+#include <lanelet2_core/Forward.h>
+#include <lanelet2_core/primitives/LaneletOrArea.h>
+#include <lanelet2_traffic_rules/TrafficRules.h>
+
+#include "lanelet2_routing/Forward.h"
+
+namespace lanelet {
+namespace routing {
+
+/** @brief Abstract class to define a framework for custom routing cost calculation modules.
+ *  This interfaces can be implemented to allow routing cost calculation based on specific needs (e.g. road conditions).
+ *  As of now, two modules are implemented which should satisfy basic needs:
+ *    1. Distance-based calculation
+ *    2. Travel time-based calculation
+ * Routing cost modules *can* be used to prohibit a certain relation by setting the cost to infinity. Still it is
+ * generally better to use regulatory elements and the TrafficRules module to achieve this. The cost can represent
+ * whatever you want it to be - seconds, meter, lane changes, ... but it must not be negative! */
+class RoutingCost {  // NOLINT
+ public:
+  virtual ~RoutingCost() = default;
+
+  /** @brief Get the cost of the transistion from one to another lanelet
+   *  @param trafficRules TrafficRules module to apply
+   *  @param from The lanelet or area the traffic participant is on (assumed to be in the middle)
+   *  @param to The lanelet or area the traffic participant is reaching for (reference is the middle again)
+   *  @return Routing cost for passing this lanelet
+      @note The calculated cost will be assigned to the edge in the graph between 'from' and 'to'. So typically this
+   should represent the cost of passing the 'from'-lanelet with regards of going into the 'to'-lanelet.*/
+  virtual double getCostSucceeding(const traffic_rules::TrafficRules& trafficRules, const ConstLaneletOrArea& from,
+                                   const ConstLaneletOrArea& to) const = 0;
+
+  /** @brief Get the cost of the lane change between two adjacent lanelets
+   *  @param trafficRules TrafficRules module to apply
+   *  @param from The lanelet or area the traffic participant is on (assumed to be in the middle)
+   *  @param to The lanelet or area the traffic participant is reaching for (reference is the middle again)
+   *  @return Routing cost of the lane change
+      @note The calculated cost will be assigned to the edge in the graph between 'from' and 'to'. So typically this
+   should represent some artificial cost for a lane change. */
+  virtual double getCostLaneChange(const traffic_rules::TrafficRules& trafficRules, const ConstLanelets& from,
+                                   const ConstLanelets& to) const = 0;
+};
+
+/** @brief A basic distance-based routing cost module.
+ *  Uses the 2D length and a fixed lane change cost to evaluate relations. */
+class RoutingCostDistance : public RoutingCost {
+ public:
+  //! Distance cost of a lane change [m]. If a lane change requires less than minLaneChangeLength, no lane change will
+  //! be possible here. Instead, relation between the involved lanelets will be "adjacent".
+  explicit RoutingCostDistance(double laneChangeCost, double minLaneChangeLength = 0.)
+      : laneChangeCost_{laneChangeCost}, minChangeLength_{minLaneChangeLength} {
+    if (laneChangeCost_ < 0.0) {
+      throw InvalidInputError("Lane change cost must be positive, but it is " + std::to_string(laneChangeCost_));
+    }
+  }
+  inline double getCostSucceeding(const traffic_rules::TrafficRules& /*trafficRules*/, const ConstLaneletOrArea& from,
+                                  const ConstLaneletOrArea& to) const override {
+    auto getLength = [this](auto& lltOrArea) -> double { return this->length(lltOrArea); };
+    return (from.applyVisitor(getLength) + to.applyVisitor(getLength)) / 2;
+  }
+  inline double getCostLaneChange(const traffic_rules::TrafficRules& /*trafficRules*/, const ConstLanelets& from,
+                                  const ConstLanelets& /*to*/) const noexcept override {
+    if (minChangeLength_ <= 0.) {
+      return laneChangeCost_;
+    }
+    auto totalLength = std::accumulate(from.begin(), from.end(), 0.,
+                                       [this](double acc, auto& llt) { return acc + this->length(llt); });
+    return totalLength >= minChangeLength_ ? laneChangeCost_ : std::numeric_limits<double>::infinity();
+  }
+
+ private:
+  static double length(const ConstLanelet& ll) noexcept;
+  static double length(const ConstArea& ar) noexcept;
+  const double laneChangeCost_, minChangeLength_;  // NOLINT
+};
+
+/** @brief A basic travel time-based routing cost module.
+ *  Uses maximum allowed speed or the maximum speed of the participant (what every is lower) and a fixed lane chance
+ * cost. */
+class RoutingCostTravelTime : public RoutingCost {
+ public:
+  RoutingCostTravelTime() = delete;
+
+  //! Time cost of a lane change [s]. If the time is not sufficient for the lane change (i.e. minLaneChangeTime is not
+  //! reached), no lane change will possible at this position. Instead the relation between the involved lanelets is
+  //! "adjacent".
+  explicit RoutingCostTravelTime(double laneChangeCost, double minLaneChangeTime = 0.)
+      : laneChangeCost_{laneChangeCost}, minChangeTime_{minLaneChangeTime} {
+    if (laneChangeCost_ < 0.0) {
+      throw InvalidInputError("Lane change cost must be positive, but it is " + std::to_string(laneChangeCost_));
+    }
+  }
+
+  inline double getCostLaneChange(const traffic_rules::TrafficRules& trafficRules, const ConstLanelets& from,
+                                  const ConstLanelets& /*to*/) const noexcept override {
+    if (minChangeTime_ <= 0.) {
+      return laneChangeCost_;
+    }
+    auto changeTime = std::accumulate(from.begin(), from.end(), 0., [this, &trafficRules](double acc, auto& llt) {
+      return acc + this->travelTime(trafficRules, llt);
+    });
+    return changeTime >= minChangeTime_ ? laneChangeCost_ : std::numeric_limits<double>::infinity();
+  }
+  inline double getCostSucceeding(const traffic_rules::TrafficRules& trafficRules, const ConstLaneletOrArea& from,
+                                  const ConstLaneletOrArea& to) const override {
+    auto getTravelTime = [&trafficRules, this](auto& lltOrArea) -> double {
+      return this->travelTime(trafficRules, lltOrArea);
+    };
+    return (from.applyVisitor(getTravelTime) + to.applyVisitor(getTravelTime)) / 2;
+  }
+
+ private:
+  static double travelTime(const traffic_rules::TrafficRules& trafficRules, const ConstLanelet& ll);
+  static double travelTime(const traffic_rules::TrafficRules& trafficRules, const ConstArea& ar);
+  const Velocity maxSpeed_;                      // NOLINT
+  const double laneChangeCost_, minChangeTime_;  // NOLINT
+};
+
+//! Returns routing cost objects for initialization of routing graph with reasonable default values (for vehicles)
+inline RoutingCostPtrs defaultRoutingCosts() {
+  return RoutingCostPtrs{std::make_shared<RoutingCostDistance>(10.), std::make_shared<RoutingCostTravelTime>(5.)};
+}
+}  // namespace routing
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_routing/RoutingGraph.h

@@ -0,0 +1,500 @@
+#pragma once
+#include <lanelet2_core/Forward.h>
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_core/primitives/LaneletOrArea.h>
+#include <lanelet2_core/utility/Optional.h>
+
+#include <map>
+
+#include "lanelet2_routing/Forward.h"
+#include "lanelet2_routing/LaneletPath.h"
+#include "lanelet2_routing/RoutingCost.h"
+#include "lanelet2_routing/Types.h"
+
+namespace lanelet {
+namespace routing {
+
+/**
+ * @brief Controls the behaviour of the different possible path algorithms in RoutingGraph
+ *
+ * Consider the following graph (- or \ connects followers, | is a lane change):
+ * ```
+ * 1-2-3-4-5
+ *  \   /
+ *   6-7
+ *   | |
+ *   8-9
+ * ```
+ * Assuming we are using lanelet 1 as start of the search you can get the following results:
+ * - routingCostLimit=50 (and the rest the default): Will give you all optimal paths that are at least 50
+ * long (wrt to routing cost module zero, i.e. the first routing cost object from the "routingCosts"
+ * parameter that the routing graph was crated with), does not include lane changes. The paths end
+ * with the first lanelet that exceeds the 50 cost limit. Depending on the costs, the returned paths
+ * might be only the path 1-2-3 (assuming 1-6-7 is too short).
+ * - routingCostLimit=70: Will give you only 1-2-3-4, because 1-6-7-4 is a suboptimal path and 1-6-7 is too short.
+ * - routingCostLimit=70, includeShorterPaths=True: Will give you 1-2-3-4 and 1-6-7. 1-6-7-4 is not included because the
+ * path to 4 is suboptimal
+ * - elementLimit=3: Will give you all optimal paths with exactly 3 lanelets (excluding lane changes). The result
+ * will be 1-2-3 and 1-6-7
+ * - elementLimit=4: Will give you only 1-2-3-4, also because 1-6-7-4 is suboptimal
+ * - elementLimit=5, includeShorterPaths=true, includeLaneChanges=true: Will give you all optimal paths that
+ * are at most 5 lanelets/areas long, including lane changes. The result might be 1-2-3-4-5, 1-6-7,
+ * 1-8-9.
+ * - routingCostLimit=50, elementLimit=3, includeLaneChanges=true:  Will give you all optimal paths (including lane
+ * changes) where the last lanelet/area exceeds the 50 cost limit or that are 3 lanelets/areas long (whatever occurs
+ * first). The result might be 1-2-3, 1-6-7 and 1-8-9
+ * - routingCostLimit=70, elementLimit=4, includeShorterPaths=true: Will give you all optimal paths as above but also
+ * include shorter/cheaper paths. Result: 1-2-3-4, 1-6-7, 1-8-9.
+ */
+struct PossiblePathsParams {
+  Optional<double> routingCostLimit;  //!< cost limit for every path. Either that or maxElements must be valid.
+  Optional<uint32_t> elementLimit;    //!< element limit for every path. Effect depends on includeShorterPaths
+  RoutingCostId routingCostId{};      //!< the routing cost module to be used for the costs
+  bool includeLaneChanges{false};     //!< if true, returned paths will include lane changes
+  bool includeShorterPaths{false};    //!< also return paths that do not reach the limits
+};
+
+/** @brief Main class of the routing module that holds routing information and can be queried.
+ *  The RoutingGraph class is the central object of this module and is initialized with a LaneletMap, TrafficRules and
+ * RoutingCost.
+ *  A routing graph with all interesting relations will be created for the traffic participant featured in the provided
+ * TrafficRules module. Routing costs will be calculated for each provided module. The routing graph can answer queries
+ * like "left", "following", "conflicting" lanelets, but also provide a shortest route or a Route.
+ *
+ * @note The direction of lanelets matters! 'lanelet' and 'lanelet.invert()' are differentiated since this matters when
+ * lanelets are passable in both directions.
+ * @note 'adjacent_left' and 'adjacent_right' means that there is a passable lanelet left/right of another passable
+ * lanelet, but a lane change is not allowed. */
+class RoutingGraph {
+ public:
+  using Errors = std::vector<std::string>;                 ///< For the checkValidity function
+  using Configuration = std::map<std::string, Attribute>;  ///< Used to provide a configuration
+  //! Defined configuration attributes
+  static constexpr const char ParticipantHeight[] = "participant_height";
+
+  /** @brief Main constructor with optional configuration.
+   *  @param laneletMap Map that should be used to build the graph
+   *  @param trafficRules Traffic rules that apply to find passable lanelets
+   *  @param routingCosts One or more ways to calculate routing costs
+   *  @param config Optional configuration */
+  static RoutingGraphUPtr build(const LaneletMap& laneletMap, const traffic_rules::TrafficRules& trafficRules,
+                                const RoutingCostPtrs& routingCosts = defaultRoutingCosts(),
+                                const Configuration& config = Configuration());
+
+  //! Similar to the above but for a LaneletSubmap
+  static RoutingGraphUPtr build(const LaneletSubmap& laneletSubmap, const traffic_rules::TrafficRules& trafficRules,
+                                const RoutingCostPtrs& routingCosts = defaultRoutingCosts(),
+                                const Configuration& config = Configuration());
+
+  //! The graph can not be copied, only moved
+  RoutingGraph() = delete;
+  RoutingGraph(const RoutingGraph&) = delete;
+  RoutingGraph& operator=(const RoutingGraph&) = delete;
+  RoutingGraph(RoutingGraph&& /*other*/) noexcept;
+  RoutingGraph& operator=(RoutingGraph&& /*other*/) noexcept;
+  ~RoutingGraph();
+
+  /** @brief Get a driving route from 'start' to 'end' lanelet
+   *  @param from Start lanelet to find a shortest path
+   *  @param to End lanelet to find a shortest path
+   *  @param routingCostId ID of RoutingCost module to determine shortest route
+   *  @param withLaneChanges if false, the shortest path will not contain lane changes and the route will only contain
+   *  lanelets that are reachable without lane changes
+   *  @return A route of all adjacent lanelets of the shortest route that lead to 'end'. Nothing if there's no route.
+   *  @see Route */
+  Optional<Route> getRoute(const ConstLanelet& from, const ConstLanelet& to, RoutingCostId routingCostId = {},
+                           bool withLaneChanges = true) const;
+
+  /** @brief Get a driving route from 'start' to 'end' lanelets while going via other lanelets
+   *  @param from Start lanelet to find a shortest path
+   *  @param via Other lanelets to visit (in this order)
+   *  @param to End lanelet to find a shortest path
+   *  @param routingCostId ID of RoutingCost module to determine shortest route
+   *  @param withLaneChanges if false, the shortest path will not contain lane changes and the route will only contain
+   *  lanelets that are reachable without lane changes
+   *  @return A route of all adjacent lanelets of the shortest route that lead to 'end'. Nothing if there's not
+   * route.
+   *
+   * If from and to are the same lanelet, the resulting route will form a loop.
+   *  @see Route */
+  Optional<Route> getRouteVia(const ConstLanelet& from, const ConstLanelets& via, const ConstLanelet& to,
+                              RoutingCostId routingCostId = {}, bool withLaneChanges = true) const;
+
+  /** @brief Retrieve a shortest path between 'start' and 'end'.
+   *
+   *  Will find a shortest path using Djikstra's shortest path algorithm and the routing cost calculated by the
+   * routing cost module with the respective ID. Be aware that the shortest path may contain lane changes,
+   * i.e. lanelets that are parallel and not only adjacent.
+   *  @param from Start lanelet to find a shortest path
+   *  @param to End lanelet to find a shortest path
+   *  @param routingCostId ID of RoutingCost module to determine shortest path
+   *  @param withLaneChanges if false, the shortest path will not contain lane changes */
+  Optional<LaneletPath> shortestPath(const ConstLanelet& from, const ConstLanelet& to, RoutingCostId routingCostId = {},
+                                     bool withLaneChanges = true) const;
+
+  /** @brief Retrieve a shortest path between 'start' and 'end' that may contain Areas.
+   *
+   *  Will find a shortest path using Djikstra's shortest path algorithm and the routing cost calculated by the
+   * routing cost module with the respective ID. Be aware that the shortest path may contain lane changes,
+   * i.e. lanelets that are parallel and not only adjacent.
+   *  @param from Start lanelet or area to find a shortest path
+   *  @param to End lanelet or area to find a shortest path to
+   *  @param routingCostId ID of RoutingCost module to determine shortest path
+   *  @param withLaneChanges if false, the shortest path will not contain lane changes */
+  Optional<LaneletOrAreaPath> shortestPathIncludingAreas(const ConstLaneletOrArea& from, const ConstLaneletOrArea& to,
+                                                         RoutingCostId routingCostId = {},
+                                                         bool withLaneChanges = true) const;
+
+  /** @brief Retrieve a shortest path between 'start' and 'end' using intermediate points.
+   *  Will find a shortest path using Djikstra's shortest path algorithm and the routing cost calculated by the
+   * routing cost module with the respective ID. Be aware that the shortest path may contain lane changes,
+   * i.e. lanelets that are parallel and not only adjacent.
+   *  @param start Start lanelet to find a shortest path
+   *  @param via Intermediate lanelets that have to be passed
+   *  @param end End lanelet to find a shortest path
+   *  @param routingCostId ID of RoutingCost module to determine shortest path
+   *  @param withLaneChanges if false, the shortest path will not contain lane changes
+   *  @see shortestPath */
+  Optional<LaneletPath> shortestPathVia(const ConstLanelet& start, const ConstLanelets& via, const ConstLanelet& end,
+                                        RoutingCostId routingCostId = {}, bool withLaneChanges = true) const;
+
+  /** @brief Retrieve a shortest path between 'start' and 'end' using intermediate points.
+   *  Will find a shortest path using Djikstra's shortest path algorithm and the routing cost calculated by the
+   * routing cost module with the respective ID. Be aware that the shortest path may contain lane changes,
+   * i.e. lanelets that are parallel and not only adjacent.
+   *  @param start Start lanelet or area to find a shortest path
+   *  @param via Intermediate lanelets or areas that have to be passed
+   *  @param end End lanelet or area to find a shortest path
+   *  @param routingCostId ID of RoutingCost module to determine shortest path
+   *  @param withLaneChanges if false, the shortest path will not contain lane changes
+   *  @see shortestPath */
+  Optional<LaneletOrAreaPath> shortestPathIncludingAreasVia(const ConstLaneletOrArea& start,
+                                                            const ConstLaneletOrAreas& via,
+                                                            const ConstLaneletOrArea& end,
+                                                            RoutingCostId routingCostId = {},
+                                                            bool withLaneChanges = true) const;
+
+  /** @brief Determines the relation between two lanelets
+   *  @param from Start lanelet
+   *  @param to Goal lanelet
+   *  @param includeConflicting if false, conflicting lanelets are not considered as relations
+   *  @return Relation between the lanelets or nothing if no relation exists. Nothing if at least one of the lanelets
+   * is not passable. */
+  Optional<RelationType> routingRelation(const ConstLanelet& from, const ConstLanelet& to,
+                                         bool includeConflicting = false) const;
+
+  /** @brief Returns the lanelets that can be reached from this lanelet.
+   *  @param lanelet Start lanelet
+   *  @param withLaneChanges Include left and right lanes or not
+   *  @return Lanelets that can be directly reached
+   *  @see followingRelations */
+  ConstLanelets following(const ConstLanelet& lanelet, bool withLaneChanges = false) const;
+
+  /** @brief Returns the lanelets that can be reached from this lanelet and the relation.
+   *  @param lanelet Start lanelet
+   *  @param withLaneChanges Include left and right lanes or not
+   *  @return Lanelets can be directly reached
+   *  @see following */
+  LaneletRelations followingRelations(const ConstLanelet& lanelet, bool withLaneChanges = false) const;
+
+  /** @brief Returns the possible previous lanelets of this lanelet.
+   *  @param lanelet Start lanelet
+   *  @param withLaneChanges Include left and right lanes or not
+   *  @return All previous lanelets
+   *  @see previousRelations */
+  ConstLanelets previous(const ConstLanelet& lanelet, bool withLaneChanges = false) const;
+
+  /** @brief Returns the possible previous lanelets of this lanelet and the relation.
+   *  @param lanelet Start lanelet
+   *  @param withLaneChanges Include left and right lanes or not
+   *  @return Lanelets that could be used to reach this lanelet
+   *  @see previous */
+  LaneletRelations previousRelations(const ConstLanelet& lanelet, bool withLaneChanges = false) const;
+
+  /** @brief Retrieve all reachable left and right lanelets
+   *  @param lanelet Start lanelet
+   *  @param routingCostId the routing cost module to be used. Can make a difference if one allows a lane change but the
+   * other one doesn't.
+   *  @return All left and right lanelets that can be reached, including lanelet, ordered left to right. */
+  ConstLanelets besides(const ConstLanelet& lanelet, RoutingCostId routingCostId = {}) const;
+
+  /** @brief Get left (routable) lanelet of a given lanelet if it exists.
+   *  @param lanelet Start lanelet
+   *  @param routingCostId the routing cost module to be used. Can make a difference if one allows a lane change but the
+   * other one doesn't.
+   *  @return Left lanelet if it exists. Nothing if it doesn't.
+   *  @see adjacentLeft, lefts, adjacentLefts */
+  Optional<ConstLanelet> left(const ConstLanelet& lanelet, RoutingCostId routingCostId = {}) const;
+
+  /** @brief Get adjacent left (non-routable) lanelet of a given lanelet if it exists.
+   *  @param lanelet Start lanelet
+   *  @param routingCostId the routing cost module to be used. Can make a difference if one allows a lane change but the
+   * other one doesn't.
+   *  @return Adjacent left lanelet if it exists. Nothing if it doesn't.
+   *  @see left, lefts, adjacentLefts */
+  Optional<ConstLanelet> adjacentLeft(const ConstLanelet& lanelet, RoutingCostId routingCostId = {}) const;
+
+  /** @brief Get right (routable) lanelet of a given lanelet if it exists.
+   *  @param lanelet Start lanelet
+   *  @param routingCostId the routing cost module to be used. Can make a difference if one allows a lane change but the
+   * other one doesn't.
+   *  @return Right lanelet if it exists. Nothing if it doesn't.
+   *  @see adjacentRight, rights, adjacentRights */
+  Optional<ConstLanelet> right(const ConstLanelet& lanelet, RoutingCostId routingCostId = {}) const;
+
+  /** @brief Get adjacent right (non-routable) lanelet of a given lanelet if it exists.
+   *  @param lanelet Start lanelet
+   *  @param routingCostId the routing cost module to be used. Can make a difference if one allows a lane change but the
+   * other one doesn't.
+   *  @return Adjacent right lanelet if it exists. Nothing if it doesn't.
+   *  @see right, rights, adjacentRights */
+  Optional<ConstLanelet> adjacentRight(const ConstLanelet& lanelet, RoutingCostId routingCostId = {}) const;
+
+  /** @brief Get all left (routable) lanelets of a given lanelet if they exist.
+   *  @param lanelet Start lanelet
+   *  @param routingCostId the routing cost module to be used. Can make a difference if one allows a lane change but the
+   * other one doesn't.
+   *  @return Left lanelets if they exists. Empty if they don't.
+   *  @see adjacentLeft, left, adjacentLefts */
+  ConstLanelets lefts(const ConstLanelet& lanelet, RoutingCostId routingCostId = {}) const;
+
+  /** @brief Get all adjacent left (non-routable) lanelets of a given lanelet if they exist.
+   *  @param lanelet Start lanelet
+   *  @param routingCostId the routing cost module to be used. Can make a difference if one allows a lane change but the
+   * other one doesn't.
+   *  @return Adjacent left lanelets if they exists. Empty if they don't.
+   *  @see adjacentLeft, left, lefts */
+  ConstLanelets adjacentLefts(const ConstLanelet& lanelet, RoutingCostId routingCostId = {}) const;
+
+  /** @brief Retrieve all lanelets and relations left of a given lanelet.
+   *  @param lanelet Start lanelet
+   *  @param routingCostId the routing cost module to be used. Can make a difference if one allows a lane change but the
+   * other one doesn't.
+   *  @return All lanelets and relations left of a given lanelet.
+   *  @see lefts, adjacentLefts */
+  LaneletRelations leftRelations(const ConstLanelet& lanelet, RoutingCostId routingCostId = {}) const;
+
+  /** @brief Get all right (routable) lanelets of a given lanelet if they exist.
+   *  @param lanelet Start lanelet
+   *  @param routingCostId the routing cost module to be used. Can make a difference if one allows a lane change but the
+   * other one doesn't.
+   *  @return Right lanelets if they exists. Empty if they don't.
+   *  @see adjacentRight, right, adjacentRights */
+  ConstLanelets rights(const ConstLanelet& lanelet, RoutingCostId routingCostId = {}) const;
+
+  /** @brief Get all adjacent right (non-routable) lanelets of a given lanelet if they exist.
+   *  @param lanelet Start lanelet
+   *  @param routingCostId the routing cost module to be used. Can make a difference if one allows a lane change but the
+   * other one doesn't.
+   *  @return Adjacent right lanelets if they exists. Empty if they don't.
+   *  @see adjacentRight, right, rights */
+  ConstLanelets adjacentRights(const ConstLanelet& lanelet, RoutingCostId routingCostId = {}) const;
+
+  /** @brief Retrieve all lanelets and relations right of a given lanelet.
+   *  @param lanelet Start lanelet
+   *  @param routingCostId the routing cost module to be used. Can make a difference if one allows a lane change but the
+   * other one doesn't.
+   *  @return All lanelets and relations right of a given lanelet.
+   *  @see rights, adjacentRights */
+  LaneletRelations rightRelations(const ConstLanelet& lanelet, RoutingCostId routingCostId = {}) const;
+
+  /** @brief Retrieve all lanelets that are conflicting with the given lanelet.
+   *
+   *  Conflicting means that their bounding boxes overlap and the height clearance is smaller than the specified
+   * "participant_height".
+   *  @param laneletOrArea Lanelet/Area to get conflicting lanelets for.
+   *  @return All conflicting lanelets. */
+  ConstLaneletOrAreas conflicting(const ConstLaneletOrArea& laneletOrArea) const;
+
+  /** @brief Retrieve a set of lanelets that can be reached from a given lanelet
+   *
+   *  Determines which lanelets can be reached from a give start lanelets within a given amount of routing cost.
+   *  @param lanelet Start lanelet
+   *  @param maxRoutingCost Maximum amount of routing cost allowed to reach other lanelets
+   *  @param routingCostId ID of the routing cost module used for routing cost.
+   *  @param allowLaneChanges Allow or forbid lane changes
+   *  @return all lanelets that are reachable in no particular orders. "lanelet" itself is always included. */
+  ConstLanelets reachableSet(const ConstLanelet& lanelet, double maxRoutingCost, RoutingCostId routingCostId = {},
+                             bool allowLaneChanges = true) const;
+
+  //! Retrieve set of lanelet or areas that are reachable without exceeding routing cost.
+  ConstLaneletOrAreas reachableSetIncludingAreas(const ConstLaneletOrArea& llOrAr, double maxRoutingCost,
+                                                 RoutingCostId routingCostId = {}) const;
+
+  /** @brief Retrieve a set of lanelets that can reach a given lanelet
+   *
+   *  Determines which reach the given lanelet with a given amount of routing cost.
+   *  @param lanelet destination lanelet
+   *  @param maxRoutingCost Maximum amount of routing cost allowed to reach other lanelets
+   *  @param routingCostId ID of the routing cost module used for routing cost.
+   *  @param allowLaneChanges Allow or forbid lane changes
+   *  @return all lanelets in range in no particular order. "lanelet" itself is always included. */
+  ConstLanelets reachableSetTowards(const ConstLanelet& lanelet, double maxRoutingCost,
+                                    RoutingCostId routingCostId = {}, bool allowLaneChanges = true) const;
+
+  /**
+   * @brief Determines possible routes from a given start lanelet that satisfy the configuration in PossiblePathsParams
+   * @param startPoint Start lanelet
+   * @param params Parameters that configure the behaviour of the algorithm; see doc on PossiblePathsParams for details.
+   * @return all valid possible paths. If a lanelet can be reached
+   * using different paths, only the one is included that requires the least number of lane changes and has minimum
+   * routing costs. "startPoint" itself is always included if it is in the routing graph.
+   * @throws InvalidInputError if neither elementLimit nor costLimit is valid in params
+   */
+  LaneletPaths possiblePaths(const ConstLanelet& startPoint, const PossiblePathsParams& params) const;
+
+  /** @brief Determines possible routes from a given start lanelet that are "minRoutingCost"-long.
+   *
+   * This behaves exactly as the PossiblePathsParams version with params.costLimit=minRoutingCost,
+   * params.includeLaneChanges=allowLaneChanges and params.routingCostId=routingCostId (and the rest as default).
+   */
+  LaneletPaths possiblePaths(const ConstLanelet& startPoint, double minRoutingCost, RoutingCostId routingCostId = {},
+                             bool allowLaneChanges = false) const;
+
+  /** @brief Determines possible paths from a given start lanelet that are "minLanelets"-long.
+   *
+   * This behaves exactly as the PossiblePathsParams version with params.elementLimit=minLanelets,
+   * params.includeLaneChanges=allowLaneChanges and params.routingCostId=routingCostId (and the rest as default).
+   */
+  LaneletPaths possiblePaths(const ConstLanelet& startPoint, uint32_t minLanelets, bool allowLaneChanges = false,
+                             RoutingCostId routingCostId = {}) const;
+
+  /**
+   * @brief Determines possible routes to reach the given lanelet which satisfy the configuration in PossiblePathsParams
+   * @return possible paths that are at least as long as specified in 'minRoutingCost'. "targetLanelet" itself is
+   * always included if it is in the routing graph.
+   * @param targetLanelet Destination lanelet
+   * @param params Parameters that configure the behaviour of the algorithm; see doc on PossiblePathsParams for details.
+   * @throws InvalidInputError if neither elementLimit nor costLimit is valid in params */
+  LaneletPaths possiblePathsTowards(const ConstLanelet& targetLanelet, const PossiblePathsParams& params) const;
+
+  /** @brief Determines possible routes that reach the given lanelet and are "minRoutingCost" long.
+   *
+   * This behaves exactly as the PossiblePathsParams version with params.costLimit=minRoutingCost,
+   * params.includeLaneChanges=allowLaneChanges and params.routingCostId=routingCostId (and the rest as default). */
+  LaneletPaths possiblePathsTowards(const ConstLanelet& targetLanelet, double minRoutingCost,
+                                    RoutingCostId routingCostId = {}, bool allowLaneChanges = false) const;
+
+  /** @brief Determines possible paths towards a destination lanelet that are "minLanelets"-long.
+   *
+   * This behaves exactly as the PossiblePathsParams version with params.elementLimit=minLanelets,
+   * params.includeLaneChanges=allowLaneChanges and params.routingCostId=routingCostId (and the rest as default). */
+  LaneletPaths possiblePathsTowards(const ConstLanelet& targetLanelet, uint32_t minLanelets,
+                                    bool allowLaneChanges = false, RoutingCostId routingCostId = {}) const;
+
+  //! Similar to RoutingGraph::possiblePaths, but also considers areas.
+  LaneletOrAreaPaths possiblePathsIncludingAreas(const ConstLaneletOrArea& startPoint,
+                                                 const PossiblePathsParams& params) const;
+
+  //! Similar to RoutingGraph::possiblePaths, but also considers areas.
+  LaneletOrAreaPaths possiblePathsIncludingAreas(const ConstLaneletOrArea& startPoint, double minRoutingCost,
+                                                 RoutingCostId routingCostId = {}, bool allowLaneChanges = false) const;
+
+  //! Similar to RoutingGraph::possiblePaths, but also considers areas.
+  LaneletOrAreaPaths possiblePathsIncludingAreas(const ConstLaneletOrArea& startPoint, uint32_t minElements,
+                                                 bool allowLaneChanges = false, RoutingCostId routingCostId = {}) const;
+
+  /** @brief Calls a function on every successor of lanelet, optionally including lane changes
+   *
+   * This function can be used to query the routing graph on a more direct level. The function will be called on
+   * lanelets with monotonically increasing cost from the start lanelet, including the start lanelet itself. If the
+   * function returns "false" on an input, its followers will not be visited through this lanelet, as if it did not
+   * exist.
+   *
+   * The search internally uses the dijkstra algorithm in order to discover the shortest path to the successors of this
+   * lanelet and calls the provided function once it is known.
+   *
+   * In order to abort the query early, an exception can be thrown. If the lanelet is not part of the graph, nothing
+   * will be called.
+   * @param lanelet the lanelet where the search starts
+   * @param f the function to be called on lanelet and its successors
+   * @param allowLaneChanges also consider lane changes
+   * @param routingCostId id for the routing cost module that is used to calculate the shortest path
+   */
+  void forEachSuccessor(const ConstLanelet& lanelet, const LaneletVisitFunction& f, bool allowLaneChanges = true,
+                        RoutingCostId routingCostId = {}) const;
+
+  //! Similar to RoutingGraph::forEachSuccessor but also includes areas into the search
+  void forEachSuccessorIncludingAreas(const ConstLaneletOrArea& lanelet, const LaneletOrAreaVisitFunction& f,
+                                      bool allowLaneChanges = true, RoutingCostId routingCostId = {}) const;
+
+  //! Similar to RoutingGraph::forEachSuccessor but goes backwards in the routing graph instead of forward. The
+  //! LaneletVisitInformation::cost will still be positive, despite going backwards.
+  void forEachPredecessor(const ConstLanelet& lanelet, const LaneletVisitFunction& f, bool allowLaneChanges = true,
+                          RoutingCostId routingCostId = {}) const;
+
+  //! Similar to RoutingGraph::forEachPredecessor but also includes areas into the search
+  void forEachPredecessorIncludingAreas(const ConstLaneletOrArea& lanelet, const LaneletOrAreaVisitFunction& f,
+                                        bool allowLaneChanges = true, RoutingCostId routingCostId = {}) const;
+
+  /** @brief Export the internal graph to graphML (xml-based) file format.
+   *  @param filename Fully qualified file name - ideally with extension (.graphml)
+   *  @param edgeTypesToExclude Exclude the specified relations. E.g. conflicting. Combine them with "|".
+   *  @param routingCostId ID of the routing cost module
+      @see exportGraphViz */
+  void exportGraphML(const std::string& filename, const RelationType& edgeTypesToExclude = RelationType::None,
+                     RoutingCostId routingCostId = {}) const;
+
+  /** @brief Export the internal graph to graphViz (DOT) file format.
+   *  This format includes coloring of the edges in the graph and bears little more information than graphML export.
+   *  @param filename Fully qualified file name - ideally with extension (.gv)
+   *  @param edgeTypesToExclude Exclude the specified relations. E.g. conflicting. Combine them with "|".
+   *  @param routingCostId ID of the routing cost module */
+  void exportGraphViz(const std::string& filename, const RelationType& edgeTypesToExclude = RelationType::None,
+                      RoutingCostId routingCostId = {}) const;
+
+  /** @brief An abstract lanelet map holding the information of the routing graph.
+   *  A good way to view the routing graph since it can be exported using the lanelet2_io module and there can be
+   * viewed in tools like JOSM. Each lanelet is represented by a point at the center of gravity of the lanelet.
+   * Relations are linestrings between points representing lanelets.
+   *  @param routingCostId ID of the routing cost module used for the cost assignment
+   *  @param includeAdjacent Also include adjacent (non-routable) relations
+   *  @param includeConflicting Also include conflicting relations
+   *  @return LaneletMap with the requested information */
+  LaneletMapPtr getDebugLaneletMap(RoutingCostId routingCostId = {}, bool includeAdjacent = false,
+                                   bool includeConflicting = false) const;
+
+  /**
+   * @brief Returns a submap that contains all lanelets and areas within this routing graph, and nothing else.
+   * You can obtain a full map of the routing graph by calling passabelSubmap()->laneletMap(), which ist a potentially
+   * costly operation.
+   */
+  inline LaneletSubmapConstPtr passableSubmap() const noexcept { return passableLaneletSubmap_; }
+
+  /** @brief LaneletSubmap that includes all passable lanelets and areas.
+   *  This map contains all passable lanelets and areas with all primitives (linestrings, points), including regulatory
+   * elements and lanelets referenced by them. It can be used to perform spacial queries e.g. for localization. When
+   * selecting a lanelet from this map please be aware that the routing graph may also contain the inverted lanelet.
+   *  @return LaneletMap with all passable lanelets and areas
+   *
+   * This function is deprecated because it was misleading that the map also contained lanelets referenced by regulatory
+   * elements and not only the lanelets from the routing graph.
+   */
+  [[deprecated(
+      "Use passableSubmap to obtain the lanelets and areas within the routing graph!")]] inline LaneletMapConstPtr
+  passableMap() const noexcept {
+    return passableSubmap()->laneletMap();
+  }
+
+  /** @brief Performs some basic sanity checks.
+   *  It is recommended to call this function after the routing graph has been generated since it can point out some
+   * mapping errors.
+   *  @throws RoutingGraphError if an error is found an 'throwOnError' is true
+   *  @param throwOnError Decide wheter to throw an exception or just return the errors
+   *  @return Possible errors if 'throwOnError' is false. */
+  Errors checkValidity(bool throwOnError = true) const;
+
+  /**
+   * Constructs the routing graph. Don't call this directly, use RoutingGraph::build instead.
+   */
+  RoutingGraph(std::unique_ptr<internal::RoutingGraphGraph>&& graph, lanelet::LaneletSubmapConstPtr&& passableMap);
+
+ private:
+  //! Documentation to be found in the cpp file.
+  std::unique_ptr<internal::RoutingGraphGraph> graph_;  ///< Wrapper of the routing graph
+  LaneletSubmapConstPtr passableLaneletSubmap_;         ///< Lanelet map of all passable lanelets
+};
+
+}  // namespace routing
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_routing/RoutingGraphContainer.h

@@ -0,0 +1,107 @@
+#pragma once
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#include <algorithm>
+#include <unordered_set>
+
+#include "lanelet2_routing/Route.h"
+#include "lanelet2_routing/RoutingGraph.h"
+
+namespace lanelet {
+namespace routing {
+
+/** @brief Container to associate multiple routing graphs to allow queries on multiple graphs
+ *  @note We cannot use the 'conflicting' relations that have been determined when creating the individual graphs
+ * because they used their respective height in 3D (e.g. 2m for a pedestrian), but the participant we want to query for
+ * could be taller (e.g. 4m truck). Therefore we can't rely on that. */
+class RoutingGraphContainer {
+ public:
+  using ConflictingInGraph = std::pair<size_t, ConstLanelets>;  //!< id of conflicing graph, lanelets in conflict there
+  using ConflictingInGraphs = std::vector<ConflictingInGraph>;
+
+  /** @brief Constructor of routing graph container
+   *  @param routingGraphs The routing graphs that should be used in the container */
+  explicit RoutingGraphContainer(std::vector<RoutingGraphConstPtr> routingGraphs) : graphs_{std::move(routingGraphs)} {}
+
+  /** @brief Constructor of routing graph container
+   *  @param routingGraphs The routing graphs that should be used in the container */
+  explicit RoutingGraphContainer(const std::vector<RoutingGraphPtr>& routingGraphs)
+      : graphs_{utils::transform(routingGraphs, [](auto& g) { return RoutingGraphConstPtr(g); })} {}
+
+  /** @brief Find the conflicting lanelets of a given lanelet within a specified graph
+   *  @param lanelet Find conflicting ones for this lanelet
+   *  @param routingGraphId ID/position in vector of the routing graph
+   *  @param participantHeight Optional height of the participant
+   *  @return Conflicting lanelets within that graph
+   *  @throws InvalidInputError if the routingGraphId is too high */
+  ConstLanelets conflictingInGraph(const ConstLanelet& lanelet, size_t routingGraphId,
+                                   double participantHeight = .0) const {
+    if (routingGraphId >= graphs_.size()) {
+      throw InvalidInputError("Routing Graph ID is higher than the number of graphs.");
+    }
+    auto overlaps = [lanelet, participantHeight](const ConstLanelet& ll) {
+      return participantHeight != .0 ? !geometry::overlaps3d(lanelet, ll, participantHeight)
+                                     : !geometry::overlaps2d(lanelet, ll);
+    };
+    const auto map{graphs_[routingGraphId]->passableSubmap()};
+    ConstLanelets conflicting{map->laneletLayer.search(geometry::boundingBox2d(lanelet))};
+    auto begin = conflicting.begin();
+    auto end = conflicting.end();
+    end = std::remove(begin, end, lanelet);
+    end = std::remove_if(begin, end, overlaps);
+    conflicting.erase(end, conflicting.end());
+    return conflicting;
+  }
+
+  /** @brief Find the conflicting lanelets of a given lanelet within all graphs
+   *  @param lanelet Find conflicting ones for this lanelet
+   *  @param participantHeight Optional height of the participant
+   *  @return Conflicting lanelets within the graphs */
+  ConflictingInGraphs conflictingInGraphs(const ConstLanelet& lanelet, double participantHeight = .0) const {
+    ConflictingInGraphs result;
+    for (size_t it = 0; it < graphs_.size(); it++) {
+      result.emplace_back(std::make_pair(it, conflictingInGraph(lanelet, it, participantHeight)));
+    }
+    return result;
+  }
+
+  /** @brief Find the conflicting lanelets of a given route within a specified graph
+   *  @param route Find conflicting lanelets for all lanelets within this route
+   *  @param routingGraphId ID/position in vector of the routing graph
+   *  @param participantHeight Optional height of the participant
+   *  @return Conflicting lanelets within that graph to any lanelet within the route
+   *  @throws InvalidInputError if the routingGraphId is too high */
+  ConstLanelets conflictingOfRouteInGraph(const Route* route, size_t routingGraphId,
+                                          double participantHeight = .0) const {
+    std::unordered_set<ConstLanelet> conflicting;
+    for (const auto& it : route->laneletSubmap()->laneletLayer) {
+      ConstLanelets tempConf{conflictingInGraph(it, routingGraphId, participantHeight)};
+      conflicting.insert(tempConf.begin(), tempConf.end());
+    }
+    return ConstLanelets(conflicting.begin(), conflicting.end());
+  }
+
+  /** @brief Find the conflicting lanelets of a given route within all graphs
+   *  @param route Find conflicting lanelets for all lanelets within this route
+   *  @param participantHeight Optional height of the participant
+   *  @return Conflicting lanelets of each graph to any lanelet within the route */
+  ConflictingInGraphs conflictingOfRouteInGraphs(const Route* route, double participantHeight = .0) const {
+    ConflictingInGraphs result;
+    for (size_t it = 0; it < graphs_.size(); it++) {
+      result.emplace_back(std::make_pair(it, conflictingOfRouteInGraph(route, it, participantHeight)));
+    }
+    return result;
+  }
+
+  //! Returns the routing graphs stored in the container
+  const std::vector<RoutingGraphConstPtr>& routingGraphs() const { return graphs_; }
+
+ private:
+  std::vector<RoutingGraphConstPtr> graphs_;  ///< Routing graphs of the container.
+};
+
+}  // namespace routing
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_routing/Types.h

@@ -0,0 +1,46 @@
+#pragma once
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_core/primitives/LaneletOrArea.h>
+
+#include <functional>
+
+#include "lanelet2_routing/Forward.h"
+
+namespace lanelet {
+namespace routing {
+
+//! This object carries the required information for the graph neighbourhood search
+struct LaneletOrAreaVisitInformation {
+  ConstLaneletOrArea laneletOrArea;  //!< The lanelet or area that is currently visited
+  ConstLaneletOrArea predecessor;    //!< Its predecessor on the shortest path
+  double cost{};                     //!< The accumulated cost from the start along shortest path
+  size_t length{};                   //!< Number of elements from start to here along shortest path (including this)
+  size_t numLaneChanges{};           //!< Number of lane changes from start to here along shortest path
+};
+
+//! This object carries the required information for the graph neighbourhood search
+struct LaneletVisitInformation {
+  ConstLanelet lanelet;      //!< The lanelet or area that is currently visited
+  ConstLanelet predecessor;  //!< Its predecessor on the shortest path
+  double cost{};             //!< The accumulated cost from the start along shortest path
+  size_t length{};           //!< The number of lanelets from start to here along shortest path (including this)
+  size_t numLaneChanges{};   //!< Number of lane changes from start to here along shortest path
+};
+
+using LaneletVisitFunction = std::function<bool(const LaneletVisitInformation&)>;
+using LaneletOrAreaVisitFunction = std::function<bool(const LaneletOrAreaVisitInformation&)>;
+
+//! Represents the relation of a lanelet to another lanelet
+struct LaneletRelation {
+  ConstLanelet lanelet;       //!< the lanelet this relation refers to
+  RelationType relationType;  //!< the type of relation to that
+};
+inline bool operator==(const LaneletRelation& lhs, const LaneletRelation& rhs) {
+  return lhs.lanelet == rhs.lanelet && lhs.relationType == rhs.relationType;
+}
+inline bool operator!=(const LaneletRelation& rhs, const LaneletRelation& lhs) { return !(rhs == lhs); }
+
+using LaneletRelations = std::vector<LaneletRelation>;
+
+}  // namespace routing
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_routing/internal/Graph.h

@@ -0,0 +1,254 @@
+#pragma once
+
+#include <lanelet2_core/primitives/LaneletOrArea.h>
+
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/graph/filtered_graph.hpp>
+#include <map>
+#include <utility>
+
+#include "lanelet2_routing/Exceptions.h"
+#include "lanelet2_routing/Forward.h"
+
+namespace lanelet {
+namespace routing {
+namespace internal {
+/** @brief Internal information of a vertex in the graph
+ *  If A* search is adapted, this could hold information about longitude and latitude. */
+struct VertexInfo {
+  // careful. You must be sure that this is indeed a lanelet
+  const ConstLanelet& lanelet() const { return static_cast<const ConstLanelet&>(laneletOrArea); }
+  const ConstArea& area() const { return static_cast<const ConstArea&>(laneletOrArea); }
+  const ConstLaneletOrArea& get() const { return laneletOrArea; }
+  ConstLaneletOrArea laneletOrArea;
+};
+
+/** @brief Internal information of a vertex in the route graph */
+struct RouteVertexInfo {
+  const ConstLanelet& get() const { return lanelet; }
+
+  ConstLanelet lanelet;
+  LaneId laneId{};
+  ConstLaneletOrAreas conflictingInMap;
+};
+
+/** @brief Internal information of an edge in the graph */
+struct EdgeInfo {
+  double routingCost;     ///< Calculcated routing cost. Infinity if not routable
+  RoutingCostId costId;   ///< ID of the routing cost module that was used to calculate cost
+  RelationType relation;  ///< Relation between the two lanelets. E.g. SUCCESSOR or CONFLICTING.
+};
+
+/// General graph type definitions
+using GraphType = boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS, VertexInfo, EdgeInfo>;
+using RouteGraphType =
+    boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS, RouteVertexInfo, EdgeInfo>;
+using GraphTraits = boost::graph_traits<GraphType>;
+
+template <typename BaseGraphT>
+struct EdgeCostFilter;
+
+/// Filtered graphs provide a filtered view on a graph. Here we can filter out types of relations (e.g. CONFLICTING) or
+/// filter by routing cost module ID.
+template <typename BaseGraphT>
+using FilteredGraphT = boost::filtered_graph<BaseGraphT, EdgeCostFilter<BaseGraphT>>;
+
+template <typename BaseGraphT>
+using FilteredGraphTraits = boost::graph_traits<FilteredGraphT<BaseGraphT>>;
+
+using FilteredRoutingGraph = FilteredGraphT<GraphType>;
+using FilteredRouteGraph = FilteredGraphT<RouteGraphType>;
+
+/** @brief An internal edge filter to get a filtered view on the graph. */
+template <typename GraphT>
+struct EdgeCostFilter {
+  /// Needed to be able to iterate edges
+  EdgeCostFilter() = default;
+
+  /// @brief Constructor for a filter that includes all relation types.
+  /// @param graph Base routing graph
+  /// @param routingCostId Edges with this routing cost ID will pass
+  EdgeCostFilter(const GraphT& graph, RoutingCostId routingCostId)
+      : routingCostId_{routingCostId},
+        pmRelation_{boost::get(&EdgeInfo::relation, graph)},
+        pmIds_{boost::get(&EdgeInfo::costId, graph)} {}
+
+  /// @brief Constructor for a filter that includes all allowed relations in 'relation'
+  /// @param graph Base routing graph
+  /// @param routingCostId Edges with this routing cost ID will pass
+  /// @param relation Relation that will pass the filter
+  EdgeCostFilter(const GraphT& graph, RoutingCostId routingCostId, const RelationType& relation)
+      : routingCostId_{routingCostId},
+        relations_{relation},
+        pmRelation_{boost::get(&EdgeInfo::relation, graph)},
+        pmIds_{boost::get(&EdgeInfo::costId, graph)} {}
+
+  /// @brief Operator that determines wheter an edge should pass the filter or not.
+  template <typename Edge>
+  inline bool operator()(const Edge& e) const {
+    return boost::get(pmIds_, e) == routingCostId_ &&
+           (relations_ == allRelations() || bool(relations_ & boost::get(pmRelation_, e)));
+  }
+
+ private:
+  RoutingCostId routingCostId_{0};          ///< Id of the routing cost functor to use
+  RelationType relations_{allRelations()};  ///< Relations that pass the filter
+  typename boost::property_map<const GraphT, RelationType EdgeInfo::*>::const_type
+      pmRelation_;  ///< Property map to the relations of edges in the graph
+  typename boost::property_map<const GraphT, RoutingCostId EdgeInfo::*>::const_type
+      pmIds_;  ///< Property map to the routing cost IDs of the edges
+};
+
+using LaneletOrAreaToVertex = std::unordered_map<ConstLaneletOrArea, std::uint32_t>;
+using FilteredGraphDesc = std::pair<size_t, RelationType>;
+
+/// @brief Manages the actual routing graph and provieds different views on the edges (lazily computed)
+template <typename BaseGraphT>
+class Graph {
+ public:
+  using FilteredGraph = FilteredGraphT<BaseGraphT>;
+  using CostFilter = EdgeCostFilter<BaseGraphT>;
+  using Vertex = typename boost::graph_traits<BaseGraphT>::vertex_descriptor;
+  using Edge = typename boost::graph_traits<BaseGraphT>::edge_descriptor;
+
+  explicit Graph(size_t numRoutingCosts) : numRoutingCosts_{numRoutingCosts} {}
+
+  inline const BaseGraphT& get() const noexcept { return graph_; }
+  inline BaseGraphT& get() noexcept { return graph_; }
+  inline size_t numRoutingCosts() const noexcept { return numRoutingCosts_; }
+  inline const LaneletOrAreaToVertex& vertexLookup() const noexcept { return laneletOrAreaToVertex_; }
+
+  FilteredGraph withLaneChanges(RoutingCostId routingCostId = 0) const {
+    return getFilteredGraph(routingCostId, RelationType::Successor | RelationType::Left | RelationType::Right);
+  }
+
+  FilteredGraph withoutLaneChanges(RoutingCostId routingCostId = 0) const {
+    return getFilteredGraph(routingCostId, RelationType::Successor);
+  }
+
+  FilteredGraph withAreasAndLaneChanges(RoutingCostId routingCostId = 0) const {
+    return getFilteredGraph(routingCostId,
+                            RelationType::Successor | RelationType::Left | RelationType::Right | RelationType::Area);
+  }
+
+  FilteredGraph withAreasWithoutLaneChanges(RoutingCostId routingCostId = 0) const {
+    return getFilteredGraph(routingCostId, RelationType::Successor | RelationType::Area);
+  }
+
+  FilteredGraph left(RoutingCostId routingCostId = 0) const {
+    return getFilteredGraph(routingCostId, RelationType::Left);
+  }
+  FilteredGraph somehowLeft(RoutingCostId routingCostId = 0) const {
+    return getFilteredGraph(routingCostId, RelationType::Left | RelationType::AdjacentLeft);
+  }
+
+  FilteredGraph right(RoutingCostId routingCostId = 0) const {
+    return getFilteredGraph(routingCostId, RelationType::Right);
+  }
+  FilteredGraph somehowRight(RoutingCostId routingCostId = 0) const {
+    return getFilteredGraph(routingCostId, RelationType::Right | RelationType::AdjacentRight);
+  }
+
+  FilteredGraph adjacentLeft(RoutingCostId routingCostId = 0) const {
+    return getFilteredGraph(routingCostId, RelationType::AdjacentLeft);
+  }
+
+  FilteredGraph adjacentRight(RoutingCostId routingCostId = 0) const {
+    return getFilteredGraph(routingCostId, RelationType::AdjacentRight);
+  }
+
+  FilteredGraph conflicting(RoutingCostId routingCostId = 0) const {
+    return getFilteredGraph(routingCostId, RelationType::Conflicting);
+  }
+
+  FilteredGraph withoutConflicting(RoutingCostId routingCostId = 0) const {
+    return getFilteredGraph(routingCostId, allRelations() | ~RelationType::Conflicting);
+  }
+
+  inline bool empty() const noexcept { return laneletOrAreaToVertex_.empty(); }
+
+  //! add new lanelet to graph
+  inline Vertex addVertex(const typename BaseGraphT::vertex_property_type& property) {
+    GraphType::vertex_descriptor vd = 0;
+    vd = boost::add_vertex(graph_);
+    graph_[vd] = property;
+    laneletOrAreaToVertex_.emplace(property.get(), vd);
+    return vd;
+  }
+
+  void addEdge(const ConstLaneletOrArea& from, const ConstLaneletOrArea& to, const EdgeInfo& edgeInfo) {
+    auto fromVertex = getVertex(from);
+    auto toVertex = getVertex(to);
+    if (!fromVertex || !toVertex) {
+      assert(false && "Lanelet/Area is not part of the graph.");  // NOLINT
+      return;
+    }
+    addEdge(*fromVertex, *toVertex, edgeInfo);
+  }
+
+  void addEdge(Vertex from, Vertex to, const EdgeInfo& edgeInfo) {
+    if (!std::isfinite(edgeInfo.routingCost)) {
+      return;
+    }
+    if (edgeInfo.routingCost < 0.) {
+      throw RoutingGraphError{"Negative costs calculated by routing cost module!"};
+    }
+    auto edge = boost::add_edge(from, to, graph_);
+    assert(edge.second && "Edge could not be added to the graph.");
+    graph_[edge.first] = edgeInfo;
+  }
+
+  /** @brief Received the edgeInfo between two given vertices
+   *  @return EdgeInfo or nothing if there's no edge */
+  Optional<EdgeInfo> getEdgeInfo(const ConstLanelet& from, const ConstLanelet& to) const noexcept {
+    return getEdgeInfoFor(from, to, graph_);
+  }
+
+  /** @brief Received the edgeInfo between two given vertices and a given (filtered)graph
+   *  @return EdgeInfo or nothing if there's no edge */
+  template <typename Graph>
+  Optional<EdgeInfo> getEdgeInfoFor(const ConstLanelet& from, const ConstLanelet& to, const Graph& g) const noexcept {
+    auto fromVertex = getVertex(from);
+    auto toVertex = getVertex(to);
+    if (!fromVertex || !toVertex) {
+      return {};
+    }
+    auto edgeToNext{boost::edge(*fromVertex, *toVertex, g)};
+    if (edgeToNext.second) {
+      return g[edgeToNext.first];
+    }
+    return {};
+  }
+
+  //! Helper function to determine the graph vertex of a given lanelet
+  Optional<typename boost::graph_traits<BaseGraphT>::vertex_descriptor> getVertex(
+      const ConstLaneletOrArea& lanelet) const noexcept {
+    try {
+      return laneletOrAreaToVertex_.at(lanelet);
+    } catch (std::out_of_range&) {
+      return {};
+    }
+  }
+
+ private:
+  FilteredGraph getFilteredGraph(RoutingCostId routingCostId, RelationType relations) const {
+    if (routingCostId >= numRoutingCosts_) {
+      throw InvalidInputError("Routing Cost ID is higher than the number of routing modules.");
+    }
+    return FilteredGraph(graph_, CostFilter(graph_, routingCostId, relations));
+  }
+  BaseGraphT graph_;                             //!< The actual graph object
+  LaneletOrAreaToVertex laneletOrAreaToVertex_;  //!< Mapping of lanelets/areas to vertices of the graph
+  size_t numRoutingCosts_;                       //!< Number of available routing cost calculation methods
+};
+
+class RoutingGraphGraph : public Graph<GraphType> {
+  using Graph::Graph;
+};
+class RouteGraph : public Graph<RouteGraphType> {
+  using Graph::Graph;
+};
+
+}  // namespace internal
+}  // namespace routing
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_routing/internal/GraphUtils.h

@@ -0,0 +1,400 @@
+#pragma once
+#include <lanelet2_core/utility/Optional.h>
+
+#include <boost/graph/depth_first_search.hpp>
+#include <boost/graph/two_bit_color_map.hpp>
+
+#include "lanelet2_routing/internal/Graph.h"
+
+namespace lanelet {
+namespace routing {
+namespace internal {
+using LaneletVertexId = GraphTraits::vertex_descriptor;
+using LaneletVertexIds = std::vector<LaneletVertexId>;
+using RouteLanelets = std::set<LaneletVertexId>;
+
+template <typename ContainerT, typename T>
+inline bool has(const ContainerT& c, const T& t) {
+  return std::find(c.begin(), c.end(), t) != c.end();
+}
+template <typename ContainerT, typename T>
+inline bool has(const std::set<T>& c, const T& t) {
+  return c.find(t) != c.end();
+}
+template <RelationType R, typename GraphT, typename EdgeT>
+inline bool hasRelation(const GraphT& g, EdgeT e) {
+  return (g[e].relation & R) != RelationType::None;
+}
+template <RelationType R, typename Graph>
+Optional<LaneletVertexId> getNext(LaneletVertexId ofVertex, const Graph& g) {
+  auto edges = boost::out_edges(ofVertex, g);
+  auto it = std::find_if(edges.first, edges.second, [&](auto e) { return hasRelation<R>(g, e); });
+  if (it != edges.second) {
+    return boost::target(*it, g);
+  }
+  return {};
+}
+
+// Class that selects between in_edges and out_edges (i wish I had c++17...)
+template <bool Backwards>
+struct GetEdges {};
+template <>
+struct GetEdges<true> {
+  template <typename Id, typename Graph>
+  inline auto operator()(Id id, Graph& g) {
+    return in_edges(id, g);
+  }
+};
+template <>
+struct GetEdges<false> {
+  template <typename Id, typename Graph>
+  inline auto operator()(Id id, Graph& g) {
+    return out_edges(id, g);
+  }
+};
+
+// Class that selects between in_edges and out_edges (i wish I had c++17...)
+template <bool Backwards>
+struct GetTarget {};
+template <>
+struct GetTarget<true> {
+  using T = FilteredRoutingGraph::vertex_descriptor;
+  template <typename Id, typename Graph>
+  inline T operator()(Id id, Graph& g) {
+    return boost::source(id, g);
+  }
+};
+template <>
+struct GetTarget<false> {
+  using T = FilteredRoutingGraph::vertex_descriptor;
+  template <typename Id, typename Graph>
+  inline T operator()(Id id, Graph& g) {
+    return boost::target(id, g);
+  }
+};
+
+template <typename Vertex, typename Graph, typename Func>
+bool anySidewayNeighbourIs(Vertex v, const Graph& g, Func&& f) {
+  Optional<LaneletVertexId> currVertex = v;
+  while (!!currVertex && !f(*currVertex)) {
+    currVertex = getNext<RelationType::Left>(*currVertex, g);
+  }
+  if (!!currVertex) {
+    return true;
+  }
+  currVertex = getNext<RelationType::Right>(v, g);
+  while (!!currVertex && !f(*currVertex)) {
+    currVertex = getNext<RelationType::Right>(*currVertex, g);
+  }
+  return !!currVertex;
+}
+
+template <typename Graph>
+std::set<LaneletVertexId> getAllNeighbourLanelets(LaneletVertexId ofVertex, const Graph& ofRoute) {
+  std::set<LaneletVertexId> result{ofVertex};
+  Optional<LaneletVertexId> currVertex = ofVertex;
+  while (!!currVertex) {
+    result.insert(*currVertex);
+    currVertex = getNext<RelationType::Left>(*currVertex, ofRoute);
+  }
+  currVertex = ofVertex;
+  while (!!currVertex) {
+    result.insert(*currVertex);
+    currVertex = getNext<RelationType::Right>(*currVertex, ofRoute);
+  }
+  return result;
+}
+
+//! Filter that reduces the original graph by edges that belong to different cost types or lane changes
+class OriginalGraphFilter {
+ public:
+  OriginalGraphFilter() = default;
+  OriginalGraphFilter(const GraphType& g, bool withLaneChange, RoutingCostId costId)
+      : g_{&g}, costId_{costId}, filterMask_{RelationType::Successor | RelationType::Conflicting} {
+    if (withLaneChange) {
+      filterMask_ |=
+          RelationType::Left | RelationType::Right | RelationType::AdjacentLeft | RelationType::AdjacentRight;
+    }
+  }
+  bool operator()(const GraphTraits::edge_descriptor& v) const {
+    const auto& edge = (*g_)[v];
+    return edge.costId == costId_ && (edge.relation & filterMask_) != RelationType::None;
+  }
+
+ private:
+  const GraphType* g_;
+  RoutingCostId costId_;
+  RelationType filterMask_;
+};
+using OriginalGraph = boost::filtered_graph<GraphType, OriginalGraphFilter>;
+
+//! Reduces the graph to a set of desired vertices
+class OnRouteFilter {
+ public:
+  OnRouteFilter() = default;
+  explicit OnRouteFilter(const RouteLanelets& onRoute) : onRoute_{&onRoute} {}
+
+  bool operator()(LaneletVertexId vertexId) const { return onRoute_->find(vertexId) != onRoute_->end(); }
+
+ private:
+  const RouteLanelets* onRoute_{};
+};
+
+template <RelationType Relation, typename GraphType>
+class EdgeRelationFilter {
+ public:
+  EdgeRelationFilter() = default;
+  explicit EdgeRelationFilter(const GraphType& graph) : graph_{&graph} {}
+  bool operator()(FilteredRoutingGraph::edge_descriptor e) const {
+    auto type = (*graph_)[e].relation;
+    return (type & Relation) != RelationType::None;
+  }
+
+ private:
+  const GraphType* graph_{};
+};
+
+//! Removes edges from the graph that are not drivable (e.g. adjacent or conflicing)
+using OnlyDrivableEdgesFilter =
+    EdgeRelationFilter<RelationType::Successor | RelationType::Left | RelationType::Right, OriginalGraph>;
+using OnlyConflictingFilter = EdgeRelationFilter<RelationType::Conflicting, OriginalGraph>;
+
+//! Removes conflicting edges from the graph
+class NoConflictingFilter {
+ public:
+  NoConflictingFilter() = default;
+  explicit NoConflictingFilter(const OriginalGraph& originalGraph) : originalGraph_{&originalGraph} {}
+  bool operator()(FilteredRoutingGraph::edge_descriptor e) const {
+    auto type = (*originalGraph_)[e].relation;
+    return type != RelationType::Conflicting;
+  }
+
+ private:
+  const OriginalGraph* originalGraph_{};
+};
+
+//! Removes vertices from the graph that are not adjacent to a set of vertices. Adjacent can also mean conflicting!
+class NextToRouteFilter {
+ public:
+  NextToRouteFilter() = default;
+  NextToRouteFilter(const RouteLanelets& onRoute, const OriginalGraph& originalGraph)
+      : onRoute_{&onRoute}, originalGraph_{&originalGraph} {}
+
+  bool operator()(LaneletVertexId vertexId) const {
+    // at least one out edge must be connected to lanelets on the route. This includes conflicting and adjacent!
+    if (onRoute_->find(vertexId) != onRoute_->end()) {
+      return true;
+    }
+    auto outEdges = boost::out_edges(vertexId, *originalGraph_);
+    auto connectedToRoute = std::any_of(outEdges.first, outEdges.second, [&](OriginalGraph::edge_descriptor e) {
+      auto dest = boost::target(e, *originalGraph_);
+      return onRoute_->find(dest) != onRoute_->end() ||
+             onRoute_->find(boost::source(e, *originalGraph_)) != onRoute_->end();
+    });
+    return connectedToRoute;
+  }
+
+ private:
+  const RouteLanelets* onRoute_{};
+  const OriginalGraph* originalGraph_{};
+};
+
+//! Removes edges from the graph that are not drivable (e.g. adjacent or conflicing) OR leave the route at its end
+class OnlyDrivableEdgesWithinFilter {
+  using SuccessorFilter = EdgeRelationFilter<RelationType::Successor, OriginalGraph>;
+
+ public:
+  OnlyDrivableEdgesWithinFilter() = default;
+  explicit OnlyDrivableEdgesWithinFilter(RouteLanelets withinLanelets, const OriginalGraph& originalGraph)
+      : drivableEdge_{originalGraph}, successorEdge_{originalGraph}, withinLanelets_{std::move(withinLanelets)} {}
+  bool operator()(FilteredRoutingGraph::edge_descriptor e) const {
+    return drivableEdge_(e) && (!successorEdge_(e) || withinLanelets_.find(e.m_source) == withinLanelets_.end());
+  }
+
+ private:
+  OnlyDrivableEdgesFilter drivableEdge_;
+  SuccessorFilter successorEdge_;
+  RouteLanelets withinLanelets_;
+};
+
+//! Finds vertices that are in conflict or adjacent to some vertices (not reachable though bidirectional lane changes)
+class ConflictingSectionFilter {
+ public:
+  ConflictingSectionFilter() = default;
+  explicit ConflictingSectionFilter(const OriginalGraph& g, const RouteLanelets& onRoute)
+      : g_{&g}, onRoute_{&onRoute} {}
+
+  bool operator()(LaneletVertexId vertexId) const {
+    // conflicting if it is not yet part of the route but in conflict with a route lanelet
+    if (has(*onRoute_, vertexId)) {
+      return false;
+    }
+    auto outEdges = boost::out_edges(vertexId, *g_);
+    bool isNeighbour = false;
+    bool isConflicting = false;
+    std::for_each(outEdges.first, outEdges.second, [&](auto edge) {
+      if (onRoute_->find(boost::target(edge, *g_)) == onRoute_->end()) {
+        return;
+      }
+      auto type = (*g_)[edge].relation;
+      auto neighbourTypes = RelationType::Left | RelationType::Right;
+      auto conflictTypes = RelationType::Conflicting | RelationType::AdjacentLeft | RelationType::AdjacentRight;
+      if ((type & (neighbourTypes)) != RelationType::None) {
+        auto outEdge = boost::edge(boost::target(edge, *g_), boost::source(edge, *g_), *g_);
+        auto reverseIsNeigh = outEdge.second && ((*g_)[outEdge.first].relation & neighbourTypes) != RelationType::None;
+        isNeighbour |= reverseIsNeigh;
+        isConflicting |= !isNeighbour;
+      }
+      isConflicting |= (type & (conflictTypes)) != RelationType::None;
+    });
+    return isConflicting && !isNeighbour;
+  }
+
+ private:
+  const OriginalGraph* g_{};
+  const RouteLanelets* onRoute_{};
+};
+
+//! Finds vertices within a set of vertices that are in conflict with another set of vertices
+class OnRouteAndConflictFilter {
+ public:
+  OnRouteAndConflictFilter() = default;
+  explicit OnRouteAndConflictFilter(const RouteLanelets& onRoute, const std::vector<LaneletVertexId>& conflictWith,
+                                    const OriginalGraph& g)
+      : onRoute_{&onRoute}, conflictWith_{&conflictWith}, g_{&g} {}
+
+  bool operator()(LaneletVertexId vertexId) const {
+    auto isOk = anySidewayNeighbourIs(vertexId, *g_, [&](auto v) {
+      if (!has(*onRoute_, vertexId)) {
+        return false;
+      }
+      auto outEdges = boost::out_edges(v, *g_);
+      auto isConflicting = [&](OriginalGraph::edge_descriptor e) {
+        auto conflictTypes = RelationType::Conflicting | RelationType::AdjacentLeft | RelationType::AdjacentRight;
+        auto type = (*g_)[e].relation;
+        return (type & conflictTypes) != RelationType::None && has(*conflictWith_, boost::target(e, *g_));
+      };
+      return v == start_ || v == end_ || std::any_of(outEdges.first, outEdges.second, isConflicting);
+    });
+    return isOk;
+  }
+
+  void setConflicting(const LaneletVertexIds& conflictWith) { conflictWith_ = &conflictWith; }
+  void setStart(LaneletVertexId start) { start_ = start; }
+  void setEnd(LaneletVertexId end) { end_ = end; }
+
+ private:
+  const RouteLanelets* onRoute_{};
+  const LaneletVertexIds* conflictWith_{};
+  LaneletVertexId start_{};
+  LaneletVertexId end_{};
+  const OriginalGraph* g_{};
+};
+
+//! For graph queries, we implement our own color map because boost's color does not perform well on sparse graphs
+struct SparseColorMap {
+  using key_type = LaneletVertexId;                     // NOLINT
+  using value_type = boost::two_bit_color_type;         // NOLINT
+  using reference = void;                               // NOLINT
+  using category = boost::read_write_property_map_tag;  // NOLINT
+  using MapT = std::map<key_type, std::uint8_t>;
+  std::shared_ptr<MapT> data{std::make_shared<MapT>()};
+};
+
+inline SparseColorMap::value_type get(const SparseColorMap& map, LaneletVertexId key) {
+  auto val = map.data->find(key);
+  if (val != map.data->end()) {
+    return static_cast<SparseColorMap::value_type>(val->second);
+  }
+  return SparseColorMap::value_type::two_bit_white;
+}
+
+inline void put(SparseColorMap& map, LaneletVertexId key, SparseColorMap::value_type value) {
+  (*map.data)[key] = static_cast<std::uint8_t>(value);
+}
+
+//! An iterator that finds paths from a start vertex to all reachable destinations.
+template <typename GraphT>
+class ConnectedPathIterator {
+ public:
+  using Vertices = std::vector<LaneletVertexId>;
+
+ private:
+  template <typename Func>
+  class PathVisitor : public boost::default_dfs_visitor {
+   public:
+    PathVisitor(Func& f, Vertices& path) : path_{&path}, f_{&f} {}
+    void discover_vertex(typename GraphT::vertex_descriptor v, const GraphT& /*g*/) {  // NOLINT
+      path_->push_back(v);
+      movingForward_ = true;
+    }
+    void finish_vertex(typename GraphT::vertex_descriptor v, const GraphT& /*g*/) {  // NOLINT
+      if (movingForward_) {
+        (*f_)(*path_);
+      }
+      movingForward_ = false;
+      assert(!path_->empty());
+      assert(path_->back() == v);
+      path_->pop_back();
+    }
+
+   private:
+    bool movingForward_{true};
+    Vertices* path_;
+    const std::decay_t<Func>* f_;
+  };
+
+ public:
+  ConnectedPathIterator() = default;
+  explicit ConnectedPathIterator(const GraphT& g) : g_{g} {}
+
+  //! Calls a function for all full paths starting from start. A full path is a path from start to either a leaf or the
+  //! last unvisited vertex in loops.
+  template <typename Func>
+  void forEachPath(LaneletVertexId start, Func&& f) {
+    assert(g_.m_vertex_pred(start));
+    path_.clear();
+    PathVisitor<Func> vis(f, path_);
+    SparseColorMap cm;
+    boost::depth_first_visit(g_, start, vis, cm);
+  }
+
+  //! Returns whether a path exists in the graph that connects from and to
+  bool hasPathFromTo(LaneletVertexId from, LaneletVertexId to) {
+    class PathFound {};
+    auto destinationReached = [&](const auto& path) {
+      return std::any_of(std::make_reverse_iterator(path.end()), std::make_reverse_iterator(path.begin()),
+                         [&](auto p) { return p == to; });
+    };
+    try {
+      forEachPath(from, [&](const auto& path) {
+        if (destinationReached(path)) {
+          throw PathFound{};
+        }
+      });
+    } catch (PathFound) {  // NOLINT(misc-throw-by-value-catch-by-reference)
+      return true;
+    }
+    return false;
+  }
+
+  GraphT& graph() { return g_; }
+
+ private:
+  GraphT g_;
+  Vertices path_;
+};
+
+// Aliases for some graphs needed by us
+using OnRouteGraph = boost::filtered_graph<OriginalGraph, boost::keep_all, OnRouteFilter>;
+using DrivableGraph = boost::filtered_graph<OriginalGraph, OnlyDrivableEdgesFilter>;
+using NoConflictingGraph = boost::filtered_graph<OriginalGraph, NoConflictingFilter>;
+using OnlyConflictingGraph = boost::filtered_graph<OriginalGraph, OnlyConflictingFilter>;
+using NextToRouteGraph = boost::filtered_graph<OriginalGraph, OnlyDrivableEdgesWithinFilter, NextToRouteFilter>;
+using ConflictOrAdjacentToRouteGraph =
+    boost::filtered_graph<OriginalGraph, OnlyDrivableEdgesFilter, ConflictingSectionFilter>;
+using ConflictsWithPathGraph = boost::filtered_graph<OriginalGraph, NoConflictingFilter, OnRouteAndConflictFilter>;
+
+}  // namespace internal
+}  // namespace routing
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_routing/internal/RouteBuilder.h

@@ -0,0 +1,44 @@
+#pragma once
+#include "lanelet2_routing/LaneletPath.h"
+#include "lanelet2_routing/Route.h"
+#include "lanelet2_routing/internal/Graph.h"
+
+namespace lanelet {
+namespace routing {
+class RoutingGraph;
+
+namespace internal {
+//! Builder class to create a route from a routing graph and the shortest path
+class RouteBuilder {
+ public:
+  /** @note Important things about pending elements:
+   *  Pending elements are lanelets that can be reached from the shortest path (e.g. a lanelet right of a shortest
+   * path lanelet), but we don't know yet if we can reach the lanelet of the shortest path using that lanelet. This
+   * could happen if one is allowed to change the lane to the right, but not back. It could happen that we could
+   * switch to the left a couple of lanelets later, but we don't know yet. Pending elements hold relations the left or
+   * right lanelet that is certainly part of the route, but a lanelet that is part of the route *never* has a
+   * relation to a pending element. These are added once we know that a pending element will become a part of the
+   * route.
+   * @note Important things about lanes:
+   * @anchor laneIdNotes
+   *  During the creation of a route we assign lane IDs. A lane is a number of consecutive lanelets that follow each
+   * other and have unambiguous relations. Relations are unambiugous when two or more lanelets are merging or if they
+   * are diverging. Judging which of the lanes is going to continue and which one ends or starts is difficult. So in
+   * both those situations we're starting new lanes. Every so often - for example in diverging situations - we don't
+   * know if both new lanes are going to be a part of the final route, but lane IDs are issued anyway. As soon as the
+   * route is determined we can replace the lane IDs with IDs that make more sense in the context of the route. For
+   * example: there's a diverging situation and we issue new lane IDs. It turns out that the left lanelet doesn't lead
+   * to the goal. So in the context of the route this is not a diverging situation but rather it's just one lanelet
+   * (the right one) is following its predecessor. We would say that they are part of the same lane since there's no
+   * other way to go. */
+  explicit RouteBuilder(const RoutingGraphGraph& g) : graph_{g} {}
+  Optional<Route> getRouteFromShortestPath(const LaneletPath& path, bool withLaneChanges = true,
+                                           RoutingCostId costId = 0);
+
+ private:
+  const RoutingGraphGraph& graph_;
+};
+
+}  // namespace internal
+}  // namespace routing
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_routing/internal/RoutingGraphBuilder.h

@@ -0,0 +1,65 @@
+#pragma once
+#include <lanelet2_core/LaneletMap.h>
+
+#include "lanelet2_routing/RoutingGraph.h"
+#include "lanelet2_routing/internal/Graph.h"
+
+namespace lanelet {
+class LaneletLayer;
+
+namespace routing {
+namespace internal {
+
+class LaneChangeLaneletsCollector;
+
+class RoutingGraphBuilder {
+ public:
+  RoutingGraphBuilder(const traffic_rules::TrafficRules& trafficRules, const RoutingCostPtrs& routingCosts,
+                      const RoutingGraph::Configuration& config);
+
+  RoutingGraphUPtr build(const LaneletMapLayers& laneletMapLayers);
+
+ private:
+  using PointsLaneletMap = std::multimap<IdPair, ConstLanelet>;
+  using PointsLaneletMapIt = PointsLaneletMap::iterator;
+  using PointsLaneletMapResult = std::pair<PointsLaneletMapIt, PointsLaneletMapIt>;
+
+  static ConstLanelets getPassableLanelets(const LaneletLayer& lanelets,
+                                           const traffic_rules::TrafficRules& trafficRules);
+  static ConstAreas getPassableAreas(const AreaLayer& areas, const traffic_rules::TrafficRules& trafficRules);
+  void appendBidirectionalLanelets(ConstLanelets& llts);
+  void addLaneletsToGraph(ConstLanelets& llts);
+  void addAreasToGraph(ConstAreas& areas);
+  void addEdges(const ConstLanelets& lanelets, const LaneletLayer& passableLanelets);
+  void addEdges(const ConstAreas& areas, const LaneletLayer& passableLanelets, const AreaLayer& passableAreas);
+  void addFollowingEdges(const ConstLanelet& ll);
+  void addSidewayEdge(LaneChangeLaneletsCollector& laneChangeLanelets, const ConstLanelet& ll,
+                      const ConstLineString3d& bound, const RelationType& relation);
+  void addConflictingEdge(const ConstLanelet& ll, const LaneletLayer& passableLanelets);
+  void addLaneChangeEdges(LaneChangeLaneletsCollector& laneChanges, const RelationType& relation);
+  void addAreaEdge(const ConstArea& area, const LaneletLayer& passableLanelets);
+  void addAreaEdge(const ConstArea& area, const AreaLayer& passableAreas);
+
+  //! Helper function to read the participant height from the configuration
+  Optional<double> participantHeight() const;
+
+  //! Adds the first and last points of a lanelet to the search index
+  void addPointsToSearchIndex(const ConstLanelet& ll);
+  bool hasEdge(const ConstLanelet& from, const ConstLanelet& to);
+  void assignLaneChangeCosts(ConstLanelets froms, ConstLanelets tos, const RelationType& relation);
+
+  /** @brief Assigns routing costs of each routing cost module to a relation between two lanelets
+   *  @param from Start lanelet
+   *  @param to Goal lanelet
+   *  @param relation Relation between the two lanelets */
+  void assignCosts(const ConstLaneletOrArea& from, const ConstLaneletOrArea& to, const RelationType& relation);
+  std::unique_ptr<RoutingGraphGraph> graph_;
+  PointsLaneletMap pointsToLanelets_;  ///< A map of tuples (first or last left and right boundary points) to lanelets
+  std::set<Id> bothWaysLaneletIds_;
+  const traffic_rules::TrafficRules& trafficRules_;
+  const RoutingCostPtrs& routingCosts_;
+  const RoutingGraph::Configuration& config_;
+};
+}  // namespace internal
+}  // namespace routing
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_routing/internal/RoutingGraphVisualization.h

@@ -0,0 +1,133 @@
+#pragma once
+
+#include <boost/graph/graphml.hpp>
+#include <boost/graph/graphviz.hpp>
+#include <iostream>
+
+#include "lanelet2_routing/Exceptions.h"
+#include "lanelet2_routing/Forward.h"
+#include "lanelet2_routing/internal/Graph.h"
+
+namespace lanelet {
+
+// This one needs to be in this namepace. Otherwise it's not found.
+inline std::istream& operator>>(std::istream& is, ConstLaneletOrArea& /*r*/) { return is; }
+
+namespace routing {
+inline std::ostream& operator<<(std::ostream& os, const RelationType& r) { return os << relationToString(r); }
+inline std::istream& operator>>(std::istream& is, const RelationType& /*r*/) { return is; }
+
+namespace internal {
+
+/** @brief Internal vertex writer for graphViz file export. */
+template <class Graph>
+class VertexWriterGraphViz {
+ public:
+  explicit VertexWriterGraphViz(const Graph* g) : graph_(g) {}
+  template <class VertexOrEdge>
+  void operator()(std::ostream& out, const VertexOrEdge& v) const {
+    const Id id{(*graph_)[v].laneletOrArea.id()};
+    out << "[label=\"" << id << "\" lanelet=\"" << id << "\"]";
+  }
+
+ private:
+  const Graph* graph_;
+};
+
+/** @brief Internal edge writer for graphViz file export.
+    Includes label, color and if existent routing cost (as 'weight') and the routing cost module ID. */
+template <class Graph>
+class EdgeWriterGraphViz {
+ public:
+  explicit EdgeWriterGraphViz(const Graph* g) : graph_(g) {}
+  template <class VertexOrEdge>
+  void operator()(std::ostream& out, const VertexOrEdge& v) const {
+    const RelationType relation{(*graph_)[v].relation};
+    out << "[label=\"" << relationToString(relation) << "\" color=\"" << relationToColor(relation);
+    if (relation != RelationType::AdjacentLeft && relation != RelationType::AdjacentRight &&
+        relation != RelationType::Conflicting) {
+      out << "\" weight=\"" << (*graph_)[v].routingCost;
+    }
+    out << "\" routingCostId=\"" << (*graph_)[v].costId << "\"]";
+  }
+
+ private:
+  const Graph* graph_;
+};
+
+/** @brief Implementation of graphViz export function
+ *  @param filename Fully qualified file name - ideally with extension (.gv)
+ *  @param g Graph to export
+ *  @param edgeFilter Edge filter that will be used to create a filtered graph
+ *  @param vertexFilter Vertex filter. Not used yet */
+template <typename G, typename E = boost::keep_all, typename V = boost::keep_all>
+inline void exportGraphVizImpl(const std::string& filename, const G& g, E edgeFilter = boost::keep_all(),
+                               V vertexFilter = boost::keep_all()) {
+  std::ofstream file;
+  file.open(filename);
+  if (!file.is_open()) {
+    throw lanelet::ExportError("Could not open file at " + filename + ".");
+  }
+
+  VertexWriterGraphViz<G> vertexWriter(&g);
+  EdgeWriterGraphViz<G> edgeWriter(&g);
+  boost::filtered_graph<G, E, V> fg(g, edgeFilter, vertexFilter);
+  boost::write_graphviz(file, fg, vertexWriter, edgeWriter);
+
+  file.close();
+}
+
+/** @brief GraphViz export function
+ *  @param filename Fully qualified file name - ideally with extension (.gv)
+ *  @param g Graph to export
+ *  @param relationTypes Relations that will be included in the export
+ *  @param routingCostId ID of the routing cost module */
+template <typename G>
+inline void exportGraphVizImpl(const std::string& filename, const G& g, const RelationType& relationTypes,
+                               RoutingCostId routingCostId = 0) {
+  auto edgeFilter = EdgeCostFilter<G>(g, routingCostId, relationTypes);
+  exportGraphVizImpl(filename, g, edgeFilter);
+}
+
+/** @brief Implementation of graphML export function
+ *  @param filename Fully qualified file name - ideally with extension (.graphml)
+ *  @param g Graph to export
+ *  @param eFilter Edge filter that will be used to create a filtered graph
+ *  @param vFilter Vertex filter. Not used yet */
+template <typename G, typename E = boost::keep_all, typename V = boost::keep_all>
+inline void exportGraphMLImpl(const std::string& filename, const G& g, E eFilter = boost::keep_all(),
+                              V vFilter = boost::keep_all()) {
+  std::ofstream file;
+  file.open(filename);
+  if (!file.is_open()) {
+    throw lanelet::ExportError("Could not open file at " + filename + ".");
+  }
+
+  boost::filtered_graph<G, EdgeCostFilter<G>> filteredGraph(g, eFilter, vFilter);
+
+  auto pmId = boost::get(&VertexInfo::laneletOrArea, filteredGraph);  // NOLINT
+  auto pmRelation = boost::get(&EdgeInfo::relation, filteredGraph);
+  auto pmRoutingCosts = boost::get(&EdgeInfo::routingCost, filteredGraph);
+
+  boost::dynamic_properties dp;
+  dp.property("info", pmId);
+  dp.property("relation", pmRelation);
+  dp.property("routingCost", pmRoutingCosts);
+
+  boost::write_graphml(file, filteredGraph, dp, false);
+}
+
+/** @brief GraphML export function
+ *  @param filename Fully qualified file name - ideally with extension (.graphml)
+ *  @param g Graph to export
+ *  @param relationTypes Relations that will be included in the export
+ *  @param routingCostId ID of the routing cost module */
+template <typename G>
+inline void exportGraphMLImpl(const std::string& filename, const G& g, const RelationType& relationTypes,
+                              RoutingCostId routingCostId = 0) {
+  auto edgeFilter = EdgeCostFilter<G>(g, routingCostId, relationTypes);
+  exportGraphMLImpl(filename, g, edgeFilter);
+}
+}  // namespace internal
+}  // namespace routing
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_routing/internal/ShortestPath.h

@@ -0,0 +1,139 @@
+#pragma once
+#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
+#include <boost/graph/filtered_graph.hpp>
+#include <boost/property_map/property_map.hpp>
+
+#include "lanelet2_routing/Exceptions.h"
+#include "lanelet2_routing/internal/Graph.h"
+#include "lanelet2_routing/internal/GraphUtils.h"
+
+namespace lanelet {
+namespace routing {
+namespace internal {
+
+//! This object carries the required information for the graph neighbourhood search
+struct VertexVisitInformation {
+  RoutingGraphGraph::Vertex vertex{};
+  RoutingGraphGraph::Vertex predecessor{};
+  double cost{};
+  size_t length{};
+  size_t numLaneChanges{};
+};
+
+struct VertexState {
+  RoutingGraphGraph::Vertex predecessor{};  //!< The vertex this refers to
+  double cost{};                            //!< Current accumulated cost
+  size_t length{};                          //!< Number of vertices to this vertex (including this one)
+  size_t numLaneChanges{};                  //!< Required lane changes along the shortest path in order to get here
+  bool predicate{true};                     //!< False if disabled by predicate
+  bool isLeaf{true};                        //!< True if it has no successor that is on the shortest path
+};
+
+template <typename VertexT>
+using DijkstraSearchMap = std::map<VertexT, VertexState>;
+
+template <typename VertexT>
+struct DijkstraCostMap {
+  using key_type = VertexT;                             // NOLINT
+  using value_type = double;                            // NOLINT
+  using reference = void;                               // NOLINT
+  using category = boost::read_write_property_map_tag;  // NOLINT
+  DijkstraSearchMap<VertexT>* map{};
+};
+
+template <typename VertexT>
+inline typename DijkstraCostMap<VertexT>::value_type get(const DijkstraCostMap<VertexT>& map, VertexT key) {
+  auto val = map.map->find(key);
+  if (val != map.map->end()) {
+    return val->second.cost;
+  }
+  return std::numeric_limits<double>::infinity();
+}
+
+template <typename VertexT>
+inline void put(DijkstraCostMap<VertexT>& map, VertexT key, typename DijkstraCostMap<VertexT>::value_type value) {
+  (*map.map)[key].cost = value;
+}
+
+template <typename G>
+class DijkstraStyleSearch {
+ public:
+  using VertexType = typename boost::graph_traits<G>::vertex_descriptor;
+  using EdgeType = typename boost::graph_traits<G>::edge_descriptor;
+  using DijkstraSearchMapType = DijkstraSearchMap<VertexType>;
+  using VisitCallback = std::function<bool(const VertexVisitInformation&)>;
+
+ private:
+  class LeafFilter {
+   public:
+    LeafFilter() = default;
+    LeafFilter(const DijkstraSearchMapType& m, const G& g) : m_{&m}, g_{&g} {}
+    bool operator()(EdgeType e) const { return (*m_).at(boost::source(e, *g_)).predicate; }
+
+   private:
+    const DijkstraSearchMapType* m_{};
+    const G* g_{};
+  };
+  using SearchGraph = boost::filtered_graph<G, LeafFilter>;
+
+  template <typename Func>
+  class DijkstraStyleVisitor : public boost::default_dijkstra_visitor {
+    using FuncT = std::remove_reference_t<Func>;
+
+   public:
+    DijkstraStyleVisitor() = default;
+    DijkstraStyleVisitor(DijkstraSearchMapType& map, FuncT* cb) : map_{&map}, cb_{cb} {}
+
+    // called whenever a minimal edge is discovered
+    void examine_vertex(VertexType v, const SearchGraph& /*g*/) {  // NOLINT
+      auto& state = map_->at(v);
+      state.predicate =
+          ((*cb_)(VertexVisitInformation{v, state.predecessor, state.cost, state.length, state.numLaneChanges}));
+      map_->at(state.predecessor).isLeaf = v == state.predecessor;  // necessary for the initial vertex
+    }
+
+    void edge_relaxed(EdgeType e, const SearchGraph& g) {  // NOLINT
+      // called whenever a shorter path to e is discovered and before "examine vertex" is called
+      // cost is automatically updated by the dijkstra algorithm
+      auto& predecessor = (*map_).at(boost::source(e, g));
+      auto& follower = (*map_)[boost::target(e, g)];
+      follower.length = predecessor.length + 1;
+      follower.predecessor = boost::source(e, g);
+      follower.numLaneChanges = predecessor.numLaneChanges + (g[e].relation != RelationType::Successor);
+    }
+
+   private:
+    DijkstraSearchMapType* map_{};
+    FuncT* cb_{};
+  };
+
+ public:
+  //! Constructor for the graph search
+  explicit DijkstraStyleSearch(const G& graph) : graph_{graph, LeafFilter{vertices_, graph}} {}
+
+  //! Performs the dijkstra style search by calling func whenever the shortest path for a certain vertex is
+  //! discovered. Whenever func returns false, the successor edges of this vertex will not be visited.
+  template <typename Func>
+  void query(VertexType start, Func&& func) {
+    vertices_.clear();
+    vertices_.emplace(start, VertexState{start, 0., 1, 0, true, true});
+    DijkstraStyleVisitor<decltype(func)> visitor{vertices_, &func};
+    auto inf = std::numeric_limits<double>::infinity();
+    // offering sane defaults seems to have been impossible...
+    boost::dijkstra_shortest_paths_no_color_map_no_init(
+        graph_, start, boost::dummy_property_map{}, DijkstraCostMap<VertexType>{&vertices_},
+        boost::get(&EdgeInfo::routingCost, graph_), boost::get(boost::vertex_index, graph_), std::less<double>{},
+        boost::closed_plus<double>{}, inf, 0., visitor);
+  }
+
+  //! Returns the result
+  const DijkstraSearchMapType& getMap() const { return vertices_; }
+
+ private:
+  SearchGraph graph_;
+  DijkstraSearchMapType vertices_;
+};
+
+}  // namespace internal
+}  // namespace routing
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_traffic_rules/Exceptions.h

@@ -0,0 +1,10 @@
+#pragma once
+#include <lanelet2_core/Exceptions.h>
+
+namespace lanelet {
+//! Thrown when unable to interpret details of a traffic rules (i.e. unknown
+//! tags, etc)
+class InterpretationError : public LaneletError {
+  using LaneletError::LaneletError;
+};
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_traffic_rules/GenericTrafficRules.h

@@ -0,0 +1,108 @@
+#pragma once
+#include <lanelet2_core/Forward.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+
+#include "lanelet2_traffic_rules/TrafficRules.h"
+
+namespace lanelet {
+namespace traffic_rules {
+
+enum class LaneChangeType { ToRight, ToLeft, Both, None };
+struct CountrySpeedLimits {
+  SpeedLimitInformation vehicleUrbanRoad;
+  SpeedLimitInformation vehicleNonurbanRoad;
+  SpeedLimitInformation vehicleUrbanHighway;
+  SpeedLimitInformation vehicleNonurbanHighway;
+  SpeedLimitInformation playStreet;
+  SpeedLimitInformation pedestrian;
+  SpeedLimitInformation bicycle;
+};
+
+class TrafficRules;
+using TrafficRulesPtr = std::shared_ptr<TrafficRules>;
+using TrafficRulesUPtr = std::unique_ptr<TrafficRules>;
+
+//! This class generically implements the TrafficRules class in the sense of the tagging specification. It is designed
+//! to make sense for most countries and participants. Country specific details (traffic signs, speed limits
+//! regulations, ...) must be implemented by inheriting classes.
+class GenericTrafficRules : public TrafficRules {  // NOLINT
+ public:
+  using TrafficRules::TrafficRules;
+
+  /**
+   * @brief returns whether it is allowed to pass/drive on this lanelet
+   *
+   * The result can differ by the type of the traffic participant. A sidewalk
+   * lanelet is passable for a pedestrian but not for a vehicle. Orientation is
+   * important. It is not possible to pass an inverted oneWay Lanelet.
+   */
+  bool canPass(const ConstLanelet& lanelet) const override;
+
+  //! returns whether it is allowed to pass/drive on this area
+  bool canPass(const ConstArea& area) const override;
+
+  /**
+   * @brief returns whether it is allowed to pass/drive from a lanelet to
+   * another lanelet.
+   *
+   * The orientation of the lanelets is important. The function first checks if
+   * lanelets are direcly adjacent, then checks if both lanelets are passable
+   * and finally checks if any traffic rule prevents to pass between the
+   * lanelets.
+   */
+  bool canPass(const ConstLanelet& from, const ConstLanelet& to) const override;
+  bool canPass(const ConstLanelet& from, const ConstArea& to) const override;
+  bool canPass(const ConstArea& from, const ConstLanelet& to) const override;
+  bool canPass(const ConstArea& from, const ConstArea& to) const override;
+
+  /**
+   * @brief determines if a lane change can be made between two lanelets
+   *
+   * The orientation of the lanelets is important here as well.
+   */
+  bool canChangeLane(const ConstLanelet& from, const ConstLanelet& to) const override;
+
+  //! returns speed limit on this lanelet.
+  SpeedLimitInformation speedLimit(const ConstLanelet& lanelet) const override;
+
+  //! returns speed limit on this area.
+  SpeedLimitInformation speedLimit(const ConstArea& area) const override;
+
+  //! returns whether a lanelet can be driven in one direction only
+  bool isOneWay(const ConstLanelet& lanelet) const override;
+
+  /**
+   * @brief returns whether dynamic traffic rules apply to this lanelet.
+   *
+   * This can be a case if e.g. a speed limit is controlled by digital signs.
+   * If this returns true, additional handling must be done to find which rules
+   * are dynamic and how to handle them. This makes the values returned by the
+   * other functions unreliable. Handling of dynamic rules is not covered here.
+   */
+  bool hasDynamicRules(const ConstLanelet& lanelet) const override;
+
+ protected:
+  //! Called by canPass to check if traffic rules make a certain primitive drivable/not drivable
+  //! If the optional is empty, there is no traffic rule that determines this.
+  virtual Optional<bool> canPass(const RegulatoryElementConstPtrs& regElems) const = 0;
+
+  //! Called by canPass to check if a certain lanelet type is drivable for this participant
+  virtual Optional<bool> canPass(const std::string& type, const std::string& /*location*/) const;
+
+  /**
+   * @brief checks which types of lange changes are allowed along this boundary
+   * to make a lane switch from one lanelet to another.
+   */
+  virtual LaneChangeType laneChangeType(const ConstLineString3d& boundary, bool virtualIsPassable) const;
+
+  //! Overloads should return a descripton of their countrie's speed limit definition
+  virtual const CountrySpeedLimits& countrySpeedLimits() const = 0;
+
+  //! Returns speed limit as defined by regiualtory elements if present
+  virtual Optional<SpeedLimitInformation> speedLimit(const RegulatoryElementConstPtrs& regelems) const = 0;
+
+ private:
+  SpeedLimitInformation speedLimit(const RegulatoryElementConstPtrs& regelems, const AttributeMap& attributes) const;
+};
+}  // namespace traffic_rules
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_traffic_rules/GermanTrafficRules.h

@@ -0,0 +1,57 @@
+#pragma once
+#include <lanelet2_core/utility/Units.h>
+
+#include "lanelet2_traffic_rules/GenericTrafficRules.h"
+
+namespace lanelet {
+namespace traffic_rules {
+
+CountrySpeedLimits germanSpeedLimits();
+
+class GermanVehicle : public GenericTrafficRules {
+ public:
+  using GenericTrafficRules::GenericTrafficRules;
+
+  // using areas is disallowed for vehicles in normal driving mode
+  bool canPass(const ConstArea& /*area*/) const override { return false; }
+
+ protected:
+  Optional<bool> canPass(const RegulatoryElementConstPtrs& /*regElems*/) const override { return {}; }
+  const CountrySpeedLimits& countrySpeedLimits() const override { return speedLimits_; }
+  Optional<SpeedLimitInformation> speedLimit(const RegulatoryElementConstPtrs& regelems) const override;
+
+ private:
+  CountrySpeedLimits speedLimits_{germanSpeedLimits()};
+};
+
+class GermanPedestrian : public GenericTrafficRules {
+ public:
+  using GenericTrafficRules::GenericTrafficRules;
+
+ protected:
+  Optional<bool> canPass(const RegulatoryElementConstPtrs& /*regElems*/) const override { return {}; }
+  const CountrySpeedLimits& countrySpeedLimits() const override { return speedLimits_; }
+  Optional<SpeedLimitInformation> speedLimit(const RegulatoryElementConstPtrs& /*regelems*/) const override {
+    return {};
+  }
+
+ private:
+  CountrySpeedLimits speedLimits_{germanSpeedLimits()};
+};
+
+class GermanBicycle : public GenericTrafficRules {
+ public:
+  using GenericTrafficRules::GenericTrafficRules;
+
+ protected:
+  Optional<bool> canPass(const RegulatoryElementConstPtrs& /*regElems*/) const override { return {}; }
+  const CountrySpeedLimits& countrySpeedLimits() const override { return speedLimits_; }
+  Optional<SpeedLimitInformation> speedLimit(const RegulatoryElementConstPtrs& /*regelems*/) const override {
+    return {};
+  }
+
+ private:
+  CountrySpeedLimits speedLimits_{germanSpeedLimits()};
+};
+}  // namespace traffic_rules
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_traffic_rules/TrafficRules.h

@@ -0,0 +1,110 @@
+#pragma once
+#include <lanelet2_core/Forward.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+
+namespace lanelet {
+namespace traffic_rules {
+
+struct SpeedLimitInformation {
+  Velocity speedLimit;     //!< The current speed limit (must not be Inf)
+  bool isMandatory{true};  //!< False if speed limit is only a recommendation
+};
+
+class TrafficRules;
+using TrafficRulesPtr = std::shared_ptr<TrafficRules>;
+using TrafficRulesUPtr = std::unique_ptr<TrafficRules>;
+
+//! Class for inferring traffic rules for lanelets and areas
+class TrafficRules {  // NOLINT
+ public:
+  using Configuration = std::map<std::string, Attribute>;
+
+  /**
+   * @brief Constructor
+   * @param config a configuration for traffic rules. This can be necessary for
+   * very specialized rule objects (ie considering the time of day or the
+   * weather). The config must have at least two entries: participant and location.
+   */
+  explicit TrafficRules(Configuration config = Configuration()) : config_{std::move(config)} {}
+  virtual ~TrafficRules();
+
+  /**
+   * @brief returns whether it is allowed to pass/drive on this lanelet
+   *
+   * The result can differ by the type of the traffic participant. A sidewalk
+   * lanelet is passable for a pedestrian but not for a vehicle. Orientation is
+   * important. It is not possible to pass an inverted oneWay Lanelet.
+   */
+  virtual bool canPass(const ConstLanelet& lanelet) const = 0;
+
+  //! returns whether it is allowed to pass/drive on this area
+  virtual bool canPass(const ConstArea& area) const = 0;
+
+  /**
+   * @brief returns whether it is allowed to pass/drive from a lanelet to
+   * another lanelet.
+   *
+   * The orientation of the lanelets is important. The function first checks if
+   * lanelets are direcly adjacent, then checks if both lanelets are passable
+   * and finally checks if any traffic rule prevents to pass between the
+   * lanelets.
+   */
+  virtual bool canPass(const ConstLanelet& from, const ConstLanelet& to) const = 0;
+  virtual bool canPass(const ConstLanelet& from, const ConstArea& to) const = 0;
+  virtual bool canPass(const ConstArea& from, const ConstLanelet& to) const = 0;
+  virtual bool canPass(const ConstArea& from, const ConstArea& to) const = 0;
+
+  /**
+   * @brief determines if a lane change can be made between two lanelets
+   *
+   * The orientation of the lanelets is important here as well.
+   */
+  virtual bool canChangeLane(const ConstLanelet& from, const ConstLanelet& to) const = 0;
+
+  //! returns speed limit on this lanelet.
+  virtual SpeedLimitInformation speedLimit(const ConstLanelet& lanelet) const = 0;
+
+  //! returns speed limit on this area.
+  virtual SpeedLimitInformation speedLimit(const ConstArea& area) const = 0;
+
+  //! returns whether a lanelet can be driven in one direction only
+  virtual bool isOneWay(const ConstLanelet& lanelet) const = 0;
+
+  /**
+   * @brief returns whether dynamic traffic rules apply to this lanelet.
+   *
+   * This can be a case if e.g. a speed limit is controlled by digital signs.
+   * If this returns true, additional handling must be done to find which rules
+   * are dynamic and how to handle them. This makes the values returned by the
+   * other functions unreliable. Handling of dynamic rules is not covered here.
+   */
+  virtual bool hasDynamicRules(const ConstLanelet& lanelet) const = 0;
+
+  /**
+   * @brief configuration for this traffic rules object
+   */
+  const Configuration& configuration() const { return config_; }
+
+  /**
+   * @brief the traffic participant the rules are valid for (e.g. vehicle, car,
+   * pedestrian, etc)
+   */
+  const std::string& participant() const;
+
+  /**
+   * @brief the the location the rules are valid for
+   *
+   * Should be ISO country code
+   */
+  const std::string& location() const;
+
+ private:
+  Configuration config_;
+};
+
+std::ostream& operator<<(std::ostream& stream, const SpeedLimitInformation& obj);
+
+std::ostream& operator<<(std::ostream& stream, const TrafficRules& obj);
+
+}  // namespace traffic_rules
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_traffic_rules/TrafficRulesFactory.h

@@ -0,0 +1,63 @@
+#pragma once
+#include "lanelet2_traffic_rules/TrafficRules.h"
+
+namespace lanelet {
+struct Locations {
+  static constexpr char Germany[] = "de";
+};
+
+// participants are defined in lanelet2_core/Attributes.h
+
+namespace traffic_rules {
+
+class TrafficRulesFactory {
+ public:
+  using FactoryFcn = std::function<TrafficRulesUPtr(const TrafficRules::Configuration&)>;
+  void registerStrategy(const std::string& location, const std::string& participant,
+                        const FactoryFcn& factoryFunction) {
+    registry_[std::make_pair(location, participant)] = factoryFunction;
+  }
+
+  /** create a traffic rule object based on location and participant
+   * @throws InvalidInputError if no traffic rules are available for a
+   * location/participant combination
+   */
+  static TrafficRulesUPtr create(const std::string& location, const std::string& participant,
+                                 TrafficRules::Configuration configuration = TrafficRules::Configuration());
+
+  /**
+   * @brief returns registered traffic rules by location and participant
+   * @return first member of pair is location, second is participant
+   */
+  static std::vector<std::pair<std::string, std::string>> availableTrafficRules();
+
+  static TrafficRulesFactory& instance();
+
+ private:
+  TrafficRulesFactory() = default;
+  std::map<std::pair<std::string, std::string>, FactoryFcn> registry_;
+};
+
+/**
+ * @brief template class for registering new TrafficRules for a certain location
+ * and type.
+ *
+ * To register a class, put
+ * RegisterTrafficRules<MyClass> regMyClass(<location>, <participant>);
+ * somewhere in your cpp file.
+ *
+ * Your class is required to have a constructor that takes a
+ * TrafficRules::Configuration as argument.
+ */
+template <class T>
+class RegisterTrafficRules {
+ public:
+  RegisterTrafficRules(const std::string& location, const std::string& participant) {
+    // initialize
+    TrafficRulesFactory::instance().registerStrategy(
+        location, participant,
+        +[](const TrafficRules::Configuration& config) -> TrafficRulesUPtr { return std::make_unique<T>(config); });
+  }
+};
+}  // namespace traffic_rules
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_validation/BasicValidator.h

@@ -0,0 +1,50 @@
+#pragma once
+#include <lanelet2_core/Forward.h>
+#include <lanelet2_traffic_rules/TrafficRules.h>
+
+#include <memory>
+
+#include "lanelet2_validation/Issue.h"
+
+namespace lanelet {
+namespace routing {
+class RoutingGraph;
+}  // namespace routing
+
+namespace validation {
+
+//! Most simple form of a validator. It gets a map once and reports errors.
+class MapValidator {  // NOLINT
+ public:
+  virtual ~MapValidator() = default;
+  constexpr static const char* name() { return ""; }
+  virtual Issues operator()(const LaneletMap& map) = 0;
+};
+using MapValidatorUPtr = std::unique_ptr<MapValidator>;
+using MapValidatorUPtrs = std::vector<MapValidatorUPtr>;
+
+//! A traffic rule validator gets a part of the map and the current traffic rules and tries to detect issues there.
+//! The object ist not destroyed between subseqent calls with different traffic rules, so that information about already
+//! reported issues can be stored.
+class TrafficRuleValidator {  // NOLINT
+ public:
+  constexpr static const char* name() { return ""; }
+  virtual Issues operator()(const LaneletMap& map, const std::vector<traffic_rules::TrafficRulesUPtr>& rules) = 0;
+  virtual ~TrafficRuleValidator() = default;
+};
+using TrafficRuleValidatorUPtr = std::unique_ptr<TrafficRuleValidator>;
+using TrafficRuleValidatorUPtrs = std::vector<TrafficRuleValidatorUPtr>;
+
+//! A routing graph validator works similar, but instead uses the routing graph of a map to detect issues
+class RoutingGraphValidator {  // NOLINTs
+ public:
+  constexpr static const char* name() { return ""; }
+  //! The RoutingGraphValidator is called together with the rules with which it was created.
+  virtual Issues operator()(const routing::RoutingGraph& graph, const traffic_rules::TrafficRules& rules) = 0;
+  virtual ~RoutingGraphValidator() = default;
+};
+using RoutingGraphValidatorUPtr = std::unique_ptr<RoutingGraphValidator>;
+using RoutingGraphValidatorUPtrs = std::vector<RoutingGraphValidatorUPtr>;
+
+}  // namespace validation
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_validation/Cli.h

@@ -0,0 +1,23 @@
+#pragma once
+#include "lanelet2_validation/Validation.h"
+
+namespace lanelet {
+namespace validation {
+struct CommandLineConfig {
+  ValidationConfig validationConfig;
+  std::string mapFile;
+  bool print{false};
+  bool help{false};
+};
+
+//! obtain the configuration from command line arguments
+CommandLineConfig parseCommandLine(int argc, const char* argv[]);
+
+//! prints a vector of issues to the command line
+void printAllIssues(const std::vector<DetectedIssues>& issues);
+
+//! Runs the configuration and returns the programs's return value (0 on success, 1 if issues found)
+int runFromConfig(const CommandLineConfig& config);
+
+}  // namespace validation
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_validation/Issue.h

@@ -0,0 +1,68 @@
+#pragma once
+#include <lanelet2_core/Forward.h>
+
+#include <sstream>
+#include <vector>
+
+namespace lanelet {
+namespace validation {
+enum class Severity { Error, Warning, Info };
+enum class Primitive { Point, LineString, Polygon, Lanelet, Area, RegulatoryElement, Primitive };
+
+inline const char* toString(Severity severity) {
+  switch (severity) {
+    case Severity::Error:
+      return "Error";
+    case Severity::Warning:
+      return "Warning";
+    case Severity::Info:
+      return "info";
+  }
+  return "";
+}
+
+inline const char* toString(Primitive primitive) {
+  switch (primitive) {
+    case Primitive::Point:
+      return "point";
+    case Primitive::LineString:
+      return "linestring";
+    case Primitive::Polygon:
+      return "polygon";
+    case Primitive::Lanelet:
+      return "lanelet";
+    case Primitive::Area:
+      return "area";
+    case Primitive::RegulatoryElement:
+      return "regulatory element";
+    case Primitive::Primitive:
+      return "primitive";
+  }
+  return "";
+}
+
+struct Issue {
+  Issue() = default;
+  Issue(Severity severity, std::string message) : severity{severity}, message{std::move(message)} {}
+  Issue(Severity severity, Primitive primitive, Id id, std::string message)
+      : severity{severity}, primitive{primitive}, id{id}, message{std::move(message)} {}
+  std::string buildReport() const {
+    std::stringstream ss;
+    ss << toString(severity) << ": ";
+    if (id != InvalId) {
+      ss << toString(primitive) << " " << std::to_string(id) << " ";
+    }
+    ss << message;
+    return ss.str();
+  }
+
+  Severity severity{Severity::Error};     //!< Severity class of issue
+  Primitive primitive{Primitive::Point};  //!< Type of primitive that caused the issue
+  Id id{InvalId};                         //!< the id of primitive that caused the issue
+  std::string message;                    //!< Message to be displayed
+};
+
+using Issues = std::vector<Issue>;
+
+}  // namespace validation
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_validation/Validation.h

@@ -0,0 +1,47 @@
+#pragma once
+#include <lanelet2_io/Projection.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+
+#include "lanelet2_validation/Issue.h"
+
+namespace lanelet {
+namespace validation {
+
+using Strings = std::vector<std::string>;
+
+//! Struct for the detected issues that were discovered by a specific check
+struct DetectedIssues {
+  DetectedIssues() = default;
+  DetectedIssues(std::string checkName, Issues issues) : checkName{std::move(checkName)}, issues{std::move(issues)} {}
+  Issues errors() const;
+  Issues warnings() const;
+  std::string checkName;
+  Issues issues;
+};
+
+//! Configuration object for the validation
+struct ValidationConfig {
+  std::string checksFilter;
+  std::string location{Locations::Germany};
+  Strings participants{Participants::Vehicle};
+  GPSPoint origin;
+};
+
+//! Contains each warning/error as formatted strings
+struct IssueReport {
+  Strings warnings;
+  Strings errors;
+};
+
+//! Generates the issue report
+IssueReport buildReport(std::vector<DetectedIssues> issues);
+
+//! Reports the available checks for the given filter. Empty will return all.
+Strings availabeChecks(const std::string& filterString);
+
+std::vector<DetectedIssues> validateMap(LaneletMap& map, const ValidationConfig& config);
+
+//! Central function here. Loads the map, runs the checks and returns the issues.
+std::vector<DetectedIssues> validateMap(const std::string& mapFilename, const ValidationConfig& config);
+}  // namespace validation
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_validation/ValidatorFactory.h

@@ -0,0 +1,90 @@
+#pragma once
+#include <lanelet2_core/utility/Utilities.h>
+
+#include <functional>
+#include <regex>
+
+#include "lanelet2_validation/BasicValidator.h"
+
+namespace lanelet {
+namespace validation {
+
+using Regexes = std::vector<std::regex>;
+
+template <typename ValidatorT>
+using ValidatorWithName = std::pair<std::string, std::unique_ptr<ValidatorT>>;
+
+template <typename ValidatorT>
+using ValidatorsWithName = std::vector<ValidatorWithName<ValidatorT>>;
+
+class ValidatorFactory {
+ public:
+  template <typename ValidatorT>
+  using CreationFcn = std::function<ValidatorT*()>;
+  static ValidatorFactory& instance();
+
+  ValidatorsWithName<MapValidator> createMapValidators(const Regexes& regexes);
+  ValidatorsWithName<TrafficRuleValidator> createTrafficRuleValidators(const Regexes& regexes);
+  ValidatorsWithName<RoutingGraphValidator> createRoutingGraphValidators(const Regexes& regexes);
+
+  /**
+   * @brief returns all available parsers as vector
+   * @return vector of parser names that match the regex, or all if empty
+   */
+  std::vector<std::string> availableValidators(const Regexes& regexes = {});
+
+  template <typename ValidatorT>
+  friend class RegisterMapValidator;
+
+  template <typename ValidatorT>
+  friend class RegisterTrafficRuleValidator;
+
+  template <typename ValidatorT>
+  friend class RegisterRoutingGraphValidator;
+
+ private:
+  void registerMapValidator(const std::string& name, const CreationFcn<MapValidator>& creator);
+  void registerTrafficRuleValidator(const std::string& name, const CreationFcn<TrafficRuleValidator>& creator);
+  void registerRoutingGraphValidator(const std::string& name, const CreationFcn<RoutingGraphValidator>& creator);
+
+  ValidatorFactory() = default;
+  std::map<std::string, CreationFcn<MapValidator>> mapValidatorRegistry_;
+  std::map<std::string, CreationFcn<TrafficRuleValidator>> trafficRuleValidatorRegistry_;
+  std::map<std::string, CreationFcn<RoutingGraphValidator>> routingGraphValidatorRegistry_;
+};
+
+/**
+ * @brief Registration object for a map validator. Needs to be instanciated as static
+ * object once to register a writer. Registration might look like this:
+ *   static RegisterWriter<Mywriter> register;
+ */
+template <class T>
+class RegisterMapValidator {
+ public:
+  RegisterMapValidator() {
+    static_assert(!utils::strequal(T::name(), ""), "You did not overload the name() function!");
+    ValidatorFactory::instance().registerMapValidator(T::name(), []() { return new T(); });
+  }
+};
+
+//! Registration object for traffic rule validators
+template <class T>
+class RegisterTrafficRuleValidator {
+ public:
+  RegisterTrafficRuleValidator() {
+    static_assert(!utils::strequal(T::name(), ""), "You did not overload the name() function!");
+    ValidatorFactory::instance().registerTrafficRuleValidator(T::name(), []() { return new T(); });
+  }
+};
+
+//! Registration object for routing graph validators
+template <class T>
+class RegisterRoutingGraphValidator {
+ public:
+  RegisterRoutingGraphValidator() {
+    static_assert(!utils::strequal(T::name(), ""), "You did not overload the name() function!");
+    ValidatorFactory::instance().registerRoutingGraphValidator(T::name(), []() { return new T(); });
+  }
+};
+}  // namespace validation
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_validation/validators/mapping/BoolTags.h

@@ -0,0 +1,17 @@
+#include <lanelet2_core/LaneletMap.h>
+
+#include "lanelet2_validation/BasicValidator.h"
+
+namespace lanelet {
+namespace validation {
+
+//! This check looks tags in primitives that should be convertible to bool but aren't.
+class BoolTags : public MapValidator {
+ public:
+  constexpr static const char* name() { return "mapping.bool_tags"; }
+
+  Issues operator()(const LaneletMap& map) override;
+};
+
+}  // namespace validation
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_validation/validators/mapping/CurvatureTooBig.h

@@ -0,0 +1,17 @@
+#include <lanelet2_core/LaneletMap.h>
+
+#include "lanelet2_validation/BasicValidator.h"
+
+namespace lanelet {
+namespace validation {
+
+class CurvatureTooBigChecker : public MapValidator {
+ public:
+  constexpr static const char* name() { return "mapping.curvature_too_big"; }
+
+  Issues operator()(const LaneletMap& map) override;
+  static void checkCurvature(Issues& issues, const ConstLineString2d& line, const Id& laneletId);
+};
+
+}  // namespace validation
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_validation/validators/mapping/DuplicatedPoints.h

@@ -0,0 +1,19 @@
+#pragma once
+#include <lanelet2_core/LaneletMap.h>
+
+#include "lanelet2_validation/BasicValidator.h"
+
+namespace lanelet {
+namespace validation {
+
+//! This check looks for points within linestrings or polygons that appear two times in succession. These are not
+//! allowed because they often confuse geometry algorithms.
+class DuplicatedPointsChecker : public MapValidator {
+ public:
+  constexpr static const char* name() { return "mapping.duplicated_points"; }
+
+  Issues operator()(const LaneletMap& map) override;
+};
+
+}  // namespace validation
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_validation/validators/mapping/MandatoryTags.h

@@ -0,0 +1,17 @@
+#include <lanelet2_core/LaneletMap.h>
+
+#include "lanelet2_validation/BasicValidator.h"
+
+namespace lanelet {
+namespace validation {
+
+//! This check looks tags that should be set on a primitive but aren't
+class MandatoryTags : public MapValidator {
+ public:
+  constexpr static const char* name() { return "mapping.mandatory_tags"; }
+
+  Issues operator()(const LaneletMap& map) override;
+};
+
+}  // namespace validation
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_validation/validators/mapping/PointsTooClose.h

@@ -0,0 +1,19 @@
+#include <lanelet2_core/LaneletMap.h>
+
+#include "lanelet2_validation/BasicValidator.h"
+
+namespace lanelet {
+namespace validation {
+
+//! This check looks for points that are suspiciosly close to each other.
+//! This is a common mapping error when users fail to click a point in josm
+//! and instead add a new one.
+class PointsTooCloseChecker : public MapValidator {
+ public:
+  constexpr static const char* name() { return "mapping.points_too_close"; }
+
+  Issues operator()(const LaneletMap& map) override;
+};
+
+}  // namespace validation
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_validation/validators/mapping/UnknownTagValue.h

@@ -0,0 +1,17 @@
+#include <lanelet2_core/LaneletMap.h>
+
+#include "lanelet2_validation/BasicValidator.h"
+
+namespace lanelet {
+namespace validation {
+
+//! This checks that known tags have known values.
+class UnknownTagValue : public MapValidator {
+ public:
+  constexpr static const char* name() { return "mapping.unknown_tag_value"; }
+
+  Issues operator()(const LaneletMap& map) override;
+};
+
+}  // namespace validation
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_validation/validators/mapping/UnknownTags.h

@@ -0,0 +1,18 @@
+#include <lanelet2_core/LaneletMap.h>
+
+#include "lanelet2_validation/BasicValidator.h"
+
+namespace lanelet {
+namespace validation {
+
+//! This check looks for tags that are not part of the lanelet2 specification. This may be very strict, since other tags
+//! are not strictly forbidden, but might make sense in some cases.
+class UnknownTags : public MapValidator {
+ public:
+  constexpr static const char* name() { return "mapping.unknown_tags"; }
+
+  Issues operator()(const LaneletMap& map) override;
+};
+
+}  // namespace validation
+}  // namespace lanelet

src/map/lanelet2/include/lanelet2_validation/validators/routing/RoutingGraphIsValid.h

@@ -0,0 +1,14 @@
+#include "lanelet2_validation/BasicValidator.h"
+
+namespace lanelet {
+namespace validation {
+
+//! This check basically calls the RoutingGraph self-check to ensure the graph is valid.
+class RoutingGraphIsValid : public RoutingGraphValidator {
+ public:
+  constexpr static const char* name() { return "routing.graph_is_valid"; }
+  Issues operator()(const routing::RoutingGraph& graph, const traffic_rules::TrafficRules& /*rules*/) override;
+};
+
+}  // namespace validation
+}  // namespace lanelet

src/map/lanelet2/include/pugiconfig.hpp

@@ -0,0 +1,77 @@
+/**
+ * pugixml parser - version 1.13
+ * --------------------------------------------------------
+ * Copyright (C) 2006-2022, by Arseny Kapoulkine (arseny.kapoulkine@gmail.com)
+ * Report bugs and download new versions at https://pugixml.org/
+ *
+ * This library is distributed under the MIT License. See notice at the end
+ * of this file.
+ *
+ * This work is based on the pugxml parser, which is:
+ * Copyright (C) 2003, by Kristen Wegner (kristen@tima.net)
+ */
+
+#ifndef HEADER_PUGICONFIG_HPP
+#define HEADER_PUGICONFIG_HPP
+
+// Uncomment this to enable wchar_t mode
+// #define PUGIXML_WCHAR_MODE
+
+// Uncomment this to enable compact mode
+// #define PUGIXML_COMPACT
+
+// Uncomment this to disable XPath
+// #define PUGIXML_NO_XPATH
+
+// Uncomment this to disable STL
+// #define PUGIXML_NO_STL
+
+// Uncomment this to disable exceptions
+// #define PUGIXML_NO_EXCEPTIONS
+
+// Set this to control attributes for public classes/functions, i.e.:
+// #define PUGIXML_API __declspec(dllexport) // to export all public symbols from DLL
+// #define PUGIXML_CLASS __declspec(dllimport) // to import all classes from DLL
+// #define PUGIXML_FUNCTION __fastcall // to set calling conventions to all public functions to fastcall
+// In absence of PUGIXML_CLASS/PUGIXML_FUNCTION definitions PUGIXML_API is used instead
+
+// Tune these constants to adjust memory-related behavior
+// #define PUGIXML_MEMORY_PAGE_SIZE 32768
+// #define PUGIXML_MEMORY_OUTPUT_STACK 10240
+// #define PUGIXML_MEMORY_XPATH_PAGE_SIZE 4096
+
+// Tune this constant to adjust max nesting for XPath queries
+// #define PUGIXML_XPATH_DEPTH_LIMIT 1024
+
+// Uncomment this to switch to header-only version
+// #define PUGIXML_HEADER_ONLY
+
+// Uncomment this to enable long long support
+// #define PUGIXML_HAS_LONG_LONG
+
+#endif
+
+/**
+ * Copyright (c) 2006-2022 Arseny Kapoulkine
+ *
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */

src/map/lanelet2/include/pugixml.hpp

@@ -0,0 +1,1506 @@
+/**
+ * pugixml parser - version 1.13
+ * --------------------------------------------------------
+ * Copyright (C) 2006-2022, by Arseny Kapoulkine (arseny.kapoulkine@gmail.com)
+ * Report bugs and download new versions at https://pugixml.org/
+ *
+ * This library is distributed under the MIT License. See notice at the end
+ * of this file.
+ *
+ * This work is based on the pugxml parser, which is:
+ * Copyright (C) 2003, by Kristen Wegner (kristen@tima.net)
+ */
+
+// Define version macro; evaluates to major * 1000 + minor * 10 + patch so that it's safe to use in less-than comparisons
+// Note: pugixml used major * 100 + minor * 10 + patch format up until 1.9 (which had version identifier 190); starting from pugixml 1.10, the minor version number is two digits
+#ifndef PUGIXML_VERSION
+#	define PUGIXML_VERSION 1130 // 1.13
+#endif
+
+// Include user configuration file (this can define various configuration macros)
+#include "pugiconfig.hpp"
+
+#ifndef HEADER_PUGIXML_HPP
+#define HEADER_PUGIXML_HPP
+
+// Include stddef.h for size_t and ptrdiff_t
+#include <stddef.h>
+
+// Include exception header for XPath
+#if !defined(PUGIXML_NO_XPATH) && !defined(PUGIXML_NO_EXCEPTIONS)
+#	include <exception>
+#endif
+
+// Include STL headers
+#ifndef PUGIXML_NO_STL
+#	include <iterator>
+#	include <iosfwd>
+#	include <string>
+#endif
+
+// Macro for deprecated features
+#ifndef PUGIXML_DEPRECATED
+#	if defined(__GNUC__)
+#		define PUGIXML_DEPRECATED __attribute__((deprecated))
+#	elif defined(_MSC_VER) && _MSC_VER >= 1300
+#		define PUGIXML_DEPRECATED __declspec(deprecated)
+#	else
+#		define PUGIXML_DEPRECATED
+#	endif
+#endif
+
+// If no API is defined, assume default
+#ifndef PUGIXML_API
+#	define PUGIXML_API
+#endif
+
+// If no API for classes is defined, assume default
+#ifndef PUGIXML_CLASS
+#	define PUGIXML_CLASS PUGIXML_API
+#endif
+
+// If no API for functions is defined, assume default
+#ifndef PUGIXML_FUNCTION
+#	define PUGIXML_FUNCTION PUGIXML_API
+#endif
+
+// If the platform is known to have long long support, enable long long functions
+#ifndef PUGIXML_HAS_LONG_LONG
+#	if __cplusplus >= 201103
+#		define PUGIXML_HAS_LONG_LONG
+#	elif defined(_MSC_VER) && _MSC_VER >= 1400
+#		define PUGIXML_HAS_LONG_LONG
+#	endif
+#endif
+
+// If the platform is known to have move semantics support, compile move ctor/operator implementation
+#ifndef PUGIXML_HAS_MOVE
+#	if __cplusplus >= 201103
+#		define PUGIXML_HAS_MOVE
+#	elif defined(_MSC_VER) && _MSC_VER >= 1600
+#		define PUGIXML_HAS_MOVE
+#	endif
+#endif
+
+// If C++ is 2011 or higher, add 'noexcept' specifiers
+#ifndef PUGIXML_NOEXCEPT
+#	if __cplusplus >= 201103
+#		define PUGIXML_NOEXCEPT noexcept
+#	elif defined(_MSC_VER) && _MSC_VER >= 1900
+#		define PUGIXML_NOEXCEPT noexcept
+#	else
+#		define PUGIXML_NOEXCEPT
+#	endif
+#endif
+
+// Some functions can not be noexcept in compact mode
+#ifdef PUGIXML_COMPACT
+#	define PUGIXML_NOEXCEPT_IF_NOT_COMPACT
+#else
+#	define PUGIXML_NOEXCEPT_IF_NOT_COMPACT PUGIXML_NOEXCEPT
+#endif
+
+// If C++ is 2011 or higher, add 'override' qualifiers
+#ifndef PUGIXML_OVERRIDE
+#	if __cplusplus >= 201103
+#		define PUGIXML_OVERRIDE override
+#	elif defined(_MSC_VER) && _MSC_VER >= 1700
+#		define PUGIXML_OVERRIDE override
+#	else
+#		define PUGIXML_OVERRIDE
+#	endif
+#endif
+
+// If C++ is 2011 or higher, use 'nullptr'
+#ifndef PUGIXML_NULL
+#	if __cplusplus >= 201103
+#		define PUGIXML_NULL nullptr
+#	elif defined(_MSC_VER) && _MSC_VER >= 1600
+#		define PUGIXML_NULL nullptr
+#	else
+#		define PUGIXML_NULL 0
+#	endif
+#endif
+
+// Character interface macros
+#ifdef PUGIXML_WCHAR_MODE
+#	define PUGIXML_TEXT(t) L ## t
+#	define PUGIXML_CHAR wchar_t
+#else
+#	define PUGIXML_TEXT(t) t
+#	define PUGIXML_CHAR char
+#endif
+
+namespace pugi
+{
+	// Character type used for all internal storage and operations; depends on PUGIXML_WCHAR_MODE
+	typedef PUGIXML_CHAR char_t;
+
+#ifndef PUGIXML_NO_STL
+	// String type used for operations that work with STL string; depends on PUGIXML_WCHAR_MODE
+	typedef std::basic_string<PUGIXML_CHAR, std::char_traits<PUGIXML_CHAR>, std::allocator<PUGIXML_CHAR> > string_t;
+#endif
+}
+
+// The PugiXML namespace
+namespace pugi
+{
+	// Tree node types
+	enum xml_node_type
+	{
+		node_null,			// Empty (null) node handle
+		node_document,		// A document tree's absolute root
+		node_element,		// Element tag, i.e. '<node/>'
+		node_pcdata,		// Plain character data, i.e. 'text'
+		node_cdata,			// Character data, i.e. '<![CDATA[text]]>'
+		node_comment,		// Comment tag, i.e. '<!-- text -->'
+		node_pi,			// Processing instruction, i.e. '<?name?>'
+		node_declaration,	// Document declaration, i.e. '<?xml version="1.0"?>'
+		node_doctype		// Document type declaration, i.e. '<!DOCTYPE doc>'
+	};
+
+	// Parsing options
+
+	// Minimal parsing mode (equivalent to turning all other flags off).
+	// Only elements and PCDATA sections are added to the DOM tree, no text conversions are performed.
+	const unsigned int parse_minimal = 0x0000;
+
+	// This flag determines if processing instructions (node_pi) are added to the DOM tree. This flag is off by default.
+	const unsigned int parse_pi = 0x0001;
+
+	// This flag determines if comments (node_comment) are added to the DOM tree. This flag is off by default.
+	const unsigned int parse_comments = 0x0002;
+
+	// This flag determines if CDATA sections (node_cdata) are added to the DOM tree. This flag is on by default.
+	const unsigned int parse_cdata = 0x0004;
+
+	// This flag determines if plain character data (node_pcdata) that consist only of whitespace are added to the DOM tree.
+	// This flag is off by default; turning it on usually results in slower parsing and more memory consumption.
+	const unsigned int parse_ws_pcdata = 0x0008;
+
+	// This flag determines if character and entity references are expanded during parsing. This flag is on by default.
+	const unsigned int parse_escapes = 0x0010;
+
+	// This flag determines if EOL characters are normalized (converted to #xA) during parsing. This flag is on by default.
+	const unsigned int parse_eol = 0x0020;
+
+	// This flag determines if attribute values are normalized using CDATA normalization rules during parsing. This flag is on by default.
+	const unsigned int parse_wconv_attribute = 0x0040;
+
+	// This flag determines if attribute values are normalized using NMTOKENS normalization rules during parsing. This flag is off by default.
+	const unsigned int parse_wnorm_attribute = 0x0080;
+
+	// This flag determines if document declaration (node_declaration) is added to the DOM tree. This flag is off by default.
+	const unsigned int parse_declaration = 0x0100;
+
+	// This flag determines if document type declaration (node_doctype) is added to the DOM tree. This flag is off by default.
+	const unsigned int parse_doctype = 0x0200;
+
+	// This flag determines if plain character data (node_pcdata) that is the only child of the parent node and that consists only
+	// of whitespace is added to the DOM tree.
+	// This flag is off by default; turning it on may result in slower parsing and more memory consumption.
+	const unsigned int parse_ws_pcdata_single = 0x0400;
+
+	// This flag determines if leading and trailing whitespace is to be removed from plain character data. This flag is off by default.
+	const unsigned int parse_trim_pcdata = 0x0800;
+
+	// This flag determines if plain character data that does not have a parent node is added to the DOM tree, and if an empty document
+	// is a valid document. This flag is off by default.
+	const unsigned int parse_fragment = 0x1000;
+
+	// This flag determines if plain character data is be stored in the parent element's value. This significantly changes the structure of
+	// the document; this flag is only recommended for parsing documents with many PCDATA nodes in memory-constrained environments.
+	// This flag is off by default.
+	const unsigned int parse_embed_pcdata = 0x2000;
+
+	// The default parsing mode.
+	// Elements, PCDATA and CDATA sections are added to the DOM tree, character/reference entities are expanded,
+	// End-of-Line characters are normalized, attribute values are normalized using CDATA normalization rules.
+	const unsigned int parse_default = parse_cdata | parse_escapes | parse_wconv_attribute | parse_eol;
+
+	// The full parsing mode.
+	// Nodes of all types are added to the DOM tree, character/reference entities are expanded,
+	// End-of-Line characters are normalized, attribute values are normalized using CDATA normalization rules.
+	const unsigned int parse_full = parse_default | parse_pi | parse_comments | parse_declaration | parse_doctype;
+
+	// These flags determine the encoding of input data for XML document
+	enum xml_encoding
+	{
+		encoding_auto,		// Auto-detect input encoding using BOM or < / <? detection; use UTF8 if BOM is not found
+		encoding_utf8,		// UTF8 encoding
+		encoding_utf16_le,	// Little-endian UTF16
+		encoding_utf16_be,	// Big-endian UTF16
+		encoding_utf16,		// UTF16 with native endianness
+		encoding_utf32_le,	// Little-endian UTF32
+		encoding_utf32_be,	// Big-endian UTF32
+		encoding_utf32,		// UTF32 with native endianness
+		encoding_wchar,		// The same encoding wchar_t has (either UTF16 or UTF32)
+		encoding_latin1
+	};
+
+	// Formatting flags
+
+	// Indent the nodes that are written to output stream with as many indentation strings as deep the node is in DOM tree. This flag is on by default.
+	const unsigned int format_indent = 0x01;
+
+	// Write encoding-specific BOM to the output stream. This flag is off by default.
+	const unsigned int format_write_bom = 0x02;
+
+	// Use raw output mode (no indentation and no line breaks are written). This flag is off by default.
+	const unsigned int format_raw = 0x04;
+
+	// Omit default XML declaration even if there is no declaration in the document. This flag is off by default.
+	const unsigned int format_no_declaration = 0x08;
+
+	// Don't escape attribute values and PCDATA contents. This flag is off by default.
+	const unsigned int format_no_escapes = 0x10;
+
+	// Open file using text mode in xml_document::save_file. This enables special character (i.e. new-line) conversions on some systems. This flag is off by default.
+	const unsigned int format_save_file_text = 0x20;
+
+	// Write every attribute on a new line with appropriate indentation. This flag is off by default.
+	const unsigned int format_indent_attributes = 0x40;
+
+	// Don't output empty element tags, instead writing an explicit start and end tag even if there are no children. This flag is off by default.
+	const unsigned int format_no_empty_element_tags = 0x80;
+
+	// Skip characters belonging to range [0; 32) instead of "&#xNN;" encoding. This flag is off by default.
+	const unsigned int format_skip_control_chars = 0x100;
+
+	// Use single quotes ' instead of double quotes " for enclosing attribute values. This flag is off by default.
+	const unsigned int format_attribute_single_quote = 0x200;
+
+	// The default set of formatting flags.
+	// Nodes are indented depending on their depth in DOM tree, a default declaration is output if document has none.
+	const unsigned int format_default = format_indent;
+
+	const int default_double_precision = 17;
+	const int default_float_precision = 9;
+
+	// Forward declarations
+	struct xml_attribute_struct;
+	struct xml_node_struct;
+
+	class xml_node_iterator;
+	class xml_attribute_iterator;
+	class xml_named_node_iterator;
+
+	class xml_tree_walker;
+
+	struct xml_parse_result;
+
+	class xml_node;
+
+	class xml_text;
+
+	#ifndef PUGIXML_NO_XPATH
+	class xpath_node;
+	class xpath_node_set;
+	class xpath_query;
+	class xpath_variable_set;
+	#endif
+
+	// Range-based for loop support
+	template <typename It> class xml_object_range
+	{
+	public:
+		typedef It const_iterator;
+		typedef It iterator;
+
+		xml_object_range(It b, It e): _begin(b), _end(e)
+		{
+		}
+
+		It begin() const { return _begin; }
+		It end() const { return _end; }
+
+		bool empty() const { return _begin == _end; }
+
+	private:
+		It _begin, _end;
+	};
+
+	// Writer interface for node printing (see xml_node::print)
+	class PUGIXML_CLASS xml_writer
+	{
+	public:
+		virtual ~xml_writer() {}
+
+		// Write memory chunk into stream/file/whatever
+		virtual void write(const void* data, size_t size) = 0;
+	};
+
+	// xml_writer implementation for FILE*
+	class PUGIXML_CLASS xml_writer_file: public xml_writer
+	{
+	public:
+		// Construct writer from a FILE* object; void* is used to avoid header dependencies on stdio
+		xml_writer_file(void* file);
+
+		virtual void write(const void* data, size_t size) PUGIXML_OVERRIDE;
+
+	private:
+		void* file;
+	};
+
+	#ifndef PUGIXML_NO_STL
+	// xml_writer implementation for streams
+	class PUGIXML_CLASS xml_writer_stream: public xml_writer
+	{
+	public:
+		// Construct writer from an output stream object
+		xml_writer_stream(std::basic_ostream<char, std::char_traits<char> >& stream);
+		xml_writer_stream(std::basic_ostream<wchar_t, std::char_traits<wchar_t> >& stream);
+
+		virtual void write(const void* data, size_t size) PUGIXML_OVERRIDE;
+
+	private:
+		std::basic_ostream<char, std::char_traits<char> >* narrow_stream;
+		std::basic_ostream<wchar_t, std::char_traits<wchar_t> >* wide_stream;
+	};
+	#endif
+
+	// A light-weight handle for manipulating attributes in DOM tree
+	class PUGIXML_CLASS xml_attribute
+	{
+		friend class xml_attribute_iterator;
+		friend class xml_node;
+
+	private:
+		xml_attribute_struct* _attr;
+
+		typedef void (*unspecified_bool_type)(xml_attribute***);
+
+	public:
+		// Default constructor. Constructs an empty attribute.
+		xml_attribute();
+
+		// Constructs attribute from internal pointer
+		explicit xml_attribute(xml_attribute_struct* attr);
+
+		// Safe bool conversion operator
+		operator unspecified_bool_type() const;
+
+		// Borland C++ workaround
+		bool operator!() const;
+
+		// Comparison operators (compares wrapped attribute pointers)
+		bool operator==(const xml_attribute& r) const;
+		bool operator!=(const xml_attribute& r) const;
+		bool operator<(const xml_attribute& r) const;
+		bool operator>(const xml_attribute& r) const;
+		bool operator<=(const xml_attribute& r) const;
+		bool operator>=(const xml_attribute& r) const;
+
+		// Check if attribute is empty
+		bool empty() const;
+
+		// Get attribute name/value, or "" if attribute is empty
+		const char_t* name() const;
+		const char_t* value() const;
+
+		// Get attribute value, or the default value if attribute is empty
+		const char_t* as_string(const char_t* def = PUGIXML_TEXT("")) const;
+
+		// Get attribute value as a number, or the default value if conversion did not succeed or attribute is empty
+		int as_int(int def = 0) const;
+		unsigned int as_uint(unsigned int def = 0) const;
+		double as_double(double def = 0) const;
+		float as_float(float def = 0) const;
+
+	#ifdef PUGIXML_HAS_LONG_LONG
+		long long as_llong(long long def = 0) const;
+		unsigned long long as_ullong(unsigned long long def = 0) const;
+	#endif
+
+		// Get attribute value as bool (returns true if first character is in '1tTyY' set), or the default value if attribute is empty
+		bool as_bool(bool def = false) const;
+
+		// Set attribute name/value (returns false if attribute is empty or there is not enough memory)
+		bool set_name(const char_t* rhs);
+		bool set_value(const char_t* rhs, size_t sz);
+		bool set_value(const char_t* rhs);
+
+		// Set attribute value with type conversion (numbers are converted to strings, boolean is converted to "true"/"false")
+		bool set_value(int rhs);
+		bool set_value(unsigned int rhs);
+		bool set_value(long rhs);
+		bool set_value(unsigned long rhs);
+		bool set_value(double rhs);
+		bool set_value(double rhs, int precision);
+		bool set_value(float rhs);
+		bool set_value(float rhs, int precision);
+		bool set_value(bool rhs);
+
+	#ifdef PUGIXML_HAS_LONG_LONG
+		bool set_value(long long rhs);
+		bool set_value(unsigned long long rhs);
+	#endif
+
+		// Set attribute value (equivalent to set_value without error checking)
+		xml_attribute& operator=(const char_t* rhs);
+		xml_attribute& operator=(int rhs);
+		xml_attribute& operator=(unsigned int rhs);
+		xml_attribute& operator=(long rhs);
+		xml_attribute& operator=(unsigned long rhs);
+		xml_attribute& operator=(double rhs);
+		xml_attribute& operator=(float rhs);
+		xml_attribute& operator=(bool rhs);
+
+	#ifdef PUGIXML_HAS_LONG_LONG
+		xml_attribute& operator=(long long rhs);
+		xml_attribute& operator=(unsigned long long rhs);
+	#endif
+
+		// Get next/previous attribute in the attribute list of the parent node
+		xml_attribute next_attribute() const;
+		xml_attribute previous_attribute() const;
+
+		// Get hash value (unique for handles to the same object)
+		size_t hash_value() const;
+
+		// Get internal pointer
+		xml_attribute_struct* internal_object() const;
+	};
+
+#ifdef __BORLANDC__
+	// Borland C++ workaround
+	bool PUGIXML_FUNCTION operator&&(const xml_attribute& lhs, bool rhs);
+	bool PUGIXML_FUNCTION operator||(const xml_attribute& lhs, bool rhs);
+#endif
+
+	// A light-weight handle for manipulating nodes in DOM tree
+	class PUGIXML_CLASS xml_node
+	{
+		friend class xml_attribute_iterator;
+		friend class xml_node_iterator;
+		friend class xml_named_node_iterator;
+
+	protected:
+		xml_node_struct* _root;
+
+		typedef void (*unspecified_bool_type)(xml_node***);
+
+	public:
+		// Default constructor. Constructs an empty node.
+		xml_node();
+
+		// Constructs node from internal pointer
+		explicit xml_node(xml_node_struct* p);
+
+		// Safe bool conversion operator
+		operator unspecified_bool_type() const;
+
+		// Borland C++ workaround
+		bool operator!() const;
+
+		// Comparison operators (compares wrapped node pointers)
+		bool operator==(const xml_node& r) const;
+		bool operator!=(const xml_node& r) const;
+		bool operator<(const xml_node& r) const;
+		bool operator>(const xml_node& r) const;
+		bool operator<=(const xml_node& r) const;
+		bool operator>=(const xml_node& r) const;
+
+		// Check if node is empty.
+		bool empty() const;
+
+		// Get node type
+		xml_node_type type() const;
+
+		// Get node name, or "" if node is empty or it has no name
+		const char_t* name() const;
+
+		// Get node value, or "" if node is empty or it has no value
+		// Note: For <node>text</node> node.value() does not return "text"! Use child_value() or text() methods to access text inside nodes.
+		const char_t* value() const;
+
+		// Get attribute list
+		xml_attribute first_attribute() const;
+		xml_attribute last_attribute() const;
+
+		// Get children list
+		xml_node first_child() const;
+		xml_node last_child() const;
+
+		// Get next/previous sibling in the children list of the parent node
+		xml_node next_sibling() const;
+		xml_node previous_sibling() const;
+
+		// Get parent node
+		xml_node parent() const;
+
+		// Get root of DOM tree this node belongs to
+		xml_node root() const;
+
+		// Get text object for the current node
+		xml_text text() const;
+
+		// Get child, attribute or next/previous sibling with the specified name
+		xml_node child(const char_t* name) const;
+		xml_attribute attribute(const char_t* name) const;
+		xml_node next_sibling(const char_t* name) const;
+		xml_node previous_sibling(const char_t* name) const;
+
+		// Get attribute, starting the search from a hint (and updating hint so that searching for a sequence of attributes is fast)
+		xml_attribute attribute(const char_t* name, xml_attribute& hint) const;
+
+		// Get child value of current node; that is, value of the first child node of type PCDATA/CDATA
+		const char_t* child_value() const;
+
+		// Get child value of child with specified name. Equivalent to child(name).child_value().
+		const char_t* child_value(const char_t* name) const;
+
+		// Set node name/value (returns false if node is empty, there is not enough memory, or node can not have name/value)
+		bool set_name(const char_t* rhs);
+		bool set_value(const char_t* rhs, size_t sz);
+		bool set_value(const char_t* rhs);
+
+		// Add attribute with specified name. Returns added attribute, or empty attribute on errors.
+		xml_attribute append_attribute(const char_t* name);
+		xml_attribute prepend_attribute(const char_t* name);
+		xml_attribute insert_attribute_after(const char_t* name, const xml_attribute& attr);
+		xml_attribute insert_attribute_before(const char_t* name, const xml_attribute& attr);
+
+		// Add a copy of the specified attribute. Returns added attribute, or empty attribute on errors.
+		xml_attribute append_copy(const xml_attribute& proto);
+		xml_attribute prepend_copy(const xml_attribute& proto);
+		xml_attribute insert_copy_after(const xml_attribute& proto, const xml_attribute& attr);
+		xml_attribute insert_copy_before(const xml_attribute& proto, const xml_attribute& attr);
+
+		// Add child node with specified type. Returns added node, or empty node on errors.
+		xml_node append_child(xml_node_type type = node_element);
+		xml_node prepend_child(xml_node_type type = node_element);
+		xml_node insert_child_after(xml_node_type type, const xml_node& node);
+		xml_node insert_child_before(xml_node_type type, const xml_node& node);
+
+		// Add child element with specified name. Returns added node, or empty node on errors.
+		xml_node append_child(const char_t* name);
+		xml_node prepend_child(const char_t* name);
+		xml_node insert_child_after(const char_t* name, const xml_node& node);
+		xml_node insert_child_before(const char_t* name, const xml_node& node);
+
+		// Add a copy of the specified node as a child. Returns added node, or empty node on errors.
+		xml_node append_copy(const xml_node& proto);
+		xml_node prepend_copy(const xml_node& proto);
+		xml_node insert_copy_after(const xml_node& proto, const xml_node& node);
+		xml_node insert_copy_before(const xml_node& proto, const xml_node& node);
+
+		// Move the specified node to become a child of this node. Returns moved node, or empty node on errors.
+		xml_node append_move(const xml_node& moved);
+		xml_node prepend_move(const xml_node& moved);
+		xml_node insert_move_after(const xml_node& moved, const xml_node& node);
+		xml_node insert_move_before(const xml_node& moved, const xml_node& node);
+
+		// Remove specified attribute
+		bool remove_attribute(const xml_attribute& a);
+		bool remove_attribute(const char_t* name);
+
+		// Remove all attributes
+		bool remove_attributes();
+
+		// Remove specified child
+		bool remove_child(const xml_node& n);
+		bool remove_child(const char_t* name);
+
+		// Remove all children
+		bool remove_children();
+
+		// Parses buffer as an XML document fragment and appends all nodes as children of the current node.
+		// Copies/converts the buffer, so it may be deleted or changed after the function returns.
+		// Note: append_buffer allocates memory that has the lifetime of the owning document; removing the appended nodes does not immediately reclaim that memory.
+		xml_parse_result append_buffer(const void* contents, size_t size, unsigned int options = parse_default, xml_encoding encoding = encoding_auto);
+
+		// Find attribute using predicate. Returns first attribute for which predicate returned true.
+		template <typename Predicate> xml_attribute find_attribute(Predicate pred) const
+		{
+			if (!_root) return xml_attribute();
+
+			for (xml_attribute attrib = first_attribute(); attrib; attrib = attrib.next_attribute())
+				if (pred(attrib))
+					return attrib;
+
+			return xml_attribute();
+		}
+
+		// Find child node using predicate. Returns first child for which predicate returned true.
+		template <typename Predicate> xml_node find_child(Predicate pred) const
+		{
+			if (!_root) return xml_node();
+
+			for (xml_node node = first_child(); node; node = node.next_sibling())
+				if (pred(node))
+					return node;
+
+			return xml_node();
+		}
+
+		// Find node from subtree using predicate. Returns first node from subtree (depth-first), for which predicate returned true.
+		template <typename Predicate> xml_node find_node(Predicate pred) const
+		{
+			if (!_root) return xml_node();
+
+			xml_node cur = first_child();
+
+			while (cur._root && cur._root != _root)
+			{
+				if (pred(cur)) return cur;
+
+				if (cur.first_child()) cur = cur.first_child();
+				else if (cur.next_sibling()) cur = cur.next_sibling();
+				else
+				{
+					while (!cur.next_sibling() && cur._root != _root) cur = cur.parent();
+
+					if (cur._root != _root) cur = cur.next_sibling();
+				}
+			}
+
+			return xml_node();
+		}
+
+		// Find child node by attribute name/value
+		xml_node find_child_by_attribute(const char_t* name, const char_t* attr_name, const char_t* attr_value) const;
+		xml_node find_child_by_attribute(const char_t* attr_name, const char_t* attr_value) const;
+
+	#ifndef PUGIXML_NO_STL
+		// Get the absolute node path from root as a text string.
+		string_t path(char_t delimiter = '/') const;
+	#endif
+
+		// Search for a node by path consisting of node names and . or .. elements.
+		xml_node first_element_by_path(const char_t* path, char_t delimiter = '/') const;
+
+		// Recursively traverse subtree with xml_tree_walker
+		bool traverse(xml_tree_walker& walker);
+
+	#ifndef PUGIXML_NO_XPATH
+		// Select single node by evaluating XPath query. Returns first node from the resulting node set.
+		xpath_node select_node(const char_t* query, xpath_variable_set* variables = PUGIXML_NULL) const;
+		xpath_node select_node(const xpath_query& query) const;
+
+		// Select node set by evaluating XPath query
+		xpath_node_set select_nodes(const char_t* query, xpath_variable_set* variables = PUGIXML_NULL) const;
+		xpath_node_set select_nodes(const xpath_query& query) const;
+
+		// (deprecated: use select_node instead) Select single node by evaluating XPath query.
+		PUGIXML_DEPRECATED xpath_node select_single_node(const char_t* query, xpath_variable_set* variables = PUGIXML_NULL) const;
+		PUGIXML_DEPRECATED xpath_node select_single_node(const xpath_query& query) const;
+
+	#endif
+
+		// Print subtree using a writer object
+		void print(xml_writer& writer, const char_t* indent = PUGIXML_TEXT("\t"), unsigned int flags = format_default, xml_encoding encoding = encoding_auto, unsigned int depth = 0) const;
+
+	#ifndef PUGIXML_NO_STL
+		// Print subtree to stream
+		void print(std::basic_ostream<char, std::char_traits<char> >& os, const char_t* indent = PUGIXML_TEXT("\t"), unsigned int flags = format_default, xml_encoding encoding = encoding_auto, unsigned int depth = 0) const;
+		void print(std::basic_ostream<wchar_t, std::char_traits<wchar_t> >& os, const char_t* indent = PUGIXML_TEXT("\t"), unsigned int flags = format_default, unsigned int depth = 0) const;
+	#endif
+
+		// Child nodes iterators
+		typedef xml_node_iterator iterator;
+
+		iterator begin() const;
+		iterator end() const;
+
+		// Attribute iterators
+		typedef xml_attribute_iterator attribute_iterator;
+
+		attribute_iterator attributes_begin() const;
+		attribute_iterator attributes_end() const;
+
+		// Range-based for support
+		xml_object_range<xml_node_iterator> children() const;
+		xml_object_range<xml_named_node_iterator> children(const char_t* name) const;
+		xml_object_range<xml_attribute_iterator> attributes() const;
+
+		// Get node offset in parsed file/string (in char_t units) for debugging purposes
+		ptrdiff_t offset_debug() const;
+
+		// Get hash value (unique for handles to the same object)
+		size_t hash_value() const;
+
+		// Get internal pointer
+		xml_node_struct* internal_object() const;
+	};
+
+#ifdef __BORLANDC__
+	// Borland C++ workaround
+	bool PUGIXML_FUNCTION operator&&(const xml_node& lhs, bool rhs);
+	bool PUGIXML_FUNCTION operator||(const xml_node& lhs, bool rhs);
+#endif
+
+	// A helper for working with text inside PCDATA nodes
+	class PUGIXML_CLASS xml_text
+	{
+		friend class xml_node;
+
+		xml_node_struct* _root;
+
+		typedef void (*unspecified_bool_type)(xml_text***);
+
+		explicit xml_text(xml_node_struct* root);
+
+		xml_node_struct* _data_new();
+		xml_node_struct* _data() const;
+
+	public:
+		// Default constructor. Constructs an empty object.
+		xml_text();
+
+		// Safe bool conversion operator
+		operator unspecified_bool_type() const;
+
+		// Borland C++ workaround
+		bool operator!() const;
+
+		// Check if text object is empty
+		bool empty() const;
+
+		// Get text, or "" if object is empty
+		const char_t* get() const;
+
+		// Get text, or the default value if object is empty
+		const char_t* as_string(const char_t* def = PUGIXML_TEXT("")) const;
+
+		// Get text as a number, or the default value if conversion did not succeed or object is empty
+		int as_int(int def = 0) const;
+		unsigned int as_uint(unsigned int def = 0) const;
+		double as_double(double def = 0) const;
+		float as_float(float def = 0) const;
+
+	#ifdef PUGIXML_HAS_LONG_LONG
+		long long as_llong(long long def = 0) const;
+		unsigned long long as_ullong(unsigned long long def = 0) const;
+	#endif
+
+		// Get text as bool (returns true if first character is in '1tTyY' set), or the default value if object is empty
+		bool as_bool(bool def = false) const;
+
+		// Set text (returns false if object is empty or there is not enough memory)
+		bool set(const char_t* rhs, size_t sz);
+		bool set(const char_t* rhs);
+
+		// Set text with type conversion (numbers are converted to strings, boolean is converted to "true"/"false")
+		bool set(int rhs);
+		bool set(unsigned int rhs);
+		bool set(long rhs);
+		bool set(unsigned long rhs);
+		bool set(double rhs);
+		bool set(double rhs, int precision);
+		bool set(float rhs);
+		bool set(float rhs, int precision);
+		bool set(bool rhs);
+
+	#ifdef PUGIXML_HAS_LONG_LONG
+		bool set(long long rhs);
+		bool set(unsigned long long rhs);
+	#endif
+
+		// Set text (equivalent to set without error checking)
+		xml_text& operator=(const char_t* rhs);
+		xml_text& operator=(int rhs);
+		xml_text& operator=(unsigned int rhs);
+		xml_text& operator=(long rhs);
+		xml_text& operator=(unsigned long rhs);
+		xml_text& operator=(double rhs);
+		xml_text& operator=(float rhs);
+		xml_text& operator=(bool rhs);
+
+	#ifdef PUGIXML_HAS_LONG_LONG
+		xml_text& operator=(long long rhs);
+		xml_text& operator=(unsigned long long rhs);
+	#endif
+
+		// Get the data node (node_pcdata or node_cdata) for this object
+		xml_node data() const;
+	};
+
+#ifdef __BORLANDC__
+	// Borland C++ workaround
+	bool PUGIXML_FUNCTION operator&&(const xml_text& lhs, bool rhs);
+	bool PUGIXML_FUNCTION operator||(const xml_text& lhs, bool rhs);
+#endif
+
+	// Child node iterator (a bidirectional iterator over a collection of xml_node)
+	class PUGIXML_CLASS xml_node_iterator
+	{
+		friend class xml_node;
+
+	private:
+		mutable xml_node _wrap;
+		xml_node _parent;
+
+		xml_node_iterator(xml_node_struct* ref, xml_node_struct* parent);
+
+	public:
+		// Iterator traits
+		typedef ptrdiff_t difference_type;
+		typedef xml_node value_type;
+		typedef xml_node* pointer;
+		typedef xml_node& reference;
+
+	#ifndef PUGIXML_NO_STL
+		typedef std::bidirectional_iterator_tag iterator_category;
+	#endif
+
+		// Default constructor
+		xml_node_iterator();
+
+		// Construct an iterator which points to the specified node
+		xml_node_iterator(const xml_node& node);
+
+		// Iterator operators
+		bool operator==(const xml_node_iterator& rhs) const;
+		bool operator!=(const xml_node_iterator& rhs) const;
+
+		xml_node& operator*() const;
+		xml_node* operator->() const;
+
+		xml_node_iterator& operator++();
+		xml_node_iterator operator++(int);
+
+		xml_node_iterator& operator--();
+		xml_node_iterator operator--(int);
+	};
+
+	// Attribute iterator (a bidirectional iterator over a collection of xml_attribute)
+	class PUGIXML_CLASS xml_attribute_iterator
+	{
+		friend class xml_node;
+
+	private:
+		mutable xml_attribute _wrap;
+		xml_node _parent;
+
+		xml_attribute_iterator(xml_attribute_struct* ref, xml_node_struct* parent);
+
+	public:
+		// Iterator traits
+		typedef ptrdiff_t difference_type;
+		typedef xml_attribute value_type;
+		typedef xml_attribute* pointer;
+		typedef xml_attribute& reference;
+
+	#ifndef PUGIXML_NO_STL
+		typedef std::bidirectional_iterator_tag iterator_category;
+	#endif
+
+		// Default constructor
+		xml_attribute_iterator();
+
+		// Construct an iterator which points to the specified attribute
+		xml_attribute_iterator(const xml_attribute& attr, const xml_node& parent);
+
+		// Iterator operators
+		bool operator==(const xml_attribute_iterator& rhs) const;
+		bool operator!=(const xml_attribute_iterator& rhs) const;
+
+		xml_attribute& operator*() const;
+		xml_attribute* operator->() const;
+
+		xml_attribute_iterator& operator++();
+		xml_attribute_iterator operator++(int);
+
+		xml_attribute_iterator& operator--();
+		xml_attribute_iterator operator--(int);
+	};
+
+	// Named node range helper
+	class PUGIXML_CLASS xml_named_node_iterator
+	{
+		friend class xml_node;
+
+	public:
+		// Iterator traits
+		typedef ptrdiff_t difference_type;
+		typedef xml_node value_type;
+		typedef xml_node* pointer;
+		typedef xml_node& reference;
+
+	#ifndef PUGIXML_NO_STL
+		typedef std::bidirectional_iterator_tag iterator_category;
+	#endif
+
+		// Default constructor
+		xml_named_node_iterator();
+
+		// Construct an iterator which points to the specified node
+		xml_named_node_iterator(const xml_node& node, const char_t* name);
+
+		// Iterator operators
+		bool operator==(const xml_named_node_iterator& rhs) const;
+		bool operator!=(const xml_named_node_iterator& rhs) const;
+
+		xml_node& operator*() const;
+		xml_node* operator->() const;
+
+		xml_named_node_iterator& operator++();
+		xml_named_node_iterator operator++(int);
+
+		xml_named_node_iterator& operator--();
+		xml_named_node_iterator operator--(int);
+
+	private:
+		mutable xml_node _wrap;
+		xml_node _parent;
+		const char_t* _name;
+
+		xml_named_node_iterator(xml_node_struct* ref, xml_node_struct* parent, const char_t* name);
+	};
+
+	// Abstract tree walker class (see xml_node::traverse)
+	class PUGIXML_CLASS xml_tree_walker
+	{
+		friend class xml_node;
+
+	private:
+		int _depth;
+
+	protected:
+		// Get current traversal depth
+		int depth() const;
+
+	public:
+		xml_tree_walker();
+		virtual ~xml_tree_walker();
+
+		// Callback that is called when traversal begins
+		virtual bool begin(xml_node& node);
+
+		// Callback that is called for each node traversed
+		virtual bool for_each(xml_node& node) = 0;
+
+		// Callback that is called when traversal ends
+		virtual bool end(xml_node& node);
+	};
+
+	// Parsing status, returned as part of xml_parse_result object
+	enum xml_parse_status
+	{
+		status_ok = 0,				// No error
+
+		status_file_not_found,		// File was not found during load_file()
+		status_io_error,			// Error reading from file/stream
+		status_out_of_memory,		// Could not allocate memory
+		status_internal_error,		// Internal error occurred
+
+		status_unrecognized_tag,	// Parser could not determine tag type
+
+		status_bad_pi,				// Parsing error occurred while parsing document declaration/processing instruction
+		status_bad_comment,			// Parsing error occurred while parsing comment
+		status_bad_cdata,			// Parsing error occurred while parsing CDATA section
+		status_bad_doctype,			// Parsing error occurred while parsing document type declaration
+		status_bad_pcdata,			// Parsing error occurred while parsing PCDATA section
+		status_bad_start_element,	// Parsing error occurred while parsing start element tag
+		status_bad_attribute,		// Parsing error occurred while parsing element attribute
+		status_bad_end_element,		// Parsing error occurred while parsing end element tag
+		status_end_element_mismatch,// There was a mismatch of start-end tags (closing tag had incorrect name, some tag was not closed or there was an excessive closing tag)
+
+		status_append_invalid_root,	// Unable to append nodes since root type is not node_element or node_document (exclusive to xml_node::append_buffer)
+
+		status_no_document_element	// Parsing resulted in a document without element nodes
+	};
+
+	// Parsing result
+	struct PUGIXML_CLASS xml_parse_result
+	{
+		// Parsing status (see xml_parse_status)
+		xml_parse_status status;
+
+		// Last parsed offset (in char_t units from start of input data)
+		ptrdiff_t offset;
+
+		// Source document encoding
+		xml_encoding encoding;
+
+		// Default constructor, initializes object to failed state
+		xml_parse_result();
+
+		// Cast to bool operator
+		operator bool() const;
+
+		// Get error description
+		const char* description() const;
+	};
+
+	// Document class (DOM tree root)
+	class PUGIXML_CLASS xml_document: public xml_node
+	{
+	private:
+		char_t* _buffer;
+
+		char _memory[192];
+
+		// Non-copyable semantics
+		xml_document(const xml_document&);
+		xml_document& operator=(const xml_document&);
+
+		void _create();
+		void _destroy();
+		void _move(xml_document& rhs) PUGIXML_NOEXCEPT_IF_NOT_COMPACT;
+
+	public:
+		// Default constructor, makes empty document
+		xml_document();
+
+		// Destructor, invalidates all node/attribute handles to this document
+		~xml_document();
+
+	#ifdef PUGIXML_HAS_MOVE
+		// Move semantics support
+		xml_document(xml_document&& rhs) PUGIXML_NOEXCEPT_IF_NOT_COMPACT;
+		xml_document& operator=(xml_document&& rhs) PUGIXML_NOEXCEPT_IF_NOT_COMPACT;
+	#endif
+
+		// Removes all nodes, leaving the empty document
+		void reset();
+
+		// Removes all nodes, then copies the entire contents of the specified document
+		void reset(const xml_document& proto);
+
+	#ifndef PUGIXML_NO_STL
+		// Load document from stream.
+		xml_parse_result load(std::basic_istream<char, std::char_traits<char> >& stream, unsigned int options = parse_default, xml_encoding encoding = encoding_auto);
+		xml_parse_result load(std::basic_istream<wchar_t, std::char_traits<wchar_t> >& stream, unsigned int options = parse_default);
+	#endif
+
+		// (deprecated: use load_string instead) Load document from zero-terminated string. No encoding conversions are applied.
+		PUGIXML_DEPRECATED xml_parse_result load(const char_t* contents, unsigned int options = parse_default);
+
+		// Load document from zero-terminated string. No encoding conversions are applied.
+		xml_parse_result load_string(const char_t* contents, unsigned int options = parse_default);
+
+		// Load document from file
+		xml_parse_result load_file(const char* path, unsigned int options = parse_default, xml_encoding encoding = encoding_auto);
+		xml_parse_result load_file(const wchar_t* path, unsigned int options = parse_default, xml_encoding encoding = encoding_auto);
+
+		// Load document from buffer. Copies/converts the buffer, so it may be deleted or changed after the function returns.
+		xml_parse_result load_buffer(const void* contents, size_t size, unsigned int options = parse_default, xml_encoding encoding = encoding_auto);
+
+		// Load document from buffer, using the buffer for in-place parsing (the buffer is modified and used for storage of document data).
+		// You should ensure that buffer data will persist throughout the document's lifetime, and free the buffer memory manually once document is destroyed.
+		xml_parse_result load_buffer_inplace(void* contents, size_t size, unsigned int options = parse_default, xml_encoding encoding = encoding_auto);
+
+		// Load document from buffer, using the buffer for in-place parsing (the buffer is modified and used for storage of document data).
+		// You should allocate the buffer with pugixml allocation function; document will free the buffer when it is no longer needed (you can't use it anymore).
+		xml_parse_result load_buffer_inplace_own(void* contents, size_t size, unsigned int options = parse_default, xml_encoding encoding = encoding_auto);
+
+		// Save XML document to writer (semantics is slightly different from xml_node::print, see documentation for details).
+		void save(xml_writer& writer, const char_t* indent = PUGIXML_TEXT("\t"), unsigned int flags = format_default, xml_encoding encoding = encoding_auto) const;
+
+	#ifndef PUGIXML_NO_STL
+		// Save XML document to stream (semantics is slightly different from xml_node::print, see documentation for details).
+		void save(std::basic_ostream<char, std::char_traits<char> >& stream, const char_t* indent = PUGIXML_TEXT("\t"), unsigned int flags = format_default, xml_encoding encoding = encoding_auto) const;
+		void save(std::basic_ostream<wchar_t, std::char_traits<wchar_t> >& stream, const char_t* indent = PUGIXML_TEXT("\t"), unsigned int flags = format_default) const;
+	#endif
+
+		// Save XML to file
+		bool save_file(const char* path, const char_t* indent = PUGIXML_TEXT("\t"), unsigned int flags = format_default, xml_encoding encoding = encoding_auto) const;
+		bool save_file(const wchar_t* path, const char_t* indent = PUGIXML_TEXT("\t"), unsigned int flags = format_default, xml_encoding encoding = encoding_auto) const;
+
+		// Get document element
+		xml_node document_element() const;
+	};
+
+#ifndef PUGIXML_NO_XPATH
+	// XPath query return type
+	enum xpath_value_type
+	{
+		xpath_type_none,	  // Unknown type (query failed to compile)
+		xpath_type_node_set,  // Node set (xpath_node_set)
+		xpath_type_number,	  // Number
+		xpath_type_string,	  // String
+		xpath_type_boolean	  // Boolean
+	};
+
+	// XPath parsing result
+	struct PUGIXML_CLASS xpath_parse_result
+	{
+		// Error message (0 if no error)
+		const char* error;
+
+		// Last parsed offset (in char_t units from string start)
+		ptrdiff_t offset;
+
+		// Default constructor, initializes object to failed state
+		xpath_parse_result();
+
+		// Cast to bool operator
+		operator bool() const;
+
+		// Get error description
+		const char* description() const;
+	};
+
+	// A single XPath variable
+	class PUGIXML_CLASS xpath_variable
+	{
+		friend class xpath_variable_set;
+
+	protected:
+		xpath_value_type _type;
+		xpath_variable* _next;
+
+		xpath_variable(xpath_value_type type);
+
+		// Non-copyable semantics
+		xpath_variable(const xpath_variable&);
+		xpath_variable& operator=(const xpath_variable&);
+
+	public:
+		// Get variable name
+		const char_t* name() const;
+
+		// Get variable type
+		xpath_value_type type() const;
+
+		// Get variable value; no type conversion is performed, default value (false, NaN, empty string, empty node set) is returned on type mismatch error
+		bool get_boolean() const;
+		double get_number() const;
+		const char_t* get_string() const;
+		const xpath_node_set& get_node_set() const;
+
+		// Set variable value; no type conversion is performed, false is returned on type mismatch error
+		bool set(bool value);
+		bool set(double value);
+		bool set(const char_t* value);
+		bool set(const xpath_node_set& value);
+	};
+
+	// A set of XPath variables
+	class PUGIXML_CLASS xpath_variable_set
+	{
+	private:
+		xpath_variable* _data[64];
+
+		void _assign(const xpath_variable_set& rhs);
+		void _swap(xpath_variable_set& rhs);
+
+		xpath_variable* _find(const char_t* name) const;
+
+		static bool _clone(xpath_variable* var, xpath_variable** out_result);
+		static void _destroy(xpath_variable* var);
+
+	public:
+		// Default constructor/destructor
+		xpath_variable_set();
+		~xpath_variable_set();
+
+		// Copy constructor/assignment operator
+		xpath_variable_set(const xpath_variable_set& rhs);
+		xpath_variable_set& operator=(const xpath_variable_set& rhs);
+
+	#ifdef PUGIXML_HAS_MOVE
+		// Move semantics support
+		xpath_variable_set(xpath_variable_set&& rhs) PUGIXML_NOEXCEPT;
+		xpath_variable_set& operator=(xpath_variable_set&& rhs) PUGIXML_NOEXCEPT;
+	#endif
+
+		// Add a new variable or get the existing one, if the types match
+		xpath_variable* add(const char_t* name, xpath_value_type type);
+
+		// Set value of an existing variable; no type conversion is performed, false is returned if there is no such variable or if types mismatch
+		bool set(const char_t* name, bool value);
+		bool set(const char_t* name, double value);
+		bool set(const char_t* name, const char_t* value);
+		bool set(const char_t* name, const xpath_node_set& value);
+
+		// Get existing variable by name
+		xpath_variable* get(const char_t* name);
+		const xpath_variable* get(const char_t* name) const;
+	};
+
+	// A compiled XPath query object
+	class PUGIXML_CLASS xpath_query
+	{
+	private:
+		void* _impl;
+		xpath_parse_result _result;
+
+		typedef void (*unspecified_bool_type)(xpath_query***);
+
+		// Non-copyable semantics
+		xpath_query(const xpath_query&);
+		xpath_query& operator=(const xpath_query&);
+
+	public:
+		// Construct a compiled object from XPath expression.
+		// If PUGIXML_NO_EXCEPTIONS is not defined, throws xpath_exception on compilation errors.
+		explicit xpath_query(const char_t* query, xpath_variable_set* variables = PUGIXML_NULL);
+
+		// Constructor
+		xpath_query();
+
+		// Destructor
+		~xpath_query();
+
+	#ifdef PUGIXML_HAS_MOVE
+		// Move semantics support
+		xpath_query(xpath_query&& rhs) PUGIXML_NOEXCEPT;
+		xpath_query& operator=(xpath_query&& rhs) PUGIXML_NOEXCEPT;
+	#endif
+
+		// Get query expression return type
+		xpath_value_type return_type() const;
+
+		// Evaluate expression as boolean value in the specified context; performs type conversion if necessary.
+		// If PUGIXML_NO_EXCEPTIONS is not defined, throws std::bad_alloc on out of memory errors.
+		bool evaluate_boolean(const xpath_node& n) const;
+
+		// Evaluate expression as double value in the specified context; performs type conversion if necessary.
+		// If PUGIXML_NO_EXCEPTIONS is not defined, throws std::bad_alloc on out of memory errors.
+		double evaluate_number(const xpath_node& n) const;
+
+	#ifndef PUGIXML_NO_STL
+		// Evaluate expression as string value in the specified context; performs type conversion if necessary.
+		// If PUGIXML_NO_EXCEPTIONS is not defined, throws std::bad_alloc on out of memory errors.
+		string_t evaluate_string(const xpath_node& n) const;
+	#endif
+
+		// Evaluate expression as string value in the specified context; performs type conversion if necessary.
+		// At most capacity characters are written to the destination buffer, full result size is returned (includes terminating zero).
+		// If PUGIXML_NO_EXCEPTIONS is not defined, throws std::bad_alloc on out of memory errors.
+		// If PUGIXML_NO_EXCEPTIONS is defined, returns empty  set instead.
+		size_t evaluate_string(char_t* buffer, size_t capacity, const xpath_node& n) const;
+
+		// Evaluate expression as node set in the specified context.
+		// If PUGIXML_NO_EXCEPTIONS is not defined, throws xpath_exception on type mismatch and std::bad_alloc on out of memory errors.
+		// If PUGIXML_NO_EXCEPTIONS is defined, returns empty node set instead.
+		xpath_node_set evaluate_node_set(const xpath_node& n) const;
+
+		// Evaluate expression as node set in the specified context.
+		// Return first node in document order, or empty node if node set is empty.
+		// If PUGIXML_NO_EXCEPTIONS is not defined, throws xpath_exception on type mismatch and std::bad_alloc on out of memory errors.
+		// If PUGIXML_NO_EXCEPTIONS is defined, returns empty node instead.
+		xpath_node evaluate_node(const xpath_node& n) const;
+
+		// Get parsing result (used to get compilation errors in PUGIXML_NO_EXCEPTIONS mode)
+		const xpath_parse_result& result() const;
+
+		// Safe bool conversion operator
+		operator unspecified_bool_type() const;
+
+		// Borland C++ workaround
+		bool operator!() const;
+	};
+
+	#ifndef PUGIXML_NO_EXCEPTIONS
+        #if defined(_MSC_VER)
+          // C4275 can be ignored in Visual C++ if you are deriving
+          // from a type in the Standard C++ Library
+          #pragma warning(push)
+          #pragma warning(disable: 4275)
+        #endif
+	// XPath exception class
+	class PUGIXML_CLASS xpath_exception: public std::exception
+	{
+	private:
+		xpath_parse_result _result;
+
+	public:
+		// Construct exception from parse result
+		explicit xpath_exception(const xpath_parse_result& result);
+
+		// Get error message
+		virtual const char* what() const throw() PUGIXML_OVERRIDE;
+
+		// Get parse result
+		const xpath_parse_result& result() const;
+	};
+        #if defined(_MSC_VER)
+          #pragma warning(pop)
+        #endif
+	#endif
+
+	// XPath node class (either xml_node or xml_attribute)
+	class PUGIXML_CLASS xpath_node
+	{
+	private:
+		xml_node _node;
+		xml_attribute _attribute;
+
+		typedef void (*unspecified_bool_type)(xpath_node***);
+
+	public:
+		// Default constructor; constructs empty XPath node
+		xpath_node();
+
+		// Construct XPath node from XML node/attribute
+		xpath_node(const xml_node& node);
+		xpath_node(const xml_attribute& attribute, const xml_node& parent);
+
+		// Get node/attribute, if any
+		xml_node node() const;
+		xml_attribute attribute() const;
+
+		// Get parent of contained node/attribute
+		xml_node parent() const;
+
+		// Safe bool conversion operator
+		operator unspecified_bool_type() const;
+
+		// Borland C++ workaround
+		bool operator!() const;
+
+		// Comparison operators
+		bool operator==(const xpath_node& n) const;
+		bool operator!=(const xpath_node& n) const;
+	};
+
+#ifdef __BORLANDC__
+	// Borland C++ workaround
+	bool PUGIXML_FUNCTION operator&&(const xpath_node& lhs, bool rhs);
+	bool PUGIXML_FUNCTION operator||(const xpath_node& lhs, bool rhs);
+#endif
+
+	// A fixed-size collection of XPath nodes
+	class PUGIXML_CLASS xpath_node_set
+	{
+	public:
+		// Collection type
+		enum type_t
+		{
+			type_unsorted,			// Not ordered
+			type_sorted,			// Sorted by document order (ascending)
+			type_sorted_reverse		// Sorted by document order (descending)
+		};
+
+		// Constant iterator type
+		typedef const xpath_node* const_iterator;
+
+		// We define non-constant iterator to be the same as constant iterator so that various generic algorithms (i.e. boost foreach) work
+		typedef const xpath_node* iterator;
+
+		// Default constructor. Constructs empty set.
+		xpath_node_set();
+
+		// Constructs a set from iterator range; data is not checked for duplicates and is not sorted according to provided type, so be careful
+		xpath_node_set(const_iterator begin, const_iterator end, type_t type = type_unsorted);
+
+		// Destructor
+		~xpath_node_set();
+
+		// Copy constructor/assignment operator
+		xpath_node_set(const xpath_node_set& ns);
+		xpath_node_set& operator=(const xpath_node_set& ns);
+
+	#ifdef PUGIXML_HAS_MOVE
+		// Move semantics support
+		xpath_node_set(xpath_node_set&& rhs) PUGIXML_NOEXCEPT;
+		xpath_node_set& operator=(xpath_node_set&& rhs) PUGIXML_NOEXCEPT;
+	#endif
+
+		// Get collection type
+		type_t type() const;
+
+		// Get collection size
+		size_t size() const;
+
+		// Indexing operator
+		const xpath_node& operator[](size_t index) const;
+
+		// Collection iterators
+		const_iterator begin() const;
+		const_iterator end() const;
+
+		// Sort the collection in ascending/descending order by document order
+		void sort(bool reverse = false);
+
+		// Get first node in the collection by document order
+		xpath_node first() const;
+
+		// Check if collection is empty
+		bool empty() const;
+
+	private:
+		type_t _type;
+
+		xpath_node _storage[1];
+
+		xpath_node* _begin;
+		xpath_node* _end;
+
+		void _assign(const_iterator begin, const_iterator end, type_t type);
+		void _move(xpath_node_set& rhs) PUGIXML_NOEXCEPT;
+	};
+#endif
+
+#ifndef PUGIXML_NO_STL
+	// Convert wide string to UTF8
+	std::basic_string<char, std::char_traits<char>, std::allocator<char> > PUGIXML_FUNCTION as_utf8(const wchar_t* str);
+	std::basic_string<char, std::char_traits<char>, std::allocator<char> > PUGIXML_FUNCTION as_utf8(const std::basic_string<wchar_t, std::char_traits<wchar_t>, std::allocator<wchar_t> >& str);
+
+	// Convert UTF8 to wide string
+	std::basic_string<wchar_t, std::char_traits<wchar_t>, std::allocator<wchar_t> > PUGIXML_FUNCTION as_wide(const char* str);
+	std::basic_string<wchar_t, std::char_traits<wchar_t>, std::allocator<wchar_t> > PUGIXML_FUNCTION as_wide(const std::basic_string<char, std::char_traits<char>, std::allocator<char> >& str);
+#endif
+
+	// Memory allocation function interface; returns pointer to allocated memory or NULL on failure
+	typedef void* (*allocation_function)(size_t size);
+
+	// Memory deallocation function interface
+	typedef void (*deallocation_function)(void* ptr);
+
+	// Override default memory management functions. All subsequent allocations/deallocations will be performed via supplied functions.
+	void PUGIXML_FUNCTION set_memory_management_functions(allocation_function allocate, deallocation_function deallocate);
+
+	// Get current memory management functions
+	allocation_function PUGIXML_FUNCTION get_memory_allocation_function();
+	deallocation_function PUGIXML_FUNCTION get_memory_deallocation_function();
+}
+
+#if !defined(PUGIXML_NO_STL) && (defined(_MSC_VER) || defined(__ICC))
+namespace std
+{
+	// Workarounds for (non-standard) iterator category detection for older versions (MSVC7/IC8 and earlier)
+	std::bidirectional_iterator_tag PUGIXML_FUNCTION _Iter_cat(const pugi::xml_node_iterator&);
+	std::bidirectional_iterator_tag PUGIXML_FUNCTION _Iter_cat(const pugi::xml_attribute_iterator&);
+	std::bidirectional_iterator_tag PUGIXML_FUNCTION _Iter_cat(const pugi::xml_named_node_iterator&);
+}
+#endif
+
+#if !defined(PUGIXML_NO_STL) && defined(__SUNPRO_CC)
+namespace std
+{
+	// Workarounds for (non-standard) iterator category detection
+	std::bidirectional_iterator_tag PUGIXML_FUNCTION __iterator_category(const pugi::xml_node_iterator&);
+	std::bidirectional_iterator_tag PUGIXML_FUNCTION __iterator_category(const pugi::xml_attribute_iterator&);
+	std::bidirectional_iterator_tag PUGIXML_FUNCTION __iterator_category(const pugi::xml_named_node_iterator&);
+}
+#endif
+
+#endif
+
+// Make sure implementation is included in header-only mode
+// Use macro expansion in #include to work around QMake (QTBUG-11923)
+#if defined(PUGIXML_HEADER_ONLY) && !defined(PUGIXML_SOURCE)
+#	define PUGIXML_SOURCE "pugixml.cpp"
+#	include PUGIXML_SOURCE
+#endif
+
+/**
+ * Copyright (c) 2006-2022 Arseny Kapoulkine
+ *
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */

src/map/lanelet2/lanelet2_core/lanelet2_core.pro

@@ -0,0 +1,48 @@
+#-------------------------------------------------
+#
+# Project created by QtCreator 2022-11-21T13:50:12
+#
+#-------------------------------------------------
+
+QT       -= core gui
+
+TARGET = lanelet2_core
+TEMPLATE = lib
+
+DEFINES += LANELET2_CORE_LIBRARY
+
+CONFIG += plugin
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which has been marked as deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+INCLUDEPATH += $$PWD/../include
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+    src/Attribute.cpp \
+    src/BasicRegulatoryElements.cpp \
+    src/Lanelet.cpp \
+    src/LaneletMap.cpp \
+    src/LaneletSequence.cpp \
+    src/LineStringGeometry.cpp \
+    src/PolygonTriangulationGeometry.cpp \
+    src/RegulatoryElement.cpp \
+    src/RegulatoryElementGeometry.cpp
+
+HEADERS +=
+
+unix {
+    target.path = /usr/lib
+    INSTALLS += target
+}
+
+unix:INCLUDEPATH += /usr/include/eigen3
+win32:INCLUDEPATH += D:\File\soft\eigen