From 42050d646a00e922068def8ceb99fdc9d0de5a14 Mon Sep 17 00:00:00 2001 From: Liangliang Zhang Date: Thu, 20 Sep 2018 20:38:14 -0700 Subject: [PATCH] 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 | 1 + .../smoothing_spline/osqp_spline_2d_solver.cc | 21 ++++- .../smoothing_spline/osqp_spline_2d_solver.h | 8 ++ .../osqp_spline_2d_solver_test.cc | 94 +++++++++++++++++++ .../smoothing_spline/spline_2d_solver_test.cc | 8 -- 5 files changed, 123 insertions(+), 9 deletions(-) create mode 100644 modules/planning/math/smoothing_spline/osqp_spline_2d_solver_test.cc diff --git a/modules/planning/math/smoothing_spline/BUILD b/modules/planning/math/smoothing_spline/BUILD index 7a443a5553..d4c7426b65 100644 --- a/modules/planning/math/smoothing_spline/BUILD +++ b/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 = [ diff --git a/modules/planning/math/smoothing_spline/osqp_spline_2d_solver.cc b/modules/planning/math/smoothing_spline/osqp_spline_2d_solver.cc index e73f6ca01e..ed934d10d4 100644 --- a/modules/planning/math/smoothing_spline/osqp_spline_2d_solver.cc +++ b/modules/planning/math/smoothing_spline/osqp_spline_2d_solver.cc @@ -22,7 +22,6 @@ #include -#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* data, + std::vector* indices, + std::vector* 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 diff --git a/modules/planning/math/smoothing_spline/osqp_spline_2d_solver.h b/modules/planning/math/smoothing_spline/osqp_spline_2d_solver.h index 7ecd4304c2..e9922b0784 100644 --- a/modules/planning/math/smoothing_spline/osqp_spline_2d_solver.h +++ b/modules/planning/math/smoothing_spline/osqp_spline_2d_solver.h @@ -24,6 +24,8 @@ #include #include +#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* data, std::vector* indices, + std::vector* indptr) const; + private: OSQPSettings* osqp_settings_ = nullptr; OSQPWorkspace* work_ = nullptr; // Workspace diff --git a/modules/planning/math/smoothing_spline/osqp_spline_2d_solver_test.cc b/modules/planning/math/smoothing_spline/osqp_spline_2d_solver_test.cc new file mode 100644 index 0000000000..3c74f8df8d --- /dev/null +++ b/modules/planning/math/smoothing_spline/osqp_spline_2d_solver_test.cc @@ -0,0 +1,94 @@ +/****************************************************************************** + * 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 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 data; + std::vector indices; + std::vector 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 data_golden = {1, 4, 5, 2, 3, 6}; + std::vector indices_golden = {0, 2, 2, 0, 1, 2}; + std::vector 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 data; + std::vector indices; + std::vector 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 data_golden = {4.0, 1.0, 1.0, 2.0}; + std::vector indices_golden = {0, 1, 0, 1}; + std::vector 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 diff --git a/modules/planning/math/smoothing_spline/spline_2d_solver_test.cc b/modules/planning/math/smoothing_spline/spline_2d_solver_test.cc index 27898dcae0..f6aa1a692b 100644 --- a/modules/planning/math/smoothing_spline/spline_2d_solver_test.cc +++ b/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 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 -- GitLab