Branch data     Line data    Source code

       1                 :            : // Copyright 2020 TierIV, Inc.
       2                 :            : //
       3                 :            : // Licensed under the Apache License, Version 2.0 (the "License");
       4                 :            : // you may not use this file except in compliance with the License.
       5                 :            : // You may obtain a copy of the License at
       6                 :            : //
       7                 :            : //     http://www.apache.org/licenses/LICENSE-2.0
       8                 :            : //
       9                 :            : // Unless required by applicable law or agreed to in writing, software
      10                 :            : // distributed under the License is distributed on an "AS IS" BASIS,
      11                 :            : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      12                 :            : // See the License for the specific language governing permissions and
      13                 :            : // limitations under the License.
      14                 :            : //
      15                 :            : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
      16                 :            : 
      17                 :            : //lint -e537 pclint vs cpplint NOLINT
      18                 :            : 
      19                 :            : 
      20                 :            : #include "had_map_utils/had_map_utils.hpp"
      21                 :            : 
      22                 :            : #include <lanelet2_core/geometry/Lanelet.h>
      23                 :            : #include <common/types.hpp>
      24                 :            : #include <utility>
      25                 :            : #include <algorithm>
      26                 :            : #include <limits>
      27                 :            : #include <memory>
      28                 :            : #include <string>
      29                 :            : #include <vector>
      30                 :            : 
      31                 :            : using autoware::common::types::float64_t;
      32                 :            : 
      33                 :            : namespace autoware
      34                 :            : {
      35                 :            : namespace common
      36                 :            : {
      37                 :            : namespace had_map_utils
      38                 :            : {
      39                 :            : 
      40         [ +  - ]:        216 : std::vector<float64_t> calculateSegmentDistances(const lanelet::ConstLineString3d & line_string)
      41                 :            : {
      42                 :            :   std::vector<float64_t> segment_distances;
      43         [ +  - ]:        216 :   segment_distances.reserve(line_string.size() - 1);
      44                 :            : 
      45         [ +  + ]:        764 :   for (size_t i = 1; i < line_string.size(); ++i) {
      46         [ +  + ]:        548 :     const auto distance = lanelet::geometry::distance(line_string[i], line_string[i - 1]);
      47         [ +  - ]:        548 :     segment_distances.push_back(distance);
      48                 :            :   }
      49                 :            : 
      50                 :        216 :   return segment_distances;
      51                 :            : }
      52                 :            : 
      53                 :        216 : std::vector<float64_t> calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string)
      54                 :            : {
      55                 :        216 :   const auto segment_distances = calculateSegmentDistances(line_string);
      56                 :            : 
      57   [ +  -  -  - ]:        216 :   std::vector<float64_t> accumulated_lengths{0};
      58         [ +  - ]:        216 :   accumulated_lengths.reserve(segment_distances.size() + 1);
      59                 :            :   std::partial_sum(
      60                 :            :     std::begin(segment_distances), std::end(segment_distances),
      61         [ +  - ]:        216 :     std::back_inserter(accumulated_lengths));
      62                 :            : 
      63                 :        216 :   return accumulated_lengths;
      64                 :            : }
      65                 :            : 
      66                 :       1828 : std::pair<size_t, size_t> findNearestIndexPair(
      67                 :            :   const std::vector<float64_t> & accumulated_lengths, const float64_t target_length)
      68                 :            : {
      69                 :            :   // List size
      70         [ -  + ]:       1828 :   const auto N = accumulated_lengths.size();
      71                 :            : 
      72                 :            :   // Front
      73         [ +  + ]:       1828 :   if (target_length < accumulated_lengths.at(1)) {
      74                 :       1318 :     return std::make_pair(0, 1);
      75                 :            :   }
      76                 :            : 
      77                 :            :   // Back
      78         [ +  + ]:        510 :   if (target_length > accumulated_lengths.at(N - 2)) {
      79                 :        232 :     return std::make_pair(N - 2, N - 1);
      80                 :            :   }
      81                 :            : 
      82                 :            :   // Middle
      83         [ +  - ]:       5002 :   for (size_t i = 1; i < N; ++i) {
      84                 :            :     if (
      85   [ -  +  +  - ]:       5002 :       accumulated_lengths.at(i - 1) <= target_length &&
      86         [ +  + ]:       5002 :       target_length <= accumulated_lengths.at(i))
      87                 :            :     {
      88                 :            :       return std::make_pair(i - 1, i);
      89                 :            :     }
      90                 :            :   }
      91                 :            : 
      92                 :            :   // Throw an exception because this never happens
      93                 :            :   throw std::runtime_error(
      94         [ #  # ]:          0 :           "findNearestIndexPair(): No nearest point found.");
      95                 :            : }
      96                 :            : 
      97                 :        216 : std::vector<lanelet::BasicPoint3d> resamplePoints(
      98                 :            :   const lanelet::ConstLineString3d & line_string, const int32_t num_segments)
      99                 :            : {
     100                 :            :   // Calculate length
     101                 :            :   const auto line_length = lanelet::geometry::length(line_string);
     102                 :            : 
     103                 :            :   // Calculate accumulated lengths
     104                 :        216 :   const auto accumulated_lengths = calculateAccumulatedLengths(line_string);
     105                 :            : 
     106                 :            :   // Create each segment
     107                 :            :   std::vector<lanelet::BasicPoint3d> resampled_points;
     108         [ +  + ]:       2044 :   for (auto i = 0; i <= num_segments; ++i) {
     109                 :            :     // Find two nearest points
     110                 :       1828 :     const float64_t target_length = (static_cast<float64_t>(i) / num_segments) *
     111                 :       1828 :       static_cast<float64_t>(line_length);
     112   [ +  -  +  + ]:       1828 :     const auto index_pair = findNearestIndexPair(accumulated_lengths, target_length);
     113                 :            : 
     114                 :            :     // Apply linear interpolation
     115                 :            :     const lanelet::BasicPoint3d back_point = line_string[index_pair.first];
     116                 :            :     const lanelet::BasicPoint3d front_point = line_string[index_pair.second];
     117                 :            :     const auto direction_vector = (front_point - back_point);
     118                 :            : 
     119         [ -  + ]:       1828 :     const auto back_length = accumulated_lengths.at(index_pair.first);
     120                 :       1828 :     const auto front_length = accumulated_lengths.at(index_pair.second);
     121                 :       1828 :     const auto segment_length = front_length - back_length;
     122                 :            :     const auto target_point =
     123                 :       1828 :       back_point + (direction_vector * (target_length - back_length) / segment_length);
     124                 :            : 
     125                 :            :     // Add to list
     126         [ -  - ]:       1828 :     resampled_points.push_back(target_point);
     127                 :            :   }
     128                 :            : 
     129                 :        216 :   return resampled_points;
     130                 :            : }
     131                 :            : 
     132                 :        108 : lanelet::LineString3d generateFineCenterline(
     133                 :            :   const lanelet::ConstLanelet & lanelet_obj, const float64_t resolution)
     134                 :            : {
     135                 :            :   // Get length of longer border
     136                 :            :   const float64_t left_length =
     137         [ +  - ]:        216 :     static_cast<float64_t>(lanelet::geometry::length(lanelet_obj.leftBound()));
     138                 :            :   const float64_t right_length =
     139         [ +  - ]:        108 :     static_cast<float64_t>(lanelet::geometry::length(lanelet_obj.rightBound()));
     140         [ +  + ]:        108 :   const float64_t longer_distance = (left_length > right_length) ? left_length : right_length;
     141                 :            :   const int32_t num_segments =
     142         [ +  + ]:        109 :     std::max(static_cast<int32_t>(ceil(longer_distance / resolution)), 1);
     143                 :            : 
     144                 :            :   // Resample points
     145         [ +  - ]:        216 :   const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments);
     146   [ +  -  +  -  :        216 :   const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments);
                   -  - ]
     147                 :            : 
     148                 :            :   // Create centerline
     149   [ +  -  -  - ]:        216 :   lanelet::LineString3d centerline(lanelet::utils::getId());
     150         [ +  + ]:       1022 :   for (size_t i = 0; i < static_cast<size_t>(num_segments + 1); i++) {
     151                 :            :     // Add ID for the average point of left and right
     152                 :            :     const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2.0;
     153                 :            :     const lanelet::Point3d center_point(
     154                 :            :       lanelet::utils::getId(), center_basic_point.x(), center_basic_point.y(),
     155   [ +  -  -  - ]:        914 :       center_basic_point.z());
     156         [ +  - ]:        914 :     centerline.push_back(center_point);
     157                 :            :   }
     158                 :        108 :   return centerline;
     159                 :            : }
     160                 :            : 
     161                 :          2 : void overwriteLaneletsCenterline(
     162                 :            :   lanelet::LaneletMapPtr lanelet_map,
     163                 :            :   const autoware::common::types::bool8_t force_overwrite)
     164                 :            : {
     165         [ +  + ]:        105 :   for (auto & lanelet_obj : lanelet_map->laneletLayer) {
     166   [ -  +  -  - ]:        103 :     if (force_overwrite || !lanelet_obj.hasCustomCenterline()) {
     167                 :        103 :       const auto fine_center_line = generateFineCenterline(lanelet_obj, 2.0);
     168         [ +  - ]:        103 :       lanelet_obj.setCenterline(fine_center_line);
     169                 :            :     }
     170                 :            :   }
     171                 :          2 : }
     172                 :            : 
     173                 :            : }  // namespace had_map_utils
     174                 :            : }  // namespace common
     175                 :            : }  // namespace autoware