Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2018-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : /**
       9                 :            :  * @file
      10                 :            :  */
      11                 :            : 
      12                 :            : #include "RssSituationCoordinateSystemConversion.hpp"
      13                 :            : 
      14                 :            : #include <algorithm>
      15                 :            : #include <limits>
      16                 :            : #include <vector>
      17                 :            : 
      18                 :            : namespace ad {
      19                 :            : namespace rss {
      20                 :            : namespace world {
      21                 :            : 
      22                 :            : using physics::Distance;
      23                 :            : using physics::MetricRange;
      24                 :            : 
      25                 :       5570 : bool calculateLateralDimensions(RoadArea const &roadArea, std::vector<MetricRange> &lateralRanges)
      26                 :            : {
      27                 :       5570 :   bool result = true;
      28         [ +  + ]:       5570 :   if (roadArea.empty())
      29                 :            :   {
      30                 :          2 :     spdlog::error("RssSituationCoordinateSystemConversion::calculateLateralDimensions>> road area empty");
      31                 :          2 :     return false;
      32                 :            :   }
      33                 :            : 
      34                 :            :   try
      35                 :            :   {
      36         [ +  - ]:       5568 :     MetricRange currentLateralPosition;
      37                 :            : 
      38                 :       5568 :     currentLateralPosition.maximum = Distance(0.);
      39                 :       5568 :     currentLateralPosition.minimum = Distance(0.);
      40                 :            : 
      41                 :       5568 :     std::size_t currentLateralIndex = 0u;
      42                 :       5568 :     bool roadSegmentFound = true;
      43         [ +  + ]:      26019 :     while (roadSegmentFound)
      44                 :            :     {
      45                 :      20496 :       roadSegmentFound = false;
      46         [ +  + ]:      20496 :       lateralRanges.push_back(currentLateralPosition);
      47                 :            : 
      48                 :      20451 :       Distance lateralDistanceMax = Distance(0.);
      49                 :      20451 :       Distance lateralDistanceMin = std::numeric_limits<Distance>::max();
      50         [ +  + ]:     176324 :       for (const auto &roadSegment : roadArea)
      51                 :            :       {
      52         [ -  + ]:     155873 :         if (roadSegment.empty())
      53                 :            :         {
      54         [ #  # ]:          0 :           spdlog::error("RssSituationCoordinateSystemConversion::calculateLateralDimensions>> road segment empty");
      55                 :          0 :           return false;
      56                 :            :         }
      57         [ +  + ]:     155873 :         if (roadSegment.size() > currentLateralIndex)
      58                 :            :         {
      59                 :     130516 :           roadSegmentFound = true;
      60         [ +  - ]:     130516 :           lateralDistanceMax = std::max(lateralDistanceMax, roadSegment[currentLateralIndex].width.maximum);
      61         [ +  - ]:     130516 :           lateralDistanceMin = std::min(lateralDistanceMin, roadSegment[currentLateralIndex].width.minimum);
      62                 :            :         }
      63                 :            :       }
      64                 :            : 
      65         [ +  + ]:      20451 :       if (roadSegmentFound)
      66                 :            :       {
      67         [ +  - ]:      14928 :         currentLateralPosition.maximum += lateralDistanceMax;
      68         [ +  - ]:      14928 :         currentLateralPosition.minimum += lateralDistanceMin;
      69                 :      14928 :         currentLateralIndex++;
      70                 :            :       }
      71                 :            :     }
      72                 :            :   }
      73                 :         45 :   catch (...)
      74                 :            :   {
      75                 :         45 :     return false;
      76                 :            :   }
      77                 :            : 
      78                 :       5523 :   return result;
      79                 :            : }
      80                 :            : 
      81                 :            : /**
      82                 :            :  * The RoadArea describes the relation between object and egoVehicle.
      83                 :            :  * The RoadArea can be regarded as as matrix.
      84                 :            :  * Each cell of the matrix is a laneSegment.
      85                 :            :  * The rows of a cell are combined to a roadSegment.
      86                 :            :  *
      87                 :            :  * The objects are defined by occupiedRegions. These are the cells that are overlapped by the objects (marked with x and
      88                 :            :  * o in the following sketch).
      89                 :            :  *
      90                 :            :  *                 ___________________________
      91                 :            :  *                |         |       _|_       |
      92                 :            :  *                |         |      |o|o|      |
      93                 :            :  *                ----------------------------
      94                 :            :  *                |        _|_     |o|o|      |
      95                 :            :  *                |       |x|x|     -|-       |
      96                 :            :  *                 ---------------------------
      97                 :            :  *                |       |x|x|      |        |
      98                 :            :  *                |        -|-       |        |   roadSegment
      99                 :            :  *                |         |        |        |
     100                 :            :  *               0 ----------------------------
     101                 :            :  *                 0
     102                 :            :  *
     103                 :            :  *
     104                 :            :  *  Each laneSegment has a minimum and maximum length and width.
     105                 :            :  *  In order to determine the dimension of the objects it is required to calculate the minimum and maximum
     106                 :            :  *  length and width towards the object borders.
     107                 :            :  *  For the longitudinal case:
     108                 :            :  *    for each roadSegment the maximum and minimum length are extracted.
     109                 :            :  *    The starting minimum and maximum length of a roadSegment is the sum of the minimum or maximum length of all the
     110                 :            :  * previous roadSegments
     111                 :            :  *  This happens on the fly in calculateObjectDimensions.
     112                 :            :  *
     113                 :            :  *  For lateral case (calculateLateralDimensions):
     114                 :            :  *   for each column of the matrix minimum and maximum width needs to be determined
     115                 :            :  *   For the first column this is the minimum and maximum of all the first laneSegments within every roadSegment.
     116                 :            :  *   For the next column this is the minimum and maximum of all the second laneSegmetns within every roadSegment plus
     117                 :            :  * the minimum / maximum value of the first column
     118                 :            :  *   and so on...
     119                 :            :  *
     120                 :            :  *  These values needs to be determined in a separate processing step (calculateLateralDimensions)
     121                 :            :  *
     122                 :            :  *  The minimum and maximum distances to the begin of each laneSegment are then passed together with the laneSegement to
     123                 :            :  * the RssObjectPostionExtractor.
     124                 :            :  *  The RssObjectPostionExtractor checks whether the laneSegment is occupied by the object and if so minimum and maximum
     125                 :            :  * object dimensions
     126                 :            :  *  of the object area in the laneSegment are calculated and combined to a global dimension of the object.
     127                 :            :  *
     128                 :            :  */
     129                 :            : 
     130                 :       5570 : bool calculateObjectDimensions(std::vector<Object> const &objects,
     131                 :            :                                RoadArea const &roadArea,
     132                 :            :                                std::vector<ObjectDimensions> &objectDimensions)
     133                 :            : {
     134                 :       5570 :   bool result = true;
     135                 :            : 
     136                 :            :   try
     137                 :            :   {
     138                 :       5656 :     std::vector<MetricRange> lateralRanges;
     139         [ +  - ]:       5570 :     result = calculateLateralDimensions(roadArea, lateralRanges);
     140         [ +  + ]:       5570 :     if (result)
     141                 :            :     {
     142         [ +  - ]:       5523 :       MetricRange longitudinalDimensions;
     143                 :            : 
     144                 :       5523 :       longitudinalDimensions.maximum = Distance(0.);
     145                 :       5523 :       longitudinalDimensions.minimum = Distance(0.);
     146                 :            : 
     147                 :       5609 :       std::vector<RssObjectPositionExtractor> extractors;
     148         [ +  + ]:      14849 :       for (const auto &object : objects)
     149                 :            :       {
     150         [ +  + ]:       9383 :         if (object.occupiedRegions.empty())
     151                 :            :         {
     152         [ +  - ]:          1 :           spdlog::error(
     153                 :            :             "RssSituationCoordinateSystemConversion::calculateObjectDimensions>> occupied region of object {} empty",
     154                 :          1 :             object.objectId);
     155                 :          1 :           return false;
     156                 :            :         }
     157   [ +  +  +  + ]:       9410 :         extractors.push_back(RssObjectPositionExtractor(object.occupiedRegions));
     158                 :            :       }
     159                 :            : 
     160   [ +  +  +  -  :      25564 :       for (auto roadSegment = roadArea.cbegin(); roadSegment != roadArea.cend() && result; roadSegment++)
                   +  + ]
     161                 :            :       {
     162                 :      20100 :         Distance longitudinalDistanceMax = Distance(0.);
     163                 :      20100 :         Distance longitudinalDistanceMin = Distance(0.);
     164         [ +  + ]:      55250 :         for (auto &extractor : extractors)
     165                 :            :         {
     166   [ +  -  +  -  :      35150 :           result = result && extractor.newRoadSegment(longitudinalDimensions.minimum, longitudinalDimensions.maximum);
                   +  - ]
     167                 :            :         }
     168                 :            : 
     169                 :            :         // This is needed, because we want to look for the minimum
     170                 :      20100 :         longitudinalDistanceMin = std::numeric_limits<Distance>::max();
     171                 :            : 
     172   [ +  +  +  -  :     150108 :         for (std::size_t i = 0u; i < roadSegment->size() && result; i++)
                   +  + ]
     173                 :            :         {
     174         [ +  - ]:     130010 :           if (i < lateralRanges.size())
     175                 :            :           {
     176         [ +  + ]:     384959 :             for (auto &extractor : extractors)
     177                 :            :             {
     178   [ +  -  +  +  :     254951 :               result = result && extractor.newLaneSegment(lateralRanges[i], (*roadSegment)[i]);
                   +  - ]
     179                 :            :             }
     180                 :            :           }
     181                 :            :           else
     182                 :            :           {
     183                 :            :             result = false; // LCOV_EXCL_LINE: unreachable code, keep to be on the safe side
     184                 :            :           }
     185                 :            : 
     186         [ +  - ]:     130008 :           longitudinalDistanceMax = std::max(longitudinalDistanceMax, (*roadSegment)[i].length.maximum);
     187         [ +  - ]:     130008 :           longitudinalDistanceMin = std::min(longitudinalDistanceMin, (*roadSegment)[i].length.minimum);
     188                 :            :         }
     189                 :            : 
     190         [ +  - ]:      20098 :         if (result)
     191                 :            :         {
     192         [ +  - ]:      20098 :           longitudinalDimensions.maximum += longitudinalDistanceMax;
     193         [ +  - ]:      20098 :           longitudinalDimensions.minimum += longitudinalDistanceMin;
     194                 :            :         }
     195                 :            :       }
     196                 :            : 
     197         [ +  - ]:       5464 :       if (result)
     198                 :            :       {
     199         [ +  + ]:      14725 :         for (auto &extractor : extractors)
     200                 :            :         {
     201         [ +  - ]:       9289 :           ObjectDimensions extractedDimensions;
     202   [ +  -  +  -  :       9289 :           result = result && extractor.getObjectDimensions(extractedDimensions);
                   +  - ]
     203         [ +  + ]:       9289 :           objectDimensions.push_back(extractedDimensions);
     204                 :            :         }
     205                 :            :       }
     206                 :            :     }
     207                 :            :     else
     208                 :            :     {
     209         [ +  - ]:         47 :       spdlog::error("RssSituationCoordinateSystemConversion::calculateObjectDimensions>> calculateLateralDimensions of "
     210                 :            :                     "object {} failed",
     211                 :         47 :                     objects.front().objectId);
     212                 :            :     }
     213                 :            :   }
     214                 :         86 :   catch (...)
     215                 :            :   {
     216                 :         86 :     return false;
     217                 :            :   }
     218                 :            : 
     219                 :       5483 :   return result;
     220                 :            : }
     221                 :            : 
     222                 :       3961 : bool calculateObjectDimensions(Scene const &currentScene,
     223                 :            :                                ObjectDimensions &egoVehiclePosition,
     224                 :            :                                ObjectDimensions &objectPosition)
     225                 :            : {
     226                 :       3961 :   bool result = true;
     227                 :            : 
     228                 :            :   try
     229                 :            :   {
     230                 :       7922 :     std::vector<Object> objects;
     231         [ +  + ]:       3961 :     objects.push_back(currentScene.egoVehicle);
     232         [ +  + ]:       3939 :     objects.push_back(currentScene.object);
     233                 :            : 
     234                 :       7834 :     std::vector<ObjectDimensions> objectDimensions;
     235         [ +  - ]:       3917 :     result = calculateObjectDimensions(objects, currentScene.egoVehicleRoad, objectDimensions);
     236                 :            : 
     237   [ +  +  +  -  :       3917 :     if (result && (objectDimensions.size() == 2))
                   +  + ]
     238                 :            :     {
     239                 :       3814 :       egoVehiclePosition = objectDimensions[0];
     240                 :       3814 :       objectPosition = objectDimensions[1];
     241                 :            :     }
     242                 :            :     else
     243                 :            :     {
     244                 :        103 :       result = false;
     245                 :            :     }
     246                 :            :   }
     247                 :         44 :   catch (...)
     248                 :            :   {
     249                 :         44 :     return false;
     250                 :            :   }
     251                 :            : 
     252                 :       3917 :   return result;
     253                 :            : }
     254                 :            : 
     255                 :       1665 : bool calculateObjectDimensions(Object const &object, RoadArea const &roadArea, ObjectDimensions &objectPosition)
     256                 :            : {
     257                 :       1665 :   bool result = true;
     258                 :            : 
     259                 :            :   try
     260                 :            :   {
     261                 :       3330 :     std::vector<Object> objects;
     262         [ +  + ]:       1665 :     objects.push_back(object);
     263                 :            : 
     264                 :       3306 :     std::vector<ObjectDimensions> objectDimensions;
     265         [ +  - ]:       1653 :     result = calculateObjectDimensions(objects, roadArea, objectDimensions);
     266                 :            : 
     267   [ +  +  +  -  :       1653 :     if (result && (objectDimensions.size() == 1))
                   +  + ]
     268                 :            :     {
     269                 :       1622 :       objectPosition = objectDimensions[0];
     270                 :            :     }
     271                 :            :     else
     272                 :            :     {
     273                 :         31 :       result = false;
     274                 :            :     }
     275                 :            :   }
     276                 :         12 :   catch (...)
     277                 :            :   {
     278                 :         12 :     return false;
     279                 :            :   }
     280                 :            : 
     281                 :       1653 :   return result;
     282                 :            : }
     283                 :            : 
     284                 :       9904 : void convertVehicleStateDynamics(Object const &object,
     285                 :            :                                  RssDynamics const &rssDynamics,
     286                 :            :                                  ::ad::rss::situation::VehicleState &vehicleState)
     287                 :            : {
     288                 :       9904 :   vehicleState.dynamics = rssDynamics;
     289                 :       9904 :   vehicleState.velocity.speedLon.minimum = object.velocity.speedLonMin;
     290                 :       9904 :   vehicleState.velocity.speedLon.maximum = object.velocity.speedLonMax;
     291                 :       9904 :   vehicleState.velocity.speedLat.minimum = object.velocity.speedLatMin;
     292                 :       9904 :   vehicleState.velocity.speedLat.maximum = object.velocity.speedLatMax;
     293                 :       9904 : }
     294                 :            : 
     295                 :            : } // namespace world
     296                 :            : } // namespace rss
     297                 :            : } // namespace ad