LaneletMap.h

Go to the documentation of this file.

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

  using iterator =  //  NOLINT
      internal::TransformIterator<typename Map::iterator, PrimitiveT, internal::PairConverter<PrimitiveT>>;

  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;

  bool exists(Id id) const;

  ConstPrimitiveT get(Id id) const;

  PrimitiveT get(Id id);

  const_iterator find(Id id) const;

  std::vector<ConstPrimitiveT> findUsages(
      const traits::ConstPrimitiveType<traits::OwnedT<PrimitiveT>>& primitive) const;

  std::vector<PrimitiveT> findUsages(const traits::ConstPrimitiveType<traits::OwnedT<PrimitiveT>>& primitive);

  const_iterator begin() const;

  const_iterator end() const;

  iterator find(Id id);

  iterator begin();

  iterator end();

  bool empty() const { return elements_.empty(); }

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

  ConstPrimitiveVec search(const BoundingBox2d& area) const;
  PrimitiveVec search(const BoundingBox2d& area);

  OptConstPrimitiveT searchUntil(const BoundingBox2d& area, const ConstSearchFunction& func) const;
  OptPrimitiveT searchUntil(const BoundingBox2d& area, const SearchFunction& func);

  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); }

  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);
  }

  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_;  
  struct Tree;
  // NOLINTNEXTLINE
  std::unique_ptr<Tree> tree_;  
};

// 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

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

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

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;

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

  bool empty() const noexcept {
    return laneletLayer.empty() && areaLayer.empty() && regulatoryElementLayer.empty() && polygonLayer.empty() &&
           lineStringLayer.empty() && pointLayer.empty();
  }

  size_t size() const noexcept {
    return laneletLayer.size() + areaLayer.size() + regulatoryElementLayer.size() + polygonLayer.size() +
           lineStringLayer.size() + pointLayer.size();
  }

  LaneletLayer laneletLayer;                      
  AreaLayer areaLayer;                            
  RegulatoryElementLayer regulatoryElementLayer;  
  PolygonLayer polygonLayer;                      
  LineStringLayer lineStringLayer;                
  PointLayer pointLayer;                          
};

class LaneletMap : public LaneletMapLayers {
 public:
  using LaneletMapLayers::LaneletMapLayers;

  void add(Lanelet lanelet);

  void add(Area area);

  void add(const RegulatoryElementPtr& regElem);

  void add(Polygon3d polygon);

  void add(LineString3d lineString);

  void add(Point3d point);
};

class LaneletSubmap : public LaneletMapLayers {
 public:
  LaneletSubmap() = default;
  using LaneletMapLayers::LaneletMapLayers;

  explicit LaneletSubmap(LaneletMap&& rhs) noexcept : LaneletMapLayers{std::move(rhs)} {}
  LaneletSubmap& operator=(LaneletMap&& rhs) {
    static_cast<LaneletMapLayers&>(*this) = std::move(rhs);
    return *this;
  }

  void add(Lanelet lanelet);

  void add(Area area);

  void add(const RegulatoryElementPtr& regElem);

  void add(Polygon3d polygon);

  void add(LineString3d lineString);

  void add(Point3d point);

  LaneletMapConstPtr laneletMap() const;
  LaneletMapUPtr laneletMap();

  void trackParameters(const RegulatoryElement& regelem);

 private:
  std::vector<boost::variant<ConstLanelet, ConstArea>> regelemObjects_;
};

namespace utils {
template <typename PrimitiveT>
using ConstLayerPrimitive = typename PrimitiveLayer<PrimitiveT>::ConstPrimitiveT;

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

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

Id getId();

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 {
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