open_space_roi_util.cc

浏览该文件的文档.

/******************************************************************************
 * Copyright 2023 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/planning/planning_open_space/utils/open_space_roi_util.h"
 
#include <limits>
#include <memory>
#include <numeric>
#include <utility>
 
#include "modules/common/util/point_factory.h"
 
namespace apollo {
namespace planning {
 
using apollo::common::math::Box2d;
using apollo::common::math::Vec2d;
 
bool OpenSpaceRoiUtil::FormulateBoundaryConstraints(
    OpenSpaceInfo *const open_space_info) {
  auto &obstacles_vertices_vec =
      *open_space_info->mutable_obstacles_vertices_vec();
  auto &obstacles_edges_num_vec =
      *open_space_info->mutable_obstacles_edges_num();
  size_t obstacles_vertices_vec_num = obstacles_vertices_vec.size();
  obstacles_edges_num_vec.resize(obstacles_vertices_vec_num, 1);
  for (size_t i = 0; i < obstacles_vertices_vec_num; i++) {
    CHECK_GT(obstacles_vertices_vec[i].size(), 1U);
    obstacles_edges_num_vec(i, 0) =
        static_cast<int>(obstacles_vertices_vec[i].size()) - 1;
  }
  open_space_info->set_obstacles_num(obstacles_vertices_vec.size());
  // Transform vertices into the form of Ax>b
  if (!LoadObstacleInHyperPlanes(open_space_info)) {
    AERROR << "fail at LoadObstacleInHyperPlanes()";
    return false;
  }
  return true;
}
 
void OpenSpaceRoiUtil::GetRoiXYBoundary(
    const std::vector<std::vector<common::math::Vec2d>> &roi_parking_boundary,
    std::vector<double> *const XYBoundary) {
  XYBoundary->clear();
  XYBoundary->resize(4);  // x_min x_max y_min y_max
  *XYBoundary = {std::numeric_limits<double>::max(),
                 std::numeric_limits<double>::lowest(),
                 std::numeric_limits<double>::max(),
                 std::numeric_limits<double>::lowest()};
  for (auto &bound_vec : roi_parking_boundary) {
    for (auto &pt : bound_vec) {
      XYBoundary->at(0) = std::min<double>(XYBoundary->at(0), pt.x());
      XYBoundary->at(1) = std::max<double>(XYBoundary->at(1), pt.x());
      XYBoundary->at(2) = std::min<double>(XYBoundary->at(2), pt.y());
      XYBoundary->at(3) = std::max<double>(XYBoundary->at(3), pt.y());
    }
  }
}
 
void OpenSpaceRoiUtil::TransformByOriginPoint(
        const common::math::Vec2d &origin_point,
        const double heading,
        std::vector<std::vector<common::math::Vec2d>> *roi_parking_boundary) {
  for (auto &bound_vec : *roi_parking_boundary) {
    TransformByOriginPoint(origin_point, heading, &bound_vec);
  }
}
 
void OpenSpaceRoiUtil::TransformByOriginPoint(
        const common::math::Vec2d &origin_point,
        const double heading,
        std::vector<common::math::Vec2d> *roi_parking_boundary) {
  for (auto &point : *roi_parking_boundary) {
    TransformByOriginPoint(origin_point, heading, &point);
  }
}
 
void OpenSpaceRoiUtil::TransformByOriginPoint(
        const common::math::Vec2d &origin_point,
        const double heading,
        common::math::Vec2d *point) {
  *point -= origin_point;
  point->SelfRotate(-heading);
}
 
bool OpenSpaceRoiUtil::LoadObstacles(
        const double filtering_distance,
        const double perception_obstacle_buffer,
        const std::vector<double> &xy_boundary,
        Frame *const frame,
        std::vector<std::vector<common::math::Vec2d>> *const
        roi_parking_boundary) {
  auto obstacles_by_frame = frame->GetObstacleList();
  size_t perception_obstacles_num = 0;
  // load vertices for perception obstacles(repeat the first vertice at the
  // last to form closed convex hull)
  for (const auto &obstacle : obstacles_by_frame->Items()) {
    if (FilterOutObstacle(filtering_distance, *frame, xy_boundary, *obstacle)) {
      continue;
    }
    ++perception_obstacles_num;
 
    Box2d original_box = obstacle->PerceptionBoundingBox();
    original_box.LongitudinalExtend(perception_obstacle_buffer);
    original_box.LateralExtend(perception_obstacle_buffer);
 
    // TODO(Jinyun): Check correctness of ExpandByDistance() in polygon
    // Polygon2d buffered_box(original_box);
    // buffered_box = buffered_box.ExpandByDistance(
    //    config_.perception_obstacle_buffer());
    // TODO(Runxin): Rotate from origin instead
    // original_box.RotateFromCenter(-1.0 * origin_heading);
    std::vector<Vec2d> vertices_ccw = original_box.GetAllCorners();
    std::vector<Vec2d> vertices_cw;
    while (!vertices_ccw.empty()) {
      auto current_corner_pt = vertices_ccw.back();
      vertices_cw.push_back(current_corner_pt);
      vertices_ccw.pop_back();
    }
    // As the perception obstacle is a closed convex set, the first vertice
    // is repeated at the end of the vector to help transform all four edges
    // to inequality constraint
    vertices_cw.push_back(vertices_cw.front());
    roi_parking_boundary->push_back(vertices_cw);
  }
 
  return true;
}
 
bool OpenSpaceRoiUtil::FilterOutObstacle(
        const double filtering_distance,
        const Frame &frame,
        const std::vector<double> &xy_boundary,
        const Obstacle &obstacle) {
  if (obstacle.IsVirtual()) {
    return true;
  }
 
  const auto &open_space_info = frame.open_space_info();
  const auto &obstacle_box = obstacle.PerceptionBoundingBox();
  auto obstacle_center_xy = obstacle_box.center();
 
  // xy_boundary in xmin, xmax, ymin, ymax.
  if (obstacle_center_xy.x() < xy_boundary[0] ||
      obstacle_center_xy.x() > xy_boundary[1] ||
      obstacle_center_xy.y() < xy_boundary[2] ||
      obstacle_center_xy.y() > xy_boundary[3]) {
    return true;
  }
 
  // Translate the end pose back to world frame with endpose in x, y, phi, v
  const auto &end_pose = open_space_info.open_space_end_pose();
  Vec2d end_pose_x_y(end_pose[0], end_pose[1]);
  const auto &vehicle_state = frame.vehicle_state();
  // Get vehicle state
  Vec2d vehicle_x_y(vehicle_state.x(), vehicle_state.y());
 
  // Use vehicle position and end position to filter out obstacle
  const double vehicle_center_to_obstacle =
      obstacle_box.DistanceTo(vehicle_x_y);
  const double end_pose_center_to_obstacle =
      obstacle_box.DistanceTo(end_pose_x_y);
  if (vehicle_center_to_obstacle > filtering_distance &&
      end_pose_center_to_obstacle > filtering_distance) {
    return true;
  }
  return false;
}
 
bool OpenSpaceRoiUtil::LoadObstacleInHyperPlanes(
    OpenSpaceInfo *const open_space_info) {
  *(open_space_info->mutable_obstacles_A()) =
      Eigen::MatrixXd::Zero(open_space_info->obstacles_edges_num().sum(), 2);
  *(open_space_info->mutable_obstacles_b()) =
      Eigen::MatrixXd::Zero(open_space_info->obstacles_edges_num().sum(), 1);
  // vertices using H-representation
  if (!GetHyperPlanes(open_space_info->obstacles_num(),
                      open_space_info->obstacles_edges_num(),
                      open_space_info->obstacles_vertices_vec(),
                      open_space_info->mutable_obstacles_A(),
                      open_space_info->mutable_obstacles_b())) {
    AERROR << "Fail to present obstacle in hyperplane";
    return false;
  }
  return true;
}
 
bool OpenSpaceRoiUtil::GetHyperPlanes(
        const size_t &obstacles_num,
        const Eigen::MatrixXi &obstacles_edges_num,
        const std::vector<std::vector<Vec2d>> &obstacles_vertices_vec,
        Eigen::MatrixXd *A_all,
        Eigen::MatrixXd *b_all) {
  if (obstacles_num != obstacles_vertices_vec.size()) {
    AERROR << "obstacles_num != obstacles_vertices_vec.size()";
    return false;
  }
 
  A_all->resize(obstacles_edges_num.sum(), 2);
  b_all->resize(obstacles_edges_num.sum(), 1);
 
  int counter = 0;
  double kEpsilon = 1.0e-5;
  // start building H representation
  for (size_t i = 0; i < obstacles_num; ++i) {
    size_t current_vertice_num = obstacles_edges_num(i, 0);
    Eigen::MatrixXd A_i(current_vertice_num, 2);
    Eigen::MatrixXd b_i(current_vertice_num, 1);
 
    // take two subsequent vertices, and computer hyperplane
    for (size_t j = 0; j < current_vertice_num; ++j) {
      Vec2d v1 = obstacles_vertices_vec[i][j];
      Vec2d v2 = obstacles_vertices_vec[i][j + 1];
 
      Eigen::MatrixXd A_tmp(2, 1), b_tmp(1, 1), ab(2, 1);
      // find hyperplane passing through v1 and v2
      if (std::abs(v1.x() - v2.x()) < kEpsilon) {
        if (v2.y() < v1.y()) {
          A_tmp << 1, 0;
          b_tmp << v1.x();
        } else {
          A_tmp << -1, 0;
          b_tmp << -v1.x();
        }
      } else if (std::abs(v1.y() - v2.y()) < kEpsilon) {
        if (v1.x() < v2.x()) {
          A_tmp << 0, 1;
          b_tmp << v1.y();
        } else {
          A_tmp << 0, -1;
          b_tmp << -v1.y();
        }
      } else {
        Eigen::MatrixXd tmp1(2, 2);
        tmp1 << v1.x(), 1, v2.x(), 1;
        Eigen::MatrixXd tmp2(2, 1);
        tmp2 << v1.y(), v2.y();
        ab = tmp1.inverse() * tmp2;
        double a = ab(0, 0);
        double b = ab(1, 0);
 
        if (v1.x() < v2.x()) {
          A_tmp << -a, 1;
          b_tmp << b;
        } else {
          A_tmp << a, -1;
          b_tmp << -b;
        }
      }
 
      // store vertices
      A_i.block(j, 0, 1, 2) = A_tmp.transpose();
      b_i.block(j, 0, 1, 1) = b_tmp;
    }
 
    A_all->block(counter, 0, A_i.rows(), 2) = A_i;
    b_all->block(counter, 0, b_i.rows(), 1) = b_i;
    counter += static_cast<int>(current_vertice_num);
  }
  return true;
}
 
void OpenSpaceRoiUtil::TransformByOriginPoint(
        const Vec2d &origin_point,
        const double &origin_heading,
        OpenSpaceInfo *open_space_info) {
  open_space_info->set_origin_heading(origin_heading);
  *open_space_info->mutable_origin_point() = origin_point;
  auto obstacles_vertices_vec =
      open_space_info->mutable_obstacles_vertices_vec();
  TransformByOriginPoint(origin_point, origin_heading, obstacles_vertices_vec);
  auto end_pose_vec = open_space_info->mutable_open_space_end_pose();
  Vec2d end_pose(end_pose_vec->at(0), end_pose_vec->at(1));
  TransformByOriginPoint(origin_point, origin_heading, &end_pose);
  end_pose_vec->at(0) = end_pose.x();
  end_pose_vec->at(1) = end_pose.y();
  end_pose_vec->at(2) -= origin_heading;
  auto xy_boundary = open_space_info->mutable_ROI_xy_boundary();
  GetRoiXYBoundary(*obstacles_vertices_vec, xy_boundary);
}
bool OpenSpaceRoiUtil::IsPolygonClockwise(const std::vector<Vec2d> &polygon) {
  double s = 0;
  for (size_t i = 0; i < polygon.size(); i++) {
    if (i + 1 < polygon.size()) {
      s += polygon.at(i).x() * polygon.at(i + 1).y() -
           polygon.at(i).y() * polygon.at(i + 1).x();
    } else {
      s += polygon.at(i).x() * polygon.at(0).y() -
           polygon.at(i).y() * polygon.at(0).x();
    }
  }
  if (s < 0) {
    return true;
  } else {
    return false;
  }
}
 
bool OpenSpaceRoiUtil::AdjustPointsOrderToClockwise(
    std::vector<Vec2d> *polygon) {
  if (!IsPolygonClockwise(*polygon)) {
    // counter clockwise reverse it
    ADEBUG << "point is anticlockwise,reverse";
    std::reverse(polygon->begin(), polygon->end());
    return true;
  } else {
    return false;
  }
}
// make points order as left top ,right top, right bottom, left bottom
bool OpenSpaceRoiUtil::UpdateParkingPointsOrder(
    const apollo::hdmap::Path &nearby_path, std::vector<Vec2d> *points) {
  AdjustPointsOrderToClockwise(points);
  double min_dist = std::numeric_limits<double>::max();
  size_t min_index = 0;
  for (size_t i = 0; i < points->size(); i++) {
    double s = 0.0, l = 0.0;
    nearby_path.GetProjection(points->at(i), &s, &l);
    ADEBUG << std::fixed << points->at(i).x() << "," << points->at(i).y()
                         << "sl" << s << "," << l;
    if (std::fabs(s) + 2.0 * std::fabs(l) < min_dist) {
      min_dist = std::fabs(s) + 2.0 * std::fabs(l);
      min_index = i;
    }
  }
  std::vector<Vec2d> tmp_points(*points);
  for (size_t i = 0; i < points->size(); i++) {
    tmp_points[i] = points->at((i + min_index) % points->size());
  }
  for (size_t i = 0; i < points->size(); i++) {
    points->at(i) = tmp_points[i];
  }
  return true;
}
 
}  // namespace planning
}  // namespace apollo