diff --git a/modules/prediction/common/validation_checker.cc b/modules/prediction/common/validation_checker.cc index ae4b07d6a8c854075bd1f653cb9cf6c12a682e65..306fffd135e319a4277c1958f8c80cd7199c391c 100644 --- a/modules/prediction/common/validation_checker.cc +++ b/modules/prediction/common/validation_checker.cc @@ -33,8 +33,8 @@ double ValidationChecker::ProbabilityByCentripetalAcceleration( double centripetal_acc_cost_sum = 0.0; double centripetal_acc_cost_sqr_sum = 0.0; for (int i = 0; i < lane_sequence.path_point_size(); ++i) { - const PathPoint& path_point = lane_sequence.path_point(i); - double centripetal_acc = speed * speed * path_point.kappa(); + const auto& path_point = lane_sequence.path_point(i); + double centripetal_acc = speed * speed * std::fabs(path_point.kappa()); double centripetal_acc_cost = centripetal_acc / FLAGS_centripedal_acc_threshold; centripetal_acc_cost_sum += centripetal_acc_cost; @@ -47,12 +47,8 @@ double ValidationChecker::ProbabilityByCentripetalAcceleration( bool ValidationChecker::ValidCentripetalAcceleration( const std::vector& trajectory_points) { - std::size_t num_point = trajectory_points.size(); - if (num_point < 2) { - return true; - } - double max_centripedal_acc = 0.0; - for (std::size_t i = 0; i + 1 < num_point; ++i) { + double max_centripetal_acc = 0.0; + for (auto i = 0; i + 1 < trajectory_points.size(); ++i) { const auto& first_point = trajectory_points[i]; const auto& second_point = trajectory_points[i + 1]; double theta_diff = std::abs(common::math::NormalizeAngle( @@ -63,11 +59,11 @@ bool ValidationChecker::ValidCentripetalAcceleration( if (time_diff < FLAGS_double_precision) { continue; } - double v = (first_point.v() + second_point.v()) / 2.0; + double v = (first_point.v() + second_point.v()) * 0.5; double centripedal_acc = v * theta_diff / time_diff; - max_centripedal_acc = std::max(max_centripedal_acc, centripedal_acc); + max_centripetal_acc = std::max(max_centripetal_acc, centripedal_acc); } - return max_centripedal_acc < FLAGS_centripedal_acc_threshold; + return max_centripetal_acc < FLAGS_centripedal_acc_threshold; } bool ValidationChecker::ValidTrajectoryPoint(