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                 :            : #include "ad/map/intersection/CoreIntersection.hpp"
      10                 :            : 
      11                 :            : #include "ad/map/access/Logging.hpp"
      12                 :            : #include "ad/map/lane/LaneOperation.hpp"
      13                 :            : #include "ad/map/match/AdMapMatching.hpp"
      14                 :            : #include "ad/map/route/RouteOperation.hpp"
      15                 :            : 
      16                 :            : namespace ad {
      17                 :            : namespace map {
      18                 :            : namespace intersection {
      19                 :            : 
      20                 :       1529 : point::ParaPoint CoreIntersection::getEntryParaPointOfExternalLane(lane::LaneId const &laneId) const
      21                 :            : {
      22         [ +  - ]:       1529 :   point::ParaPoint p;
      23                 :       1529 :   p.laneId = laneId;
      24                 :            :   p.parametricOffset
      25   [ +  -  +  + ]:       1529 :     = (lane::isLaneDirectionNegative(laneId) ? physics::ParametricValue(0.) : physics::ParametricValue(1.));
      26                 :       1529 :   return p;
      27                 :            : }
      28                 :            : 
      29                 :        611 : point::ParaPoint CoreIntersection::getExitParaPointOfExternalLane(lane::LaneId const &laneId) const
      30                 :            : {
      31         [ +  - ]:        611 :   point::ParaPoint p;
      32                 :        611 :   p.laneId = laneId;
      33                 :            :   p.parametricOffset
      34   [ +  -  +  + ]:        611 :     = (lane::isLaneDirectionNegative(laneId) ? physics::ParametricValue(1.) : physics::ParametricValue(0.));
      35                 :        611 :   return p;
      36                 :            : }
      37                 :            : 
      38                 :          0 : point::ParaPoint CoreIntersection::getEntryParaPointOfInternalLane(lane::LaneId const &laneId) const
      39                 :            : {
      40                 :            :   // for internal lanes the para point logic is vice-versa to external lanes
      41                 :          0 :   return getExitParaPointOfExternalLane(laneId);
      42                 :            : }
      43                 :            : 
      44                 :          1 : point::ParaPoint CoreIntersection::getExitParaPointOfInternalLane(lane::LaneId const &laneId) const
      45                 :            : {
      46                 :            :   // for internal lanes the para point logic is vice-versa to external lanes
      47                 :          1 :   return getEntryParaPointOfExternalLane(laneId);
      48                 :            : }
      49                 :            : 
      50                 :       7143 : bool CoreIntersection::isLanePartOfIntersection(lane::LaneId const laneId, SuccessorMode const successorMode) const
      51                 :            : {
      52         [ +  + ]:       7143 :   if (successorMode == SuccessorMode::OwnIntersection)
      53                 :            :   {
      54                 :       7142 :     return isLanePartOfCoreIntersection(laneId);
      55                 :            :   }
      56                 :            :   else
      57                 :            :   {
      58                 :          1 :     return isLanePartOfAnIntersection(laneId);
      59                 :            :   }
      60                 :            : }
      61                 :            : 
      62                 :        572 : lane::LaneIdSet const &CoreIntersection::internalLanes() const
      63                 :            : {
      64                 :        572 :   return mInternalLanes;
      65                 :            : }
      66                 :            : 
      67                 :        116 : lane::LaneIdSet const &CoreIntersection::entryLanes() const
      68                 :            : {
      69                 :        116 :   return mEntryLanes;
      70                 :            : }
      71                 :            : 
      72                 :          3 : point::ParaPointList const &CoreIntersection::entryParaPoints() const
      73                 :            : {
      74                 :          3 :   return mEntryParaPoints;
      75                 :            : }
      76                 :            : 
      77                 :          5 : lane::LaneIdSet const &CoreIntersection::exitLanes() const
      78                 :            : {
      79                 :          5 :   return mExitLanes;
      80                 :            : }
      81                 :            : 
      82                 :          3 : point::ParaPointList const &CoreIntersection::exitParaPoints() const
      83                 :            : {
      84                 :          3 :   return mExitParaPoints;
      85                 :            : }
      86                 :            : 
      87                 :      22034 : bool CoreIntersection::isLanePartOfCoreIntersection(lane::LaneId const laneId) const
      88                 :            : {
      89         [ +  - ]:      22034 :   return (mInternalLanes.find(laneId) != mInternalLanes.end());
      90                 :            : }
      91                 :            : 
      92                 :      14781 : void CoreIntersection::extractLanesOfCoreIntersection(lane::LaneId const laneId)
      93                 :            : {
      94   [ +  -  +  + ]:      14781 :   if (isLanePartOfCoreIntersection(laneId))
      95                 :            :   {
      96                 :      13297 :     return;
      97                 :            :   }
      98   [ +  -  +  - ]:       4530 :   auto lane = lane::getLane(laneId);
      99         [ +  + ]:       4530 :   if (!lane::isLanePartOfAnIntersection(lane))
     100                 :            :   {
     101                 :            :     // restrict ourselves to lanes inside the intersection
     102                 :       3046 :     return;
     103                 :            :   }
     104         [ +  - ]:       1484 :   mInternalLanes.insert(laneId);
     105         [ +  + ]:       1484 :   if (mInternalLanes.size() == 1u)
     106                 :            :   {
     107                 :        126 :     mInternalLanesBoundingSphere = lane.boundingSphere;
     108                 :            :   }
     109                 :            :   else
     110                 :            :   {
     111         [ +  - ]:       1358 :     mInternalLanesBoundingSphere = mInternalLanesBoundingSphere + lane.boundingSphere;
     112                 :            :   }
     113         [ +  + ]:      16139 :   for (auto const &contact : lane.contactLanes)
     114                 :            :   {
     115         [ +  - ]:      14655 :     processContactsForLane(lane, contact);
     116         [ +  - ]:      14655 :     extractLanesOfCoreIntersection(contact.toLane);
     117                 :            :   }
     118                 :            : }
     119                 :            : 
     120                 :      14655 : void CoreIntersection::processContactsForLane(lane::Lane const &lane, lane::ContactLane const &contact)
     121                 :            : {
     122                 :      14655 :   auto const &toId = contact.toLane;
     123   [ +  +  +  + ]:      14655 :   switch (contact.location)
     124                 :            :   {
     125                 :       9244 :     case lane::ContactLocation::OVERLAP:
     126                 :       9244 :       mOverlapping[lane.id].insert(toId);
     127                 :       9244 :       break;
     128                 :       1874 :     case lane::ContactLocation::SUCCESSOR:
     129         [ +  + ]:       1874 :       if (isLaneDirectionPositive(lane))
     130                 :            :       {
     131                 :        990 :         mSuccessor[lane.id].insert(toId);
     132                 :        990 :         checkAndInsertExitLane(toId);
     133                 :            :       }
     134                 :            :       else
     135                 :            :       {
     136                 :        884 :         mPredecessor[lane.id].insert(toId);
     137                 :        884 :         checkAndInsertEntryLane(toId);
     138                 :            :       }
     139                 :       1874 :       break;
     140                 :       2050 :     case lane::ContactLocation::PREDECESSOR:
     141         [ +  + ]:       2050 :       if (isLaneDirectionNegative(lane))
     142                 :            :       {
     143                 :       1015 :         mSuccessor[lane.id].insert(toId);
     144                 :       1015 :         checkAndInsertExitLane(toId);
     145                 :            :       }
     146                 :            :       else
     147                 :            :       {
     148                 :       1035 :         mPredecessor[lane.id].insert(toId);
     149                 :       1035 :         checkAndInsertEntryLane(toId);
     150                 :            :       }
     151                 :       2050 :       break;
     152                 :       1487 :     default:
     153                 :       1487 :       break; // ignored for now
     154                 :            :   }
     155                 :      14655 : }
     156                 :            : 
     157                 :       1919 : void CoreIntersection::checkAndInsertEntryLane(lane::LaneId const laneId)
     158                 :            : {
     159         [ +  + ]:       1919 :   if (!isLanePartOfAnIntersection(laneId))
     160                 :            :   {
     161         [ +  + ]:       1574 :     if (mEntryLanes.insert(laneId).second)
     162                 :            :     {
     163         [ +  - ]:        529 :       mEntryParaPoints.push_back(getEntryParaPointOfExternalLane(laneId));
     164                 :            :     }
     165                 :            :   }
     166                 :       1919 : }
     167                 :            : 
     168                 :       2005 : void CoreIntersection::checkAndInsertExitLane(lane::LaneId const laneId)
     169                 :            : {
     170         [ +  + ]:       2005 :   if (!isLanePartOfAnIntersection(laneId))
     171                 :            :   {
     172         [ +  + ]:       1315 :     if (mExitLanes.insert(laneId).second)
     173                 :            :     {
     174         [ +  - ]:        486 :       mExitParaPoints.push_back(getExitParaPointOfExternalLane(laneId));
     175                 :            :     }
     176                 :            :   }
     177                 :       2005 : }
     178                 :            : 
     179                 :            : std::pair<lane::LaneIdSet, lane::LaneIdSet>
     180                 :       3181 : CoreIntersection::getDirectSuccessorsInLaneDirection(lane::LaneId const laneId, SuccessorMode const successorMode) const
     181                 :            : {
     182   [ +  -  +  - ]:       6362 :   auto lane = lane::getLane(laneId);
     183                 :       3181 :   auto location = lane::ContactLocation::SUCCESSOR;
     184         [ +  + ]:       3181 :   if (lane.direction == lane::LaneDirection::NEGATIVE)
     185                 :            :   {
     186                 :       1398 :     location = lane::ContactLocation::PREDECESSOR;
     187                 :            :   }
     188                 :       3181 :   std::pair<lane::LaneIdSet, lane::LaneIdSet> result;
     189   [ +  -  +  + ]:      10048 :   for (auto const &contact : lane::getContactLanes(lane, location))
     190                 :            :   {
     191                 :       6867 :     lane::LaneId const successorLane = contact.toLane;
     192   [ +  -  +  + ]:       6867 :     if (isLanePartOfIntersection(successorLane, successorMode))
     193                 :            :     {
     194                 :            :       // restrict first to within intersection
     195         [ +  - ]:       4845 :       result.first.insert(successorLane);
     196                 :            :     }
     197                 :            :     else
     198                 :            :     {
     199                 :            :       // restrict second to outside intersection
     200         [ +  - ]:       2022 :       result.second.insert(successorLane);
     201                 :            :     }
     202                 :            :   }
     203                 :       6362 :   return result;
     204                 :            : }
     205                 :            : 
     206                 :            : lane::LaneIdSet
     207                 :       3136 : CoreIntersection::getDirectSuccessorsInLaneDirectionWithinIntersection(lane::LaneId const laneId,
     208                 :            :                                                                        SuccessorMode const successorMode) const
     209                 :            : {
     210                 :       3136 :   return getDirectSuccessorsInLaneDirection(laneId, successorMode).first;
     211                 :            : }
     212                 :            : 
     213                 :            : lane::LaneIdSet
     214                 :       2875 : CoreIntersection::getAllSuccessorsInLaneDirectionWithinIntersection(lane::LaneId const laneId,
     215                 :            :                                                                     SuccessorMode const successorMode) const
     216                 :            : {
     217                 :       2875 :   lane::LaneIdSet result;
     218         [ +  - ]:       5750 :   auto directSuccessors = getDirectSuccessorsInLaneDirectionWithinIntersection(laneId, successorMode);
     219         [ +  + ]:       4848 :   for (auto directSuccessorId : directSuccessors)
     220                 :            :   {
     221                 :            :     auto directSuccessorRecursiveResult
     222         [ +  - ]:       3946 :       = getAllSuccessorsInLaneDirectionWithinIntersection(directSuccessorId, successorMode);
     223         [ +  - ]:       1973 :     result.insert(directSuccessorRecursiveResult.begin(), directSuccessorRecursiveResult.end());
     224                 :            :   }
     225         [ +  - ]:       2875 :   result.insert(directSuccessors.begin(), directSuccessors.end());
     226                 :       5750 :   return result;
     227                 :            : }
     228                 :            : 
     229                 :            : lane::LaneIdSet
     230                 :        276 : CoreIntersection::getLaneAndAllSuccessorsInLaneDirectionWithinIntersection(lane::LaneId const laneId,
     231                 :            :                                                                            SuccessorMode const successorMode) const
     232                 :            : {
     233                 :        276 :   auto result = getAllSuccessorsInLaneDirectionWithinIntersection(laneId, successorMode);
     234   [ +  -  +  + ]:        276 :   if (isLanePartOfIntersection(laneId, successorMode))
     235                 :            :   {
     236         [ +  - ]:        137 :     result.insert(laneId);
     237                 :            :   }
     238                 :        276 :   return result;
     239                 :            : }
     240                 :            : 
     241                 :         44 : lane::LaneIdSet CoreIntersection::getAllReachableOutgoingLanes(lane::LaneId const laneId,
     242                 :            :                                                                SuccessorMode const successorMode) const
     243                 :            : {
     244                 :         44 :   lane::LaneIdSet result;
     245         [ +  - ]:         88 :   auto directSuccessors = getDirectSuccessorsInLaneDirection(laneId, successorMode);
     246                 :            :   // recursive until end of intersection
     247         [ -  + ]:         44 :   for (auto directSuccessorId : directSuccessors.first)
     248                 :            :   {
     249         [ #  # ]:          0 :     auto directSuccessorRecursiveResult = getAllReachableOutgoingLanes(directSuccessorId, successorMode);
     250         [ #  # ]:          0 :     result.insert(directSuccessorRecursiveResult.begin(), directSuccessorRecursiveResult.end());
     251                 :            :   }
     252         [ +  - ]:         44 :   result.insert(directSuccessors.second.begin(), directSuccessors.second.end());
     253                 :         88 :   return result;
     254                 :            : }
     255                 :            : 
     256                 :            : std::pair<lane::LaneIdSet, lane::LaneIdSet>
     257                 :          1 : CoreIntersection::getAllReachableInternalAndOutgoingLanes(lane::LaneId const laneId,
     258                 :            :                                                           SuccessorMode const successorMode) const
     259                 :            : {
     260                 :          1 :   std::pair<lane::LaneIdSet, lane::LaneIdSet> result;
     261         [ +  - ]:          2 :   auto directSuccessors = getDirectSuccessorsInLaneDirection(laneId, successorMode);
     262                 :            :   // recursive until end of intersection
     263         [ -  + ]:          1 :   for (auto directSuccessorId : directSuccessors.first)
     264                 :            :   {
     265         [ #  # ]:          0 :     auto directSuccessorRecursiveResult = getAllReachableInternalAndOutgoingLanes(directSuccessorId, successorMode);
     266         [ #  # ]:          0 :     result.first.insert(directSuccessorRecursiveResult.first.begin(), directSuccessorRecursiveResult.first.end());
     267         [ #  # ]:          0 :     result.second.insert(directSuccessorRecursiveResult.second.begin(), directSuccessorRecursiveResult.second.end());
     268                 :            :   }
     269         [ +  - ]:          1 :   result.first.insert(directSuccessors.first.begin(), directSuccessors.first.end());
     270         [ +  - ]:          1 :   result.second.insert(directSuccessors.second.begin(), directSuccessors.second.end());
     271                 :          2 :   return result;
     272                 :            : }
     273                 :            : 
     274                 :       4951 : bool CoreIntersection::isLanePartOfAnIntersection(lane::LaneId const laneId)
     275                 :            : {
     276   [ +  -  +  - ]:       9902 :   auto lane = lane::getLane(laneId);
     277                 :       9902 :   return lane::isLanePartOfAnIntersection(lane);
     278                 :            : }
     279                 :            : 
     280                 :          0 : bool CoreIntersection::isRoutePartOfAnIntersection(route::FullRoute const &route)
     281                 :            : {
     282         [ #  # ]:          0 :   for (auto const &roadSegment : route.roadSegments)
     283                 :            :   {
     284         [ #  # ]:          0 :     for (auto const &laneSegment : roadSegment.drivableLaneSegments)
     285                 :            :     {
     286   [ #  #  #  # ]:          0 :       if (isLanePartOfAnIntersection(laneSegment.laneInterval.laneId))
     287                 :            :       {
     288                 :          0 :         return true;
     289                 :            :       }
     290                 :            :     }
     291                 :            :   }
     292                 :          0 :   return false;
     293                 :            : }
     294                 :            : 
     295                 :        265 : bool laneEntersIntersection(lane::LaneId const &from, lane::LaneId const &to)
     296                 :            : {
     297         [ +  + ]:        530 :   return ((from != to) && !CoreIntersection::isLanePartOfAnIntersection(from)
     298   [ +  -  +  + ]:        530 :           && CoreIntersection::isLanePartOfAnIntersection(to));
     299                 :            : }
     300                 :            : 
     301                 :        380 : bool CoreIntersection::isRoadSegmentEnteringIntersection(
     302                 :            :   route::RouteIterator const &routeIterator, route::RoadSegmentList::const_iterator &routePreviousSegmentIter)
     303                 :            : {
     304                 :        380 :   if ((routeIterator.roadSegmentIterator != routeIterator.route.roadSegments.end())
     305   [ +  -  +  +  :        380 :       && (routeIterator.roadSegmentIterator != routeIterator.route.roadSegments.begin()))
                   +  + ]
     306                 :            :   {
     307                 :        265 :     auto previousSegmentIter = routeIterator.roadSegmentIterator;
     308                 :        265 :     previousSegmentIter--;
     309                 :            :     // only look at the first parapoint per index
     310                 :        265 :     auto const fromLane = previousSegmentIter->drivableLaneSegments.front().laneInterval.laneId;
     311                 :        265 :     auto const toLane = routeIterator.roadSegmentIterator->drivableLaneSegments.front().laneInterval.laneId;
     312   [ +  -  +  + ]:        265 :     if (laneEntersIntersection(fromLane, toLane))
     313                 :            :     {
     314                 :        117 :       routePreviousSegmentIter = previousSegmentIter;
     315                 :        117 :       return true;
     316                 :            :     }
     317                 :            :   }
     318                 :        263 :   return false;
     319                 :            : }
     320                 :            : 
     321                 :          0 : bool CoreIntersection::isIntersectionOnRoute(route::FullRoute const &route)
     322                 :            : {
     323         [ #  # ]:          0 :   for (auto roadSegmentIter = route.roadSegments.begin(); roadSegmentIter != route.roadSegments.end();
     324                 :          0 :        roadSegmentIter++)
     325                 :            :   {
     326                 :          0 :     route::RoadSegmentList::const_iterator routePreviousSegmentIter;
     327                 :            :     // @todo: also consider segments leaving intersection if first segment is already within intersection
     328   [ #  #  #  # ]:          0 :     if (isRoadSegmentEnteringIntersection(route::RouteIterator(route, roadSegmentIter), routePreviousSegmentIter))
     329                 :            :     {
     330                 :          0 :       return true;
     331                 :            :     }
     332                 :            :   }
     333                 :          0 :   return false;
     334                 :            : }
     335                 :            : 
     336                 :          3 : bool CoreIntersection::objectWithinIntersection(match::MapMatchedObjectBoundingBox const &object) const
     337                 :            : {
     338         [ +  + ]:          3 :   for (auto const &occupiedLane : object.laneOccupiedRegions)
     339                 :            :   {
     340   [ +  -  +  - ]:          2 :     if (mInternalLanes.count(occupiedLane.laneId) > 0)
     341                 :            :     {
     342                 :          2 :       return true;
     343                 :            :     }
     344                 :            :   }
     345                 :          1 :   return false;
     346                 :            : }
     347                 :            : 
     348                 :          0 : bool CoreIntersection::objectRouteCrossesIntersection(route::FullRoute const &objectRoute) const
     349                 :            : {
     350         [ #  # ]:          0 :   for (auto internalLaneId : mInternalLanes)
     351                 :            :   {
     352         [ #  # ]:          0 :     auto findResult = route::findWaypoint(internalLaneId, objectRoute);
     353         [ #  # ]:          0 :     if (findResult.isValid())
     354                 :            :     {
     355                 :          0 :       return true;
     356                 :            :     }
     357                 :            :   }
     358                 :          0 :   return false;
     359                 :            : }
     360                 :            : 
     361                 :          1 : physics::Distance CoreIntersection::objectDistanceToIntersection(match::Object const &object) const
     362                 :            : {
     363                 :          1 :   physics::Distance minDistance = std::numeric_limits<physics::Distance>::max();
     364   [ +  -  +  - ]:          1 :   if (objectWithinIntersection(object.mapMatchedBoundingBox))
     365                 :            :   {
     366                 :          1 :     minDistance = physics::Distance(0.);
     367                 :            :   }
     368                 :            :   else
     369                 :            :   {
     370         [ #  # ]:          0 :     for (auto internalLaneId : mInternalLanes)
     371                 :            :     {
     372   [ #  #  #  # ]:          0 :       minDistance = std::min(minDistance, lane::getDistanceToLane(internalLaneId, object));
     373                 :            :     }
     374                 :            :   }
     375                 :          1 :   return minDistance;
     376                 :            : }
     377                 :            : 
     378         [ +  - ]:         15 : CoreIntersection::CoreIntersection(lane::LaneId const &laneId)
     379                 :            : {
     380         [ +  - ]:         15 :   extractLanesOfCoreIntersection(laneId);
     381                 :         15 : }
     382                 :            : 
     383                 :          1 : CoreIntersectionPtr CoreIntersection::getCoreIntersectionFor(lane::LaneId const &laneId)
     384                 :            : {
     385                 :          1 :   CoreIntersectionPtr result;
     386   [ +  -  +  - ]:          1 :   if (isLanePartOfAnIntersection(laneId))
     387                 :            :   {
     388   [ +  -  +  -  :          1 :     result = CoreIntersectionPtr(new CoreIntersection(laneId));
                   +  - ]
     389                 :            :   }
     390                 :          1 :   return result;
     391                 :            : }
     392                 :            : 
     393                 :            : template <typename CONTAINER>
     394                 :          4 : std::vector<CoreIntersectionPtr> CoreIntersection::getCoreIntersectionsForLaneIds(CONTAINER const &laneIds)
     395                 :            : {
     396                 :          4 :   std::vector<CoreIntersectionPtr> result;
     397         [ +  + ]:        327 :   for (auto &laneId : laneIds)
     398                 :            :   {
     399   [ +  -  +  + ]:        323 :     if (isLanePartOfAnIntersection(laneId))
     400                 :            :     {
     401                 :         86 :       bool createIntersection = true;
     402         [ +  + ]:        482 :       for (auto const &intersection : result)
     403                 :            :       {
     404   [ +  -  +  - ]:        468 :         auto const internalLanes = intersection->internalLanes();
     405   [ +  -  +  + ]:        468 :         if (internalLanes.find(laneId) != internalLanes.end())
     406                 :            :         {
     407                 :         72 :           createIntersection = false;
     408                 :         72 :           break;
     409                 :            :         }
     410                 :            :       }
     411         [ +  + ]:         86 :       if (createIntersection)
     412                 :            :       {
     413   [ +  -  +  -  :         14 :         result.push_back(CoreIntersectionPtr(new CoreIntersection(laneId)));
             +  -  +  - ]
     414                 :            :       }
     415                 :            :     }
     416                 :            :   }
     417                 :          4 :   return result;
     418                 :            : }
     419                 :            : 
     420                 :          0 : std::vector<CoreIntersectionPtr> CoreIntersection::getCoreIntersectionsFor(lane::LaneIdSet const &laneIds)
     421                 :            : {
     422                 :          0 :   return getCoreIntersectionsForLaneIds(laneIds);
     423                 :            : }
     424                 :            : 
     425                 :          2 : std::vector<CoreIntersectionPtr> CoreIntersection::getCoreIntersectionsFor(lane::LaneIdList const &laneIds)
     426                 :            : {
     427                 :          2 :   return getCoreIntersectionsForLaneIds(laneIds);
     428                 :            : }
     429                 :            : 
     430                 :          0 : CoreIntersectionPtr CoreIntersection::getCoreIntersectionFor(match::MapMatchedPosition const &mapMatchedPosition)
     431                 :            : {
     432                 :          0 :   return CoreIntersection::getCoreIntersectionFor(mapMatchedPosition.lanePoint.paraPoint.laneId);
     433                 :            : }
     434                 :            : 
     435                 :          2 : std::vector<CoreIntersectionPtr> CoreIntersection::getCoreIntersectionsForInLaneMatches(point::ENUPoint const &position)
     436                 :            : {
     437         [ +  - ]:          4 :   match::AdMapMatching mapMatching;
     438   [ +  -  +  - ]:          4 :   auto mapMatchedPositionConfidenceList = mapMatching.findLanes(toECEF(position), physics::Distance(2.));
     439         [ +  - ]:          4 :   return CoreIntersection::getCoreIntersectionsForInLaneMatches(mapMatchedPositionConfidenceList);
     440                 :            : }
     441                 :            : 
     442                 :          2 : std::vector<CoreIntersectionPtr> CoreIntersection::getCoreIntersectionsForInLaneMatches(
     443                 :            :   match::MapMatchedPositionConfidenceList const &mapMatchedPositionConfidenceList)
     444                 :            : {
     445                 :          4 :   lane::LaneIdSet inLaneMatches;
     446         [ +  + ]:          8 :   for (auto const &mapMatchedPosition : mapMatchedPositionConfidenceList)
     447                 :            :   {
     448         [ +  + ]:          6 :     if (mapMatchedPosition.type == match::MapMatchedPositionType::LANE_IN)
     449                 :            :     {
     450         [ +  - ]:          3 :       inLaneMatches.insert(mapMatchedPosition.lanePoint.paraPoint.laneId);
     451                 :            :     }
     452                 :            :   }
     453                 :            : 
     454         [ +  - ]:          4 :   return CoreIntersection::getCoreIntersectionsForLaneIds(inLaneMatches);
     455                 :            : }
     456                 :            : 
     457                 :            : std::vector<CoreIntersectionPtr>
     458                 :          0 : CoreIntersection::getCoreIntersectionsForInLaneMatches(match::MapMatchedObjectBoundingBox const &object)
     459                 :            : {
     460                 :          0 :   lane::LaneIdSet inLaneMatches;
     461         [ #  # ]:          0 :   for (auto const &occupiedRegion : object.laneOccupiedRegions)
     462                 :            :   {
     463         [ #  # ]:          0 :     inLaneMatches.insert(occupiedRegion.laneId);
     464                 :            :   }
     465                 :            : 
     466         [ #  # ]:          0 :   return CoreIntersection::getCoreIntersectionsForLaneIds(inLaneMatches);
     467                 :            : }
     468                 :            : 
     469                 :          2 : std::vector<CoreIntersectionPtr> CoreIntersection::getCoreIntersectionsForMap()
     470                 :            : {
     471         [ +  - ]:          4 :   return getCoreIntersectionsFor(lane::getLanes());
     472                 :            : }
     473                 :            : 
     474                 :            : } // namespace intersection
     475                 :            : } // namespace map
     476                 :            : } // namespace ad
     477                 :            : 
     478                 :            : namespace std {
     479                 :            : 
     480                 :          0 : std::ostream &operator<<(std::ostream &os, ::ad::map::intersection::CoreIntersection const &intersection)
     481                 :            : {
     482                 :          0 :   os << "CoreIntersection(";
     483         [ #  # ]:          0 :   os << " boundingSphere(center=" << ::ad::map::point::toENU(intersection.getBoundingSphere().center)
     484   [ #  #  #  #  :          0 :      << ", radius=" << intersection.getBoundingSphere().radius << ")";
                   #  # ]
     485                 :          0 :   os << std::endl;
     486                 :          0 :   os << "->internalLanes: ";
     487                 :          0 :   os << intersection.internalLanes();
     488                 :          0 :   os << std::endl;
     489                 :          0 :   os << "->entryLanes: ";
     490                 :          0 :   os << intersection.entryLanes();
     491                 :          0 :   os << std::endl;
     492                 :          0 :   os << " -> entryParaPoints: ";
     493                 :          0 :   os << intersection.entryParaPoints();
     494                 :          0 :   os << std::endl;
     495                 :          0 :   os << " -> exitLanes: ";
     496                 :          0 :   os << intersection.exitLanes();
     497                 :          0 :   os << std::endl;
     498                 :          0 :   os << " -> exitParaPoints: ";
     499                 :          0 :   os << intersection.exitParaPoints();
     500                 :          0 :   os << ")" << std::endl;
     501                 :          0 :   return os;
     502                 :            : }
     503                 :            : 
     504                 :            : } // namespace std