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/match/MapMatchedOperation.hpp"
      10                 :            : 
      11                 :            : #include <algorithm>
      12                 :            : #include "ad/map/point/Operation.hpp"
      13                 :            : 
      14                 :            : namespace ad {
      15                 :            : namespace map {
      16                 :            : namespace match {
      17                 :            : 
      18                 :          2 : point::ParaPointList getParaPoints(MapMatchedPositionConfidenceList const &inMapMatchedPositions)
      19                 :            : {
      20                 :          2 :   point::ParaPointList result;
      21         [ +  - ]:          2 :   result.reserve(inMapMatchedPositions.size());
      22         [ +  + ]:          5 :   for (auto const &mapMatchedPosition : inMapMatchedPositions)
      23                 :            :   {
      24         [ +  - ]:          3 :     result.push_back(mapMatchedPosition.lanePoint.paraPoint);
      25                 :            :   }
      26                 :          2 :   return result;
      27                 :            : }
      28                 :            : 
      29                 :         20 : point::ENUHeading getObjectENUHeading(const match::MapMatchedObjectBoundingBox &mapMatchedBoundingBox)
      30                 :            : {
      31         [ +  - ]:         20 :   point::ECEFPoint rearLeftPosition;
      32                 :            :   bool const rearLeftAvailable
      33                 :         20 :     = mapMatchedBoundingBox.referencePointPositions[int32_t(match::ObjectReferencePoints::RearLeft)].size() > 0u;
      34         [ +  + ]:         20 :   if (rearLeftAvailable)
      35                 :            :   {
      36                 :         34 :     rearLeftPosition = mapMatchedBoundingBox.referencePointPositions[int32_t(match::ObjectReferencePoints::RearLeft)]
      37                 :         17 :                          .front()
      38                 :            :                          .queryPoint;
      39                 :            :   }
      40         [ +  - ]:         20 :   point::ECEFPoint rearRightPosition;
      41                 :            :   bool const rearRightAvailable
      42                 :         20 :     = mapMatchedBoundingBox.referencePointPositions[int32_t(match::ObjectReferencePoints::RearRight)].size() > 0u;
      43         [ +  + ]:         20 :   if (rearRightAvailable)
      44                 :            :   {
      45                 :         34 :     rearRightPosition = mapMatchedBoundingBox.referencePointPositions[int32_t(match::ObjectReferencePoints::RearRight)]
      46                 :         17 :                           .front()
      47                 :            :                           .queryPoint;
      48                 :            :   }
      49         [ +  - ]:         20 :   point::ECEFPoint frontLeftPosition;
      50                 :            :   bool const frontLeftAvailable
      51                 :         20 :     = mapMatchedBoundingBox.referencePointPositions[int32_t(match::ObjectReferencePoints::FrontLeft)].size() > 0u;
      52         [ +  + ]:         20 :   if (frontLeftAvailable)
      53                 :            :   {
      54                 :         34 :     frontLeftPosition = mapMatchedBoundingBox.referencePointPositions[int32_t(match::ObjectReferencePoints::FrontLeft)]
      55                 :         17 :                           .front()
      56                 :            :                           .queryPoint;
      57                 :            :   }
      58         [ +  - ]:         20 :   point::ECEFPoint frontRightPosition;
      59                 :            :   bool const frontRightAvailable
      60                 :         20 :     = mapMatchedBoundingBox.referencePointPositions[int32_t(match::ObjectReferencePoints::FrontRight)].size() > 0u;
      61         [ +  + ]:         20 :   if (frontRightAvailable)
      62                 :            :   {
      63                 :            :     frontRightPosition
      64                 :         34 :       = mapMatchedBoundingBox.referencePointPositions[int32_t(match::ObjectReferencePoints::FrontRight)]
      65                 :         17 :           .front()
      66                 :            :           .queryPoint;
      67                 :            :   }
      68                 :            : 
      69         [ +  - ]:         20 :   point::ECEFHeading ecefHeading;
      70                 :         20 :   bool rotateENUHeading = false;
      71   [ +  +  +  +  :         20 :   if (rearLeftAvailable && rearRightAvailable && frontLeftAvailable && frontRightAvailable)
             +  +  +  - ]
      72                 :            :   {
      73   [ +  -  +  - ]:         15 :     auto const rearCenterPosition = 0.5 * (rearLeftPosition + rearRightPosition);
      74   [ +  -  +  - ]:         15 :     auto const frontCenterPosition = 0.5 * (frontLeftPosition + frontRightPosition);
      75         [ +  - ]:         15 :     ecefHeading = point::createECEFHeading(rearCenterPosition, frontCenterPosition);
      76                 :            :   }
      77   [ +  +  +  + ]:          5 :   else if (rearLeftAvailable && frontLeftAvailable)
      78                 :            :   {
      79         [ +  - ]:          1 :     ecefHeading = point::createECEFHeading(rearLeftPosition, frontLeftPosition);
      80                 :            :   }
      81   [ +  +  +  + ]:          4 :   else if (rearRightAvailable && frontRightAvailable)
      82                 :            :   {
      83         [ +  - ]:          1 :     ecefHeading = point::createECEFHeading(rearRightPosition, frontRightPosition);
      84                 :            :   }
      85   [ +  +  +  - ]:          3 :   else if (frontLeftAvailable && frontRightAvailable)
      86                 :            :   {
      87         [ +  - ]:          1 :     ecefHeading = point::createECEFHeading(frontLeftPosition, frontRightPosition);
      88                 :          1 :     rotateENUHeading = true;
      89                 :            :   }
      90   [ +  +  +  - ]:          2 :   else if (rearLeftAvailable && rearRightAvailable)
      91                 :            :   {
      92         [ +  - ]:          1 :     ecefHeading = point::createECEFHeading(rearLeftPosition, rearRightPosition);
      93                 :          1 :     rotateENUHeading = true;
      94                 :            :   }
      95                 :            :   else
      96                 :            :   {
      97         [ +  - ]:          1 :     throw std::runtime_error("point::createENUHeading no two corner points available. Heading estimate is impossible");
      98                 :            :   }
      99         [ +  - ]:         19 :   auto enuHeading = point::createENUHeading(ecefHeading);
     100         [ +  + ]:         19 :   if (rotateENUHeading)
     101                 :            :   {
     102         [ +  - ]:          2 :     enuHeading = point::createENUHeading(static_cast<double>(enuHeading) + M_PI_2);
     103                 :            :   }
     104                 :         19 :   return enuHeading;
     105                 :            : }
     106                 :            : 
     107                 :          9 : physics::Distance signedDistanceToLane(lane::LaneId const checkLaneId,
     108                 :            :                                        MapMatchedPositionConfidenceList const &mapMatchedPositions)
     109                 :            : {
     110                 :          9 :   physics::Distance distance = std::numeric_limits<physics::Distance>::max();
     111                 :            : 
     112                 :            :   auto findCheckPosition = std::find_if(std::begin(mapMatchedPositions),
     113                 :            :                                         std::end(mapMatchedPositions),
     114                 :          9 :                                         [checkLaneId](match::MapMatchedPosition const &position) {
     115                 :          9 :                                           return position.lanePoint.paraPoint.laneId == checkLaneId;
     116         [ +  - ]:          9 :                                         });
     117                 :            : 
     118         [ +  + ]:          9 :   if (findCheckPosition != std::end(mapMatchedPositions))
     119                 :            :   {
     120                 :            :     // found position
     121         [ +  + ]:          8 :     if (findCheckPosition->type == match::MapMatchedPositionType::LANE_IN)
     122                 :            :     {
     123                 :          3 :       distance = physics::Distance(0.);
     124                 :            :     }
     125         [ +  + ]:          5 :     else if (findCheckPosition->type == match::MapMatchedPositionType::LANE_LEFT)
     126                 :            :     {
     127   [ +  -  +  + ]:          2 :       if (findCheckPosition->lanePoint.lateralT > physics::RatioValue(0.))
     128                 :            :       {
     129                 :            :         throw std::runtime_error("ad::map::match::signedDistanceToLane: inconsistent map matched positions."
     130         [ +  - ]:          1 :                                  " Expected lateralT to be < 0. for LANE_LEFT");
     131                 :            :       }
     132         [ +  - ]:          1 :       distance = findCheckPosition->lanePoint.lateralT * findCheckPosition->lanePoint.laneWidth;
     133                 :            :     }
     134         [ +  + ]:          3 :     else if (findCheckPosition->type == match::MapMatchedPositionType::LANE_RIGHT)
     135                 :            :     {
     136   [ +  -  +  + ]:          2 :       if (findCheckPosition->lanePoint.lateralT < physics::RatioValue(1.))
     137                 :            :       {
     138                 :            :         throw std::runtime_error("ad::map::match::signedDistanceToLane: inconsistent map matched positions."
     139         [ +  - ]:          1 :                                  " Expected lateralT to be > 1. for LANE_RIGHT");
     140                 :            :       }
     141                 :            :       distance
     142   [ +  -  +  - ]:          1 :         = (findCheckPosition->lanePoint.lateralT - physics::RatioValue(1.)) * findCheckPosition->lanePoint.laneWidth;
     143                 :            :     }
     144                 :            :     else
     145                 :            :     {
     146                 :            :       throw std::runtime_error("ad::map::match::signedDistanceToLane: inconsistent map matched positions."
     147         [ +  - ]:          1 :                                " Expected type to be one of LANE_IN, LANE_LEFT, LEFT_RIGHT");
     148                 :            :     }
     149                 :            :   }
     150                 :            : 
     151                 :          6 :   return distance;
     152                 :            : }
     153                 :            : 
     154                 :            : } // namespace match
     155                 :            : } // namespace map
     156                 :            : } // namespace ad