提交 5ed85f98 编写于 作者: Y Yajia Zhang 提交者: Jiangtao Hu

prediction: deprecate pedestrian acceleration

上级 63b18bb2
......@@ -98,7 +98,6 @@ DEFINE_int32(max_num_nearby_lane_in_junction, 1,
"Max number to search nearby lanes");
DEFINE_double(max_lane_angle_diff_in_junction, M_PI / 6.0,
"Max angle difference for a candidate lane");
DEFINE_bool(enable_pedestrian_acc, false, "Enable calculating speed by acc");
DEFINE_double(coeff_mul_sigma, 2.0, "coefficient multiply standard deviation");
DEFINE_double(pedestrian_max_speed, 10.0, "speed upper bound for pedestrian");
DEFINE_double(pedestrian_max_acc, 2.0, "maximum pedestrian acceleration");
......
......@@ -68,7 +68,6 @@ DECLARE_double(max_lane_angle_diff);
DECLARE_int32(max_num_current_lane_in_junction);
DECLARE_int32(max_num_nearby_lane_in_junction);
DECLARE_double(max_lane_angle_diff_in_junction);
DECLARE_bool(enable_pedestrian_acc);
DECLARE_double(coeff_mul_sigma);
DECLARE_double(pedestrian_max_speed);
DECLARE_double(pedestrian_max_acc);
......
......@@ -894,10 +894,8 @@ void Obstacle::UpdateKFPedestrianTracker(const Feature& feature) {
Eigen::Matrix<double, 4, 1> u;
u(0, 0) = feature.velocity().x();
u(1, 0) = feature.velocity().y();
if (FLAGS_enable_pedestrian_acc) {
u(2, 0) = feature.acceleration().x();
u(3, 0) = feature.acceleration().y();
}
u(2, 0) = 0.0;
u(3, 0) = 0.0;
kf_pedestrian_tracker_.Predict(u);
......
......@@ -30,13 +30,12 @@
#include "Eigen/Dense"
#include "modules/common/filters/digital_filter.h"
#include "modules/common/math/kalman_filter.h"
#include "modules/common/proto/error_code.pb.h"
#include "modules/map/hdmap/hdmap_common.h"
#include "modules/perception/proto/perception_obstacle.pb.h"
#include "modules/prediction/proto/feature.pb.h"
#include "modules/common/math/kalman_filter.h"
#include "modules/map/hdmap/hdmap_common.h"
/**
* @namespace apollo::prediction
* @brief apollo::prediction
......@@ -303,14 +302,22 @@ class Obstacle {
private:
int id_ = -1;
perception::PerceptionObstacle::Type type_ =
perception::PerceptionObstacle::UNKNOWN_UNMOVABLE;
std::deque<Feature> feature_history_;
common::math::KalmanFilter<double, 6, 2, 0> kf_motion_tracker_;
common::math::KalmanFilter<double, 2, 2, 4> kf_pedestrian_tracker_;
common::DigitalFilter heading_filter_;
std::vector<std::shared_ptr<const hdmap::LaneInfo>> current_lanes_;
std::vector<Eigen::MatrixXf> rnn_states_;
bool rnn_enabled_ = false;
};
......
......@@ -131,9 +131,6 @@ void RegionalPredictor::GenerateMovingTrajectory(const Obstacle* obstacle,
Eigen::Vector2d position(feature.position().x(), feature.position().y());
Eigen::Vector2d velocity(feature.velocity().x(), feature.velocity().y());
Eigen::Vector2d acc(0.0, 0.0);
if (FLAGS_enable_pedestrian_acc) {
acc = {feature.acceleration().x(), feature.acceleration().y()};
}
const double total_time = FLAGS_prediction_trajectory_time_length;
std::vector<TrajectoryPoint> left_points;
......@@ -239,10 +236,6 @@ void RegionalPredictor::GetTrajectoryCandidatePoints(
u.setZero();
u(0, 0) = velocity.x();
u(1, 0) = velocity.y();
if (FLAGS_enable_pedestrian_acc) {
u(2, 0) = acceleration.x();
u(3, 0) = acceleration.y();
}
kf.SetControlMatrix(B);
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册