prediction_map.cc

浏览该文件的文档.

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
 
#include "modules/prediction/common/prediction_map.h"
 
#include <algorithm>
#include <limits>
#include <unordered_set>
#include <utility>
 
#include "modules/prediction/common/prediction_gflags.h"
 
namespace apollo {
namespace prediction {
 
using apollo::common::math::Polygon2d;
using apollo::common::math::Vec2d;
using apollo::hdmap::HDMapUtil;
using apollo::hdmap::JunctionInfo;
using apollo::hdmap::LaneInfo;
using apollo::hdmap::MapPathPoint;
using apollo::hdmap::OverlapInfo;
using apollo::hdmap::PNCJunctionInfo;
 
bool PredictionMap::Ready() { return HDMapUtil::BaseMapPtr() != nullptr; }
 
Eigen::Vector2d PredictionMap::PositionOnLane(
    const std::shared_ptr<const LaneInfo> lane_info, const double s) {
  common::PointENU point = lane_info->GetSmoothPoint(s);
  return {point.x(), point.y()};
}
 
double PredictionMap::HeadingOnLane(
    const std::shared_ptr<const LaneInfo> lane_info, const double s) {
  return lane_info->Heading(s);
}
 
double PredictionMap::CurvatureOnLane(const std::string& lane_id,
                                      const double s) {
  std::shared_ptr<const hdmap::LaneInfo> lane_info = LaneById(lane_id);
  if (lane_info == nullptr) {
    AERROR << "Null lane_info ptr found";
    return 0.0;
  }
  return lane_info->Curvature(s);
}
 
double PredictionMap::LaneTotalWidth(
    const std::shared_ptr<const hdmap::LaneInfo> lane_info, const double s) {
  double left = 0.0;
  double right = 0.0;
  lane_info->GetWidth(s, &left, &right);
  return left + right;
}
 
std::shared_ptr<const LaneInfo> PredictionMap::LaneById(
    const std::string& str_id) {
  return HDMapUtil::BaseMap().GetLaneById(hdmap::MakeMapId(str_id));
}
 
std::shared_ptr<const JunctionInfo> PredictionMap::JunctionById(
    const std::string& str_id) {
  return HDMapUtil::BaseMap().GetJunctionById(hdmap::MakeMapId(str_id));
}
 
std::shared_ptr<const OverlapInfo> PredictionMap::OverlapById(
    const std::string& str_id) {
  return HDMapUtil::BaseMap().GetOverlapById(hdmap::MakeMapId(str_id));
}
 
bool PredictionMap::GetProjection(
    const Eigen::Vector2d& pos, const std::shared_ptr<const LaneInfo> lane_info,
    double* s, double* l) {
  if (lane_info == nullptr) {
    return false;
  }
  return lane_info->GetProjection({pos.x(), pos.y()}, s, l);
}
 
bool PredictionMap::HasNearbyLane(const double x, const double y,
                                  const double radius) {
  common::PointENU point_enu;
  point_enu.set_x(x);
  point_enu.set_y(y);
  std::vector<std::shared_ptr<const LaneInfo>> lanes;
  HDMapUtil::BaseMap().GetLanes(point_enu, radius, &lanes);
  return (!lanes.empty());
}
 
bool PredictionMap::ProjectionFromLane(
    std::shared_ptr<const LaneInfo> lane_info, const double s,
    MapPathPoint* path_point) {
  if (lane_info == nullptr) {
    return false;
  }
  const common::PointENU point = lane_info->GetSmoothPoint(s);
  const double heading = HeadingOnLane(lane_info, s);
  path_point->set_x(point.x());
  path_point->set_y(point.y());
  path_point->set_heading(heading);
  return true;
}
 
bool PredictionMap::IsVirtualLane(const std::string& lane_id) {
  std::shared_ptr<const LaneInfo> lane_info =
      HDMapUtil::BaseMap().GetLaneById(hdmap::MakeMapId(lane_id));
  if (lane_info == nullptr) {
    return false;
  }
  const hdmap::Lane& lane = lane_info->lane();
  bool left_virtual = lane.has_left_boundary() &&
                      lane.left_boundary().has_virtual_() &&
                      lane.left_boundary().virtual_();
  bool right_virtual = lane.has_right_boundary() &&
                       lane.right_boundary().has_virtual_() &&
                       lane.right_boundary().virtual_();
  return left_virtual && right_virtual;
}
 
bool PredictionMap::OnVirtualLane(const Eigen::Vector2d& point,
                                  const double radius) {
  std::vector<std::shared_ptr<const LaneInfo>> lanes;
  common::PointENU hdmap_point;
  hdmap_point.set_x(point[0]);
  hdmap_point.set_y(point[1]);
  HDMapUtil::BaseMap().GetLanes(hdmap_point, radius, &lanes);
  for (const auto& lane : lanes) {
    if (IsVirtualLane(lane->id().id())) {
      return true;
    }
  }
  return false;
}
 
void PredictionMap::OnLane(
    const std::vector<std::shared_ptr<const LaneInfo>>& prev_lanes,
    const Eigen::Vector2d& point, const double heading, const double radius,
    const bool on_lane, const int max_num_lane,
    const double max_lane_angle_diff,
    std::vector<std::shared_ptr<const LaneInfo>>* lanes) {
  std::vector<std::shared_ptr<const LaneInfo>> candidate_lanes;
 
  common::PointENU hdmap_point;
  hdmap_point.set_x(point.x());
  hdmap_point.set_y(point.y());
  if (HDMapUtil::BaseMap().GetLanesWithHeading(hdmap_point, radius, heading,
                                               max_lane_angle_diff,
                                               &candidate_lanes) != 0) {
    return;
  }
 
  std::vector<std::pair<std::shared_ptr<const LaneInfo>, double>> lane_pairs;
  for (const auto& candidate_lane : candidate_lanes) {
    if (candidate_lane == nullptr) {
      continue;
    }
    if (on_lane && !candidate_lane->IsOnLane({point.x(), point.y()})) {
      continue;
    }
    if (!FLAGS_use_navigation_mode &&
        !IsIdenticalLane(candidate_lane, prev_lanes) &&
        !IsSuccessorLane(candidate_lane, prev_lanes) &&
        !IsLeftNeighborLane(candidate_lane, prev_lanes) &&
        !IsRightNeighborLane(candidate_lane, prev_lanes)) {
      continue;
    }
    double distance = 0.0;
    common::PointENU nearest_point =
        candidate_lane->GetNearestPoint({point.x(), point.y()}, &distance);
    double nearest_point_heading = PathHeading(candidate_lane, nearest_point);
    double diff =
        std::fabs(common::math::AngleDiff(heading, nearest_point_heading));
    if (diff <= max_lane_angle_diff) {
      lane_pairs.emplace_back(candidate_lane, diff);
    }
  }
  if (lane_pairs.empty()) {
    return;
  }
  std::sort(lane_pairs.begin(), lane_pairs.end(),
            [](const std::pair<std::shared_ptr<const LaneInfo>, double>& p1,
               const std::pair<std::shared_ptr<const LaneInfo>, double>& p2) {
              return p1.second < p2.second;
            });
 
  int count = 0;
  for (const auto& lane_pair : lane_pairs) {
    lanes->push_back(lane_pair.first);
    ++count;
    if (count >= max_num_lane) {
      break;
    }
  }
}
 
std::shared_ptr<const LaneInfo> PredictionMap::GetMostLikelyCurrentLane(
    const common::PointENU& position, const double radius, const double heading,
    const double angle_diff_threshold) {
  std::vector<std::shared_ptr<const LaneInfo>> candidate_lanes;
  if (HDMapUtil::BaseMap().GetLanesWithHeading(position, radius, heading,
                                               angle_diff_threshold,
                                               &candidate_lanes) != 0) {
    return nullptr;
  }
  double min_angle_diff = 2.0 * M_PI;
  std::shared_ptr<const LaneInfo> curr_lane_ptr = nullptr;
  for (auto candidate_lane : candidate_lanes) {
    if (!candidate_lane->IsOnLane({position.x(), position.y()})) {
      continue;
    }
    double distance = 0.0;
    common::PointENU nearest_point = candidate_lane->GetNearestPoint(
        {position.x(), position.y()}, &distance);
    double nearest_point_heading = PathHeading(candidate_lane, nearest_point);
    double angle_diff =
        std::fabs(common::math::AngleDiff(heading, nearest_point_heading));
    if (angle_diff < min_angle_diff) {
      min_angle_diff = angle_diff;
      curr_lane_ptr = candidate_lane;
    }
  }
  return curr_lane_ptr;
}
 
bool PredictionMap::IsProjectionApproximateWithinLane(
    const Eigen::Vector2d& ego_position, const std::string& lane_id) {
  auto ptr_lane = LaneById(lane_id);
  const auto& lane_points = ptr_lane->points();
  if (lane_points.size() < 2) {
    return false;
  }
 
  const auto& start_point = lane_points.front();
  const auto& end_point = lane_points.back();
 
  auto lane_vec = end_point - start_point;
 
  auto approx_lane_length = lane_vec.Length();
  if (approx_lane_length < 1.0e-3) {
    return false;
  }
 
  auto dist_vec =
      common::math::Vec2d(ego_position.x(), ego_position.y()) - start_point;
 
  auto projection_length = dist_vec.InnerProd(lane_vec) / approx_lane_length;
 
  if (projection_length < 0.0 || projection_length > approx_lane_length) {
    return false;
  }
  return true;
}
 
bool PredictionMap::NearJunction(const Eigen::Vector2d& point,
                                 const double radius) {
  common::PointENU hdmap_point;
  hdmap_point.set_x(point.x());
  hdmap_point.set_y(point.y());
  std::vector<std::shared_ptr<const JunctionInfo>> junctions;
  HDMapUtil::BaseMap().GetJunctions(hdmap_point, radius, &junctions);
  return junctions.size() > 0;
}
 
bool PredictionMap::IsPointInJunction(
    const double x, const double y,
    const std::shared_ptr<const JunctionInfo> junction_info_ptr) {
  if (junction_info_ptr == nullptr) {
    return false;
  }
  const Polygon2d& polygon = junction_info_ptr->polygon();
  return polygon.IsPointIn({x, y});
}
 
std::vector<std::shared_ptr<const JunctionInfo>> PredictionMap::GetJunctions(
    const Eigen::Vector2d& point, const double radius) {
  common::PointENU hdmap_point;
  hdmap_point.set_x(point.x());
  hdmap_point.set_y(point.y());
  std::vector<std::shared_ptr<const JunctionInfo>> junctions;
  HDMapUtil::BaseMap().GetJunctions(hdmap_point, radius, &junctions);
  return junctions;
}
 
std::vector<std::shared_ptr<const PNCJunctionInfo>>
PredictionMap::GetPNCJunctions(const Eigen::Vector2d& point,
                               const double radius) {
  common::PointENU hdmap_point;
  hdmap_point.set_x(point.x());
  hdmap_point.set_y(point.y());
  std::vector<std::shared_ptr<const PNCJunctionInfo>> junctions;
  HDMapUtil::BaseMap().GetPNCJunctions(hdmap_point, radius, &junctions);
  return junctions;
}
 
bool PredictionMap::InJunction(const Eigen::Vector2d& point,
                               const double radius) {
  auto junction_infos = GetJunctions(point, radius);
  if (junction_infos.empty()) {
    return false;
  }
  for (const auto junction_info : junction_infos) {
    if (junction_info == nullptr || !junction_info->junction().has_polygon()) {
      continue;
    }
    std::vector<Vec2d> vertices;
    for (const auto& point : junction_info->junction().polygon().point()) {
      vertices.emplace_back(point.x(), point.y());
    }
    if (vertices.size() < 3) {
      continue;
    }
    Polygon2d junction_polygon{vertices};
    if (junction_polygon.IsPointIn({point.x(), point.y()})) {
      return true;
    }
  }
  return false;
}
 
bool PredictionMap::IsLaneInJunction(
    const std::shared_ptr<const LaneInfo> lane_info,
    const std::string& junction_id) {
  if (lane_info == nullptr) {
    return false;
  }
 
  // first, check whether the lane is virtual
  if (!PredictionMap::IsVirtualLane(lane_info->lane().id().id())) {
    return false;
  }
 
  // second, use junction from lane
  if (lane_info->lane().has_junction_id() &&
      lane_info->lane().junction_id().id() == junction_id) {
    return true;
  }
 
  // third, use junction from road
  auto ptr_road_info = HDMapUtil::BaseMap().GetRoadById(lane_info->road_id());
  if (ptr_road_info->has_junction_id() &&
      ptr_road_info->junction_id().id() == junction_id) {
    return true;
  }
 
  return false;
}
 
double PredictionMap::PathHeading(std::shared_ptr<const LaneInfo> lane_info,
                                  const common::PointENU& point) {
  double s = 0.0;
  double l = 0.0;
  if (lane_info->GetProjection({point.x(), point.y()}, &s, &l)) {
    return HeadingOnLane(lane_info, s);
  } else {
    return M_PI;
  }
}
 
bool PredictionMap::SmoothPointFromLane(const std::string& id, const double s,
                                        const double l, Eigen::Vector2d* point,
                                        double* heading) {
  if (point == nullptr || heading == nullptr) {
    return false;
  }
  std::shared_ptr<const LaneInfo> lane = LaneById(id);
  common::PointENU hdmap_point = lane->GetSmoothPoint(s);
  *heading = PathHeading(lane, hdmap_point);
  point->x() = hdmap_point.x() - std::sin(*heading) * l;
  point->y() = hdmap_point.y() + std::cos(*heading) * l;
  return true;
}
 
void PredictionMap::NearbyLanesByCurrentLanes(
    const Eigen::Vector2d& point, const double heading, const double radius,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes,
    const int max_num_lane,
    std::vector<std::shared_ptr<const LaneInfo>>* nearby_lanes) {
  if (lanes.empty()) {
    std::vector<std::shared_ptr<const LaneInfo>> prev_lanes;
    OnLane(prev_lanes, point, heading, radius, false, max_num_lane,
           FLAGS_max_lane_angle_diff, nearby_lanes);
  } else {
    std::unordered_set<std::string> lane_ids;
    for (auto& lane_ptr : lanes) {
      if (lane_ptr == nullptr) {
        continue;
      }
      for (auto& lane_id : lane_ptr->lane().left_neighbor_forward_lane_id()) {
        const std::string& id = lane_id.id();
        if (lane_ids.find(id) != lane_ids.end()) {
          continue;
        }
        std::shared_ptr<const LaneInfo> nearby_lane = LaneById(id);
        double s = -1.0;
        double l = 0.0;
        GetProjection(point, nearby_lane, &s, &l);
        if (s < 0.0 || s >= nearby_lane->total_length() ||
            std::fabs(l) > radius) {
          continue;
        }
        lane_ids.insert(id);
        nearby_lanes->push_back(nearby_lane);
      }
      for (auto& lane_id : lane_ptr->lane().right_neighbor_forward_lane_id()) {
        const std::string& id = lane_id.id();
        if (lane_ids.find(id) != lane_ids.end()) {
          continue;
        }
        std::shared_ptr<const LaneInfo> nearby_lane = LaneById(id);
        double s = -1.0;
        double l = 0.0;
        GetProjection(point, nearby_lane, &s, &l);
        if (s < 0.0 || s >= nearby_lane->total_length() ||
            std::fabs(l) > radius) {
          continue;
        }
        lane_ids.insert(id);
        nearby_lanes->push_back(nearby_lane);
      }
    }
  }
}
 
std::shared_ptr<const LaneInfo> PredictionMap::GetLeftNeighborLane(
    const std::shared_ptr<const LaneInfo>& ptr_ego_lane,
    const Eigen::Vector2d& ego_position, const double threshold) {
  std::vector<std::string> neighbor_ids;
  for (const auto& lane_id :
       ptr_ego_lane->lane().left_neighbor_forward_lane_id()) {
    neighbor_ids.push_back(lane_id.id());
  }
 
  return GetNeighborLane(ptr_ego_lane, ego_position, neighbor_ids, threshold);
}
 
std::shared_ptr<const LaneInfo> PredictionMap::GetRightNeighborLane(
    const std::shared_ptr<const LaneInfo>& ptr_ego_lane,
    const Eigen::Vector2d& ego_position, const double threshold) {
  std::vector<std::string> neighbor_ids;
  for (const auto& lane_id :
       ptr_ego_lane->lane().right_neighbor_forward_lane_id()) {
    neighbor_ids.push_back(lane_id.id());
  }
 
  return GetNeighborLane(ptr_ego_lane, ego_position, neighbor_ids, threshold);
}
 
std::shared_ptr<const LaneInfo> PredictionMap::GetNeighborLane(
    const std::shared_ptr<const LaneInfo>& ptr_ego_lane,
    const Eigen::Vector2d& ego_position,
    const std::vector<std::string>& neighbor_lane_ids, const double threshold) {
  double ego_s = 0.0;
  double ego_l = 0.0;
  GetProjection(ego_position, ptr_ego_lane, &ego_s, &ego_l);
 
  double s_diff_min = std::numeric_limits<double>::max();
  std::shared_ptr<const LaneInfo> ptr_lane_min = nullptr;
 
  for (auto& lane_id : neighbor_lane_ids) {
    std::shared_ptr<const LaneInfo> ptr_lane = LaneById(lane_id);
    double s = -1.0;
    double l = 0.0;
    GetProjection(ego_position, ptr_lane, &s, &l);
 
    double s_diff = std::fabs(s - ego_s);
    if (s_diff < s_diff_min) {
      s_diff_min = s_diff;
      ptr_lane_min = ptr_lane;
    }
  }
 
  if (s_diff_min > threshold) {
    return nullptr;
  }
  return ptr_lane_min;
}
 
std::vector<std::string> PredictionMap::NearbyLaneIds(
    const Eigen::Vector2d& point, const double radius) {
  std::vector<std::string> lane_ids;
  std::vector<std::shared_ptr<const LaneInfo>> lanes;
  common::PointENU hdmap_point;
  hdmap_point.set_x(point[0]);
  hdmap_point.set_y(point[1]);
  HDMapUtil::BaseMap().GetLanes(hdmap_point, radius, &lanes);
  for (const auto& lane : lanes) {
    lane_ids.push_back(lane->id().id());
  }
  return lane_ids;
}
 
bool PredictionMap::IsLeftNeighborLane(
    std::shared_ptr<const LaneInfo> target_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (target_lane == nullptr) {
    return false;
  }
  for (const auto& left_lane_id :
       curr_lane->lane().left_neighbor_forward_lane_id()) {
    if (target_lane->id().id() == left_lane_id.id()) {
      return true;
    }
  }
  return false;
}
 
bool PredictionMap::IsLeftNeighborLane(
    std::shared_ptr<const LaneInfo> target_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.empty()) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsLeftNeighborLane(target_lane, lane)) {
      return true;
    }
  }
  return false;
}
 
bool PredictionMap::IsRightNeighborLane(
    std::shared_ptr<const LaneInfo> target_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (target_lane == nullptr) {
    return false;
  }
  for (auto& right_lane_id :
       curr_lane->lane().right_neighbor_forward_lane_id()) {
    if (target_lane->id().id() == right_lane_id.id()) {
      return true;
    }
  }
  return false;
}
 
bool PredictionMap::IsRightNeighborLane(
    std::shared_ptr<const LaneInfo> target_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.empty()) {
    return true;
  }
  for (const auto& lane : lanes) {
    if (IsRightNeighborLane(target_lane, lane)) {
      return true;
    }
  }
  return false;
}
 
bool PredictionMap::IsSuccessorLane(std::shared_ptr<const LaneInfo> target_lane,
                                    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (target_lane == nullptr) {
    return false;
  }
  for (const auto& successor_lane_id : curr_lane->lane().successor_id()) {
    if (target_lane->id().id() == successor_lane_id.id()) {
      return true;
    }
  }
  return false;
}
 
bool PredictionMap::IsSuccessorLane(
    std::shared_ptr<const LaneInfo> target_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.empty()) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsSuccessorLane(target_lane, lane)) {
      return true;
    }
  }
  return false;
}
 
bool PredictionMap::IsPredecessorLane(
    std::shared_ptr<const LaneInfo> target_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (target_lane == nullptr) {
    return false;
  }
  for (const auto& predecessor_lane_id : curr_lane->lane().predecessor_id()) {
    if (target_lane->id().id() == predecessor_lane_id.id()) {
      return true;
    }
  }
  return false;
}
 
bool PredictionMap::IsPredecessorLane(
    std::shared_ptr<const LaneInfo> target_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.empty()) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsPredecessorLane(target_lane, lane)) {
      return true;
    }
  }
  return false;
}
 
bool PredictionMap::IsIdenticalLane(std::shared_ptr<const LaneInfo> other_lane,
                                    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr || other_lane == nullptr) {
    return true;
  }
  return other_lane->id().id() == curr_lane->id().id();
}
 
bool PredictionMap::IsIdenticalLane(
    std::shared_ptr<const LaneInfo> other_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.empty()) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsIdenticalLane(other_lane, lane)) {
      return true;
    }
  }
  return false;
}
 
int PredictionMap::LaneTurnType(const std::string& lane_id) {
  std::shared_ptr<const LaneInfo> lane = LaneById(lane_id);
  if (lane != nullptr) {
    return static_cast<int>(lane->lane().turn());
  }
  return 1;
}
 
std::vector<std::shared_ptr<const LaneInfo>> PredictionMap::GetNearbyLanes(
    const common::PointENU& position, const double nearby_radius) {
  ACHECK(position.has_x() && position.has_y() && position.has_z());
  ACHECK(nearby_radius > 0.0);
 
  std::vector<std::shared_ptr<const LaneInfo>> nearby_lanes;
 
  HDMapUtil::BaseMap().GetLanes(position, nearby_radius, &nearby_lanes);
  return nearby_lanes;
}
 
std::shared_ptr<const LaneInfo> PredictionMap::LaneWithSmallestAverageCurvature(
    const std::vector<std::shared_ptr<const LaneInfo>>& lane_infos) {
  ACHECK(!lane_infos.empty());
  size_t sample_size = FLAGS_sample_size_for_average_lane_curvature;
  std::shared_ptr<const hdmap::LaneInfo> selected_lane_info = lane_infos[0];
  if (selected_lane_info == nullptr) {
    AERROR << "Lane Vector first element: selected_lane_info is nullptr.";
    return nullptr;
  }
  double smallest_curvature =
      AverageCurvature(selected_lane_info->id().id(), sample_size);
  for (size_t i = 1; i < lane_infos.size(); ++i) {
    std::shared_ptr<const hdmap::LaneInfo> lane_info = lane_infos[i];
    if (lane_info == nullptr) {
      AWARN << "Lane vector element: one lane_info is nullptr.";
      continue;
    }
    double curvature = AverageCurvature(lane_info->id().id(), sample_size);
    if (curvature < smallest_curvature) {
      smallest_curvature = curvature;
      selected_lane_info = lane_info;
    }
  }
  return selected_lane_info;
}
 
double PredictionMap::AverageCurvature(const std::string& lane_id,
                                       const size_t sample_size) {
  CHECK_GT(sample_size, 0U);
  std::shared_ptr<const hdmap::LaneInfo> lane_info_ptr =
      PredictionMap::LaneById(lane_id);
  if (lane_info_ptr == nullptr) {
    return 0.0;
  }
  double lane_length = lane_info_ptr->total_length();
  double s_gap = lane_length / static_cast<double>(sample_size);
  double curvature_sum = 0.0;
  for (size_t i = 0; i < sample_size; ++i) {
    double s = s_gap * static_cast<double>(i);
    curvature_sum += std::abs(PredictionMap::CurvatureOnLane(lane_id, s));
  }
  return curvature_sum / static_cast<double>(sample_size);
}
 
}  // namespace prediction
}  // namespace apollo