#include <semantic_lstm_evaluator.h>

类 apollo::prediction::SemanticLSTMEvaluator 继承关系图:

apollo::prediction::SemanticLSTMEvaluator 的协作图:

Public 成员函数
	SemanticLSTMEvaluator ()=delete
	Constructor

	SemanticLSTMEvaluator (SemanticMap *semantic_map)

virtual	~SemanticLSTMEvaluator ()=default
	Destructor

void	Clear ()
	Clear obstacle feature map

bool	Evaluate (Obstacle obstacle_ptr, ObstaclesContainer obstacles_container) override
	Override Evaluate

bool	ExtractObstacleHistory (Obstacle obstacle_ptr, std::vector< std::pair< double, double > > pos_history)
	Extract obstacle history

std::string	GetName () override
	Get the name of evaluator.

Public 成员函数继承自 apollo::prediction::Evaluator
	Evaluator ()=default
	Constructor

virtual	~Evaluator ()=default
	Destructor

virtual bool	Evaluate (Obstacle obstacle, ObstaclesContainer obstacles_container, std::vector< Obstacle * > dynamic_env)
	Evaluate an obstacle

virtual bool	Evaluate (const ADCTrajectoryContainer adc_trajectory_container, Obstacle obstacle, ObstaclesContainer *obstacles_container)
	Evaluate an obstacle

额外继承的成员函数
Protected 成员函数继承自 apollo::prediction::Evaluator
std::pair< double, double >	WorldCoordToObjCoord (std::pair< double, double > input_world_coord, std::pair< double, double > obj_world_coord, double obj_world_angle)

std::pair< double, double >	WorldCoordToObjCoordNorth (std::pair< double, double > input_world_coord, std::pair< double, double > obj_world_coord, double obj_world_angle)

double	WorldAngleToObjAngle (double input_world_angle, double obj_world_angle)

Eigen::MatrixXf	VectorToMatrixXf (const std::vector< double > &nums, const int start_index, const int end_index)

Eigen::MatrixXf	VectorToMatrixXf (const std::vector< double > &nums, const int start_index, const int end_index, const int output_num_row, const int output_num_col)

Protected 属性继承自 apollo::prediction::Evaluator
ObstacleConf::EvaluatorType	evaluator_type_

详细描述

在文件 semantic_lstm_evaluator.h 第 32 行定义.

构造及析构函数说明

◆ SemanticLSTMEvaluator() [1/2]

apollo::prediction::SemanticLSTMEvaluator::SemanticLSTMEvaluator ( )

delete

Constructor

◆ SemanticLSTMEvaluator() [2/2]

apollo::prediction::SemanticLSTMEvaluator::SemanticLSTMEvaluator ( SemanticMap * semantic_map )

explicit

在文件 semantic_lstm_evaluator.cc 第 34 行定义.

    : semantic_map_(semantic_map) {
  evaluator_type_ = ObstacleConf::SEMANTIC_LSTM_EVALUATOR;
  torch_default_output_tensor_ = torch::zeros({1, 30, 2});
  model_manager_ = ModelManager();
  model_manager_.Init();
  LoadModel();
}

◆ ~SemanticLSTMEvaluator()

virtual apollo::prediction::SemanticLSTMEvaluator::~SemanticLSTMEvaluator ( )

virtualdefault

Destructor

成员函数说明

◆ Clear()

void apollo::prediction::SemanticLSTMEvaluator::Clear ( )

Clear obstacle feature map

在文件 semantic_lstm_evaluator.cc 第 43 行定义.

43{}

◆ Evaluate()

bool apollo::prediction::SemanticLSTMEvaluator::Evaluate	(	Obstacle *	obstacle_ptr,
		ObstaclesContainer *	obstacles_container
	)

overridevirtual

Override Evaluate

参数

Obstacle	pointer
Obstacles	container

实现了 apollo::prediction::Evaluator.

在文件 semantic_lstm_evaluator.cc 第 45 行定义.

                                                                              {
  omp_set_num_threads(1);
 
  obstacle_ptr->SetEvaluatorType(evaluator_type_);
 
  Clear();
  CHECK_NOTNULL(obstacle_ptr);
  int id = obstacle_ptr->id();
  if (!obstacle_ptr->latest_feature().IsInitialized()) {
    AERROR << "Obstacle [" << id << "] has no latest feature.";
    return false;
  }
  Feature* latest_feature_ptr = obstacle_ptr->mutable_latest_feature();
  CHECK_NOTNULL(latest_feature_ptr);
 
  if (!FLAGS_enable_semantic_map) {
    ADEBUG << "Not enable semantic map, exit semantic_lstm_evaluator.";
    return false;
  }
  cv::Mat feature_map;
  if (!semantic_map_->GetMapById(id, &feature_map)) {
    return false;
  }
  // Process the feature_map
  cv::cvtColor(feature_map, feature_map, cv::COLOR_BGR2RGB);
  cv::Mat img_float;
  feature_map.convertTo(img_float, CV_32F, 1.0 / 255);
  torch::Tensor img_tensor = torch::from_blob(img_float.data, {1, 224, 224, 3});
  img_tensor = img_tensor.permute({0, 3, 1, 2});
  img_tensor[0][0] = img_tensor[0][0].sub(0.485).div(0.229);
  img_tensor[0][1] = img_tensor[0][1].sub(0.456).div(0.224);
  img_tensor[0][2] = img_tensor[0][2].sub(0.406).div(0.225);
 
  // Extract features of pos_history
  std::vector<std::pair<double, double>> pos_history(20, {0.0, 0.0});
  if (!ExtractObstacleHistory(obstacle_ptr, &pos_history)) {
    ADEBUG << "Obstacle [" << id << "] failed to extract obstacle history";
    return false;
  }
  // Process obstacle_history
  // TODO(Hongyi): move magic numbers to parameters and gflags
  torch::Tensor obstacle_pos = torch::zeros({1, 20, 2});
  torch::Tensor obstacle_pos_step = torch::zeros({1, 20, 2});
  for (int i = 0; i < 20; ++i) {
    obstacle_pos[0][19 - i][0] = pos_history[i].first;
    obstacle_pos[0][19 - i][1] = pos_history[i].second;
    if (i == 19 || (i > 0 && pos_history[i].first == 0.0)) {
      break;
    }
    obstacle_pos_step[0][19 - i][0] =
        pos_history[i].first - pos_history[i + 1].first;
    obstacle_pos_step[0][19 - i][1] =
        pos_history[i].second - pos_history[i + 1].second;
  }
 
  // Build input features for torch
  std::vector<void*> input_buffers{
      img_tensor.data_ptr<float>(),
      obstacle_pos.data_ptr<float>(),
      obstacle_pos_step.data_ptr<float>()
  };
  std::vector<void*> output_buffers{
    torch_default_output_tensor_.data_ptr<float>()};
 
  // Compute pred_traj
  std::vector<double> pred_traj;
 
  auto start_time = std::chrono::system_clock::now();
  if (obstacle_ptr->IsPedestrian()) {
    if (!model_manager_.SelectModel(
          device_, ObstacleConf::SEMANTIC_LSTM_EVALUATOR,
          apollo::perception::PerceptionObstacle::PEDESTRIAN)->Inference(
            input_buffers, 3, &output_buffers, 1)) {
      return false;
    }
  } else {
    if (!model_manager_.SelectModel(
          device_, ObstacleConf::SEMANTIC_LSTM_EVALUATOR,
          apollo::perception::PerceptionObstacle::VEHICLE)->Inference(
            input_buffers, 3, &output_buffers, 1)) {
      return false;
    }
  }
  at::Tensor torch_output_tensor = torch::from_blob(
      output_buffers[0], {1, 30, 2});
 
  auto end_time = std::chrono::system_clock::now();
  std::chrono::duration<double> diff = end_time - start_time;
  ADEBUG << "Semantic_LSTM_evaluator used time: " << diff.count() * 1000
         << " ms.";
  auto torch_output = torch_output_tensor.accessor<float, 3>();
 
  // Get the trajectory
  double pos_x = latest_feature_ptr->position().x();
  double pos_y = latest_feature_ptr->position().y();
  Trajectory* trajectory = latest_feature_ptr->add_predicted_trajectory();
  trajectory->set_probability(1.0);
 
  for (int i = 0; i < 30; ++i) {
    double prev_x = pos_x;
    double prev_y = pos_y;
    if (i > 0) {
      const auto& last_point = trajectory->trajectory_point(i - 1).path_point();
      prev_x = last_point.x();
      prev_y = last_point.y();
    }
    TrajectoryPoint* point = trajectory->add_trajectory_point();
    double dx = static_cast<double>(torch_output[0][i][0]);
    double dy = static_cast<double>(torch_output[0][i][1]);
 
    double heading = latest_feature_ptr->velocity_heading();
    Vec2d offset(dx, dy);
    Vec2d rotated_offset = offset.rotate(heading);
    double point_x = pos_x + rotated_offset.x();
    double point_y = pos_y + rotated_offset.y();
    point->mutable_path_point()->set_x(point_x);
    point->mutable_path_point()->set_y(point_y);
 
    if (torch_output_tensor.sizes()[2] == 5) {
      double sigma_xr = std::abs(static_cast<double>(torch_output[0][i][2]));
      double sigma_yr = std::abs(static_cast<double>(torch_output[0][i][3]));
      double corr_r = static_cast<double>(torch_output[0][i][4]);
      Eigen::Matrix2d cov_matrix_r;
      cov_matrix_r(0, 0) = sigma_xr * sigma_xr;
      cov_matrix_r(0, 1) = corr_r * sigma_xr * sigma_yr;
      cov_matrix_r(1, 0) = corr_r * sigma_xr * sigma_yr;
      cov_matrix_r(1, 1) = sigma_yr * sigma_yr;
 
      Eigen::Matrix2d rotation_matrix;
      rotation_matrix(0, 0) = std::cos(heading);
      rotation_matrix(0, 1) = -std::sin(heading);
      rotation_matrix(1, 0) = std::sin(heading);
      rotation_matrix(1, 1) = std::cos(heading);
 
      Eigen::Matrix2d cov_matrix;
      cov_matrix =
          rotation_matrix * cov_matrix_r * (rotation_matrix.transpose());
      double sigma_x = std::sqrt(std::abs(cov_matrix(0, 0)));
      double sigma_y = std::sqrt(std::abs(cov_matrix(1, 1)));
      double corr = cov_matrix(0, 1) / (sigma_x + FLAGS_double_precision) /
                    (sigma_y + FLAGS_double_precision);
 
      point->mutable_gaussian_info()->set_sigma_x(sigma_x);
      point->mutable_gaussian_info()->set_sigma_y(sigma_y);
      point->mutable_gaussian_info()->set_correlation(corr);
 
      if (i > 0) {
        Eigen::EigenSolver<Eigen::Matrix2d> eigen_solver(cov_matrix);
        const auto& eigen_values = eigen_solver.eigenvalues();
        const auto& eigen_vectors = eigen_solver.eigenvectors();
        point->mutable_gaussian_info()->set_ellipse_a(
            std::sqrt(std::abs(eigen_values(0).real())));
        point->mutable_gaussian_info()->set_ellipse_b(
            std::sqrt(std::abs(eigen_values(1).real())));
        double cos_theta_a = eigen_vectors(0, 0).real();
        double sin_theta_a = eigen_vectors(1, 0).real();
        point->mutable_gaussian_info()->set_theta_a(
            std::atan2(sin_theta_a, cos_theta_a));
      }
    }
 
    if (i < 10) {  // use origin heading for the first second
      point->mutable_path_point()->set_theta(
          latest_feature_ptr->velocity_heading());
    } else {
      point->mutable_path_point()->set_theta(
          std::atan2(trajectory->trajectory_point(i).path_point().y() -
                         trajectory->trajectory_point(i - 1).path_point().y(),
                     trajectory->trajectory_point(i).path_point().x() -
                         trajectory->trajectory_point(i - 1).path_point().x()));
    }
    point->set_relative_time(static_cast<double>(i) *
                             FLAGS_prediction_trajectory_time_resolution);
    if (i == 0) {
      point->set_v(latest_feature_ptr->speed());
    } else {
      double diff_x = point_x - prev_x;
      double diff_y = point_y - prev_y;
      point->set_v(std::hypot(diff_x, diff_y) /
                   FLAGS_prediction_trajectory_time_resolution);
    }
  }
 
  return true;
}

◆ ExtractObstacleHistory()

bool apollo::prediction::SemanticLSTMEvaluator::ExtractObstacleHistory	(	Obstacle *	obstacle_ptr,
		std::vector< std::pair< double, double > > *	pos_history
	)

Extract obstacle history

参数

Obstacle pointer Feature container in a vector for receiving the obstacle history

在文件 semantic_lstm_evaluator.cc 第 232 行定义.

                                                     {
  pos_history->resize(20, {0.0, 0.0});
  const Feature& obs_curr_feature = obstacle_ptr->latest_feature();
  double obs_curr_heading = obs_curr_feature.velocity_heading();
  std::pair<double, double> obs_curr_pos = std::make_pair(
      obs_curr_feature.position().x(), obs_curr_feature.position().y());
  for (std::size_t i = 0; i < obstacle_ptr->history_size() && i < 20; ++i) {
    const Feature& feature = obstacle_ptr->feature(i);
    if (!feature.IsInitialized()) {
      break;
    }
    pos_history->at(i) = WorldCoordToObjCoord(
        std::make_pair(feature.position().x(), feature.position().y()),
        obs_curr_pos, obs_curr_heading);
  }
  return true;
}

◆ GetName()

std::string apollo::prediction::SemanticLSTMEvaluator::GetName ( )

inlineoverridevirtual

Get the name of evaluator.

实现了 apollo::prediction::Evaluator.

在文件 semantic_lstm_evaluator.h 第 70 行定义.

70{ return "SEMANTIC_LSTM_EVALUATOR"; }

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

modules/prediction/evaluator/vehicle/semantic_lstm_evaluator.h
modules/prediction/evaluator/vehicle/semantic_lstm_evaluator.cc

Public 成员函数

额外继承的成员函数