From dadd20fb6bc5c40a06733561c6d83c361aa5b357 Mon Sep 17 00:00:00 2001 From: kechxu Date: Sun, 8 Dec 2019 12:59:49 -0800 Subject: [PATCH] Prediction: deprecate kf motion and rnn evaluator --- .../container/obstacles/obstacle.cc | 181 ---------- .../prediction/container/obstacles/obstacle.h | 41 --- .../container/obstacles/obstacle_test.cc | 34 +- modules/prediction/evaluator/BUILD | 1 - .../prediction/evaluator/evaluator_manager.cc | 6 - modules/prediction/evaluator/vehicle/BUILD | 28 -- .../evaluator/vehicle/rnn_evaluator.cc | 309 ------------------ .../evaluator/vehicle/rnn_evaluator.h | 94 ------ .../evaluator/vehicle/rnn_evaluator_test.cc | 59 ---- .../prediction/proto/prediction_conf.proto | 2 +- .../submodules/predictor_submodule.cc | 2 +- 11 files changed, 13 insertions(+), 744 deletions(-) delete mode 100644 modules/prediction/evaluator/vehicle/rnn_evaluator.cc delete mode 100644 modules/prediction/evaluator/vehicle/rnn_evaluator.h delete mode 100644 modules/prediction/evaluator/vehicle/rnn_evaluator_test.cc diff --git a/modules/prediction/container/obstacles/obstacle.cc b/modules/prediction/container/obstacles/obstacle.cc index 95a5f226dd..88e63b1a4a 100644 --- a/modules/prediction/container/obstacles/obstacle.cc +++ b/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& Obstacle::kf_motion_tracker() const { - return kf_motion_tracker_; -} - const KalmanFilter& 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 F; - F.setIdentity(); - kf_motion_tracker_.SetTransitionMatrix(F); - - // Set observation matrix H - Eigen::Matrix 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 Q; - Q.setIdentity(); - Q *= FLAGS_q_var; - kf_motion_tracker_.SetTransitionNoise(Q); - - // Set observation noise matrix R - Eigen::Matrix 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 P; - P.setIdentity(); - P *= FLAGS_p_var; - - // Set initial state - Eigen::Matrix 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 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 F; @@ -1644,29 +1486,6 @@ void Obstacle::DiscardOutdatedHistory() { } } -void Obstacle::SetRNNStates(const std::vector& rnn_states) { - rnn_states_ = rnn_states; -} - -void Obstacle::GetRNNStates(std::vector* 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(); diff --git a/modules/prediction/container/obstacles/obstacle.h b/modules/prediction/container/obstacles/obstacle.h index 9ff7dd00c0..73c2059c4a 100644 --- a/modules/prediction/container/obstacles/obstacle.h +++ b/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& 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& rnn_states); - - /** - * @brief Get RNN state - * @param A pointer to RNN state matrix - */ - void GetRNNStates(std::vector* 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_history_; - common::math::KalmanFilter kf_motion_tracker_; - common::math::KalmanFilter kf_pedestrian_tracker_; std::vector> current_lanes_; - std::vector rnn_states_; - - bool rnn_enabled_ = false; - ObstacleConf obstacle_conf_; }; diff --git a/modules/prediction/container/obstacles/obstacle_test.cc b/modules/prediction/container/obstacles/obstacle_test.cc index 60f78ede29..fccf2e92da 100644 --- a/modules/prediction/container/obstacles/obstacle_test.cc +++ b/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) { diff --git a/modules/prediction/evaluator/BUILD b/modules/prediction/evaluator/BUILD index 32ba82fe57..c3c9dcb5f1 100644 --- a/modules/prediction/evaluator/BUILD +++ b/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", ], diff --git a/modules/prediction/evaluator/evaluator_manager.cc b/modules/prediction/evaluator/evaluator_manager.cc index 621b4d8428..bd94946c31 100644 --- a/modules/prediction/evaluator/evaluator_manager.cc +++ b/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 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; diff --git a/modules/prediction/evaluator/vehicle/BUILD b/modules/prediction/evaluator/vehicle/BUILD index c068693de7..d764b552f5 100644 --- a/modules/prediction/evaluator/vehicle/BUILD +++ b/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"], diff --git a/modules/prediction/evaluator/vehicle/rnn_evaluator.cc b/modules/prediction/evaluator/vehicle/rnn_evaluator.cc deleted file mode 100644 index 3e8bdda17e..0000000000 --- a/modules/prediction/evaluator/vehicle/rnn_evaluator.cc +++ /dev/null @@ -1,309 +0,0 @@ -/****************************************************************************** - * 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 -#include - -#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 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 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* const lane_feature_mats) { - std::vector obstacle_features; - std::vector lane_features; - if (SetupObstacleFeature(obstacle, &obstacle_features) != 0) { - ADEBUG << "Reset rnn state"; - obstacle->InitRNNStates(); - } - if (static_cast(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(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* 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(p_lane_fea->lane_l()); - theta = static_cast(p_lane_fea->angle_diff()); - dist_lb = static_cast(p_lane_fea->dist_to_left_boundary()); - dist_rb = static_cast(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(fea->speed()); - heading = static_cast(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* const feature_values) { - CHECK_LE(LENGTH_LANE_POINT_SEQUENCE, FLAGS_max_num_lane_point); - feature_values->clear(); - feature_values->reserve(static_cast(DIM_LANE_POINT_FEATURE) * - static_cast(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(p_lane_point->position().x() - - feature.position().x()); - float diff_y = static_cast(p_lane_point->position().y() - - feature.position().y()); - float angle = std::atan2(diff_y, diff_x); - feature_values->push_back(static_cast(p_lane_point->heading())); - feature_values->push_back(static_cast(p_lane_point->angle_diff())); - feature_values->push_back(static_cast( - 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 curr_lane_info = - PredictionMap::LaneById(curr_lane_id); - std::shared_ptr 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 diff --git a/modules/prediction/evaluator/vehicle/rnn_evaluator.h b/modules/prediction/evaluator/vehicle/rnn_evaluator.h deleted file mode 100644 index d7532c60ac..0000000000 --- a/modules/prediction/evaluator/vehicle/rnn_evaluator.h +++ /dev/null @@ -1,94 +0,0 @@ -/****************************************************************************** - * 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 -#include -#include - -#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* 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* const feature_values); - - int SetupLaneFeature(const Feature& feature, - const LaneSequence& lane_sequence, - std::vector* 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 diff --git a/modules/prediction/evaluator/vehicle/rnn_evaluator_test.cc b/modules/prediction/evaluator/vehicle/rnn_evaluator_test.cc deleted file mode 100644 index 577a3ca353..0000000000 --- a/modules/prediction/evaluator/vehicle/rnn_evaluator_test.cc +++ /dev/null @@ -1,59 +0,0 @@ -/****************************************************************************** - * 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 diff --git a/modules/prediction/proto/prediction_conf.proto b/modules/prediction/proto/prediction_conf.proto index dd23b7e4de..ca2d425f6d 100644 --- a/modules/prediction/proto/prediction_conf.proto +++ b/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; diff --git a/modules/prediction/submodules/predictor_submodule.cc b/modules/prediction/submodules/predictor_submodule.cc index 5919300f3a..62240fb0b7 100644 --- a/modules/prediction/submodules/predictor_submodule.cc +++ b/modules/prediction/submodules/predictor_submodule.cc @@ -78,7 +78,7 @@ bool PredictorSubmodule::Proc( std::make_shared(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; -- GitLab