From e79ee61a2872a3f22bba9a2bdd8e58e2f94fda9d Mon Sep 17 00:00:00 2001 From: kechxu Date: Thu, 1 Mar 2018 21:38:45 -0800 Subject: [PATCH] Prediction: implement cost evaluator --- .../prediction/common/prediction_gflags.cc | 4 + modules/prediction/common/prediction_gflags.h | 3 + modules/prediction/evaluator/vehicle/BUILD | 21 ++++ .../evaluator/vehicle/cost_evaluator.cc | 95 +++++++++++++++++++ .../evaluator/vehicle/cost_evaluator.h | 57 +++++++++++ 5 files changed, 180 insertions(+) create mode 100644 modules/prediction/evaluator/vehicle/cost_evaluator.cc create mode 100644 modules/prediction/evaluator/vehicle/cost_evaluator.h diff --git a/modules/prediction/common/prediction_gflags.cc b/modules/prediction/common/prediction_gflags.cc index 28c17ba1b1..09d0021f07 100644 --- a/modules/prediction/common/prediction_gflags.cc +++ b/modules/prediction/common/prediction_gflags.cc @@ -105,6 +105,10 @@ DEFINE_bool(enable_adjust_velocity_heading, false, "adjust velocity heading to lane heading"); DEFINE_double(heading_filter_param, 0.99, "heading filter parameter"); +// Cost evaluator +DEFINE_double(cost_exp_coeff, 1.5, + "Coefficient of the exponential term in cost evaluator."); + // Obstacle trajectory DEFINE_double(lane_sequence_threshold, 0.5, "Threshold for trimming lane sequence trajectories"); diff --git a/modules/prediction/common/prediction_gflags.h b/modules/prediction/common/prediction_gflags.h index 048244e632..476a43df99 100644 --- a/modules/prediction/common/prediction_gflags.h +++ b/modules/prediction/common/prediction_gflags.h @@ -78,6 +78,9 @@ DECLARE_double(rnn_min_lane_relatice_s); DECLARE_bool(enable_adjust_velocity_heading); DECLARE_double(heading_filter_param); +// Cost evaluator +DECLARE_double(cost_exp_coeff); + // Obstacle trajectory DECLARE_double(lane_sequence_threshold); DECLARE_double(lane_change_dist); diff --git a/modules/prediction/evaluator/vehicle/BUILD b/modules/prediction/evaluator/vehicle/BUILD index 8916d6cd60..6af084a124 100644 --- a/modules/prediction/evaluator/vehicle/BUILD +++ b/modules/prediction/evaluator/vehicle/BUILD @@ -84,5 +84,26 @@ cc_test( ], ) +cc_library( + name = "cost_evaluator", + srcs = [ + "cost_evaluator.cc", + ], + hdrs = [ + "cost_evaluator.h", + ], + deps = [ + "//modules/common/math:math_utils", + "//modules/common/util", + "//modules/map/proto:map_proto", + "//modules/prediction/common:prediction_gflags", + "//modules/prediction/common:prediction_util", + "//modules/prediction/common:feature_output", + "//modules/prediction/container/obstacles:obstacle", + "//modules/prediction/evaluator", + "//modules/prediction/proto:feature_proto", + "//modules/prediction/proto:lane_graph_proto", + ], +) cpplint() diff --git a/modules/prediction/evaluator/vehicle/cost_evaluator.cc b/modules/prediction/evaluator/vehicle/cost_evaluator.cc new file mode 100644 index 0000000000..d09a5399d2 --- /dev/null +++ b/modules/prediction/evaluator/vehicle/cost_evaluator.cc @@ -0,0 +1,95 @@ +/****************************************************************************** + * Copyright 2018 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/cost_evaluator.h" +#include "modules/prediction/common/prediction_gflags.h" + +namespace apollo { +namespace prediction { + +void CostEvaluator::Evaluate(Obstacle* obstacle_ptr) { + CHECK_NOTNULL(obstacle_ptr); + int id = obstacle_ptr->id(); + if (!obstacle_ptr->latest_feature().IsInitialized()) { + AERROR << "Obstacle [" << id << "] has no latest feature."; + return; + } + + 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; + } + + double obstacle_length = 0.0; + if (latest_feature_ptr->has_length()) { + obstacle_length = latest_feature_ptr->length(); + } + double obstacle_width = 0.0; + if (latest_feature_ptr->has_width()) { + obstacle_width = latest_feature_ptr->width(); + } + CHECK(latest_feature_ptr->has_velocity_heading()); + double obstacle_heading = latest_feature_ptr->velocity_heading(); + + LaneGraph* lane_graph_ptr = + latest_feature_ptr->mutable_lane()->mutable_lane_graph(); + CHECK_NOTNULL(lane_graph_ptr); + if (lane_graph_ptr->lane_sequence_size() == 0) { + AERROR << "Obstacle [" << id << "] has no lane sequences."; + return; + } + + for (int i = 0; i < lane_graph_ptr->lane_sequence_size(); ++i) { + LaneSequence* lane_sequence_ptr = lane_graph_ptr->mutable_lane_sequence(i); + CHECK_NOTNULL(lane_sequence_ptr); + double probability = ComputeProbability(obstacle_length, + obstacle_width, obstacle_heading, *lane_sequence_ptr); + lane_sequence_ptr->set_probability(probability); + } +} + +double CostEvaluator::ComputeProbability( + const double obstacle_length, + const double obstacle_width, + const double obstacle_heading, + const LaneSequence& lane_sequence) { + if (lane_sequence.lane_segment_size() == 0 || + lane_sequence.lane_segment(0).lane_point_size() == 0) { + return 0.0; + } + const LanePoint& lane_point = + lane_sequence.lane_segment(0).lane_point(0); + double lane_l = -lane_point.relative_l(); + double distance = + lane_l - obstacle_length / 2.0 * std::sin(lane_point.angle_diff()); + if (lane_l > 0.0) { + distance -= obstacle_width / 2.0 * std::cos(lane_point.angle_diff()); + } else { + distance += obstacle_width / 2.0 * std::cos(lane_point.angle_diff()); + } + distance = std::abs(distance); + double half_lane_width = lane_point.width() / 2.0; + if (distance < half_lane_width + FLAGS_double_precision) { + return 1.0; + } + return std::exp(FLAGS_cost_exp_coeff * (half_lane_width - distance)); +} + +} // namespace prediction +} // namespace apollo diff --git a/modules/prediction/evaluator/vehicle/cost_evaluator.h b/modules/prediction/evaluator/vehicle/cost_evaluator.h new file mode 100644 index 0000000000..b7d7539d7f --- /dev/null +++ b/modules/prediction/evaluator/vehicle/cost_evaluator.h @@ -0,0 +1,57 @@ +/****************************************************************************** + * Copyright 2018 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. + *****************************************************************************/ + +#ifndef MODULES_PREDICTION_EVALUATOR_VEHICLE_COST_EVALUATOR_H_ +#define MODULES_PREDICTION_EVALUATOR_VEHICLE_COST_EVALUATOR_H_ + +#include "modules/prediction/container/obstacles/obstacle.h" +#include "modules/prediction/evaluator/evaluator.h" +#include "modules/prediction/proto/feature.pb.h" +#include "modules/prediction/proto/lane_graph.pb.h" + +namespace apollo { +namespace prediction { + +class CostEvaluator : public Evaluator { + public: + /** + * @brief Constructor + */ + CostEvaluator() = default; + + /** + * @brief Destructor + */ + virtual ~CostEvaluator() = default; + + /** + * @brief Override Evaluate + * @param Obstacle pointer + */ + void Evaluate(Obstacle* obstacle_ptr) override; + + private: + double ComputeProbability( + const double obstacle_length, + const double obstacle_width, + const double obstacle_heading, + const LaneSequence& lane_sequence); +}; + +} // namespace prediction +} // namespace apollo + +#endif // MODULES_PREDICTION_EVALUATOR_VEHICLE_COST_EVALUATOR_H_ -- GitLab