Planning: added calculation of csc format matrix in osqp solver. (#392)

* Planning: added calculation of csc format matrix in osqp solver.

* Update osqp_spline_2d_solver_test.cc

modules/planning/math/smoothing_spline/BUILD

@@ -307,6 +307,7 @@ cc_test(
     name = "spline_2d_solver_test",
     size = "small",
     srcs = [
+        "osqp_spline_2d_solver_test.cc",
         "spline_2d_solver_test.cc",
     ],
     deps = [

modules/planning/math/smoothing_spline/osqp_spline_2d_solver.cc

@@ -22,7 +22,6 @@
 
 #include <algorithm>
 
-#include "Eigen/Core"
 #include "cybertron/common/log.h"
 
 #include "modules/common/math/qp_solver/qp_solver_gflags.h"
@@ -77,5 +76,25 @@ bool OsqpSpline2dSolver::Solve() {
 // extract
 const Spline2d& OsqpSpline2dSolver::spline() const { return spline_; }
 
+void OsqpSpline2dSolver::ToCSCMatrix(const MatrixXd& dense_matrix,
+                                     std::vector<double>* data,
+                                     std::vector<double>* indices,
+                                     std::vector<double>* indptr) const {
+  constexpr double epsilon = 1e-9;
+  int data_count = 0;
+  for (int c = 0; c < dense_matrix.cols(); ++c) {
+    indptr->emplace_back(data_count);
+    for (int r = 0; r < dense_matrix.cols(); ++r) {
+      if (std::fabs(dense_matrix(r, c)) < epsilon) {
+        continue;
+      }
+      data->emplace_back(dense_matrix(r, c));
+      ++data_count;
+      indices->emplace_back(r);
+    }
+  }
+  indptr->emplace_back(data_count);
+}
+
 }  // namespace planning
 }  // namespace apollo

modules/planning/math/smoothing_spline/osqp_spline_2d_solver.h

@@ -24,6 +24,8 @@
 #include <memory>
 #include <vector>
 
+#include "Eigen/Core"
+#include "gtest/gtest_prod.h"
 #include "osqp/include/osqp.h"
 
 #include "modules/planning/math/smoothing_spline/spline_2d.h"
@@ -49,6 +51,12 @@ class OsqpSpline2dSolver final : public Spline2dSolver {
   // extract
   const Spline2d& spline() const override;
 
+ private:
+  FRIEND_TEST(OSQPSolverTest, basic_test);
+  void ToCSCMatrix(const Eigen::MatrixXd& dense_matrix,
+                   std::vector<double>* data, std::vector<double>* indices,
+                   std::vector<double>* indptr) const;
+
  private:
   OSQPSettings* osqp_settings_ = nullptr;
   OSQPWorkspace* work_ = nullptr;  // Workspace

modules/planning/math/smoothing_spline/osqp_spline_2d_solver_test.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.
+ *****************************************************************************/
+
+/**
+ * @file
+ **/
+#include "modules/planning/math/smoothing_spline/osqp_spline_2d_solver.h"
+
+#include "gtest/gtest.h"
+
+#include "modules/planning/math/curve_math.h"
+
+namespace apollo {
+namespace planning {
+
+using apollo::common::math::Vec2d;
+using Eigen::MatrixXd;
+
+TEST(OSQPSolverTest, basic_test) {
+  std::vector<double> t_knots{0, 1, 2, 3, 4, 5};
+  std::size_t order = 5;
+  OsqpSpline2dSolver spline_solver(t_knots, order);
+  EXPECT_TRUE(spline_solver.Solve());
+
+  {
+    std::vector<double> data;
+    std::vector<double> indices;
+    std::vector<double> indptr;
+    Eigen::MatrixXd dense_matrix(3, 3);
+    dense_matrix << 1, 0, 2, 0, 0, 3, 4, 5, 6;
+    spline_solver.ToCSCMatrix(dense_matrix, &data, &indices, &indptr);
+
+    std::vector<double> data_golden = {1, 4, 5, 2, 3, 6};
+    std::vector<double> indices_golden = {0, 2, 2, 0, 1, 2};
+    std::vector<double> indptr_golden = {0, 2, 3, 6};
+
+    EXPECT_EQ(data.size(), data_golden.size());
+    EXPECT_EQ(indices.size(), indices_golden.size());
+    EXPECT_EQ(indptr.size(), indptr_golden.size());
+
+    for (size_t i = 0; i < data.size(); ++i) {
+      EXPECT_DOUBLE_EQ(data[i], data_golden[i]);
+    }
+    for (size_t i = 0; i < indices.size(); ++i) {
+      EXPECT_DOUBLE_EQ(indices[i], indices_golden[i]);
+    }
+    for (size_t i = 0; i < indptr.size(); ++i) {
+      EXPECT_DOUBLE_EQ(indptr[i], indptr_golden[i]);
+    }
+  }
+
+  {
+    std::vector<double> data;
+    std::vector<double> indices;
+    std::vector<double> indptr;
+    Eigen::MatrixXd dense_matrix(2, 2);
+    dense_matrix << 4.0, 1.0, 1.0, 2.0;
+    spline_solver.ToCSCMatrix(dense_matrix, &data, &indices, &indptr);
+
+    std::vector<double> data_golden = {4.0, 1.0, 1.0, 2.0};
+    std::vector<double> indices_golden = {0, 1, 0, 1};
+    std::vector<double> indptr_golden = {0, 2, 4};
+
+    EXPECT_EQ(data.size(), data_golden.size());
+    EXPECT_EQ(indices.size(), indices_golden.size());
+    EXPECT_EQ(indptr.size(), indptr_golden.size());
+
+    for (size_t i = 0; i < data.size(); ++i) {
+      EXPECT_DOUBLE_EQ(data[i], data_golden[i]);
+    }
+    for (size_t i = 0; i < indices.size(); ++i) {
+      EXPECT_DOUBLE_EQ(indices[i], indices_golden[i]);
+    }
+    for (size_t i = 0; i < indptr.size(); ++i) {
+      EXPECT_DOUBLE_EQ(indptr[i], indptr_golden[i]);
+    }
+  }
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/math/smoothing_spline/spline_2d_solver_test.cc

@@ -22,7 +22,6 @@
 #include "gtest/gtest.h"
 
 #include "modules/planning/math/curve_math.h"
-#include "modules/planning/math/smoothing_spline/osqp_spline_2d_solver.h"
 
 namespace apollo {
 namespace planning {
@@ -149,12 +148,5 @@ TEST(constraint_test, test_suit_one) {
   }
 }
 
-TEST(OSQPSolverTest, basic_test) {
-  std::vector<double> t_knots{0, 1, 2, 3, 4, 5};
-  std::size_t order = 5;
-  OsqpSpline2dSolver spline_solver(t_knots, order);
-  EXPECT_TRUE(spline_solver.Solve());
-}
-
 }  // namespace planning
 }  // namespace apollo