From 12d2f5ef9026fa7c33ad41022fade2b50385b146 Mon Sep 17 00:00:00 2001 From: Shu Jiang Date: Fri, 11 Sep 2020 10:31:40 -0700 Subject: [PATCH] Planning: using math util gaussian function --- .../piecewise_jerk_path_optimizer.cc | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/modules/planning/tasks/optimizers/piecewise_jerk_path/piecewise_jerk_path_optimizer.cc b/modules/planning/tasks/optimizers/piecewise_jerk_path/piecewise_jerk_path_optimizer.cc index 21a882064c..80a91f5125 100644 --- a/modules/planning/tasks/optimizers/piecewise_jerk_path/piecewise_jerk_path_optimizer.cc +++ b/modules/planning/tasks/optimizers/piecewise_jerk_path/piecewise_jerk_path_optimizer.cc @@ -23,6 +23,7 @@ #include #include +#include "modules/common/math/math_utils.h" #include "modules/common/util/point_factory.h" #include "modules/planning/common/planning_context.h" #include "modules/planning/common/planning_gflags.h" @@ -36,6 +37,7 @@ namespace planning { using apollo::common::ErrorCode; using apollo::common::Status; using apollo::common::VehicleConfigHelper; +using apollo::common::math::Gaussian; PiecewiseJerkPathOptimizer::PiecewiseJerkPathOptimizer( const TaskConfig& config, @@ -368,8 +370,13 @@ double PiecewiseJerkPathOptimizer::EstimateJerkBoundary( double PiecewiseJerkPathOptimizer::GaussianWeighting( const double x, const double peak_weighting, const double peak_weighting_x) const { - return peak_weighting * - exp(-0.5 * (x - peak_weighting_x) * (x - peak_weighting_x)); + double std = 1 / (std::sqrt(2 * M_PI) * peak_weighting); + double u = peak_weighting_x * std; + double x_updated = x * std; + ADEBUG << peak_weighting * + exp(-0.5 * (x - peak_weighting_x) * (x - peak_weighting_x)); + ADEBUG << Gaussian(u, std, x_updated); + return Gaussian(u, std, x_updated); } } // namespace planning -- GitLab