From a896368edd3164b6e97512ea40a0419445ebdd24 Mon Sep 17 00:00:00 2001 From: Zhang Liangliang Date: Mon, 11 Sep 2017 21:35:29 -0700 Subject: [PATCH] Planning: implemented second and third order derivative kernel in piecewise linear kernel. --- .../piecewise_linear_generator.cc | 2 +- .../piecewise_linear_kernel.cc | 77 ++++++++++++++++++- .../piecewise_linear_kernel.h | 3 +- 3 files changed, 77 insertions(+), 5 deletions(-) diff --git a/modules/planning/math/smoothing_spline/piecewise_linear_generator.cc b/modules/planning/math/smoothing_spline/piecewise_linear_generator.cc index 401daa2d4d..f76ec22096 100644 --- a/modules/planning/math/smoothing_spline/piecewise_linear_generator.cc +++ b/modules/planning/math/smoothing_spline/piecewise_linear_generator.cc @@ -35,7 +35,7 @@ PiecewiseLinearGenerator::PiecewiseLinearGenerator( unit_segment_(unit_segment), total_t_(num_of_segments * unit_segment), constraint_(num_of_segments + 1, unit_segment), - kernel_(num_of_segments + 1) { + kernel_(num_of_segments + 1, unit_segment) { CHECK_GE(num_of_segments, 3); } diff --git a/modules/planning/math/smoothing_spline/piecewise_linear_kernel.cc b/modules/planning/math/smoothing_spline/piecewise_linear_kernel.cc index e792606154..2c68a160d2 100644 --- a/modules/planning/math/smoothing_spline/piecewise_linear_kernel.cc +++ b/modules/planning/math/smoothing_spline/piecewise_linear_kernel.cc @@ -24,8 +24,10 @@ namespace apollo { namespace planning { -PiecewiseLinearKernel::PiecewiseLinearKernel(const uint32_t dimension) +PiecewiseLinearKernel::PiecewiseLinearKernel(const uint32_t dimension, + const double unit_segment) : dimension_(dimension), + unit_segment_(unit_segment), kernel_matrix_(Eigen::MatrixXd::Zero(dimension_, dimension_)), offset_matrix_(Eigen::MatrixXd::Zero(dimension_, 1)) {} @@ -49,11 +51,80 @@ void PiecewiseLinearKernel::AddDerivativeKernelMatrix(const double weight) { void PiecewiseLinearKernel::AddSecondOrderDerivativeMatrix( const double weight) { - // TODO(Liangliang): Implement this function. + Eigen::MatrixXd second_derivative_matrix = + Eigen::MatrixXd::Zero(dimension_, dimension_); + for (std::size_t i = 1; i < dimension_; ++i) { + if (i == 1) { + second_derivative_matrix(i, i) += 1.0; + } else if (i == 2) { + second_derivative_matrix(i, i) += 1.0; + second_derivative_matrix(i - 1, i - 1) += 4.0; + second_derivative_matrix(i - 1, i) += -2.0; + second_derivative_matrix(i, i - 1) += -2.0; + } else { + second_derivative_matrix(i, i) += 1.0; + second_derivative_matrix(i - 1, i - 1) += 4.0; + second_derivative_matrix(i - 2, i - 2) += 1.0; + second_derivative_matrix(i - 1, i) += -2.0; + second_derivative_matrix(i, i - 1) += -2.0; + second_derivative_matrix(i - 2, i - 1) += -2.0; + second_derivative_matrix(i - 1, i - 2) += -2.0; + second_derivative_matrix(i, i - 2) += 1.0; + second_derivative_matrix(i - 2, i) += 1.0; + } + } + second_derivative_matrix *= 2.0 * weight / std::pow(unit_segment_, 4); + kernel_matrix_ += second_derivative_matrix; } void PiecewiseLinearKernel::AddThirdOrderDerivativeMatrix(const double weight) { - // TODO(Liangliang): Implement this function. + Eigen::MatrixXd jerk_matrix = Eigen::MatrixXd::Zero(dimension_, dimension_); + for (std::size_t i = 0; i < dimension_; ++i) { + if (i == 1) { + jerk_matrix(i, i) += 1.0; + } else if (i == 2) { + jerk_matrix(i - 1, i - 1) += 9.0; + jerk_matrix(i, i) += 1.0; + + jerk_matrix(i - 1, i) += -3.0; + jerk_matrix(i, i - 1) += -3.0; + } else if (i == 3) { + jerk_matrix(i - 2, i - 2) += 9.0; + jerk_matrix(i - 1, i - 1) += 9.0; + jerk_matrix(i, i) += 1.0; + + jerk_matrix(i - 1, i) += -3.0; + jerk_matrix(i, i - 1) += -3.0; + jerk_matrix(i - 2, i) += 3.0; + jerk_matrix(i, i - 2) += 3.0; + + jerk_matrix(i - 2, i - 1) += -9.0; + jerk_matrix(i - 1, i - 2) += -9.0; + } else { + jerk_matrix(i - 3, i - 3) += 1.0; + jerk_matrix(i - 2, i - 2) += 9.0; + jerk_matrix(i - 1, i - 1) += 9.0; + jerk_matrix(i, i) += 1.0; + + jerk_matrix(i - 1, i) += -3.0; + jerk_matrix(i, i - 1) += -3.0; + jerk_matrix(i - 2, i) += 3.0; + jerk_matrix(i, i - 2) += 3.0; + jerk_matrix(i - 3, i) += -1.0; + jerk_matrix(i, i - 3) += -1.0; + + jerk_matrix(i - 2, i - 1) += -9.0; + jerk_matrix(i - 1, i - 2) += -9.0; + jerk_matrix(i - 3, i - 1) += 3.0; + jerk_matrix(i - 1, i - 3) += 3.0; + + jerk_matrix(i - 3, i - 2) += -3.0; + jerk_matrix(i - 2, i - 3) += -3.0; + } + } + + jerk_matrix *= 2.0 * weight / std::pow(unit_segment_, 4); + kernel_matrix_ += jerk_matrix; } // reference line kernel diff --git a/modules/planning/math/smoothing_spline/piecewise_linear_kernel.h b/modules/planning/math/smoothing_spline/piecewise_linear_kernel.h index 1e7c5b50a5..a71ce6be55 100644 --- a/modules/planning/math/smoothing_spline/piecewise_linear_kernel.h +++ b/modules/planning/math/smoothing_spline/piecewise_linear_kernel.h @@ -31,7 +31,7 @@ namespace planning { class PiecewiseLinearKernel { public: - explicit PiecewiseLinearKernel(const uint32_t dimension); + PiecewiseLinearKernel(const uint32_t dimension, const double unit_segment); void AddRegularization(const double regularized_param); @@ -49,6 +49,7 @@ class PiecewiseLinearKernel { private: const uint32_t dimension_; + const double unit_segment_; Eigen::MatrixXd kernel_matrix_; Eigen::MatrixXd offset_matrix_; }; -- GitLab