Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2018-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: MIT
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : /**
       9                 :            :  * @file
      10                 :            :  */
      11                 :            : 
      12                 :            : #pragma once
      13                 :            : 
      14                 :            : #include <list>
      15                 :            : 
      16                 :            : #include "ad/map/match/Types.hpp"
      17                 :            : #include "ad/map/point/Operation.hpp"
      18                 :            : #include "ad/map/route/Types.hpp"
      19                 :            : 
      20                 :            : /** @brief namespace admap */
      21                 :            : namespace ad {
      22                 :            : /** @brief namespace map */
      23                 :            : namespace map {
      24                 :            : /** @brief namespace match */
      25                 :            : namespace match {
      26                 :            : 
      27                 :            : /**
      28                 :            :  * @class AdMapMatching
      29                 :            :  * @brief performs map matching of a given point to the map
      30                 :            :  *
      31                 :            :  * In general, there are multiple possible matches for a given point available,
      32                 :            :  * especially if the point is e.g. located within an intersection area.
      33                 :            :  * Therefore, map matching always returns a std::multimap of map matched positions ordered by their probabilities.
      34                 :            :  *
      35                 :            :  * Besides the actual position, there are additional input parameters influencing the probabilistic calculation.
      36                 :            :  * Observed heading or possible routes of a vehicle/object can be provided to influence the map matching result.
      37                 :            :  */
      38                 :            : class AdMapMatching
      39                 :            : {
      40                 :            : public:
      41                 :            :   /**
      42                 :            :    * @brief default constructor
      43                 :            :    */
      44                 :            :   AdMapMatching();
      45                 :            : 
      46                 :            :   /**
      47                 :            :    * @brief destructor
      48                 :            :    */
      49                 :       1990 :   ~AdMapMatching() = default;
      50                 :            : 
      51                 :            :   /**
      52                 :            :    * @brief set the maximum probability multiplier for heading hints
      53                 :            :    */
      54                 :            :   void setMaxHeadingHintFactor(double newHeadingHintFactor)
      55                 :            :   {
      56                 :            :     mHeadingHintFactor = std::max(1., newHeadingHintFactor);
      57                 :            :   }
      58                 :            : 
      59                 :            :   /**
      60                 :            :    * @brief get the maximum probability multiplier for heading hints
      61                 :            :    */
      62                 :            :   double getMaxHeadingHintFactor() const
      63                 :            :   {
      64                 :            :     return mHeadingHintFactor;
      65                 :            :   }
      66                 :            : 
      67                 :            :   /**
      68                 :            :    * @brief set the probability multiplier for valid route hints
      69                 :            :    */
      70                 :            :   void setRouteHintFactor(double newRouteHintFactor)
      71                 :            :   {
      72                 :            :     mRouteHintFactor = std::max(1., newRouteHintFactor);
      73                 :            :   }
      74                 :            : 
      75                 :            :   /**
      76                 :            :    * @brief get the probability multiplier for valid route hints
      77                 :            :    */
      78                 :          5 :   double getRouteHintFactor() const
      79                 :            :   {
      80                 :          5 :     return mRouteHintFactor;
      81                 :            :   }
      82                 :            : 
      83                 :            :   /**
      84                 :            :    * @brief add a hint for the heading of the vehicle/object
      85                 :            :    *
      86                 :            :    * It is possible to provide hints on the heading of the vehicle/object to be considered on map matching.
      87                 :            :    * This increases the map matching probabilities for lanes with respective direction.
      88                 :            :    * The matches are multiplied with a headingFactor:
      89                 :            :    * 1. <= headingFactor <= getMaxHeadingHintFactor()
      90                 :            :    *
      91                 :            :    * @param[in] headingHint the heading hint of the object/vehicle to consider
      92                 :            :    */
      93                 :            :   void addHeadingHint(point::ECEFHeading const &headingHint)
      94                 :            :   {
      95                 :            :     mHeadingHints.push_back(headingHint);
      96                 :            :   }
      97                 :            : 
      98                 :            :   /**
      99                 :            :    * @brief add a hint for the heading of the vehicle/object
     100                 :            :    *
     101                 :            :    * It is possible to provide hints on the heading of the vehicle/object to be considered on map matching.
     102                 :            :    * This increases the map matching probabilities for lanes with respective direction.
     103                 :            :    *
     104                 :            :    * @param[in] yaw the yaw of the object/vehicle to consider
     105                 :            :    * @param[in] enuReferencePoint the reference point of the corresponding ENUCoordinate system
     106                 :            :    */
     107                 :        314 :   void addHeadingHint(point::ENUHeading const &yaw, point::GeoPoint const &enuReferencePoint)
     108                 :            :   {
     109         [ +  - ]:        314 :     mHeadingHints.push_back(point::createECEFHeading(yaw, enuReferencePoint));
     110                 :        314 :   }
     111                 :            : 
     112                 :            :   /**
     113                 :            :    * @brief clears the list of heading hints
     114                 :            :    */
     115                 :          2 :   void clearHeadingHints()
     116                 :            :   {
     117                 :          2 :     mHeadingHints.clear();
     118                 :          2 :   }
     119                 :            : 
     120                 :            :   /**
     121                 :            :    * @brief add a hint for the route of the vehicle/object
     122                 :            :    *
     123                 :            :    * This function allows to provide a hint for the current route of the object.
     124                 :            :    * This increases the map matching probabilities for lanes along the route.
     125                 :            :    *
     126                 :            :    * @param[in] routeHint the route hint to consider
     127                 :            :    */
     128                 :         26 :   void addRouteHint(route::FullRoute const &routeHint)
     129                 :            :   {
     130                 :         26 :     mRouteHints.push_back(routeHint);
     131                 :         26 :   }
     132                 :            : 
     133                 :            :   /**
     134                 :            :    * @brief clears the list of route hints
     135                 :            :    */
     136                 :            :   void clearRouteHints()
     137                 :            :   {
     138                 :            :     mRouteHints.clear();
     139                 :            :   }
     140                 :            : 
     141                 :            :   /**
     142                 :            :    * @brief clears route and heading hints at once
     143                 :            :    */
     144                 :            :   void clearHints()
     145                 :            :   {
     146                 :            :     clearRouteHints();
     147                 :            :     clearHeadingHints();
     148                 :            :   }
     149                 :            : 
     150                 :            :   /**
     151                 :            :    * @brief set the lanes that are relevant for map matching
     152                 :            :    * All the rest of the lanes in the map are ignored.
     153                 :            :    */
     154                 :       1156 :   void setRelevantLanes(::ad::map::lane::LaneIdSet const &relevantLanes)
     155                 :            :   {
     156                 :       1156 :     mRelevantLanes = relevantLanes;
     157                 :       1156 :   }
     158                 :            : 
     159                 :            :   /**
     160                 :            :    * @brief clear the list of relevant lanes.
     161                 :            :    */
     162                 :            :   void clearRelevantLanes()
     163                 :            :   {
     164                 :            :     mRelevantLanes.clear();
     165                 :            :   }
     166                 :            : 
     167                 :            :   /**
     168                 :            :    * @brief get the map matched positions
     169                 :            :    *
     170                 :            :    * Calculate the map matched positions and return these.
     171                 :            :    *
     172                 :            :    * @param[in] geoPoint position to match against the map
     173                 :            :    * @param[in] distance search radius around geoPoint to select a lane as a match
     174                 :            :    * @param[in] minProbabilty A probability threshold to be considered for the results.
     175                 :            :    */
     176                 :            :   MapMatchedPositionConfidenceList getMapMatchedPositions(point::GeoPoint const &geoPoint,
     177                 :            :                                                           physics::Distance const &distance,
     178                 :            :                                                           physics::Probability const &minProbability) const;
     179                 :            : 
     180                 :            :   /**
     181                 :            :    * @brief get the map matched positions
     182                 :            :    *
     183                 :            :    * Calculate the map matched positions and return these.
     184                 :            :    *
     185                 :            :    * @param[in] enuPoint position to match against the map in ENU coordinate frame
     186                 :            :    * @param[in] enuReferencePoint the enu reference point
     187                 :            :    * @param[in] distance search radius around geoPoint to select a lane as a match
     188                 :            :    * @param[in] minProbabilty A probability threshold to be considered for the results.
     189                 :            :    */
     190                 :            :   MapMatchedPositionConfidenceList getMapMatchedPositions(point::ENUPoint const &enuPoint,
     191                 :            :                                                           point::GeoPoint const &enuReferencePoint,
     192                 :            :                                                           physics::Distance const &distance,
     193                 :            :                                                           physics::Probability const &minProbability) const;
     194                 :            : 
     195                 :            :   /**
     196                 :            :    * @brief get the map matched positions
     197                 :            :    *
     198                 :            :    * Calculate the map matched positions and return these.
     199                 :            :    *
     200                 :            :    * @param[in] enuPoint position to match against the map in ENU coordinate frame
     201                 :            :    * @param[in] distance search radius around geoPoint to select a lane as a match
     202                 :            :    * @param[in] minProbabilty A probability threshold to be considered for the results.
     203                 :            :    *
     204                 :            :    * This function makes use of the globally set ENUReferencePoint
     205                 :            :    * (see AdMapAccess::setENUReferencePoint())
     206                 :            :    */
     207                 :            :   MapMatchedPositionConfidenceList getMapMatchedPositions(point::ENUPoint const &enuPoint,
     208                 :            :                                                           physics::Distance const &distance,
     209                 :            :                                                           physics::Probability const &minProbability) const;
     210                 :            : 
     211                 :            :   /**
     212                 :            :    * @brief get the map matched positions
     213                 :            :    *
     214                 :            :    * Calculate the map matched positions and return these.
     215                 :            :    *
     216                 :            :    * @param[in] enuObjectPosition object position, orientation, dimensions and ENRReferencePoint
     217                 :            :    *  to match against the map in ENU coordinate frame
     218                 :            :    * @param[in] distance search radius around geoPoint to select a lane as a match
     219                 :            :    * @param[in] minProbabilty A probability threshold to be considered for the results.
     220                 :            :    *
     221                 :            :    * The orientation of the ENUObjectPosition is set as heading hint before matching and cleared afterwards
     222                 :            :    * This function makes use of the ENUReferencePoint of the provided ENUObjectPosition.
     223                 :            :    * The dimensions of the ENUObjectPosition is not required.
     224                 :            :    */
     225                 :            :   MapMatchedPositionConfidenceList getMapMatchedPositions(ENUObjectPosition const &enuObjectPosition,
     226                 :            :                                                           physics::Distance const &distance,
     227                 :            :                                                           physics::Probability const &minProbability);
     228                 :            : 
     229                 :            :   /**
     230                 :            :    * @brief get the map matched bounding box
     231                 :            :    *
     232                 :            :    * Calculate the map matched bounding box.
     233                 :            :    * This will calculate the map matched positions of all the corner points and the center point
     234                 :            :    * In addition it will calculate all lane regions that are covered by the bounding box by sampling
     235                 :            :    * the objects geometry in between with the provided \c samplingDistance
     236                 :            :    *
     237                 :            :    * @param[in] enuObjectPosition object position, orientation, dimensions and ENRReferencePoint
     238                 :            :    *  to match against the map in ENU coordinate frame
     239                 :            :    * @param[in] samplingDistance The step size to be used to perform map matching in between the vehicle boundaries
     240                 :            :    *   This parameter is heavily influencing the performance of this function:
     241                 :            :    *   A samplingDistance of 0.1 at a car (3x5m) means 1500x map matching. With a distance of 1.0 we get only 15x map
     242                 :            :    * matching.
     243                 :            :    *
     244                 :            :    * @returns the map matched bounding box of the object
     245                 :            :    */
     246                 :            :   MapMatchedObjectBoundingBox getMapMatchedBoundingBox(ENUObjectPosition const &enuObjectPosition,
     247                 :            :                                                        physics::Distance const &samplingDistance
     248                 :            :                                                        = physics::Distance(1.)) const;
     249                 :            : 
     250                 :            :   /**
     251                 :            :    * @brief get the lane occupied regions from a list of ENUObjectPositionList
     252                 :            :    *
     253                 :            :    * Merge the lane occupied regions of the getMapMatchedBoundingBox() results of all
     254                 :            :    * position entries. See getMapMatchedBoundingBox() for a detailed description.
     255                 :            :    * For a correct handling of the inner borders crossed on matching a bigger object, this function
     256                 :            :    * only works as expected if the the provided enuObjectPositionList covers the whole object.
     257                 :            :    *
     258                 :            :    * @param[in] enuObjectPositionList list of ENUObjectPosition entries
     259                 :            :    * @param[in] samplingDistance The step size to be used to perform map matching in between the vehicle boundaries
     260                 :            :    *   A samplingDistance of 0.1 at a car (3x5m) means 1500x map matching. With a distance of 1.0 we get only 15x map
     261                 :            :    * matching.
     262                 :            :    *
     263                 :            :    * @returns the map matched bounding box of the object
     264                 :            :    */
     265                 :            :   LaneOccupiedRegionList getLaneOccupiedRegions(ENUObjectPositionList enuObjectPositionList,
     266                 :            :                                                 physics::Distance const &samplingDistance
     267                 :            :                                                 = physics::Distance(1.)) const;
     268                 :            : 
     269                 :            :   /**
     270                 :            :    * @brief Method to be called to retrieve the lane heading at a mapMatchedPosition
     271                 :            :    */
     272                 :            :   point::ENUHeading getLaneENUHeading(MapMatchedPosition const &mapMatchedPosition) const;
     273                 :            : 
     274                 :            :   /**
     275                 :            :    * @brief Spatial Lane Search.
     276                 :            :    *        Returns all Lanes where any part of surface is less than specified physics::Distance
     277                 :            :    *        from given point.
     278                 :            :    * @param[in] ecefPoint Point that is used as base for the search.
     279                 :            :    * @param[in] distance Search radius.
     280                 :            :    * @param[in] relevantLanes if not empty, the function restricts the search to the given set of lanes
     281                 :            :    *
     282                 :            :    * This static function doesn't make use of any matching hints.
     283                 :            :    *
     284                 :            :    * @returns Map matching results that satisfy search criteria.
     285                 :            :    */
     286                 :            :   static MapMatchedPositionConfidenceList findLanes(point::ECEFPoint const &ecefPoint,
     287                 :            :                                                     physics::Distance const &distance,
     288                 :            :                                                     ::ad::map::lane::LaneIdSet const &relevantLanes
     289                 :            :                                                     = ::ad::map::lane::LaneIdSet());
     290                 :            : 
     291                 :            :   /**
     292                 :            :    * @brief Spatial Lane Search.
     293                 :            :    *        Returns all Lanes where any part of surface is less than specified physics::Distance
     294                 :            :    *        from given point.
     295                 :            :    * @param[in] geoPoint Point that is used as base for the search.
     296                 :            :    * @param[in] distance Search radius.
     297                 :            :    * @param[in] relevantLanes if not empty, the function restricts the search to the given set of lanes
     298                 :            :    *
     299                 :            :    * This static function doesn't make use of any matching hints.
     300                 :            :    *
     301                 :            :    * @returns Map matching results that satisfy search criteria. The individual matching result probabilities are equal.
     302                 :            :    */
     303                 :            :   static MapMatchedPositionConfidenceList findLanes(point::GeoPoint const &geoPoint,
     304                 :            :                                                     physics::Distance const &distance,
     305                 :            :                                                     ::ad::map::lane::LaneIdSet const &relevantLanes
     306                 :            :                                                     = ::ad::map::lane::LaneIdSet());
     307                 :            : 
     308                 :            :   /**
     309                 :            :    * @brief Spatial Lane Search.
     310                 :            :    *        Returns the map matched position in respect to all Lanes of the given route.
     311                 :            :    * @param[in] ecefPoint Point that is used as base for the search.
     312                 :            :    * @param[in] route The route providing the lane subset to be searched.
     313                 :            :    *
     314                 :            :    * This static function doesn't make use of any matching hints.
     315                 :            :    *
     316                 :            :    * @returns The individual matching result probabilities are relative to the actual distance of the matchedPoint to
     317                 :            :    * the queryPoint.
     318                 :            :    */
     319                 :            :   static MapMatchedPositionConfidenceList findRouteLanes(point::ECEFPoint const &ecefPoint,
     320                 :            :                                                          route::FullRoute const &route);
     321                 :            : 
     322                 :            : private:
     323                 :            :   // Copy operators and constructors are deleted to avoid accidental copies
     324                 :            :   AdMapMatching(AdMapMatching const &) = delete;
     325                 :            :   AdMapMatching(AdMapMatching &&) = delete;
     326                 :            :   AdMapMatching &operator=(AdMapMatching &&) = delete;
     327                 :            :   AdMapMatching &operator=(AdMapMatching const &) = delete;
     328                 :            : 
     329                 :            :   static match::MapMatchedPositionConfidenceList findLanesInputChecked(point::ECEFPoint const &ecefPoint,
     330                 :            :                                                                        physics::Distance const &distance,
     331                 :            :                                                                        ::ad::map::lane::LaneIdSet const &relevantLanes);
     332                 :            : 
     333                 :            :   static match::MapMatchedPositionConfidenceList
     334                 :            :   findLanesInputChecked(std::vector<lane::Lane::ConstPtr> const &relevantLanes,
     335                 :            :                         point::ECEFPoint const &ecefPoint,
     336                 :            :                         physics::Distance const &distance);
     337                 :            : 
     338                 :            :   static std::vector<lane::Lane::ConstPtr> getRelevantLanesInputChecked(point::ECEFPoint const &ecefPoint,
     339                 :            :                                                                         physics::Distance const &distance);
     340                 :            :   static void normalizeResults(match::MapMatchedPositionConfidenceList &mapMatchingResults,
     341                 :            :                                physics::Probability const &probabilitySum);
     342                 :            : 
     343                 :            :   static match::MapMatchedPositionConfidenceList
     344                 :            :   findLanesInputCheckedAltitudeUnknown(point::GeoPoint const &geoPoint,
     345                 :            :                                        physics::Distance const &distance,
     346                 :            :                                        ::ad::map::lane::LaneIdSet const &relevantLanes);
     347                 :            : 
     348                 :            :   static std::vector<lane::Lane::ConstPtr>
     349                 :            :   getRelevantLanesInputChecked(point::ECEFPoint const &ecefPoint,
     350                 :            :                                physics::Distance const &distance,
     351                 :            :                                ::ad::map::lane::LaneIdSet const &relevantLanes);
     352                 :            : 
     353                 :            :   /**
     354                 :            :    * @brief extract the mapMatchedPositions and write them into a map of occuppied regions
     355                 :            :    *
     356                 :            :    * @param[in,out] laneOccupiedRegions vector containing the occupied regions
     357                 :            :    * @param[in] mapMatchedPositions
     358                 :            :    */
     359                 :            : 
     360                 :            :   void addLaneRegions(LaneOccupiedRegionList &laneOccupiedRegions,
     361                 :            :                       MapMatchedPositionConfidenceList const &mapMatchedPositions) const;
     362                 :            :   void addLaneRegions(LaneOccupiedRegionList &laneOccupiedRegions,
     363                 :            :                       LaneOccupiedRegionList const &otherLaneOccupiedRegions) const;
     364                 :            : 
     365                 :            :   MapMatchedPositionConfidenceList considerMapMatchingHints(MapMatchedPositionConfidenceList const &mapMatchedPositions,
     366                 :            :                                                             physics::Probability const &minProbability) const;
     367                 :            :   bool isLanePartOfRouteHints(lane::LaneId const &laneId) const;
     368                 :            :   double getHeadingFactor(MapMatchedPosition const &matchedPosition) const;
     369                 :            : 
     370                 :            :   std::list<point::ECEFHeading> mHeadingHints;
     371                 :            :   double mHeadingHintFactor{2.};
     372                 :            :   std::list<route::FullRoute> mRouteHints;
     373                 :            :   double mRouteHintFactor{10.};
     374                 :            : 
     375                 :            :   ::ad::map::lane::LaneIdSet mRelevantLanes;
     376                 :            : };
     377                 :            : 
     378                 :            : } // namespace match
     379                 :            : } // namespace map
     380                 :            : } // namespace ad