apollo::planning::OpenSpaceRoiUtil类 参考

#include <open_space_roi_util.h>

apollo::planning::OpenSpaceRoiUtil 的协作图:

静态 Public 成员函数
static bool	FormulateBoundaryConstraints (OpenSpaceInfo *const open_space_info)
	main process to compute and load info needed by open space planner
static void	GetRoiXYBoundary (const std::vector< std::vector< common::math::Vec2d > > &roi_parking_boundary, std::vector< double > *const XYBoundary)
	get XY boundary which is minmax x or y in roi boundary
static void	TransformByOriginPoint (const common::math::Vec2d &origin_point, const double heading, std::vector< std::vector< common::math::Vec2d > > *roi_parking_boundary)
static void	TransformByOriginPoint (const common::math::Vec2d &origin_point, const double heading, std::vector< common::math::Vec2d > *roi_parking_boundary)
static void	TransformByOriginPoint (const common::math::Vec2d &origin_point, const double heading, common::math::Vec2d *point)
static bool	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)
	Load obstacle polygon to roi_boundary in clock wise order
static bool	FilterOutObstacle (const double filtering_distance, const Frame &frame, const std::vector< double > &xy_boundary, const Obstacle &obstacle)
	is obstacle can be filterd out, filtered by distance and roi XY bound
static bool	LoadObstacleInHyperPlanes (OpenSpaceInfo *const open_space_info)
	brief Transform the vertice presentation of the obstacles into linear inequality as Ax>b
static bool	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)
static void	TransformByOriginPoint (const Vec2d &origin_point, const double &origin_heading, OpenSpaceInfo *open_space_info)
static bool	IsPolygonClockwise (const std::vector< Vec2d > &polygon)
static bool	AdjustPointsOrderToClockwise (std::vector< Vec2d > *polygon)
static bool	UpdateParkingPointsOrder (const apollo::hdmap::Path &nearby_path, std::vector< Vec2d > *points)

详细描述

在文件 open_space_roi_util.h 第 36 行定义.

成员函数说明

AdjustPointsOrderToClockwise()

bool apollo::planning::OpenSpaceRoiUtil::AdjustPointsOrderToClockwise ( std::vector< Vec2d > *  polygon )

static

在文件 open_space_roi_util.cc 第 316 行定义.

                               {
  if (!IsPolygonClockwise(*polygon)) {
    // counter clockwise reverse it
    ADEBUG << "point is anticlockwise,reverse";
    std::reverse(polygon->begin(), polygon->end());
    return true;
  } else {
    return false;
  }
}

FilterOutObstacle()

bool apollo::planning::OpenSpaceRoiUtil::FilterOutObstacle ( const double  filtering_distance, const Frame &  frame, const std::vector< double > &  xy_boundary, const Obstacle &  obstacle  )

static

is obstacle can be filterd out, filtered by distance and roi XY bound

参数

xy_boundary	fileter obstacle which out of xy boundary
filtering_distance	filter out distance which is far away from adc

返回

true can be filtered out

在文件 open_space_roi_util.cc 第 147 行定义.

                                  {
  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;
}

FormulateBoundaryConstraints()

bool apollo::planning::OpenSpaceRoiUtil::FormulateBoundaryConstraints ( OpenSpaceInfo *const  open_space_info )

static

main process to compute and load info needed by open space planner

在文件 open_space_roi_util.cc 第 36 行定义.

                                          {
  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;
}

GetHyperPlanes()

bool apollo::planning::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  )

static

在文件 open_space_roi_util.cc 第 205 行定义.

                              {
  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;
}

GetRoiXYBoundary()

void apollo::planning::OpenSpaceRoiUtil::GetRoiXYBoundary ( const std::vector< std::vector< common::math::Vec2d > > &  roi_parking_boundary, std::vector< double > *const  XYBoundary  )

static

get XY boundary which is minmax x or y in roi boundary

参数

XYBoundary

x_min x_max y_min y_max

返回

在文件 open_space_roi_util.cc 第 58 行定义.

                                         {
  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());
    }
  }
}

IsPolygonClockwise()

bool apollo::planning::OpenSpaceRoiUtil::IsPolygonClockwise ( const std::vector< Vec2d > &  polygon )

static

在文件 open_space_roi_util.cc 第 298 行定义.

                                                                         {
  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;
  }
}

LoadObstacleInHyperPlanes()

bool apollo::planning::OpenSpaceRoiUtil::LoadObstacleInHyperPlanes ( OpenSpaceInfo *const  open_space_info )

static

brief Transform the vertice presentation of the obstacles into linear inequality as Ax>b

返回

true is success

在文件 open_space_roi_util.cc 第 187 行定义.

                                          {
  *(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;
}

LoadObstacles()

bool apollo::planning::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  )

static

Load obstacle polygon to roi_boundary in clock wise order

参数

filtering_distance	filter out distance which is far away from adc
perception_obstacle_buffer	add extend buffer for obstacle box
roi_boundary	the roi polygon which adc can drive

返回

true is running success

在文件 open_space_roi_util.cc 第 103 行定义.

                              {
  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;
}

TransformByOriginPoint() [1/4]

void apollo::planning::OpenSpaceRoiUtil::TransformByOriginPoint ( const common::math::Vec2d &  origin_point, const double  heading, common::math::Vec2d *  point  )

static

在文件 open_space_roi_util.cc 第 95 行定义.

                                {
  *point -= origin_point;
  point->SelfRotate(-heading);
}

TransformByOriginPoint() [2/4]

void apollo::planning::OpenSpaceRoiUtil::TransformByOriginPoint ( const common::math::Vec2d &  origin_point, const double  heading, std::vector< common::math::Vec2d > *  roi_parking_boundary  )

static

在文件 open_space_roi_util.cc 第 86 行定义.

                                                          {
  for (auto &point : *roi_parking_boundary) {
    TransformByOriginPoint(origin_point, heading, &point);
  }
}

TransformByOriginPoint() [3/4]

void apollo::planning::OpenSpaceRoiUtil::TransformByOriginPoint ( const common::math::Vec2d &  origin_point, const double  heading, std::vector< std::vector< common::math::Vec2d > > *  roi_parking_boundary  )

static

在文件 open_space_roi_util.cc 第 77 行定义.

                                                                     {
  for (auto &bound_vec : *roi_parking_boundary) {
    TransformByOriginPoint(origin_point, heading, &bound_vec);
  }
}

TransformByOriginPoint() [4/4]

void apollo::planning::OpenSpaceRoiUtil::TransformByOriginPoint ( const Vec2d &  origin_point, const double &  origin_heading, OpenSpaceInfo *  open_space_info  )

static

在文件 open_space_roi_util.cc 第 280 行定义.

                                        {
  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);
}

UpdateParkingPointsOrder()

bool apollo::planning::OpenSpaceRoiUtil::UpdateParkingPointsOrder ( const apollo::hdmap::Path &  nearby_path, std::vector< Vec2d > *  points  )

static

在文件 open_space_roi_util.cc 第 328 行定义.

                                                                  {
  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;
}

---

该类的文档由以下文件生成:

• modules/planning/planning_open_space/utils/open_space_roi_util.h
• modules/planning/planning_open_space/utils/open_space_roi_util.cc