Prediction: deprecate kf motion and rnn evaluator

modules/prediction/container/obstacles/obstacle.cc

@@ -90,10 +90,6 @@ Feature* Obstacle::mutable_latest_feature() {
 
 size_t Obstacle::history_size() const { return feature_history_.size(); }
 
-const KalmanFilter<double, 6, 2, 0>& Obstacle::kf_motion_tracker() const {
-  return kf_motion_tracker_;
-}
-
 const KalmanFilter<double, 2, 2, 4>& Obstacle::kf_pedestrian_tracker() const {
   return kf_pedestrian_tracker_;
 }
@@ -171,21 +167,12 @@ bool Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
 
   // Set obstacle observation for KF tracking
   if (!FLAGS_use_navigation_mode) {
-    // Update KF
-    if (!kf_motion_tracker_.IsInitialized()) {
-      InitKFMotionTracker(feature);
-    }
-    UpdateKFMotionTracker(feature);
     if (type_ == PerceptionObstacle::PEDESTRIAN) {
       if (!kf_pedestrian_tracker_.IsInitialized()) {
         InitKFPedestrianTracker(feature);
       }
       UpdateKFPedestrianTracker(feature);
     }
-    // Update obstacle status based on KF if enabled
-    if (FLAGS_enable_kf_tracking) {
-      UpdateStatus(&feature);
-    }
   }
 
   // Set obstacle lane features
@@ -344,81 +331,6 @@ void Obstacle::SetStatus(const PerceptionObstacle& perception_obstacle,
   SetIsNearJunction(perception_obstacle, feature);
 }
 
-void Obstacle::UpdateStatus(Feature* feature) {
-  // Update motion belief
-  if (!kf_motion_tracker_.IsInitialized()) {
-    ADEBUG << "Obstacle [" << id_ << "] has not initialized motion tracker.";
-    return;
-  }
-  auto state = kf_motion_tracker_.GetStateEstimate();
-
-  feature->mutable_t_position()->set_x(state(0, 0));
-  feature->mutable_t_position()->set_y(state(1, 0));
-  feature->mutable_t_position()->set_z(feature->position().z());
-
-  double velocity_x = state(2, 0);
-  double velocity_y = state(3, 0);
-  double speed = std::hypot(velocity_x, velocity_y);
-  double velocity_heading = std::atan2(velocity_y, velocity_x);
-  if (FLAGS_adjust_velocity_by_position_shift) {
-    UpdateVelocity(feature->theta(), &velocity_x, &velocity_y,
-                   &velocity_heading, &speed);
-  }
-  feature->mutable_velocity()->set_x(velocity_x);
-  feature->mutable_velocity()->set_y(velocity_y);
-  feature->mutable_velocity()->set_z(velocity_heading);
-  feature->set_speed(speed);
-  feature->set_velocity_heading(std::atan2(state(3, 0), state(2, 0)));
-
-  double acc_x = common::math::Clamp(state(4, 0), FLAGS_vehicle_min_linear_acc,
-                                     FLAGS_vehicle_max_linear_acc);
-  double acc_y = common::math::Clamp(state(5, 0), FLAGS_vehicle_min_linear_acc,
-                                     FLAGS_vehicle_max_linear_acc);
-  double acc =
-      acc_x * std::cos(velocity_heading) + acc_y * std::sin(velocity_heading);
-  feature->mutable_acceleration()->set_x(acc_x);
-  feature->mutable_acceleration()->set_y(acc_y);
-  feature->mutable_acceleration()->set_z(feature->acceleration().z());
-  feature->set_acc(acc);
-
-  ADEBUG << "Obstacle [" << id_ << "] has tracked position [" << std::fixed
-         << std::setprecision(6) << feature->t_position().x() << ", "
-         << std::fixed << std::setprecision(6) << feature->t_position().y()
-         << ", " << std::fixed << std::setprecision(6)
-         << feature->t_position().z() << "]";
-  ADEBUG << "Obstacle [" << id_ << "] has tracked velocity [" << std::fixed
-         << std::setprecision(6) << feature->velocity().x() << ", "
-         << std::fixed << std::setprecision(6) << feature->velocity().y()
-         << ", " << std::fixed << std::setprecision(6)
-         << feature->velocity().z() << "]";
-  ADEBUG << "Obstacle [" << id_ << "] has tracked acceleration [" << std::fixed
-         << std::setprecision(6) << feature->acceleration().x() << ", "
-         << std::fixed << std::setprecision(6) << feature->acceleration().y()
-         << ", " << std::fixed << std::setprecision(6)
-         << feature->acceleration().z() << "]";
-  ADEBUG << "Obstacle [" << id_ << "] has tracked velocity heading ["
-         << std::fixed << std::setprecision(6) << feature->velocity_heading()
-         << "].";
-
-  // Update pedestrian motion belief
-  if (type_ == PerceptionObstacle::PEDESTRIAN) {
-    if (!kf_pedestrian_tracker_.IsInitialized()) {
-      ADEBUG << "Obstacle [" << id_
-             << "] has not initialized pedestrian tracker.";
-      return;
-    }
-    feature->mutable_t_position()->set_x(
-        kf_pedestrian_tracker_.GetStateEstimate()(0, 0));
-    feature->mutable_t_position()->set_y(
-        kf_pedestrian_tracker_.GetStateEstimate()(1, 0));
-    ADEBUG << "Obstacle [" << id_ << "] has tracked pedestrian position ["
-           << std::setprecision(6) << feature->t_position().x() << std::fixed
-           << ", " << std::setprecision(6) << feature->t_position().y()
-           << std::fixed << ", " << std::setprecision(6)
-           << feature->t_position().z() << std::fixed << "]";
-  }
-}
-
 bool Obstacle::SetId(const PerceptionObstacle& perception_obstacle,
                      Feature* feature, const int prediction_obstacle_id) {
   int id = prediction_obstacle_id > 0 ? prediction_obstacle_id
@@ -703,76 +615,6 @@ bool Obstacle::HasJunctionFeatureWithExits() const {
          latest_feature().junction_feature().junction_exit_size() > 0;
 }
 
-void Obstacle::InitKFMotionTracker(const Feature& feature) {
-  // Set transition matrix F
-  // constant acceleration dynamic model
-  Eigen::Matrix<double, 6, 6> F;
-  F.setIdentity();
-  kf_motion_tracker_.SetTransitionMatrix(F);
-
-  // Set observation matrix H
-  Eigen::Matrix<double, 2, 6> H;
-  H.setIdentity();
-  kf_motion_tracker_.SetObservationMatrix(H);
-
-  // Set covariance of transition noise matrix Q
-  // make the noise this order:
-  // noise(x/y) < noise(vx/vy) < noise(ax/ay)
-  Eigen::Matrix<double, 6, 6> Q;
-  Q.setIdentity();
-  Q *= FLAGS_q_var;
-  kf_motion_tracker_.SetTransitionNoise(Q);
-
-  // Set observation noise matrix R
-  Eigen::Matrix<double, 2, 2> R;
-  R.setIdentity();
-  R *= FLAGS_r_var;
-  kf_motion_tracker_.SetObservationNoise(R);
-
-  // Set current state covariance matrix P
-  // make the covariance this order:
-  // cov(x/y) < cov(vx/vy) < cov(ax/ay)
-  Eigen::Matrix<double, 6, 6> P;
-  P.setIdentity();
-  P *= FLAGS_p_var;
-
-  // Set initial state
-  Eigen::Matrix<double, 6, 1> x;
-  x(0, 0) = feature.position().x();
-  x(1, 0) = feature.position().y();
-  x(2, 0) = feature.velocity().x();
-  x(3, 0) = feature.velocity().y();
-  x(4, 0) = feature.acceleration().x();
-  x(5, 0) = feature.acceleration().y();
-
-  kf_motion_tracker_.SetStateEstimate(x, P);
-}
-
-void Obstacle::UpdateKFMotionTracker(const Feature& feature) {
-  double delta_ts = 0.0;
-  if (feature_history_.size() > 0) {
-    delta_ts = feature.timestamp() - feature_history_.front().timestamp();
-  }
-  if (delta_ts > FLAGS_double_precision) {
-    // Set transition matrix and predict
-    auto F = kf_motion_tracker_.GetTransitionMatrix();
-    F(0, 2) = delta_ts;
-    F(0, 4) = 0.5 * delta_ts * delta_ts;
-    F(1, 3) = delta_ts;
-    F(1, 5) = 0.5 * delta_ts * delta_ts;
-    F(2, 4) = delta_ts;
-    F(3, 5) = delta_ts;
-    kf_motion_tracker_.SetTransitionMatrix(F);
-    kf_motion_tracker_.Predict();
-
-    // Set observation and correct
-    Eigen::Matrix<double, 2, 1> z;
-    z(0, 0) = feature.position().x();
-    z(1, 0) = feature.position().y();
-    kf_motion_tracker_.Correct(z);
-  }
-}
-
 void Obstacle::InitKFPedestrianTracker(const Feature& feature) {
   // Set transition matrix F
   Eigen::Matrix<double, 2, 2> F;
@@ -1644,29 +1486,6 @@ void Obstacle::DiscardOutdatedHistory() {
   }
 }
 
-void Obstacle::SetRNNStates(const std::vector<Eigen::MatrixXf>& rnn_states) {
-  rnn_states_ = rnn_states;
-}
-
-void Obstacle::GetRNNStates(std::vector<Eigen::MatrixXf>* rnn_states) {
-  rnn_states->clear();
-  rnn_states->insert(rnn_states->end(), rnn_states_.begin(), rnn_states_.end());
-}
-
-void Obstacle::InitRNNStates() {
-  if (network::RnnModel::Instance()->IsOk()) {
-    network::RnnModel::Instance()->ResetState();
-    network::RnnModel::Instance()->State(&rnn_states_);
-    rnn_enabled_ = true;
-    ADEBUG << "Success to initialize rnn model.";
-  } else {
-    AWARN << "Fail to initialize rnn model.";
-    rnn_enabled_ = false;
-  }
-}
-
-bool Obstacle::RNNEnabled() const { return rnn_enabled_; }
-
 void Obstacle::SetCaution() {
   CHECK_GT(feature_history_.size(), 0);
   Feature* feature = mutable_latest_feature();

modules/prediction/container/obstacles/obstacle.h

@@ -142,12 +142,6 @@ class Obstacle {
    */
   size_t history_size() const;
 
-  /**
-   * @brief Get the motion Kalman filter.
-   * @return The motion Kalman filter.
-   */
-  const common::math::KalmanFilter<double, 6, 2, 0>& kf_motion_tracker() const;
-
   /**
    * @brief Get the pedestrian Kalman filter.
    * @return The pedestrian Kalman filter.
@@ -220,29 +214,6 @@ class Obstacle {
    */
   void BuildLaneGraphFromLeftToRight();
 
-  /**
-   * @brief Set RNN state
-   * @param RNN state matrix
-   */
-  void SetRNNStates(const std::vector<Eigen::MatrixXf>& rnn_states);
-
-  /**
-   * @brief Get RNN state
-   * @param A pointer to RNN state matrix
-   */
-  void GetRNNStates(std::vector<Eigen::MatrixXf>* rnn_states);
-
-  /**
-   * @brief Initialize RNN state
-   */
-  void InitRNNStates();
-
-  /**
-   * @brief Check if RNN is enabled
-   * @return True if RNN is enabled
-   */
-  bool RNNEnabled() const;
-
   /**
    * @brief Set the obstacle as caution level
    */
@@ -262,8 +233,6 @@ class Obstacle {
   void SetStatus(const perception::PerceptionObstacle& perception_obstacle,
                  double timestamp, Feature* feature);
 
-  void UpdateStatus(Feature* feature);
-
   bool SetId(const perception::PerceptionObstacle& perception_obstacle,
              Feature* feature, const int prediction_id = -1);
 
@@ -302,10 +271,6 @@ class Obstacle {
       const perception::PerceptionObstacle& perception_obstacle,
       Feature* feature);
 
-  void InitKFMotionTracker(const Feature& feature);
-
-  void UpdateKFMotionTracker(const Feature& feature);
-
   void UpdateLaneBelief(Feature* feature);
 
   void SetCurrentLanes(Feature* feature);
@@ -361,16 +326,10 @@ class Obstacle {
 
   std::deque<Feature> feature_history_;
 
-  common::math::KalmanFilter<double, 6, 2, 0> kf_motion_tracker_;
-
   common::math::KalmanFilter<double, 2, 2, 4> kf_pedestrian_tracker_;
 
   std::vector<std::shared_ptr<const hdmap::LaneInfo>> current_lanes_;
 
-  std::vector<Eigen::MatrixXf> rnn_states_;
-
-  bool rnn_enabled_ = false;
-
   ObstacleConf obstacle_conf_;
 };
 

modules/prediction/container/obstacles/obstacle_test.cc

@@ -29,7 +29,7 @@ class ObstacleTest : public KMLMapBasedTest {
     FLAGS_p_var = 0.1;
     FLAGS_q_var = 0.1;
     FLAGS_r_var = 0.001;
-    FLAGS_enable_kf_tracking = true;
+    FLAGS_enable_kf_tracking = false;
     FLAGS_min_prediction_trajectory_spatial_length = 50.0;
     FLAGS_adjust_velocity_by_position_shift = false;
     FLAGS_adjust_vehicle_heading_by_lane = false;
@@ -66,22 +66,16 @@ TEST_F(ObstacleTest, VehiclePosition) {
   EXPECT_DOUBLE_EQ(start_feature.timestamp(), 0.0);
   EXPECT_DOUBLE_EQ(start_feature.position().x(), -458.941);
   EXPECT_DOUBLE_EQ(start_feature.position().y(), -159.240);
-  EXPECT_NEAR(start_feature.t_position().x(), -458.941, 0.001);
-  EXPECT_NEAR(start_feature.t_position().y(), -159.240, 0.001);
 
   Feature* mid_feature_ptr = obstacle_ptr->mutable_feature(1);
   EXPECT_DOUBLE_EQ(mid_feature_ptr->timestamp(), 0.1);
   EXPECT_DOUBLE_EQ(mid_feature_ptr->position().x(), -457.010);
   EXPECT_DOUBLE_EQ(mid_feature_ptr->position().y(), -160.023);
-  EXPECT_NEAR(mid_feature_ptr->t_position().x(), -457.010, 0.1);
-  EXPECT_NEAR(mid_feature_ptr->t_position().y(), -160.023, 0.1);
 
   const Feature& latest_feature = obstacle_ptr->latest_feature();
   EXPECT_DOUBLE_EQ(latest_feature.timestamp(), 0.2);
   EXPECT_DOUBLE_EQ(latest_feature.position().x(), -455.182);
   EXPECT_DOUBLE_EQ(latest_feature.position().y(), -160.608);
-  EXPECT_NEAR(latest_feature.t_position().x(), -455.182, 0.1);
-  EXPECT_NEAR(latest_feature.t_position().y(), -160.608, 0.1);
 }
 
 TEST_F(ObstacleTest, VehicleVelocity) {
@@ -94,15 +88,15 @@ TEST_F(ObstacleTest, VehicleVelocity) {
 
   const Feature& mid_feature = obstacle_ptr->feature(1);
   EXPECT_DOUBLE_EQ(mid_feature.timestamp(), 0.1);
-  EXPECT_DOUBLE_EQ(mid_feature.velocity().x(), 18.796567195950544);
-  EXPECT_DOUBLE_EQ(mid_feature.velocity().y(), -6.8439304092771129);
+  EXPECT_DOUBLE_EQ(mid_feature.velocity().x(), 17.994);
+  EXPECT_DOUBLE_EQ(mid_feature.velocity().y(), -6.8390000000000004);
 
   Feature* latest_feature_ptr = obstacle_ptr->mutable_latest_feature();
   EXPECT_DOUBLE_EQ(latest_feature_ptr->timestamp(), 0.2);
-  EXPECT_DOUBLE_EQ(latest_feature_ptr->velocity().x(), 18.786524599512003);
-  EXPECT_DOUBLE_EQ(latest_feature_ptr->velocity().y(), -6.8246071588835804);
+  EXPECT_DOUBLE_EQ(latest_feature_ptr->velocity().x(), 17.994);
+  EXPECT_DOUBLE_EQ(latest_feature_ptr->velocity().y(), -6.8390000000000004);
 
-  EXPECT_NEAR(latest_feature_ptr->speed(), 19.987715462282193, 0.001);
+  EXPECT_NEAR(latest_feature_ptr->speed(), 19.249830051197854, 0.001);
 }
 
 TEST_F(ObstacleTest, VehicleHeading) {
@@ -144,22 +138,16 @@ TEST_F(ObstacleTest, PedestrianPosition) {
   EXPECT_DOUBLE_EQ(start_feature.timestamp(), 0.0);
   EXPECT_DOUBLE_EQ(start_feature.position().x(), -438.879);
   EXPECT_DOUBLE_EQ(start_feature.position().y(), -161.931);
-  EXPECT_NEAR(start_feature.t_position().x(), -438.879, 0.001);
-  EXPECT_NEAR(start_feature.t_position().y(), -161.931, 0.001);
 
   Feature* mid_feature_ptr = obstacle_ptr->mutable_feature(1);
   EXPECT_DOUBLE_EQ(mid_feature_ptr->timestamp(), 0.1);
   EXPECT_DOUBLE_EQ(mid_feature_ptr->position().x(), -438.610);
   EXPECT_DOUBLE_EQ(mid_feature_ptr->position().y(), -161.521);
-  EXPECT_NEAR(mid_feature_ptr->t_position().x(), -438.610, 0.05);
-  EXPECT_NEAR(mid_feature_ptr->t_position().y(), -161.521, 0.05);
 
   const Feature& latest_feature = obstacle_ptr->latest_feature();
   EXPECT_DOUBLE_EQ(latest_feature.timestamp(), 0.2);
   EXPECT_DOUBLE_EQ(latest_feature.position().x(), -438.537);
   EXPECT_DOUBLE_EQ(latest_feature.position().y(), -160.991);
-  EXPECT_NEAR(latest_feature.t_position().x(), -438.537, 0.05);
-  EXPECT_NEAR(latest_feature.t_position().y(), -160.991, 0.05);
 }
 
 TEST_F(ObstacleTest, PedestrianVelocity) {
@@ -172,15 +160,15 @@ TEST_F(ObstacleTest, PedestrianVelocity) {
 
   const Feature& mid_feature = obstacle_ptr->feature(1);
   EXPECT_DOUBLE_EQ(mid_feature.timestamp(), 0.1);
-  EXPECT_DOUBLE_EQ(mid_feature.velocity().x(), 1.7148756822316562);
-  EXPECT_DOUBLE_EQ(mid_feature.velocity().y(), 4.6960198636155503);
+  EXPECT_DOUBLE_EQ(mid_feature.velocity().x(), 1.710);
+  EXPECT_DOUBLE_EQ(mid_feature.velocity().y(), 4.699);
 
   Feature* latest_feature_ptr = obstacle_ptr->mutable_latest_feature();
   EXPECT_DOUBLE_EQ(latest_feature_ptr->timestamp(), 0.2);
-  EXPECT_DOUBLE_EQ(latest_feature_ptr->velocity().x(), 1.6957282277561021);
-  EXPECT_DOUBLE_EQ(latest_feature_ptr->velocity().y(), 4.7077623811976848);
+  EXPECT_DOUBLE_EQ(latest_feature_ptr->velocity().x(), 1.710);
+  EXPECT_DOUBLE_EQ(latest_feature_ptr->velocity().y(), 4.699);
 
-  EXPECT_NEAR(latest_feature_ptr->speed(), 5.0038506033083108, 0.001);
+  EXPECT_NEAR(latest_feature_ptr->speed(), 5.0004700779026763, 0.001);
 }
 
 TEST_F(ObstacleTest, PedestrianHeading) {

modules/prediction/evaluator/BUILD

@@ -25,7 +25,6 @@ cc_library(
         "//modules/prediction/evaluator/vehicle:lane_aggregating_evaluator",
         "//modules/prediction/evaluator/vehicle:lane_scanning_evaluator",
         "//modules/prediction/evaluator/vehicle:mlp_evaluator",
-        "//modules/prediction/evaluator/vehicle:rnn_evaluator",
         "//modules/prediction/evaluator/vehicle:semantic_lstm_evaluator",
         "//modules/prediction/proto:prediction_conf_proto",
     ],

modules/prediction/evaluator/evaluator_manager.cc

@@ -38,7 +38,6 @@
 #include "modules/prediction/evaluator/vehicle/lane_aggregating_evaluator.h"
 #include "modules/prediction/evaluator/vehicle/lane_scanning_evaluator.h"
 #include "modules/prediction/evaluator/vehicle/mlp_evaluator.h"
-#include "modules/prediction/evaluator/vehicle/rnn_evaluator.h"
 #include "modules/prediction/evaluator/vehicle/semantic_lstm_evaluator.h"
 
 namespace apollo {
@@ -102,7 +101,6 @@ EvaluatorManager::EvaluatorManager() { RegisterEvaluators(); }
 
 void EvaluatorManager::RegisterEvaluators() {
   RegisterEvaluator(ObstacleConf::MLP_EVALUATOR);
-  RegisterEvaluator(ObstacleConf::RNN_EVALUATOR);
   RegisterEvaluator(ObstacleConf::COST_EVALUATOR);
   RegisterEvaluator(ObstacleConf::CRUISE_MLP_EVALUATOR);
   RegisterEvaluator(ObstacleConf::JUNCTION_MLP_EVALUATOR);
@@ -379,10 +377,6 @@ std::unique_ptr<Evaluator> EvaluatorManager::CreateEvaluator(
       evaluator_ptr.reset(new JunctionMLPEvaluator());
       break;
     }
-    case ObstacleConf::RNN_EVALUATOR: {
-      evaluator_ptr.reset(new RNNEvaluator());
-      break;
-    }
     case ObstacleConf::COST_EVALUATOR: {
       evaluator_ptr.reset(new CostEvaluator());
       break;

modules/prediction/evaluator/vehicle/BUILD

@@ -34,34 +34,6 @@ cc_test(
     ],
 )
 
-cc_library(
-    name = "rnn_evaluator",
-    srcs = ["rnn_evaluator.cc"],
-    hdrs = ["rnn_evaluator.h"],
-    copts = [
-        "-DMODULE_NAME=\\\"prediction\\\"",
-    ],
-    deps = [
-        "//modules/prediction/container/obstacles:obstacles_container",
-        "//modules/prediction/evaluator",
-    ],
-)
-
-cc_test(
-    name = "rnn_evaluator_test",
-    size = "small",
-    srcs = ["rnn_evaluator_test.cc"],
-    data = [
-        "//modules/prediction:prediction_data",
-        "//modules/prediction:prediction_testdata",
-    ],
-    deps = [
-        "//modules/prediction/common:kml_map_based_test",
-        "//modules/prediction/evaluator/vehicle:rnn_evaluator",
-        "@gtest//:main",
-    ],
-)
-
 cc_library(
     name = "cost_evaluator",
     srcs = ["cost_evaluator.cc"],

modules/prediction/evaluator/vehicle/rnn_evaluator.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/evaluator/vehicle/rnn_evaluator.h"
-
-#include <memory>
-#include <utility>
-
-#include "cyber/common/file.h"
-#include "modules/prediction/common/prediction_gflags.h"
-#include "modules/prediction/common/prediction_map.h"
-
-namespace apollo {
-namespace prediction {
-
-using apollo::hdmap::LaneInfo;
-
-RNNEvaluator::RNNEvaluator() {
-  evaluator_type_ = ObstacleConf::RNN_EVALUATOR;
-  LoadModel(FLAGS_evaluator_vehicle_rnn_file);
-}
-
-bool RNNEvaluator::Evaluate(Obstacle* obstacle_ptr,
-                            ObstaclesContainer* obstacles_container) {
-  Clear();
-  CHECK_NOTNULL(obstacle_ptr);
-
-  obstacle_ptr->SetEvaluatorType(evaluator_type_);
-
-  int id = obstacle_ptr->id();
-  if (!obstacle_ptr->latest_feature().IsInitialized()) {
-    ADEBUG << "Obstacle [" << id << "] has no latest feature.";
-    return false;
-  }
-
-  Feature* latest_feature_ptr = obstacle_ptr->mutable_latest_feature();
-  CHECK_NOTNULL(latest_feature_ptr);
-  if (!latest_feature_ptr->has_lane() ||
-      !latest_feature_ptr->lane().has_lane_graph()) {
-    ADEBUG << "Obstacle [" << id << "] has no lane graph.";
-    return false;
-  }
-
-  LaneGraph* lane_graph_ptr =
-      latest_feature_ptr->mutable_lane()->mutable_lane_graph();
-  CHECK_NOTNULL(lane_graph_ptr);
-  if (lane_graph_ptr->lane_sequence().empty()) {
-    ADEBUG << "Obstacle [" << id << "] has no lane sequences.";
-    return false;
-  }
-
-  Eigen::MatrixXf obstacle_feature_mat;
-  std::unordered_map<int, Eigen::MatrixXf> lane_feature_mats;
-  if (ExtractFeatureValues(obstacle_ptr, &obstacle_feature_mat,
-                           &lane_feature_mats) != 0) {
-    ADEBUG << "Fail to extract feature from obstacle";
-    return false;
-  }
-  if (obstacle_feature_mat.rows() != 1 ||
-      obstacle_feature_mat.size() != DIM_OBSTACLE_FEATURE) {
-    ADEBUG << "Dim of obstacle feature is wrong!";
-    return true;
-  }
-
-  Eigen::MatrixXf pred_mat;
-  std::vector<Eigen::MatrixXf> states;
-  if (!obstacle_ptr->RNNEnabled()) {
-    obstacle_ptr->InitRNNStates();
-  }
-  obstacle_ptr->GetRNNStates(&states);
-  for (int i = 0; i < lane_graph_ptr->lane_sequence_size(); ++i) {
-    LaneSequence* lane_sequence_ptr = lane_graph_ptr->mutable_lane_sequence(i);
-    int seq_id = lane_sequence_ptr->lane_sequence_id();
-    if (lane_feature_mats.find(seq_id) == lane_feature_mats.end()) {
-      ADEBUG << "Fail to access seq-" << seq_id << " feature!";
-      continue;
-    }
-    const Eigen::MatrixXf& lane_feature_mat = lane_feature_mats[seq_id];
-    if (lane_feature_mat.cols() != DIM_LANE_POINT_FEATURE) {
-      ADEBUG << "Lane feature dim of seq-" << seq_id << " is wrong!";
-      continue;
-    }
-    model_ptr_->SetState(states);
-    model_ptr_->Run({obstacle_feature_mat, lane_feature_mat}, &pred_mat);
-    double probability = pred_mat(0, 0);
-    ADEBUG << "Probability = " << probability;
-    double acceleration = pred_mat(0, 1);
-    if (std::isnan(probability) || std::isinf(probability)) {
-      ADEBUG << "Fail to compute probability.";
-      continue;
-    }
-    if (std::isnan(acceleration) || std::isinf(acceleration)) {
-      ADEBUG << "Fail to compute acceleration.";
-      continue;
-    }
-    lane_sequence_ptr->set_probability(probability);
-    lane_sequence_ptr->set_acceleration(acceleration);
-  }
-  model_ptr_->State(&states);
-  obstacle_ptr->SetRNNStates(states);
-  return true;
-}
-
-void RNNEvaluator::Clear() {}
-
-void RNNEvaluator::LoadModel(const std::string& model_file) {
-  NetParameter net_parameter;
-  CHECK(cyber::common::GetProtoFromFile(model_file, &net_parameter))
-      << "Unable to load model file: " << model_file << ".";
-
-  ADEBUG << "Succeeded in loading the model file: " << model_file << ".";
-  model_ptr_ = network::RnnModel::Instance();
-  model_ptr_->LoadModel(net_parameter);
-}
-
-int RNNEvaluator::ExtractFeatureValues(
-    Obstacle* obstacle, Eigen::MatrixXf* const obstacle_feature_mat,
-    std::unordered_map<int, Eigen::MatrixXf>* const lane_feature_mats) {
-  std::vector<float> obstacle_features;
-  std::vector<float> lane_features;
-  if (SetupObstacleFeature(obstacle, &obstacle_features) != 0) {
-    ADEBUG << "Reset rnn state";
-    obstacle->InitRNNStates();
-  }
-  if (static_cast<int>(obstacle_features.size()) != DIM_OBSTACLE_FEATURE) {
-    AWARN << "Obstacle feature size: " << obstacle_features.size();
-    return -1;
-  }
-  obstacle_feature_mat->resize(1, obstacle_features.size());
-  for (size_t i = 0; i < obstacle_features.size(); ++i) {
-    (*obstacle_feature_mat)(0, i) = obstacle_features[i];
-  }
-
-  Feature* feature = obstacle->mutable_latest_feature();
-  if (!feature->has_lane() || !feature->lane().has_lane_graph()) {
-    AWARN << "Fail to access lane graph!";
-    return -1;
-  }
-  int routes = feature->lane().lane_graph().lane_sequence_size();
-  for (int i = 0; i < routes; ++i) {
-    LaneSequence lane_seq = feature->lane().lane_graph().lane_sequence(i);
-    int seq_id = lane_seq.lane_sequence_id();
-    if (SetupLaneFeature(*feature, lane_seq, &lane_features) != 0) {
-      ADEBUG << "Fail to setup lane sequence feature!";
-      continue;
-    }
-    int dim = DIM_LANE_POINT_FEATURE;
-    Eigen::MatrixXf mat(lane_features.size() / dim, dim);
-    for (int j = 0; j < static_cast<int>(lane_features.size()); ++j) {
-      mat(j / dim, j % dim) = lane_features[j];
-    }
-    (*lane_feature_mats)[seq_id] = std::move(mat);
-  }
-  return 0;
-}
-
-int RNNEvaluator::SetupObstacleFeature(
-    Obstacle* obstacle, std::vector<float>* const feature_values) {
-  feature_values->clear();
-  feature_values->reserve(DIM_OBSTACLE_FEATURE);
-
-  float heading = 0.0f;
-  float speed = 0.0f;
-  float lane_l = 0.0f;
-  float theta = 0.0f;
-  float dist_lb = 1.0f;
-  float dist_rb = 1.0f;
-  if (obstacle->history_size() < 1) {
-    AWARN << "Size of feature less than 1!";
-    return -1;
-  }
-
-  bool success_setup = false;
-  int ret = 0;
-  size_t num = obstacle->history_size() > 3 ? 3 : obstacle->history_size();
-  for (size_t i = 0; i < num; ++i) {
-    Feature* fea = obstacle->mutable_feature(i);
-    if (fea == nullptr) {
-      ADEBUG << "Fail to get " << i << "-th feature from obstacle";
-      continue;
-    }
-    if (!fea->has_lane() || !fea->lane().has_lane_feature() ||
-        !fea->lane().lane_feature().has_lane_id()) {
-      ADEBUG << "Fail to access lane feature from " << i << "-the feature";
-      continue;
-    }
-    LaneFeature* p_lane_fea = fea->mutable_lane()->mutable_lane_feature();
-    lane_l = static_cast<float>(p_lane_fea->lane_l());
-    theta = static_cast<float>(p_lane_fea->angle_diff());
-    dist_lb = static_cast<float>(p_lane_fea->dist_to_left_boundary());
-    dist_rb = static_cast<float>(p_lane_fea->dist_to_right_boundary());
-
-    if (!fea->has_speed() || !fea->velocity_heading()) {
-      ADEBUG << "Fail to access speed and velocity heading from " << i
-             << "-the feature";
-      continue;
-    }
-    speed = static_cast<float>(fea->speed());
-    heading = static_cast<float>(fea->velocity_heading());
-    success_setup = true;
-    ADEBUG << "Success to setup obstacle feature!";
-
-    Feature* fea_pre = nullptr;
-    if (i + 1 < obstacle->history_size()) {
-      fea_pre = obstacle->mutable_feature(i + 1);
-    }
-    if (fea_pre != nullptr) {
-      if (fea_pre->lane().has_lane_feature() &&
-          fea_pre->lane().lane_feature().has_lane_id()) {
-        std::string lane_id_pre = fea_pre->lane().lane_feature().lane_id();
-        if (lane_id_pre != p_lane_fea->lane_id() &&
-            IsCutinInHistory(p_lane_fea->lane_id(), lane_id_pre)) {
-          ADEBUG << "Obstacle [" << fea->id() << "] cut in from " << lane_id_pre
-                 << " to " << p_lane_fea->lane_id() << ", reset";
-          ret = -1;
-        }
-      }
-    }
-    break;
-  }
-
-  if (!success_setup) {
-    return -1;
-  }
-  feature_values->push_back(heading);
-  feature_values->push_back(speed);
-  feature_values->push_back(lane_l);
-  feature_values->push_back(dist_lb);
-  feature_values->push_back(dist_rb);
-  feature_values->push_back(theta);
-  return ret;
-}
-
-int RNNEvaluator::SetupLaneFeature(const Feature& feature,
-                                   const LaneSequence& lane_sequence,
-                                   std::vector<float>* const feature_values) {
-  CHECK_LE(LENGTH_LANE_POINT_SEQUENCE, FLAGS_max_num_lane_point);
-  feature_values->clear();
-  feature_values->reserve(static_cast<size_t>(DIM_LANE_POINT_FEATURE) *
-                          static_cast<size_t>(LENGTH_LANE_POINT_SEQUENCE));
-  LanePoint* p_lane_point = nullptr;
-  int counter = 0;
-  for (int seg_i = 0; seg_i < lane_sequence.lane_segment_size(); ++seg_i) {
-    if (counter > LENGTH_LANE_POINT_SEQUENCE) {
-      break;
-    }
-    LaneSegment lane_seg = lane_sequence.lane_segment(seg_i);
-    for (int pt_i = 0; pt_i < lane_seg.lane_point_size(); ++pt_i) {
-      p_lane_point = lane_seg.mutable_lane_point(pt_i);
-      if (p_lane_point->has_relative_s() &&
-          p_lane_point->relative_s() < FLAGS_rnn_min_lane_relatice_s) {
-        continue;
-      }
-      if (!feature.has_position() || !p_lane_point->has_position()) {
-        ADEBUG << "Feature or lane_point has no position!";
-        continue;
-      }
-      float diff_x = static_cast<float>(p_lane_point->position().x() -
-                                        feature.position().x());
-      float diff_y = static_cast<float>(p_lane_point->position().y() -
-                                        feature.position().y());
-      float angle = std::atan2(diff_y, diff_x);
-      feature_values->push_back(static_cast<float>(p_lane_point->heading()));
-      feature_values->push_back(static_cast<float>(p_lane_point->angle_diff()));
-      feature_values->push_back(static_cast<float>(
-          p_lane_point->relative_l() - feature.lane().lane_feature().lane_l()));
-      feature_values->push_back(angle);
-      ++counter;
-      if (counter > LENGTH_LANE_POINT_SEQUENCE) {
-        ADEBUG << "Full the lane point sequence";
-        break;
-      }
-    }
-  }
-  ADEBUG << "Lane sequence feature size: " << counter;
-  if (counter < DIM_LANE_POINT_FEATURE) {
-    AWARN << "Fail to setup lane feature!";
-    return -1;
-  }
-  return 0;
-}
-
-bool RNNEvaluator::IsCutinInHistory(const std::string& curr_lane_id,
-                                    const std::string& prev_lane_id) {
-  std::shared_ptr<const LaneInfo> curr_lane_info =
-      PredictionMap::LaneById(curr_lane_id);
-  std::shared_ptr<const LaneInfo> prev_lane_info =
-      PredictionMap::LaneById(prev_lane_id);
-  if (!PredictionMap::IsSuccessorLane(curr_lane_info, prev_lane_info)) {
-    return true;
-  }
-  return false;
-}
-
-}  // namespace prediction
-}  // namespace apollo

modules/prediction/evaluator/vehicle/rnn_evaluator.h

-/******************************************************************************
- * 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.
- *****************************************************************************/
-
-#pragma once
-
-#include <string>
-#include <unordered_map>
-#include <vector>
-
-#include "modules/prediction/evaluator/evaluator.h"
-#include "modules/prediction/network/rnn_model/rnn_model.h"
-
-namespace apollo {
-namespace prediction {
-
-class RNNEvaluator : public Evaluator {
- public:
-  /**
-   * @brief Constructor
-   */
-  RNNEvaluator();
-
-  /**
-   * @brief Destructor
-   */
-  virtual ~RNNEvaluator() = default;
-
-  /**
-   * @brief Override Evaluate
-   * @param Obstacle pointer
-   * @param Obstacles container
-   */
-  bool Evaluate(Obstacle* obstacle_ptr,
-                ObstaclesContainer* obstacles_container) override;
-
-  /**
-   * @brief Extract feature vector
-   * @param obstacle a pointer to the target obstacle
-   * @param obstacle_feature_mat feature matrix
-   * @param lane_feature_mats lane feature matrices
-   */
-  int ExtractFeatureValues(
-      Obstacle* obstacle, Eigen::MatrixXf* const obstacle_feature_mat,
-      std::unordered_map<int, Eigen::MatrixXf>* const lane_feature_mats);
-
-  /**
-   * @brief Get the name of evaluator.
-   */
-  std::string GetName() override { return "RNN_EVALUATOR"; }
-
-  /**
-   * @brief Clear
-   */
-  void Clear();
-
- private:
-  /**
-   * @brief Load model file
-   * @param Model file name
-   */
-  void LoadModel(const std::string& model_file);
-
-  int SetupObstacleFeature(Obstacle* obstacle,
-                           std::vector<float>* const feature_values);
-
-  int SetupLaneFeature(const Feature& feature,
-                       const LaneSequence& lane_sequence,
-                       std::vector<float>* const feature_values);
-
-  bool IsCutinInHistory(const std::string& curr_lane_id,
-                        const std::string& prev_lane_id);
-
- private:
-  static const int DIM_OBSTACLE_FEATURE = 6;
-  static const int DIM_LANE_POINT_FEATURE = 4;
-  static const int LENGTH_LANE_POINT_SEQUENCE = 20;
-  network::RnnModel* model_ptr_ = nullptr;
-};
-
-}  // namespace prediction
-}  // namespace apollo

modules/prediction/evaluator/vehicle/rnn_evaluator_test.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/evaluator/vehicle/rnn_evaluator.h"
-
-#include "cyber/common/file.h"
-#include "modules/prediction/common/kml_map_based_test.h"
-#include "modules/prediction/container/obstacles/obstacles_container.h"
-
-namespace apollo {
-namespace prediction {
-
-class RNNEvaluatorTest : public KMLMapBasedTest {
- public:
-  void SetUp() override {
-    const std::string file =
-        "modules/prediction/testdata/single_perception_vehicle_onlane.pb.txt";
-    CHECK(cyber::common::GetProtoFromFile(file, &perception_obstacles_));
-  }
-
- protected:
-  apollo::perception::PerceptionObstacles perception_obstacles_;
-};
-
-TEST_F(RNNEvaluatorTest, OnLaneCase) {
-  EXPECT_DOUBLE_EQ(perception_obstacles_.header().timestamp_sec(),
-                   1501183430.161906);
-  apollo::perception::PerceptionObstacle perception_obstacle =
-      perception_obstacles_.perception_obstacle(0);
-  EXPECT_EQ(perception_obstacle.id(), 1);
-  RNNEvaluator rnn_evaluator;
-  ObstaclesContainer container;
-  container.Insert(perception_obstacles_);
-  Obstacle* obstacle_ptr = container.GetObstacle(1);
-  EXPECT_NE(obstacle_ptr, nullptr);
-  rnn_evaluator.Evaluate(obstacle_ptr, &container);
-  const Feature& feature = obstacle_ptr->latest_feature();
-  const LaneGraph& lane_graph = feature.lane().lane_graph();
-  for (const auto& lane_sequence : lane_graph.lane_sequence()) {
-    EXPECT_TRUE(lane_sequence.has_probability());
-  }
-  rnn_evaluator.Clear();
-}
-
-}  // namespace prediction
-}  // namespace apollo

modules/prediction/proto/prediction_conf.proto

@@ -16,7 +16,7 @@ message ObstacleConf {
 
   enum EvaluatorType {
     MLP_EVALUATOR = 0;
-    RNN_EVALUATOR = 1;
+    RNN_EVALUATOR = 1 [deprecated = true];
     COST_EVALUATOR = 2;  // navi mode can only support this evaluator
     CRUISE_MLP_EVALUATOR = 3;
     JUNCTION_MLP_EVALUATOR = 4;

modules/prediction/submodules/predictor_submodule.cc

@@ -78,7 +78,7 @@ bool PredictorSubmodule::Proc(
       std::make_shared<PredictionObstacles>(prediction_obstacles));
 
   const absl::Time& end_time = absl::Now();
-  ADEBUG << "End to end time = "
+  AERROR << "End to end time = "
          << absl::ToDoubleMilliseconds(end_time - frame_start_time) << " ms";
 
   return true;