diff --git a/modules/prediction/common/feature_output.cc b/modules/prediction/common/feature_output.cc index 6c84f0a8989174534cea53c9a6461a81422c52c9..0fc79db8076c710627cb13b7942df7683bb2464f 100644 --- a/modules/prediction/common/feature_output.cc +++ b/modules/prediction/common/feature_output.cc @@ -90,7 +90,8 @@ void FeatureOutput::InsertDataForLearning( data_for_learning->add_features_for_learning(feature_values[i]); } data_for_learning->set_category(category); - ADEBUG << "Insert [" << category << "] data for learning"; + ADEBUG << "Insert [" << category << "] data for learning with size = " + << feature_values.size(); } void FeatureOutput::InsertPredictionResult( diff --git a/modules/prediction/common/prediction_gflags.cc b/modules/prediction/common/prediction_gflags.cc index 50093cd8113e814353796130a6e16ae0a92214ee..e09c3542217e0b7d368c4d5b21b4ef4271fb0ce9 100644 --- a/modules/prediction/common/prediction_gflags.cc +++ b/modules/prediction/common/prediction_gflags.cc @@ -158,6 +158,9 @@ DEFINE_double(centripetal_acc_coeff, 0.5, "Coefficient of centripetal acceleration probability"); // Junction Scenario +DEFINE_uint32(junction_historical_frame_length, 5, + "The number of historical frames of the obstacle" + "that the junction model will look at."); DEFINE_double(junction_exit_lane_threshold, 0.1, "If a lane extends out of the junction by this value," "consider it as a exit_lane."); diff --git a/modules/prediction/common/prediction_gflags.h b/modules/prediction/common/prediction_gflags.h index 1eb9aa9f6b1feb8c4a68822dcc4addc4266732d7..b9ff4d0d6af517123e2eb3df7c8614b27cb12d42 100644 --- a/modules/prediction/common/prediction_gflags.h +++ b/modules/prediction/common/prediction_gflags.h @@ -103,6 +103,7 @@ DECLARE_double(angle_threshold_to_junction_exit); DECLARE_double(centripetal_acc_coeff); // Junction Scenario +DECLARE_uint32(junction_historical_frame_length); DECLARE_double(junction_exit_lane_threshold); DECLARE_double(distance_beyond_junction); DECLARE_double(defualt_junction_range); diff --git a/modules/prediction/evaluator/vehicle/junction_mlp_evaluator.cc b/modules/prediction/evaluator/vehicle/junction_mlp_evaluator.cc index bbc0dc45efbfc89240a12bbcc41c30fc94dec9f8..f226da4fe3078f68f21ffd7b9b5a157f3557444f 100644 --- a/modules/prediction/evaluator/vehicle/junction_mlp_evaluator.cc +++ b/modules/prediction/evaluator/vehicle/junction_mlp_evaluator.cc @@ -18,6 +18,7 @@ #include #include +#include #include "cyber/common/file.h" #include "modules/common/adapters/proto/adapter_config.pb.h" @@ -91,14 +92,14 @@ void JunctionMLPEvaluator::Evaluate(Obstacle* obstacle_ptr) { probability = ComputeProbability(feature_values); } else { for (int i = 0; i < 12; ++i) { - probability.push_back(feature_values[3 + 6 * i]); + probability.push_back(feature_values[OBSTACLE_FEATURE_SIZE + + EGO_VEHICLE_FEATURE_SIZE + 8 * i]); } } for (double prob : probability) { latest_feature_ptr->mutable_junction_feature() ->add_junction_mlp_probability(prob); } - // assign all lane_sequence probability LaneGraph* lane_graph_ptr = latest_feature_ptr->mutable_lane()->mutable_lane_graph(); @@ -191,9 +192,41 @@ void JunctionMLPEvaluator::SetObstacleFeatureValues( ADEBUG << "Obstacle [" << obstacle_ptr->id() << "] has no position."; return; } + std::pair obs_curr_pos = std::make_pair( + feature.position().x(), feature.position().y()); + double obs_curr_heading = feature.velocity_heading(); + bool has_history = false; + std::vector> pos_history( + FLAGS_junction_historical_frame_length, std::make_pair(0.0, 0.0)); + + if (obstacle_ptr->history_size() > FLAGS_junction_historical_frame_length) { + has_history = true; + for (std::size_t i = 0; i < FLAGS_junction_historical_frame_length; ++i) { + const Feature& feature = obstacle_ptr->feature(i+1); + if (!feature.IsInitialized()) { + has_history = false; + break; + } + if (feature.has_position()) { + pos_history[i] = WorldCoordToObjCoord( + std::make_pair(feature.position().x(), feature.position().y()), + obs_curr_pos, obs_curr_heading); + } + } + } + feature_values->push_back(feature.speed()); feature_values->push_back(feature.acc()); feature_values->push_back(feature.junction_feature().junction_range()); + if (has_history) { + feature_values->push_back(1.0); + } else { + feature_values->push_back(0.0); + } + for (std::size_t i = 0; i < FLAGS_junction_historical_frame_length; i++) { + feature_values->push_back(pos_history[i].first); + feature_values->push_back(pos_history[i].second); + } } void JunctionMLPEvaluator::SetEgoVehicleFeatureValues( @@ -249,6 +282,8 @@ void JunctionMLPEvaluator::SetJunctionFeatureValues( std::string junction_id = feature_ptr->junction_feature().junction_id(); double junction_range = feature_ptr->junction_feature().junction_range(); for (int i = 0; i < 12; ++i) { + feature_values->push_back(0.0); + feature_values->push_back(0.0); feature_values->push_back(0.0); feature_values->push_back(1.0); feature_values->push_back(1.0); @@ -291,13 +326,15 @@ void JunctionMLPEvaluator::SetJunctionFeatureValues( std::abs(x_1 * y_2 - y_1 * x_2) / std::hypot(x_1, y_1)); t += FLAGS_prediction_trajectory_time_resolution; } - feature_values->operator[](idx * 6) = 1.0; - feature_values->operator[](idx * 6 + 1) = diff_x / junction_range; - feature_values->operator[](idx * 6 + 2) = diff_y / junction_range; - feature_values->operator[](idx * 6 + 3) = + feature_values->operator[](idx * 8) = 1.0; + feature_values->operator[](idx * 8 + 1) = diff_x / junction_range; + feature_values->operator[](idx * 8 + 2) = diff_y / junction_range; + feature_values->operator[](idx * 8 + 3) = std::sqrt(diff_x * diff_x + diff_y * diff_y) / junction_range; - feature_values->operator[](idx * 6 + 4) = diff_heading; - feature_values->operator[](idx * 6 + 5) = cost; + feature_values->operator[](idx * 8 + 4) = diff_x; + feature_values->operator[](idx * 8 + 5) = diff_y; + feature_values->operator[](idx * 8 + 6) = diff_heading; + feature_values->operator[](idx * 8 + 7) = cost; } } diff --git a/modules/prediction/evaluator/vehicle/junction_mlp_evaluator.h b/modules/prediction/evaluator/vehicle/junction_mlp_evaluator.h index 2f4781085c08f56ca4612fac8d92328fafd9826b..43167cf8c16241a294770d14667a97798ce66c04 100644 --- a/modules/prediction/evaluator/vehicle/junction_mlp_evaluator.h +++ b/modules/prediction/evaluator/vehicle/junction_mlp_evaluator.h @@ -100,9 +100,9 @@ class JunctionMLPEvaluator : public Evaluator { const std::vector& feature_values); private: - static const size_t OBSTACLE_FEATURE_SIZE = 3; + static const size_t OBSTACLE_FEATURE_SIZE = 4 + 2 * 5; static const size_t EGO_VEHICLE_FEATURE_SIZE = 4; - static const size_t JUNCTION_FEATURE_SIZE = 72; + static const size_t JUNCTION_FEATURE_SIZE = 12 * 8; std::unique_ptr model_ptr_; };