Planning: implemented second and third order derivative kernel in piecewise linear kernel.

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

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

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_;
 };