diff --git a/modules/prediction/common/prediction_gflags.cc b/modules/prediction/common/prediction_gflags.cc index c53096d441ce63d29727a956e6713591fe445c82..5398574fd71ce427c39d4d1d6d71acbba01e6d3f 100644 --- a/modules/prediction/common/prediction_gflags.cc +++ b/modules/prediction/common/prediction_gflags.cc @@ -178,6 +178,10 @@ DEFINE_double(defualt_junction_range, 10.0, // Evaluator DEFINE_double(time_to_center_if_not_reach, 10.0, "Default value of time to lane center of not reach."); +DEFINE_double(default_s_if_no_obstacle_in_lane_sequence, 1000.0, + "The default s value if no obstacle in the lane sequence."); +DEFINE_double(default_l_if_no_obstacle_in_lane_sequence, 10.0, + "The default l value if no obstacle in the lane sequence."); // Obstacle trajectory DEFINE_bool(enable_cruise_regression, false, diff --git a/modules/prediction/common/prediction_gflags.h b/modules/prediction/common/prediction_gflags.h index 124f7f7a7027ded66cb0eb725db1a338b14801b0..3011f77e31a0c3ce50dd06af66ff3395272c8083 100644 --- a/modules/prediction/common/prediction_gflags.h +++ b/modules/prediction/common/prediction_gflags.h @@ -114,6 +114,8 @@ DECLARE_double(defualt_junction_range); // Evaluator DECLARE_double(time_to_center_if_not_reach); +DECLARE_double(default_s_if_no_obstacle_in_lane_sequence); +DECLARE_double(default_l_if_no_obstacle_in_lane_sequence); // Obstacle trajectory DECLARE_bool(enable_cruise_regression); diff --git a/modules/prediction/container/obstacles/obstacles_container.cc b/modules/prediction/container/obstacles/obstacles_container.cc index 6c8d869ae1894369c2be02fddfcb70af141467c6..d6962b8232d0f2ebfc4281d201b51ad40d7cd6df 100644 --- a/modules/prediction/container/obstacles/obstacles_container.cc +++ b/modules/prediction/container/obstacles/obstacles_container.cc @@ -143,11 +143,6 @@ void ObstaclesContainer::BuildLaneGraph() { ADEBUG << "Ignore obstacle [" << obstacle_ptr->id() << "]"; continue; } - // TODO(all) check - // if (obstacle_ptr->HasJunctionFeatureWithExits() && - // !obstacle_ptr->IsClosedToJunctionExit()) { - // continue; - // } obstacle_ptr->BuildLaneGraph(); } } diff --git a/modules/prediction/evaluator/vehicle/cruise_mlp_evaluator.cc b/modules/prediction/evaluator/vehicle/cruise_mlp_evaluator.cc index b0bc9370076f937114d0919bd209291c53f0c978..f24ad88ae8f1f22e3254ec3376caf655abddda01 100644 --- a/modules/prediction/evaluator/vehicle/cruise_mlp_evaluator.cc +++ b/modules/prediction/evaluator/vehicle/cruise_mlp_evaluator.cc @@ -143,17 +143,17 @@ void CruiseMLPEvaluator::Evaluate(Obstacle* obstacle_ptr) { ExtractFeatureValues(obstacle_ptr, lane_sequence_ptr, &feature_values); if (feature_values.size() != OBSTACLE_FEATURE_SIZE + INTERACTION_FEATURE_SIZE + - LANE_FEATURE_SIZE) { + SINGLE_LANE_FEATURE_SIZE * LANE_POINTS_SIZE) { lane_sequence_ptr->set_probability(0.0); ADEBUG << "Skip lane sequence due to incorrect feature size"; continue; } Eigen::MatrixXf obs_feature_mat = VectorToMatrixXf(feature_values, 0, OBSTACLE_FEATURE_SIZE); - // TODO(kechxu) move 5 and 30 to gflags Eigen::MatrixXf lane_feature_mat = VectorToMatrixXf(feature_values, OBSTACLE_FEATURE_SIZE + INTERACTION_FEATURE_SIZE, - static_cast(feature_values.size()), 5, 30); + static_cast(feature_values.size()), SINGLE_LANE_FEATURE_SIZE, + LANE_POINTS_SIZE); Eigen::MatrixXf model_output; if (lane_sequence_ptr->vehicle_on_lane()) { go_model_ptr_->Run({lane_feature_mat, obs_feature_mat}, &model_output); @@ -214,7 +214,8 @@ void CruiseMLPEvaluator::ExtractFeatureValues // Extract lane related features. std::vector lane_feature_values; SetLaneFeatureValues(obstacle_ptr, lane_sequence_ptr, &lane_feature_values); - if (lane_feature_values.size() != LANE_FEATURE_SIZE) { + if (lane_feature_values.size() != + SINGLE_LANE_FEATURE_SIZE * LANE_POINTS_SIZE) { ADEBUG << "Obstacle [" << id << "] has fewer than " << "expected lane feature_values" << lane_feature_values.size() << "."; @@ -439,10 +440,10 @@ void CruiseMLPEvaluator::SetInteractionFeatureValues(Obstacle* obstacle_ptr, // Initialize forward and backward obstacles NearbyObstacle forward_obstacle; NearbyObstacle backward_obstacle; - forward_obstacle.set_s(1000.0); // TODO(kechxu) move to gflags - forward_obstacle.set_l(10.0); // TODO(kechxu) move to gflags - backward_obstacle.set_s(-1000.0); // TODO(kechxu) move to gflags - backward_obstacle.set_l(10.0); // TODO(kechxu) move to gflags + forward_obstacle.set_s(FLAGS_default_s_if_no_obstacle_in_lane_sequence); + forward_obstacle.set_l(FLAGS_default_l_if_no_obstacle_in_lane_sequence); + backward_obstacle.set_s(-FLAGS_default_s_if_no_obstacle_in_lane_sequence); + backward_obstacle.set_l(FLAGS_default_l_if_no_obstacle_in_lane_sequence); for (const auto& nearby_obstacle : lane_sequence_ptr->nearby_obstacle()) { if (nearby_obstacle.s() < 0.0) { @@ -496,7 +497,7 @@ void CruiseMLPEvaluator::SetLaneFeatureValues std::vector* feature_values) { // Sanity checks. feature_values->clear(); - feature_values->reserve(LANE_FEATURE_SIZE); + feature_values->reserve(SINGLE_LANE_FEATURE_SIZE * LANE_POINTS_SIZE); const Feature& feature = obstacle_ptr->latest_feature(); if (!feature.IsInitialized()) { ADEBUG << "Obstacle [" << obstacle_ptr->id() << "] has no latest feature."; @@ -509,12 +510,13 @@ void CruiseMLPEvaluator::SetLaneFeatureValues double heading = feature.velocity_heading(); double speed = feature.speed(); for (int i = 0; i < lane_sequence_ptr->lane_segment_size(); ++i) { - if (feature_values->size() >= LANE_FEATURE_SIZE) { + if (feature_values->size() >= SINGLE_LANE_FEATURE_SIZE * LANE_POINTS_SIZE) { break; } const LaneSegment& lane_segment = lane_sequence_ptr->lane_segment(i); for (int j = 0; j < lane_segment.lane_point_size(); ++j) { - if (feature_values->size() >= LANE_FEATURE_SIZE) { + if (feature_values->size() >= + SINGLE_LANE_FEATURE_SIZE * LANE_POINTS_SIZE) { break; } const LanePoint& lane_point = lane_segment.lane_point(j); @@ -535,30 +537,12 @@ void CruiseMLPEvaluator::SetLaneFeatureValues feature_values->push_back(relative_ang); feature_values->push_back(speed * speed * lane_point.kappa()); feature_values->push_back(lane_point.kappa()); - /* - double diff_x = lane_point.position().x() - feature.position().x(); - double diff_y = lane_point.position().y() - feature.position().y(); - double angle = std::atan2(diff_y, diff_x); - double deviation_angle = angle - heading; - double relative_l = std::sqrt(diff_x * diff_x + diff_y * diff_y) - * std::sin(deviation_angle); - - // The lateral deviation of the lane-point from vehicle facing: - feature_values->push_back(relative_l); - // The extrapolated normal acceleration: - feature_values->push_back(speed * speed * lane_point.kappa()); - // The lane-point's heading w.r.t. vehicle's facing direction: - feature_values->push_back(common::math::AngleDiff - (heading, lane_point.heading())); - feature_values->push_back(diff_x); - feature_values->push_back(diff_y); - */ } } // If the lane points are not sufficient, apply a linear extrapolation. std::size_t size = feature_values->size(); - while (size >= 10 && size < LANE_FEATURE_SIZE) { + while (size >= 10 && size < SINGLE_LANE_FEATURE_SIZE * LANE_POINTS_SIZE) { double relative_l_new = 2 * feature_values->operator[](size - 5) - feature_values->operator[](size - 10); double relative_s_new = 2 * feature_values->operator[](size - 4) - @@ -573,30 +557,9 @@ void CruiseMLPEvaluator::SetLaneFeatureValues feature_values->push_back(0.0); size = feature_values->size(); - /* - double diff_x_new = 2 * feature_values->operator[](size - 2) - - feature_values->operator[](size - 7); - double diff_y_new = 2 * feature_values->operator[](size - 1) - - feature_values->operator[](size - 6); - double angle_new = std::atan2(diff_y_new, diff_x_new); - double deviation_angle_new = angle_new - heading; - double relative_l_new = std::sqrt(diff_x_new * diff_x_new + - diff_y_new * diff_y_new) - * std::sin(deviation_angle_new); - double centri_acc_new = 0.0; - double angle_diff_new = feature_values->operator[](size - 3); - - feature_values->push_back(relative_l_new); - feature_values->push_back(centri_acc_new); - feature_values->push_back(angle_diff_new); - feature_values->push_back(diff_x_new); - feature_values->push_back(diff_y_new); - size = feature_values->size(); - */ } } -// TODO(all): uncomment this once the model is trained and ready. void CruiseMLPEvaluator::LoadModels(const std::string& go_model_file, const std::string& cutin_model_file) { go_model_ptr_.reset(new network::CruiseModel()); diff --git a/modules/prediction/evaluator/vehicle/cruise_mlp_evaluator.h b/modules/prediction/evaluator/vehicle/cruise_mlp_evaluator.h index 9bb2a8bdea80fb11764d3b17c927fb8d337a0759..35ac348bfc6cee30af174d58f88bde71ef092d25 100644 --- a/modules/prediction/evaluator/vehicle/cruise_mlp_evaluator.h +++ b/modules/prediction/evaluator/vehicle/cruise_mlp_evaluator.h @@ -112,7 +112,8 @@ class CruiseMLPEvaluator : public Evaluator { private: static const size_t OBSTACLE_FEATURE_SIZE = 23 + 24; static const size_t INTERACTION_FEATURE_SIZE = 8; - static const size_t LANE_FEATURE_SIZE = 150; + static const size_t SINGLE_LANE_FEATURE_SIZE = 5; + static const size_t LANE_POINTS_SIZE = 30; std::shared_ptr go_model_ptr_; std::shared_ptr cutin_model_ptr_; diff --git a/modules/prediction/prediction_component.cc b/modules/prediction/prediction_component.cc index a454c42f0de3ea043a79692bc26396bd5cc70c51..0b8e2da9a2d45bf1a444d93fea691e179c65a816 100644 --- a/modules/prediction/prediction_component.cc +++ b/modules/prediction/prediction_component.cc @@ -220,7 +220,6 @@ void PredictionComponent::OnPerception( diff = end_time4 - end_time3; ADEBUG << "Time to build junction features: " << diff.count() * 1000 << " msec."; - // TODO(kechxu) refactor logic of build lane graph and build junction feature ptr_obstacles_container->BuildLaneGraph(); auto end_time5 = std::chrono::system_clock::now(); diff = end_time5 - end_time4;