Planning: used sparse matrix kernel, reduced calculation time from 84ms to 71ms

in the test; removed active_set related methord as planned.
Todo: replace affine matrix to sparse to further reduce calculation time.

modules/common/math/matrix_operations_test.cc

@@ -327,6 +327,59 @@ TEST(DENSE_TO_CSC_MATRIX, dense_to_csc_matrix_test) {
   }
 }
 
+TEST(DENSE_TO_CSC_MATRIX, patterned_dense_to_csc_matrix_test) {
+  int N = 5;
+  Eigen::MatrixXd dense_matrix = Eigen::MatrixXd::Zero(N * 3, N * 3);
+  for (int i = 0; i < N; ++i) {
+    dense_matrix(i, i) = 1.0;
+    if (i + 1 < N) {
+      dense_matrix(i, i + 1) = 1.0;
+    }
+    if (i > 0) {
+      dense_matrix(i, i - 1) = 1.0;
+    }
+  }
+
+  for (int i = 0; i < N; ++i) {
+    dense_matrix(i + N, i + N) = 1.0;
+  }
+
+  for (int i = 0; i < N; ++i) {
+    dense_matrix(i + 2 * N, i + 2 * N) = 1.0;
+    if (i + 1 < N) {
+      dense_matrix(i + 2 * N, i + 2 * N + 1) = 1.0;
+    }
+    if (i > 0) {
+      dense_matrix(i + 2 * N, i + 2 * N - 1) = 1.0;
+    }
+  }
+
+  for (int i = 0; i < dense_matrix.rows(); ++i) {
+    for (int j = 0; j < dense_matrix.cols(); ++j) {
+      std::cout << dense_matrix(i, j) << ", ";
+    }
+    std::cout << std::endl;
+  }
+  std::cout << std::endl;
+
+  std::vector<double> data;
+  std::vector<int> indices;
+  std::vector<int> indptr;
+
+  DenseToCSCMatrix(dense_matrix, &data, &indices, &indptr);
+  EXPECT_EQ(data.size(), indices.size());
+
+  for (const auto ind : indices) {
+    std::cout << ind << ", ";
+  }
+  std::cout << std::endl;
+
+  for (const auto ipr : indptr) {
+    std::cout << ipr << ", ";
+  }
+  std::cout << std::endl;
+}
+
 }  // namespace math
 }  // namespace common
 }  // namespace apollo

modules/planning/lattice/trajectory_generation/trajectory1d_generator.cc

@@ -31,7 +31,6 @@
 #include "modules/planning/lattice/trajectory1d/piecewise_jerk_trajectory1d.h"
 #include "modules/planning/lattice/trajectory1d/standing_still_trajectory1d.h"
 #include "modules/planning/lattice/trajectory_generation/lateral_trajectory_optimizer.h"
-#include "modules/planning/math/finite_element_qp/active_set_lateral_qp_optimizer.h"
 #include "modules/planning/math/finite_element_qp/osqp_lateral_qp_optimizer.h"
 
 namespace apollo {

modules/planning/math/finite_element_qp/BUILD

@@ -5,8 +5,6 @@ package(default_visibility = ["//visibility:public"])
 cc_library(
     name = "finite_element_qp",
     deps = [
-        ":active_set_augmented_lateral_qp_optimizer",
-        ":active_set_lateral_qp_optimizer",
         ":lateral_qp_optimizer",
         ":osqp_lateral_jerk_qp_optimizer",
         ":osqp_lateral_linear_qp_optimizer",
@@ -28,20 +26,6 @@ cc_library(
     ],
 )
 
-cc_library(
-    name = "active_set_lateral_qp_optimizer",
-    srcs = [
-        "active_set_lateral_qp_optimizer.cc",
-    ],
-    hdrs = [
-        "active_set_lateral_qp_optimizer.h",
-    ],
-    deps = [
-        ":lateral_qp_optimizer",
-        "@qpOASES",
-    ],
-)
-
 cc_library(
     name = "osqp_lateral_qp_optimizer",
     srcs = [
@@ -72,20 +56,6 @@ cc_library(
     ],
 )
 
-cc_library(
-    name = "active_set_augmented_lateral_qp_optimizer",
-    srcs = [
-        "active_set_augmented_lateral_qp_optimizer.cc",
-    ],
-    hdrs = [
-        "active_set_augmented_lateral_qp_optimizer.h",
-    ],
-    deps = [
-        ":lateral_qp_optimizer",
-        "@qpOASES",
-    ],
-)
-
 cc_library(
     name = "osqp_lateral_jerk_qp_optimizer",
     srcs = [

modules/planning/math/finite_element_qp/active_set_augmented_lateral_qp_optimizer.cc

-/******************************************************************************
- * Copyright 2018 The Apollo Authors. All Rights Reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *****************************************************************************/
-
-#include "modules/planning/math/finite_element_qp/active_set_augmented_lateral_qp_optimizer.h"
-
-#include "cybertron/common/log.h"
-#include "qpOASES.hpp"
-
-#include "modules/planning/common/planning_gflags.h"
-
-namespace apollo {
-namespace planning {
-
-bool ActiveSetAugmentedLateralQPOptimizer::optimize(
-    const std::array<double, 3>& d_state, const double delta_s,
-    const std::vector<std::pair<double, double>>& d_bounds) {
-  opt_d_.clear();
-  opt_d_prime_.clear();
-  opt_d_pprime_.clear();
-
-  delta_s_ = delta_s;
-  const int kNumVariable = d_bounds.size();
-
-  const int kNumConstraint = (kNumVariable - 3);
-  ::qpOASES::HessianType hessian_type = ::qpOASES::HST_POSDEF;
-  ::qpOASES::QProblem qp_problem(kNumVariable, kNumConstraint, hessian_type);
-  ::qpOASES::Options my_options;
-  my_options.enableRegularisation = ::qpOASES::BT_TRUE;
-
-  my_options.epsNum = -1e-3;
-  my_options.epsDen = 1e-3;
-  my_options.epsIterRef = 1e-3;
-  my_options.terminationTolerance = 1e-3;
-  qp_problem.setOptions(my_options);
-  qp_problem.setPrintLevel(qpOASES::PL_NONE);
-  int max_iter = 1000;
-
-  // Construct kernel matrix
-  const int kNumOfMatrixElement = kNumVariable * kNumVariable;
-  double h_matrix[kNumOfMatrixElement];
-  std::fill(h_matrix, h_matrix + kNumOfMatrixElement, 0.0);
-
-  // pre-calculate some const
-  const double delta_s_sq = delta_s * delta_s;
-  const double delta_s_quad = delta_s_sq * delta_s_sq;
-  const double one_over_delta_s_sq_coeff =
-      1.0 / delta_s_sq * FLAGS_weight_lateral_derivative;
-  const double one_over_delta_s_quad_coeff =
-      1.0 / delta_s_quad * FLAGS_weight_lateral_second_order_derivative;
-
-  for (int i = 0; i < kNumVariable; ++i) {
-    h_matrix[i * kNumVariable + i] += 2.0 * FLAGS_weight_lateral_offset;
-    h_matrix[i * kNumVariable + i] +=
-        2.0 * FLAGS_weight_lateral_obstacle_distance;
-
-    // first order derivative
-    int idx = (i + 1) * kNumVariable + (i + 1);
-    if (idx < kNumOfMatrixElement) {
-      h_matrix[idx] += 2 * one_over_delta_s_sq_coeff;
-      h_matrix[i * kNumVariable + i] += 2 * one_over_delta_s_sq_coeff;
-      h_matrix[i * kNumVariable + i + 1] += 2.0 * one_over_delta_s_sq_coeff;
-      h_matrix[(i + 1) * kNumVariable + i] += 2.0 * one_over_delta_s_sq_coeff;
-    }
-
-    // second order derivative
-    idx = (i + 2) * kNumVariable + (i + 2);
-    if (idx < kNumOfMatrixElement) {
-      h_matrix[idx] += 2.0 * one_over_delta_s_quad_coeff;
-      h_matrix[(i + 1) * kNumVariable + i + 1] +=
-          8.0 * one_over_delta_s_quad_coeff;
-      h_matrix[i * kNumVariable + i] += 2.0 * one_over_delta_s_quad_coeff;
-
-      h_matrix[(i)*kNumVariable + (i + 1)] +=
-          -4.0 * one_over_delta_s_quad_coeff;
-      h_matrix[(i + 1) * kNumVariable + i] +=
-          -4.0 * one_over_delta_s_quad_coeff;
-
-      h_matrix[(i + 1) * kNumVariable + (i + 2)] +=
-          -4.0 * one_over_delta_s_quad_coeff;
-      h_matrix[(i + 2) * kNumVariable + (i + 1)] +=
-          -4.0 * one_over_delta_s_quad_coeff;
-
-      h_matrix[i * kNumVariable + (i + 2)] += 2.0 * one_over_delta_s_quad_coeff;
-      h_matrix[(i + 2) * kNumVariable + i] += 2.0 * one_over_delta_s_quad_coeff;
-    }
-  }
-
-  // Construct offset vector
-  double g_matrix[kNumVariable];
-  for (int i = 0; i < kNumVariable; ++i) {
-    g_matrix[i] = -2.0 * FLAGS_weight_lateral_obstacle_distance *
-                  (d_bounds[i].first + d_bounds[i].second);
-  }
-
-  // Construct variable bounds
-  double param_lower_bounds[kNumVariable];
-  double param_upper_bounds[kNumVariable];
-  for (int i = 0; i < kNumVariable; ++i) {
-    param_lower_bounds[i] = d_bounds[i].first;
-    param_upper_bounds[i] = d_bounds[i].second;
-  }
-
-  // Construct constraint matrix
-  const int kNumOfConstraint = kNumVariable * kNumConstraint;
-  double affine_constraint_matrix[kNumOfConstraint];
-  std::fill(affine_constraint_matrix,
-            affine_constraint_matrix + kNumOfConstraint, 0.0);
-  double constraint_lower_bounds[kNumConstraint];
-  double constraint_upper_bounds[kNumConstraint];
-
-  int constraint_index = 0;
-
-  const double third_order_derivative_max_coff =
-      FLAGS_lateral_third_order_derivative_max * delta_s * delta_s * delta_s;
-  for (int i = 0; i + 3 < kNumVariable; ++i) {
-    int idx = i * kNumVariable + i;
-    affine_constraint_matrix[idx] = -1.0;
-    affine_constraint_matrix[idx + 1] = 3.0;
-    affine_constraint_matrix[idx + 2] = -3.0;
-    affine_constraint_matrix[idx + 3] = 1.0;
-    constraint_lower_bounds[constraint_index] =
-        -third_order_derivative_max_coff;
-    constraint_upper_bounds[constraint_index] = third_order_derivative_max_coff;
-    ++constraint_index;
-  }
-
-  const double kDeltaL = 1.0e-7;
-  int var_index = constraint_index * kNumVariable;
-  bool set_init_status_constraints = false;
-
-  // TODO(lianglia-apollo):
-  // Add initial status constraints here
-  if (set_init_status_constraints) {
-    affine_constraint_matrix[var_index] = 1.0;
-    constraint_lower_bounds[constraint_index] = d_state[0] - kDeltaL;
-    constraint_upper_bounds[constraint_index] = d_state[0] + kDeltaL;
-    ++constraint_index;
-
-    var_index = constraint_index * kNumVariable;
-    affine_constraint_matrix[var_index] = -1.0 / delta_s;
-    affine_constraint_matrix[var_index + 1] = 1.0 / delta_s;
-    constraint_lower_bounds[constraint_index] = d_state[1] - kDeltaL;
-    constraint_upper_bounds[constraint_index] = d_state[1] + kDeltaL;
-    ++constraint_index;
-
-    var_index = constraint_index * kNumVariable;
-    affine_constraint_matrix[var_index] = 1.0 / delta_s_sq;
-    affine_constraint_matrix[var_index + 1] = -2.0 / delta_s_sq;
-    affine_constraint_matrix[var_index + 2] = 1.0 / delta_s_sq;
-    constraint_lower_bounds[constraint_index] = d_state[2] - kDeltaL;
-    constraint_upper_bounds[constraint_index] = d_state[2] + kDeltaL;
-    ++constraint_index;
-  }
-
-  // TODO(lianglia-apollo):
-  // Verify whether it is necessary to keep final dl and ddl to 0.0
-
-  CHECK_EQ(constraint_index, kNumConstraint);
-
-  auto ret = qp_problem.init(h_matrix, g_matrix, affine_constraint_matrix,
-                             param_lower_bounds, param_upper_bounds,
-                             constraint_lower_bounds, constraint_upper_bounds,
-                             max_iter);
-  if (ret != qpOASES::SUCCESSFUL_RETURN) {
-    if (ret == qpOASES::RET_MAX_NWSR_REACHED) {
-      AERROR << "qpOASES solver failed due to reached max iteration";
-    } else {
-      AERROR << "qpOASES solver failed due to infeasibility or other internal "
-                "reasons";
-    }
-  }
-  double result[kNumVariable];
-  qp_problem.getPrimalSolution(result);
-  for (int i = 0; i + 2 < kNumVariable; ++i) {
-    opt_d_.push_back(result[i]);
-    if (i > 0) {
-      opt_d_prime_.push_back((result[i] - result[i - 1]) / delta_s);
-    }
-    if (i > 1) {
-      opt_d_pprime_.push_back((result[i + 2] - 2 * result[i + 1] + result[i]) /
-                              delta_s_sq);
-    }
-  }
-  opt_d_prime_.push_back(0.0);
-  opt_d_pprime_.push_back(0.0);
-  return qp_problem.isSolved() == qpOASES::BT_TRUE;
-}
-
-}  // namespace planning
-}  // namespace apollo

modules/planning/math/finite_element_qp/active_set_augmented_lateral_qp_optimizer.h

-/******************************************************************************
- * Copyright 2018 The Apollo Authors. All Rights Reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *****************************************************************************/
-
-/**
- * @file
- **/
-
-#pragma once
-
-#include <array>
-#include <memory>
-#include <utility>
-#include <vector>
-
-#include "modules/planning/lattice/trajectory1d/piecewise_jerk_trajectory1d.h"
-#include "modules/planning/math/finite_element_qp/lateral_qp_optimizer.h"
-
-namespace apollo {
-namespace planning {
-
-class ActiveSetAugmentedLateralQPOptimizer : public LateralQPOptimizer {
- public:
-  ActiveSetAugmentedLateralQPOptimizer() = default;
-
-  virtual ~ActiveSetAugmentedLateralQPOptimizer() = default;
-
-  bool optimize(
-      const std::array<double, 3>& d_state, const double delta_s,
-      const std::vector<std::pair<double, double>>& d_bounds) override;
-};
-
-}  // namespace planning
-}  // namespace apollo

modules/planning/math/finite_element_qp/active_set_lateral_qp_optimizer.cc

-/******************************************************************************
- * Copyright 2018 The Apollo Authors. All Rights Reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *****************************************************************************/
-
-#include "modules/planning/math/finite_element_qp/active_set_lateral_qp_optimizer.h"
-
-#include "cybertron/common/log.h"
-#include "qpOASES.hpp"
-
-#include "modules/planning/common/planning_gflags.h"
-
-namespace apollo {
-namespace planning {
-
-bool ActiveSetLateralQPOptimizer::optimize(
-    const std::array<double, 3>& d_state, const double delta_s,
-    const std::vector<std::pair<double, double>>& d_bounds) {
-  delta_s_ = delta_s;
-  const int num_var = static_cast<int>(d_bounds.size());
-  const int kNumParam = 3 * num_var;
-  const int kNumConstraint = 3 * (num_var - 1) + 5;
-  ::qpOASES::HessianType hessian_type = ::qpOASES::HST_POSDEF;
-  ::qpOASES::QProblem qp_problem(kNumParam, kNumConstraint, hessian_type);
-  ::qpOASES::Options my_options;
-  my_options.enableRegularisation = ::qpOASES::BT_TRUE;
-  // TODO(kechxu) move to gflags
-  my_options.epsNum = -1e-3;
-  my_options.epsDen = 1e-3;
-  my_options.epsIterRef = 1e-3;
-  my_options.terminationTolerance = 1e-3;
-  qp_problem.setOptions(my_options);
-  qp_problem.setPrintLevel(qpOASES::PL_NONE);
-  int max_iter = 1000;
-
-  // Construct kernel matrix
-  const int kNumOfMatrixElement = kNumParam * kNumParam;
-  double h_matrix[kNumOfMatrixElement];
-  std::fill(h_matrix, h_matrix + kNumOfMatrixElement, 0.0);
-  for (int i = 0; i < kNumParam; ++i) {
-    if (i < num_var) {
-      h_matrix[i * kNumParam + i] =
-          2.0 * FLAGS_weight_lateral_offset +
-          2.0 * FLAGS_weight_lateral_obstacle_distance;
-    } else if (i < 2 * num_var) {
-      h_matrix[i * kNumParam + i] = 2.0 * FLAGS_weight_lateral_derivative;
-    } else {
-      h_matrix[i * kNumParam + i] =
-          2.0 * FLAGS_weight_lateral_second_order_derivative;
-    }
-  }
-
-  // Construct offset vector
-  const int kNumOfOffsetRow = kNumParam;
-  double g_matrix[kNumOfOffsetRow];
-  for (int i = 0; i < kNumParam; ++i) {
-    if (i < num_var) {
-      g_matrix[i] = -2.0 * FLAGS_weight_lateral_obstacle_distance *
-                    (d_bounds[i].first + d_bounds[i].second);
-    } else {
-      g_matrix[i] = 0.0;
-    }
-  }
-
-  // Construct variable bounds
-  double param_lower_bounds[kNumParam];
-  double param_upper_bounds[kNumParam];
-  const double LARGE_VALUE = 2.0;
-  for (int i = 0; i < kNumParam; ++i) {
-    if (i < num_var) {
-      param_lower_bounds[i] = d_bounds[i].first;
-      param_upper_bounds[i] = d_bounds[i].second;
-    } else {
-      param_lower_bounds[i] = -LARGE_VALUE;
-      param_upper_bounds[i] = LARGE_VALUE;
-    }
-  }
-
-  // Construct constraint matrix
-  const int kNumOfConstraint = kNumParam * kNumConstraint;
-  double affine_constraint_matrix[kNumOfConstraint];
-  std::fill(affine_constraint_matrix,
-            affine_constraint_matrix + kNumOfConstraint, 0.0);
-  double constraint_lower_bounds[kNumConstraint];
-  double constraint_upper_bounds[kNumConstraint];
-  const int prime_offset = num_var;
-  const int pprime_offset = 2 * num_var;
-  int constraint_index = 0;
-  // d_i+1'' - d_i''
-  for (int i = 0; i + 1 < num_var; ++i) {
-    int pprime_index = constraint_index * kNumParam + pprime_offset + i;
-    affine_constraint_matrix[pprime_index + 1] = 1.0;
-    affine_constraint_matrix[pprime_index] = -1.0;
-    constraint_lower_bounds[constraint_index] =
-        -FLAGS_lateral_third_order_derivative_max * delta_s;
-    constraint_upper_bounds[constraint_index] =
-        FLAGS_lateral_third_order_derivative_max * delta_s;
-    ++constraint_index;
-  }
-  // d_i+1' - d_i' - 0.5 * ds * (d_i'' + d_i+1'')
-  for (int i = 0; i + 1 < num_var; ++i) {
-    int pprime_index = constraint_index * kNumParam + pprime_offset + i;
-    int prime_index = constraint_index * kNumParam + prime_offset + i;
-    affine_constraint_matrix[prime_index + 1] = 1.0;
-    affine_constraint_matrix[prime_index] = -1.0;
-    affine_constraint_matrix[pprime_index + 1] = -0.5 * delta_s;
-    affine_constraint_matrix[pprime_index] = -0.5 * delta_s;
-    constraint_lower_bounds[constraint_index] = 0.0;
-    constraint_upper_bounds[constraint_index] = 0.0;
-    ++constraint_index;
-  }
-  // d_i+1 - d_i - d_i' * ds - 1/3 * d_i'' * ds^2 - 1/6 * d_i+1'' * ds^2
-  for (int i = 0; i + 1 < num_var; ++i) {
-    int var_index = constraint_index * kNumParam + i;
-    int pprime_index = constraint_index * kNumParam + pprime_offset + i;
-    int prime_index = constraint_index * kNumParam + prime_offset + i;
-    affine_constraint_matrix[var_index + 1] = 1.0;
-    affine_constraint_matrix[var_index] = -1.0;
-    affine_constraint_matrix[prime_index] = -delta_s;
-    affine_constraint_matrix[pprime_index] = -delta_s * delta_s / 3.0;
-    affine_constraint_matrix[pprime_index + 1] = -delta_s * delta_s / 6.0;
-    constraint_lower_bounds[constraint_index] = 0.0;
-    constraint_upper_bounds[constraint_index] = 0.0;
-    ++constraint_index;
-  }
-
-  int var_index = constraint_index * kNumParam;
-  affine_constraint_matrix[var_index] = 1.0;
-  constraint_lower_bounds[constraint_index] = d_state[0];
-  constraint_upper_bounds[constraint_index] = d_state[0];
-  ++constraint_index;
-
-  int prime_index = constraint_index * kNumParam + prime_offset;
-  affine_constraint_matrix[prime_index] = 1.0;
-  constraint_lower_bounds[constraint_index] = d_state[1];
-  constraint_upper_bounds[constraint_index] = d_state[1];
-  ++constraint_index;
-
-  int pprime_index = constraint_index * kNumParam + pprime_offset;
-  affine_constraint_matrix[pprime_index] = 1.0;
-  constraint_lower_bounds[constraint_index] = d_state[2];
-  constraint_upper_bounds[constraint_index] = d_state[2];
-  ++constraint_index;
-
-  affine_constraint_matrix[prime_index + num_var - 1] = 1.0;
-  constraint_lower_bounds[constraint_index] = 0.0;
-  constraint_upper_bounds[constraint_index] = 0.0;
-  ++constraint_index;
-
-  affine_constraint_matrix[pprime_index + num_var - 1] = 1.0;
-  constraint_lower_bounds[constraint_index] = 0.0;
-  constraint_upper_bounds[constraint_index] = 0.0;
-  ++constraint_index;
-
-  CHECK_EQ(constraint_index, kNumConstraint);
-
-  auto ret = qp_problem.init(h_matrix, g_matrix, affine_constraint_matrix,
-                             param_lower_bounds, param_upper_bounds,
-                             constraint_lower_bounds, constraint_upper_bounds,
-                             max_iter);
-  if (ret != qpOASES::SUCCESSFUL_RETURN) {
-    if (ret == qpOASES::RET_MAX_NWSR_REACHED) {
-      AERROR << "qpOASES solver failed due to reached max iteration";
-    } else {
-      AERROR << "qpOASES solver failed due to infeasibility or other internal "
-                "reasons";
-    }
-  }
-  double result[kNumParam];
-  qp_problem.getPrimalSolution(result);
-  opt_d_.resize(num_var);
-  opt_d_prime_.resize(num_var);
-  opt_d_pprime_.resize(num_var);
-  for (int i = 0; i < num_var; ++i) {
-    opt_d_[i] = result[i];
-    opt_d_prime_[i] = result[num_var + i];
-    opt_d_pprime_[i] = result[2 * num_var + i];
-  }
-  opt_d_prime_[num_var - 1] = 0.0;
-  opt_d_pprime_[num_var - 1] = 0.0;
-  return qp_problem.isSolved() == qpOASES::BT_TRUE;
-}
-
-}  // namespace planning
-}  // namespace apollo

modules/planning/math/finite_element_qp/active_set_lateral_qp_optimizer.h

-/******************************************************************************
- * Copyright 2018 The Apollo Authors. All Rights Reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *****************************************************************************/
-
-/**
- * @file
- **/
-
-#pragma once
-
-#include <array>
-#include <memory>
-#include <utility>
-#include <vector>
-
-#include "modules/planning/lattice/trajectory1d/piecewise_jerk_trajectory1d.h"
-#include "modules/planning/math/finite_element_qp/lateral_qp_optimizer.h"
-
-namespace apollo {
-namespace planning {
-
-class ActiveSetLateralQPOptimizer : public LateralQPOptimizer {
- public:
-  ActiveSetLateralQPOptimizer() = default;
-
-  virtual ~ActiveSetLateralQPOptimizer() = default;
-
-  bool optimize(
-      const std::array<double, 3>& d_state, const double delta_s,
-      const std::vector<std::pair<double, double>>& d_bounds) override;
-};
-
-}  // namespace planning
-}  // namespace apollo

modules/planning/math/finite_element_qp/fem_1d_expanded_qp_problem.cc

@@ -84,20 +84,32 @@ bool Fem1dExpandedQpProblem::Optimize() {
 void Fem1dExpandedQpProblem::CalcualteKernel(std::vector<c_float>* P_data,
                                              std::vector<c_int>* P_indices,
                                              std::vector<c_int>* P_indptr) {
-  const size_t kNumParam = 3 * x_bounds_.size();
+  P_data->clear();
+  P_indices->clear();
+  P_indptr->clear();
+
+  const int kNumParam = 3 * num_var_;
+  const int N = num_var_;
 
   MatrixXd kernel = MatrixXd::Zero(kNumParam, kNumParam);  // dense matrix
 
-  for (size_t i = 0; i < kNumParam; ++i) {
-    if (i < num_var_) {
-      kernel(i, i) = 2.0 * weight_.x_w + 2.0 * weight_.x_mid_line_w;
-    } else if (i < 2 * num_var_) {
-      kernel(i, i) = 2.0 * weight_.x_derivative_w;
+  int idx_ptr = 0;
+  for (int i = 0; i < kNumParam; ++i) {
+    if (i < N) {
+      P_data->emplace_back(2.0 * weight_.x_w + 2.0 * weight_.x_mid_line_w);
+      P_indices->emplace_back(i);
+    } else if (i < 2 * N) {
+      P_data->emplace_back(2.0 * weight_.x_derivative_w);
+      P_indices->emplace_back(i);
     } else {
-      kernel(i, i) = 2.0 * weight_.x_second_order_derivative_w;
+      P_indices->emplace_back(i);
+      P_data->emplace_back(2.0 * weight_.x_second_order_derivative_w);
     }
+    P_indptr->emplace_back(idx_ptr);
+    idx_ptr += 1;
   }
-  DenseToCSCMatrix(kernel, P_data, P_indices, P_indptr);
+  P_indptr->emplace_back(idx_ptr);
+  DCHECK_EQ(P_data->size(), P_indices->size());
 }
 
 void Fem1dExpandedQpProblem::CalcualteOffset(std::vector<c_float>* q) {

modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/BUILD

@@ -30,7 +30,6 @@ cc_library(
         "//modules/planning/math/curve1d:polynomial_curve1d",
         "//modules/planning/math/curve1d:quintic_polynomial_curve1d",
         "//modules/planning/math/finite_element_qp",
-        "//modules/planning/math/smoothing_spline:active_set_spline_1d_solver",
         "//modules/planning/proto:planning_proto",
         "//modules/planning/reference_line",
         "//modules/planning/toolkits/optimizers:path_optimizer",

modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc

@@ -23,8 +23,6 @@
 #include <algorithm>
 
 #include "modules/common/time/time.h"
-#include "modules/planning/math/finite_element_qp/active_set_augmented_lateral_qp_optimizer.h"
-#include "modules/planning/math/finite_element_qp/active_set_lateral_qp_optimizer.h"
 #include "modules/planning/math/finite_element_qp/osqp_lateral_jerk_qp_optimizer.h"
 #include "modules/planning/math/finite_element_qp/osqp_lateral_linear_qp_optimizer.h"
 #include "modules/planning/math/finite_element_qp/osqp_lateral_qp_optimizer.h"
@@ -82,9 +80,7 @@ bool QpPiecewiseJerkPathOptimizer::Init(
     config_ = config.qp_piecewise_jerk_path_config();
   }
   // TODO(all): use gflags or config to turn on/off new algorithms
-  // lateral_qp_optimizer_.reset(new ActiveSetLateralQPOptimizer());
   // lateral_qp_optimizer_.reset(new OsqpLateralJerkQPOptimizer());
-  // lateral_qp_optimizer_.reset(new ActiveSetAugmentedLateralQPOptimizer());
   lateral_qp_optimizer_.reset(new OsqpLateralLinearQPOptimizer());
 
   is_init_ = true;