Prediction: implement cost evaluator

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");

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);

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()

modules/prediction/evaluator/vehicle/cost_evaluator.cc

+/******************************************************************************
+ * 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

modules/prediction/evaluator/vehicle/cost_evaluator.h

+/******************************************************************************
+ * 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_