diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index cc976d668d7d..9914e008b048 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -83,4 +83,4 @@ repos: args: ["--quiet"] exclude: ".cu" -exclude: ".svg|common/autoware_auto_perception_rviz_plugin" +exclude: ".svg|control/trajectory_follower|control/trajectory_follower_nodes|common/autoware_auto_dependencies|common/autoware_auto_perception_rviz_plugin|common/osqp_interface|simulator/simple_planning_simulator|planning/freespace_planner|planning/astar_search|planning/costmap_generator" diff --git a/common/interpolation/CMakeLists.txt b/common/interpolation/CMakeLists.txt new file mode 100644 index 000000000000..64abec9a6a27 --- /dev/null +++ b/common/interpolation/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.5) +project(interpolation) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(interpolation SHARED + src/linear_interpolation.cpp + src/spline_interpolation.cpp +) + +# Test +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + + file(GLOB_RECURSE test_files test/**/*.cpp) + + ament_add_gtest(test_interpolation ${test_files}) + + target_link_libraries(test_interpolation + interpolation + ) +endif() + +ament_auto_package() diff --git a/common/interpolation/README.md b/common/interpolation/README.md new file mode 100644 index 000000000000..51922bd85440 --- /dev/null +++ b/common/interpolation/README.md @@ -0,0 +1,109 @@ +# Interpolation package + +This package supplies linear and spline interpolation functions. + +## Linear Interpolation + +`lerp(src_val, dst_val, ratio)` (for scalar interpolation) interpolates `src_val` and `dst_val` with `ratio`. +This will be replaced with `std::lerp(src_val, dst_val, ratio)` in `C++20`. + +`lerp(base_keys, base_values, query_keys)` (for vector interpolation) applies linear regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`. +Then it calculates interpolated values on y-axis for `query_keys` on x-axis. + +## Spline Interpolation + +`slerp(base_keys, base_values, query_keys)` (for vector interpolation) applies spline regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`. +Then it calculates interpolated values on y-axis for `query_keys` on x-axis. + +### Evaluation of calculation cost + +We evaluated calculation cost of spline interpolation for 100 points, and adopted the best one which is tridiagonal matrix algorithm. +Methods except for tridiagonal matrix algorithm exists in `spline_interpolation` package, which has been removed from Autoware. + +| Method | Calculation time | +| --------------------------------- | ---------------- | +| Tridiagonal Matrix Algorithm | 0.007 [ms] | +| Preconditioned Conjugate Gradient | 0.024 [ms] | +| Successive Over-Relaxation | 0.074 [ms] | + +### Spline Interpolation Algorithm + +Assuming that the size of `base_keys` ($x_i$) and `base_values` ($y_i$) are $N + 1$, we aim to calculate spline interpolation with the following equation to interpolate between $y_i$ and $y_{i+1}$. + +$$ +Y_i(x) = a_i (x - x_i)^3 + b_i (x - x_i)^2 + c_i (x - x_i) + d_i \ \ \ (i = 0, \dots, N-1) +$$ + +Constraints on spline interpolation are as follows. +The number of constraints is $4N$, which is equal to the number of variables of spline interpolation. + +$$ +\begin{align} +Y_i (x_i) & = y_i \ \ \ (i = 0, \dots, N-1) \\ +Y_i (x_{i+1}) & = y_{i+1} \ \ \ (i = 0, \dots, N-1) \\ +Y'_i (x_{i+1}) & = Y'_{i+1} (x_{i+1}) \ \ \ (i = 0, \dots, N-2) \\ +Y''_i (x_{i+1}) & = Y''_{i+1} (x_{i+1}) \ \ \ (i = 0, \dots, N-2) \\ +Y''_0 (x_0) & = 0 \\ +Y''_{N-1} (x_N) & = 0 +\end{align} +$$ + +According to [this article](https://www.mk-mode.com/rails/docs/INTERPOLATION_SPLINE.pdf), spline interpolation is formulated as the following linear equation. + +$$ +\begin{align} + \begin{pmatrix} + 2(h_0 + h_1) & h_1 \\ + h_0 & 2 (h_1 + h_2) & h_2 & & O \\ + & & & \ddots \\ + O & & & & h_{N-2} & 2 (h_{N-2} + h_{N-1}) + \end{pmatrix} + \begin{pmatrix} + v_1 \\ v_2 \\ v_3 \\ \vdots \\ v_{N-1} + \end{pmatrix}= + \begin{pmatrix} + w_1 \\ w_2 \\ w_3 \\ \vdots \\ w_{N-1} + \end{pmatrix} +\end{align} +$$ + +where + +$$ +\begin{align} +h_i & = x_{i+1} - x_i \ \ \ (i = 0, \dots, N-1) \\ +w_i & = 6 \left(\frac{y_{i+1} - y_{i+1}}{h_i} - \frac{y_i - y_{i-1}}{h_{i-1}}\right) \ \ \ (i = 1, \dots, N-1) +\end{align} +$$ + +The coefficient matrix of this linear equation is tridiagonal matrix. Therefore, it can be solve with tridiagonal matrix algorithm, which can solve linear equations without gradient descent methods. + +Solving this linear equation with tridiagonal matrix algorithm, we can calculate coefficients of spline interpolation as follows. + +$$ +\begin{align} +a_i & = \frac{v_{i+1} - v_i}{6 (x_{i+1} - x_i)} \ \ \ (i = 0, \dots, N-1) \\ +b_i & = \frac{v_i}{2} \ \ \ (i = 0, \dots, N-1) \\ +c_i & = \frac{y_{i+1} - y_i}{x_{i+1} - x_i} - \frac{1}{6}(x_{i+1} - x_i)(2 v_i + v_{i+1}) \ \ \ (i = 0, \dots, N-1) \\ +d_i & = y_i \ \ \ (i = 0, \dots, N-1) +\end{align} +$$ + +### Tridiagonal Matrix Algorithm + +We solve tridiagonal linear equation according to [this article](https://mathlang.hatenablog.com/entry/2018/10/14/232741) where variables of linear equation are expressed as follows in the implementation. + +$$ +\begin{align} + \begin{pmatrix} + b_0 & c_0 & & \\ + a_0 & b_1 & c_2 & O \\ + & & \ddots \\ + O & & a_{N-2} & b_{N-1} + \end{pmatrix} +x = +\begin{pmatrix} + d_0 \\ d_2 \\ d_3 \\ \vdots \\ d_{N-1} + \end{pmatrix} +\end{align} +$$ diff --git a/common/interpolation/include/interpolation/interpolation_utils.hpp b/common/interpolation/include/interpolation/interpolation_utils.hpp new file mode 100644 index 000000000000..98049fbd715b --- /dev/null +++ b/common/interpolation/include/interpolation/interpolation_utils.hpp @@ -0,0 +1,87 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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. + +#ifndef INTERPOLATION__INTERPOLATION_UTILS_HPP_ +#define INTERPOLATION__INTERPOLATION_UTILS_HPP_ + +#include +#include +#include + +namespace interpolation_utils +{ +inline bool isIncreasing(const std::vector & x) +{ + if (x.empty()) { + throw std::invalid_argument("Points is empty."); + } + + for (size_t i = 0; i < x.size() - 1; ++i) { + if (x.at(i) >= x.at(i + 1)) { + return false; + } + } + + return true; +} + +inline bool isNotDecreasing(const std::vector & x) +{ + if (x.empty()) { + throw std::invalid_argument("Points is empty."); + } + + for (size_t i = 0; i < x.size() - 1; ++i) { + if (x.at(i) > x.at(i + 1)) { + return false; + } + } + + return true; +} + +inline void validateInput( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys) +{ + // when vectors are empty + if (base_keys.empty() || base_values.empty() || query_keys.empty()) { + throw std::invalid_argument("Points is empty."); + } + + // when size of vectors are less than 2 + if (base_keys.size() < 2 || base_values.size() < 2) { + throw std::invalid_argument( + "The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()) + + ", base_values.size() = " + std::to_string(base_values.size())); + } + + // when indices are not sorted + if (!isIncreasing(base_keys) || !isNotDecreasing(query_keys)) { + throw std::invalid_argument("Either base_keys or query_keys is not sorted."); + } + + // when query_keys is out of base_keys (This function does not allow exterior division.) + if (query_keys.front() < base_keys.front() || base_keys.back() < query_keys.back()) { + throw std::invalid_argument("query_keys is out of base_keys"); + } + + // when sizes of indices and values are not same + if (base_keys.size() != base_values.size()) { + throw std::invalid_argument("The size of base_keys and base_values are not the same."); + } +} +} // namespace interpolation_utils + +#endif // INTERPOLATION__INTERPOLATION_UTILS_HPP_ diff --git a/common/interpolation/include/interpolation/linear_interpolation.hpp b/common/interpolation/include/interpolation/linear_interpolation.hpp new file mode 100644 index 000000000000..181e4dd302b6 --- /dev/null +++ b/common/interpolation/include/interpolation/linear_interpolation.hpp @@ -0,0 +1,31 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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. + +#ifndef INTERPOLATION__LINEAR_INTERPOLATION_HPP_ +#define INTERPOLATION__LINEAR_INTERPOLATION_HPP_ + +#include "interpolation/interpolation_utils.hpp" + +#include + +namespace interpolation +{ +double lerp(const double src_val, const double dst_val, const double ratio); + +std::vector lerp( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys); +} // namespace interpolation + +#endif // INTERPOLATION__LINEAR_INTERPOLATION_HPP_ diff --git a/common/interpolation/include/interpolation/spline_interpolation.hpp b/common/interpolation/include/interpolation/spline_interpolation.hpp new file mode 100644 index 000000000000..d429fe750297 --- /dev/null +++ b/common/interpolation/include/interpolation/spline_interpolation.hpp @@ -0,0 +1,48 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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. + +#ifndef INTERPOLATION__SPLINE_INTERPOLATION_HPP_ +#define INTERPOLATION__SPLINE_INTERPOLATION_HPP_ + +#include +#include +#include +#include +#include + +namespace interpolation +{ +// NOTE: X(s) = a_i (s - s_i)^3 + b_i (s - s_i)^2 + c_i (s - s_i) + d_i : (i = 0, 1, ... N-1) +struct MultiSplineCoef +{ + explicit MultiSplineCoef(const size_t num_spline) + { + a.resize(num_spline); + b.resize(num_spline); + c.resize(num_spline); + d.resize(num_spline); + } + + std::vector a; + std::vector b; + std::vector c; + std::vector d; +}; + +std::vector slerp( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys); +} // namespace interpolation + +#endif // INTERPOLATION__SPLINE_INTERPOLATION_HPP_ diff --git a/common/interpolation/package.xml b/common/interpolation/package.xml new file mode 100644 index 000000000000..0d1090567f91 --- /dev/null +++ b/common/interpolation/package.xml @@ -0,0 +1,18 @@ + + + + interpolation + 0.1.0 + The spline interpolation package + Fumiya Watanabe + Takayuki Murooka + Apache License 2.0 + ament_cmake_auto + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/interpolation/src/linear_interpolation.cpp b/common/interpolation/src/linear_interpolation.cpp new file mode 100644 index 000000000000..8f5544e74ebb --- /dev/null +++ b/common/interpolation/src/linear_interpolation.cpp @@ -0,0 +1,52 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 "interpolation/linear_interpolation.hpp" + +#include + +namespace interpolation +{ +double lerp(const double src_val, const double dst_val, const double ratio) +{ + return src_val + (dst_val - src_val) * ratio; +} + +std::vector lerp( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys) +{ + // throw exception for invalid arguments + interpolation_utils::validateInput(base_keys, base_values, query_keys); + + // calculate linear interpolation + std::vector query_values; + size_t key_index = 0; + for (const auto query_key : query_keys) { + while (base_keys.at(key_index + 1) < query_key) { + ++key_index; + } + + const double src_val = base_values.at(key_index); + const double dst_val = base_values.at(key_index + 1); + const double ratio = (query_key - base_keys.at(key_index)) / + (base_keys.at(key_index + 1) - base_keys.at(key_index)); + + const double interpolated_val = lerp(src_val, dst_val, ratio); + query_values.push_back(interpolated_val); + } + + return query_values; +} +} // namespace interpolation diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/interpolation/src/spline_interpolation.cpp new file mode 100644 index 000000000000..525784adf600 --- /dev/null +++ b/common/interpolation/src/spline_interpolation.cpp @@ -0,0 +1,169 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 "interpolation/spline_interpolation.hpp" + +#include "interpolation/interpolation_utils.hpp" + +#include + +namespace +{ +// solve Ax = d +// where A is tridiagonal matrix +// [b_0 c_0 ... ] +// [a_0 b_1 c_1 ... O ] +// A = [ ... ] +// [ O ... a_N-3 b_N-2 c_N-2] +// [ ... a_N-2 b_N-1] +struct TDMACoef +{ + explicit TDMACoef(const size_t num_row) + { + a.resize(num_row - 1); + b.resize(num_row); + c.resize(num_row - 1); + d.resize(num_row); + } + + std::vector a; + std::vector b; + std::vector c; + std::vector d; +}; + +std::vector solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma_coef) +{ + const auto & a = tdma_coef.a; + const auto & b = tdma_coef.b; + const auto & c = tdma_coef.c; + const auto & d = tdma_coef.d; + + const size_t num_row = b.size(); + + std::vector x(num_row); + if (num_row != 1) { + // calculate p and q + std::vector p; + std::vector q; + p.push_back(-c[0] / b[0]); + q.push_back(d[0] / b[0]); + + for (size_t i = 1; i < num_row; ++i) { + const double den = b[i] + a[i - 1] * p[i - 1]; + p.push_back(-c[i - 1] / den); + q.push_back((d[i] - a[i - 1] * q[i - 1]) / den); + } + + // calculate solution + x[num_row - 1] = q[num_row - 1]; + + for (size_t i = 1; i < num_row; ++i) { + const size_t j = num_row - 1 - i; + x[j] = p[j] * x[j + 1] + q[j]; + } + } else { + x.push_back(d[0] / b[0]); + } + + return x; +} + +interpolation::MultiSplineCoef generateSplineCoefficients( + const std::vector & base_keys, const std::vector & base_values) +{ + const size_t num_base = base_keys.size(); // N+1 + + std::vector diff_keys; // N + std::vector diff_values; // N + for (size_t i = 0; i < num_base - 1; ++i) { + diff_keys.push_back(base_keys.at(i + 1) - base_keys.at(i)); + diff_values.push_back(base_values.at(i + 1) - base_values.at(i)); + } + + std::vector v = {0.0}; + if (num_base > 2) { + // solve tridiagonal matrix algorithm + TDMACoef tdma_coef(num_base - 2); // N-1 + + for (size_t i = 0; i < num_base - 2; ++i) { + tdma_coef.b[i] = 2 * (diff_keys[i] + diff_keys[i + 1]); + if (i != num_base - 3) { + tdma_coef.a[i] = diff_keys[i + 1]; + tdma_coef.c[i] = diff_keys[i + 1]; + } + tdma_coef.d[i] = + 6.0 * (diff_values[i + 1] / diff_keys[i + 1] - diff_values[i] / diff_keys[i]); + } + + const std::vector tdma_res = solveTridiagonalMatrixAlgorithm(tdma_coef); + + // calculate v + v.insert(v.end(), tdma_res.begin(), tdma_res.end()); + } + v.push_back(0.0); + + // calculate a, b, c, d of spline coefficients + interpolation::MultiSplineCoef multi_spline_coef(num_base - 1); // N + for (size_t i = 0; i < num_base - 1; ++i) { + multi_spline_coef.a[i] = (v[i + 1] - v[i]) / 6.0 / diff_keys[i]; + multi_spline_coef.b[i] = v[i] / 2.0; + multi_spline_coef.c[i] = + diff_values[i] / diff_keys[i] - diff_keys[i] * (2 * v[i] + v[i + 1]) / 6.0; + multi_spline_coef.d[i] = base_values[i]; + } + + return multi_spline_coef; +} + +std::vector getSplineInterpolatedValues( + const std::vector & base_keys, const std::vector & query_keys, + const interpolation::MultiSplineCoef & multi_spline_coef) +{ + const auto & a = multi_spline_coef.a; + const auto & b = multi_spline_coef.b; + const auto & c = multi_spline_coef.c; + const auto & d = multi_spline_coef.d; + + std::vector res; + size_t j = 0; + for (const auto & query_key : query_keys) { + while (base_keys.at(j + 1) < query_key) { + ++j; + } + + const double ds = query_key - base_keys.at(j); + res.push_back(d.at(j) + (c.at(j) + (b.at(j) + a.at(j) * ds) * ds) * ds); + } + + return res; +} +} // namespace + +namespace interpolation +{ +std::vector slerp( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys) +{ + // throw exceptions for invalid arguments + interpolation_utils::validateInput(base_keys, base_values, query_keys); + + // calculate spline coefficients + const auto multi_spline_coef = generateSplineCoefficients(base_keys, base_values); + + // interpolate base_keys at query_keys + return getSplineInterpolatedValues(base_keys, query_keys, multi_spline_coef); +} +} // namespace interpolation diff --git a/common/interpolation/test/src/test_interpolation.cpp b/common/interpolation/test/src/test_interpolation.cpp new file mode 100644 index 000000000000..ebb93a0a8568 --- /dev/null +++ b/common/interpolation/test/src/test_interpolation.cpp @@ -0,0 +1,21 @@ +// Copyright 2021 TierIV +// +// 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 + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/interpolation/test/src/test_interpolation_utils.cpp b/common/interpolation/test/src/test_interpolation_utils.cpp new file mode 100644 index 000000000000..c2d8fc9dd4a6 --- /dev/null +++ b/common/interpolation/test/src/test_interpolation_utils.cpp @@ -0,0 +1,108 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 "interpolation/interpolation_utils.hpp" + +#include + +#include + +constexpr double epsilon = 1e-6; + +TEST(interpolation_utils, isIncreasing) +{ + // empty + const std::vector empty_vec; + EXPECT_THROW(interpolation_utils::isIncreasing(empty_vec), std::invalid_argument); + + // increase + const std::vector increasing_vec{0.0, 1.5, 3.0, 4.5, 6.0}; + EXPECT_EQ(interpolation_utils::isIncreasing(increasing_vec), true); + + // not decrease + const std::vector not_increasing_vec{0.0, 1.5, 1.5, 4.5, 6.0}; + EXPECT_EQ(interpolation_utils::isIncreasing(not_increasing_vec), false); + + // decrease + const std::vector decreasing_vec{0.0, 1.5, 1.2, 4.5, 6.0}; + EXPECT_EQ(interpolation_utils::isIncreasing(decreasing_vec), false); +} + +TEST(interpolation_utils, isNotDecreasing) +{ + // empty + const std::vector empty_vec; + EXPECT_THROW(interpolation_utils::isNotDecreasing(empty_vec), std::invalid_argument); + + // increase + const std::vector increasing_vec{0.0, 1.5, 3.0, 4.5, 6.0}; + EXPECT_EQ(interpolation_utils::isNotDecreasing(increasing_vec), true); + + // not decrease + const std::vector not_increasing_vec{0.0, 1.5, 1.5, 4.5, 6.0}; + EXPECT_EQ(interpolation_utils::isNotDecreasing(not_increasing_vec), true); + + // decrease + const std::vector decreasing_vec{0.0, 1.5, 1.2, 4.5, 6.0}; + EXPECT_EQ(interpolation_utils::isNotDecreasing(decreasing_vec), false); +} + +TEST(interpolation_utils, validateInput) +{ + using interpolation_utils::validateInput; + + const std::vector base_keys{0.0, 1.0, 2.0, 3.0}; + const std::vector base_values{0.0, 1.0, 2.0, 3.0}; + const std::vector query_keys{0.0, 1.0, 2.0, 3.0}; + + // valid + EXPECT_NO_THROW(validateInput(base_keys, base_values, query_keys)); + + // empty + const std::vector empty_vec; + EXPECT_THROW(validateInput(empty_vec, base_values, query_keys), std::invalid_argument); + EXPECT_THROW(validateInput(base_keys, empty_vec, query_keys), std::invalid_argument); + EXPECT_THROW(validateInput(base_keys, base_values, empty_vec), std::invalid_argument); + + // size is less than 2 + const std::vector short_vec{0.0}; + EXPECT_THROW(validateInput(short_vec, base_values, query_keys), std::invalid_argument); + EXPECT_THROW(validateInput(base_keys, short_vec, query_keys), std::invalid_argument); + EXPECT_THROW(validateInput(short_vec, short_vec, query_keys), std::invalid_argument); + + // partly not increase + const std::vector partly_not_increasing_vec{0.0, 0.0, 2.0, 3.0}; + // NOTE: base_keys must be strictly monotonous increasing vector + EXPECT_THROW( + validateInput(partly_not_increasing_vec, base_values, query_keys), std::invalid_argument); + // NOTE: query_keys is allowed to be monotonous non-decreasing vector + EXPECT_NO_THROW(validateInput(base_keys, base_values, partly_not_increasing_vec)); + + // decrease + const std::vector decreasing_vec{0.0, -1.0, 2.0, 3.0}; + EXPECT_THROW(validateInput(decreasing_vec, base_values, query_keys), std::invalid_argument); + EXPECT_THROW(validateInput(base_keys, base_values, decreasing_vec), std::invalid_argument); + + // out of range + const std::vector front_out_query_keys{-1.0, 1.0, 2.0, 3.0}; + EXPECT_THROW(validateInput(base_keys, base_values, front_out_query_keys), std::invalid_argument); + + const std::vector back_out_query_keys{0.0, 1.0, 2.0, 4.0}; + EXPECT_THROW(validateInput(base_keys, base_values, back_out_query_keys), std::invalid_argument); + + // size is different + const std::vector different_size_base_values{0.0, 1.0, 2.0}; + EXPECT_THROW( + validateInput(base_keys, different_size_base_values, query_keys), std::invalid_argument); +} diff --git a/common/interpolation/test/src/test_linear_interpolation.cpp b/common/interpolation/test/src/test_linear_interpolation.cpp new file mode 100644 index 000000000000..49a9352f3daa --- /dev/null +++ b/common/interpolation/test/src/test_linear_interpolation.cpp @@ -0,0 +1,79 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 "interpolation/linear_interpolation.hpp" + +#include + +#include +#include + +constexpr double epsilon = 1e-6; + +TEST(linear_interpolation, lerp_scalar) +{ + EXPECT_EQ(interpolation::lerp(0.0, 1.0, 0.3), 0.3); + EXPECT_EQ(interpolation::lerp(-0.5, 12.3, 0.3), 3.34); +} + +TEST(linear_interpolation, lerp_vector) +{ + { // straight: query_keys is same as base_keys + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{0.0, 1.5, 3.0, 4.5, 6.0}; + const std::vector query_keys = base_keys; + const std::vector ans = base_values; + + const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } + + { // straight: query_keys is random + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{0.0, 1.5, 3.0, 4.5, 6.0}; + const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; + const std::vector ans{0.0, 1.05, 2.85, 6.0}; + + const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } + + { // curve: query_keys is same as base_keys + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys = base_keys; + const std::vector ans = base_values; + + const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } + + { // curve: query_keys is same as random + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys{0.0, 8.0, 18.0}; + const std::vector ans{-0.18, 1.12, 1.4}; + + const auto query_values = interpolation::lerp(base_keys, base_values, query_keys); + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } +} diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp new file mode 100644 index 000000000000..90f08df57884 --- /dev/null +++ b/common/interpolation/test/src/test_spline_interpolation.cpp @@ -0,0 +1,97 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 "interpolation/spline_interpolation.hpp" + +#include + +#include +#include + +constexpr double epsilon = 1e-6; + +TEST(spline_interpolation, slerp) +{ + { // straight: query_keys is same as base_keys + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{0.0, 1.5, 3.0, 4.5, 6.0}; + const std::vector query_keys = base_keys; + const std::vector ans = base_values; + + const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } + + { // straight: query_keys is random + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{0.0, 1.5, 3.0, 4.5, 6.0}; + const std::vector query_keys{0.0, 0.7, 1.9, 4.0}; + const std::vector ans{0.0, 1.05, 2.85, 6.0}; + + const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } + + { // curve: query_keys is same as base_keys + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys = base_keys; + const std::vector ans = base_values; + + const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } + + { // curve: query_keys is same as random + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys{0.0, 8.0, 18.0}; + const std::vector ans{-0.075611, 0.997242, 1.573258}; + + const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } + + { // straight: size of base_keys is 2 + const std::vector base_keys{0.0, 1.0}; + const std::vector base_values{0.0, 1.5}; + const std::vector query_keys = base_keys; + const std::vector ans = base_values; + + const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } + + { // straight: size of base_keys is 3 + const std::vector base_keys{0.0, 1.0, 2.0}; + const std::vector base_values{0.0, 1.5, 3.0}; + const std::vector query_keys = base_keys; + const std::vector ans = base_values; + + const auto query_values = interpolation::slerp(base_keys, base_values, query_keys); + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } +} diff --git a/common/kalman_filter/CMakeLists.txt b/common/kalman_filter/CMakeLists.txt new file mode 100644 index 000000000000..4419e3d7761e --- /dev/null +++ b/common/kalman_filter/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.5) +project(kalman_filter) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +ament_auto_add_library(kalman_filter SHARED + src/kalman_filter.cpp + src/time_delay_kalman_filter.cpp + include/kalman_filter/kalman_filter.hpp + include/kalman_filter/time_delay_kalman_filter.hpp +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/common/kalman_filter/README.md b/common/kalman_filter/README.md new file mode 100644 index 000000000000..7c0feb9c2a61 --- /dev/null +++ b/common/kalman_filter/README.md @@ -0,0 +1,9 @@ +# kalman_filter + +## Purpose + +This common package contains the kalman filter with time delay and the calculation of the kalman filter. + +## Assumptions / Known limits + +TBD. diff --git a/common/kalman_filter/include/kalman_filter/kalman_filter.hpp b/common/kalman_filter/include/kalman_filter/kalman_filter.hpp new file mode 100644 index 000000000000..2ae8877aeea6 --- /dev/null +++ b/common/kalman_filter/include/kalman_filter/kalman_filter.hpp @@ -0,0 +1,210 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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. + +#ifndef KALMAN_FILTER__KALMAN_FILTER_HPP_ +#define KALMAN_FILTER__KALMAN_FILTER_HPP_ + +#include +#include + +/** + * @file kalman_filter.h + * @brief kalman filter class + * @author Takamasa Horibe + * @date 2019.05.01 + */ + +class KalmanFilter +{ +public: + /** + * @brief No initialization constructor. + */ + KalmanFilter(); + + /** + * @brief constructor with initialization + * @param x initial state + * @param A coefficient matrix of x for process model + * @param B coefficient matrix of u for process model + * @param C coefficient matrix of x for measurement model + * @param Q covariance matrix for process model + * @param R covariance matrix for measurement model + * @param P initial covariance of estimated state + */ + KalmanFilter( + const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, + const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, + const Eigen::MatrixXd & P); + + /** + * @brief destructor + */ + ~KalmanFilter(); + + /** + * @brief initialization of kalman filter + * @param x initial state + * @param A coefficient matrix of x for process model + * @param B coefficient matrix of u for process model + * @param C coefficient matrix of x for measurement model + * @param Q covariance matrix for process model + * @param R covariance matrix for measurement model + * @param P initial covariance of estimated state + */ + bool init( + const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, + const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, + const Eigen::MatrixXd & P); + + /** + * @brief initialization of kalman filter + * @param x initial state + * @param P initial covariance of estimated state + */ + bool init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0); + + /** + * @brief set A of process model + * @param A coefficient matrix of x for process model + */ + void setA(const Eigen::MatrixXd & A); + + /** + * @brief set B of process model + * @param B coefficient matrix of u for process model + */ + void setB(const Eigen::MatrixXd & B); + + /** + * @brief set C of measurement model + * @param C coefficient matrix of x for measurement model + */ + void setC(const Eigen::MatrixXd & C); + + /** + * @brief set covariance matrix Q for process model + * @param Q covariance matrix for process model + */ + void setQ(const Eigen::MatrixXd & Q); + + /** + * @brief set covariance matrix R for measurement model + * @param R covariance matrix for measurement model + */ + void setR(const Eigen::MatrixXd & R); + + /** + * @brief get current kalman filter state + * @param x kalman filter state + */ + void getX(Eigen::MatrixXd & x); + + /** + * @brief get current kalman filter covariance + * @param P kalman filter covariance + */ + void getP(Eigen::MatrixXd & P); + + /** + * @brief get component of current kalman filter state + * @param i index of kalman filter state + * @return value of i's component of the kalman filter state x[i] + */ + double getXelement(unsigned int i); + + /** + * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix. + * This is mainly for EKF with variable matrix. + * @param u input for model + * @param A coefficient matrix of x for process model + * @param B coefficient matrix of u for process model + * @param Q covariance matrix for process model + * @return bool to check matrix operations are being performed properly + */ + bool predict( + const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, + const Eigen::MatrixXd & Q); + + /** + * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is + * mainly for EKF with variable matrix. + * @param x_next predicted state + * @param A coefficient matrix of x for process model + * @param Q covariance matrix for process model + * @return bool to check matrix operations are being performed properly + */ + bool predict( + const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q); + + /** + * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is + * mainly for EKF with variable matrix. + * @param x_next predicted state + * @param A coefficient matrix of x for process model + * @return bool to check matrix operations are being performed properly + */ + bool predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A); + + /** + * @brief calculate kalman filter state by prediction model with A, B and Q being class member + * variables. + * @param u input for the model + * @return bool to check matrix operations are being performed properly + */ + bool predict(const Eigen::MatrixXd & u); + + /** + * @brief calculate kalman filter state by measurement model with y_pred, C and R matrix. This is + * mainly for EKF with variable matrix. + * @param y measured values + * @param y output values expected from measurement model + * @param C coefficient matrix of x for measurement model + * @param R covariance matrix for measurement model + * @return bool to check matrix operations are being performed properly + */ + bool update( + const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C, + const Eigen::MatrixXd & R); + + /** + * @brief calculate kalman filter state by measurement model with C and R matrix. This is mainly + * for EKF with variable matrix. + * @param y measured values + * @param C coefficient matrix of x for measurement model + * @param R covariance matrix for measurement model + * @return bool to check matrix operations are being performed properly + */ + bool update(const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R); + + /** + * @brief calculate kalman filter state by measurement model with C and R being class member + * variables. + * @param y measured values + * @return bool to check matrix operations are being performed properly + */ + bool update(const Eigen::MatrixXd & y); + +protected: + Eigen::MatrixXd x_; //!< @brief current estimated state + Eigen::MatrixXd + A_; //!< @brief coefficient matrix of x for process model x[k+1] = A*x[k] + B*u[k] + Eigen::MatrixXd + B_; //!< @brief coefficient matrix of u for process model x[k+1] = A*x[k] + B*u[k] + Eigen::MatrixXd C_; //!< @brief coefficient matrix of x for measurement model y[k] = C * x[k] + Eigen::MatrixXd Q_; //!< @brief covariance matrix for process model x[k+1] = A*x[k] + B*u[k] + Eigen::MatrixXd R_; //!< @brief covariance matrix for measurement model y[k] = C * x[k] + Eigen::MatrixXd P_; //!< @brief covariance of estimated state +}; +#endif // KALMAN_FILTER__KALMAN_FILTER_HPP_ diff --git a/common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp b/common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp new file mode 100644 index 000000000000..5fb615647207 --- /dev/null +++ b/common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp @@ -0,0 +1,88 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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. + +#ifndef KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ +#define KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ + +#include "kalman_filter/kalman_filter.hpp" + +#include +#include + +#include + +/** + * @file time_delay_kalman_filter.h + * @brief kalman filter with delayed measurement class + * @author Takamasa Horibe + * @date 2019.05.01 + */ + +class TimeDelayKalmanFilter : public KalmanFilter +{ +public: + /** + * @brief No initialization constructor. + */ + TimeDelayKalmanFilter(); + + /** + * @brief initialization of kalman filter + * @param x initial state + * @param P0 initial covariance of estimated state + * @param max_delay_step Maximum number of delay steps, which determines the dimension of the + * extended kalman filter + */ + void init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P, const int max_delay_step); + + /** + * @brief get latest time estimated state + * @param x latest time estimated state + */ + void getLatestX(Eigen::MatrixXd & x); + + /** + * @brief get latest time estimation covariance + * @param P latest time estimation covariance + */ + void getLatestP(Eigen::MatrixXd & P); + + /** + * @brief calculate kalman filter covariance by precision model with time delay. This is mainly + * for EKF of nonlinear process model. + * @param x_next predicted state by prediction model + * @param A coefficient matrix of x for process model + * @param Q covariance matrix for process model + */ + bool predictWithDelay( + const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q); + + /** + * @brief calculate kalman filter covariance by measurement model with time delay. This is mainly + * for EKF of nonlinear process model. + * @param y measured values + * @param C coefficient matrix of x for measurement model + * @param R covariance matrix for measurement model + * @param delay_step measurement delay + */ + bool updateWithDelay( + const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R, + const int delay_step); + +private: + int max_delay_step_; //!< @brief maximum number of delay steps + int dim_x_; //!< @brief dimension of latest state + int dim_x_ex_; //!< @brief dimension of extended state with dime delay +}; +#endif // KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ diff --git a/common/kalman_filter/package.xml b/common/kalman_filter/package.xml new file mode 100644 index 000000000000..9e10cb10538e --- /dev/null +++ b/common/kalman_filter/package.xml @@ -0,0 +1,18 @@ + + + kalman_filter + 0.1.0 + The kalman filter package + Yukihiro Saito Horibe + Takamasa Horibe + + Apache License 2.0 + ament_cmake_auto + + ament_cmake_cppcheck + ament_lint_auto + + + ament_cmake + + diff --git a/common/kalman_filter/src/kalman_filter.cpp b/common/kalman_filter/src/kalman_filter.cpp new file mode 100644 index 000000000000..e6e01a5bb1bd --- /dev/null +++ b/common/kalman_filter/src/kalman_filter.cpp @@ -0,0 +1,124 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 "kalman_filter/kalman_filter.hpp" + +KalmanFilter::KalmanFilter() {} +KalmanFilter::KalmanFilter( + const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, + const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, + const Eigen::MatrixXd & P) +{ + init(x, A, B, C, Q, R, P); +} +KalmanFilter::~KalmanFilter() {} +bool KalmanFilter::init( + const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, + const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, + const Eigen::MatrixXd & P) +{ + if ( + x.cols() == 0 || x.rows() == 0 || A.cols() == 0 || A.rows() == 0 || B.cols() == 0 || + B.rows() == 0 || C.cols() == 0 || C.rows() == 0 || Q.cols() == 0 || Q.rows() == 0 || + R.cols() == 0 || R.rows() == 0 || P.cols() == 0 || P.rows() == 0) { + return false; + } + x_ = x; + A_ = A; + B_ = B; + C_ = C; + Q_ = Q; + R_ = R; + P_ = P; + return true; +} +bool KalmanFilter::init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0) +{ + if (x.cols() == 0 || x.rows() == 0 || P0.cols() == 0 || P0.rows() == 0) { + return false; + } + x_ = x; + P_ = P0; + return true; +} + +void KalmanFilter::setA(const Eigen::MatrixXd & A) { A_ = A; } +void KalmanFilter::setB(const Eigen::MatrixXd & B) { B_ = B; } +void KalmanFilter::setC(const Eigen::MatrixXd & C) { C_ = C; } +void KalmanFilter::setQ(const Eigen::MatrixXd & Q) { Q_ = Q; } +void KalmanFilter::setR(const Eigen::MatrixXd & R) { R_ = R; } +void KalmanFilter::getX(Eigen::MatrixXd & x) { x = x_; } +void KalmanFilter::getP(Eigen::MatrixXd & P) { P = P_; } +double KalmanFilter::getXelement(unsigned int i) { return x_(i); } + +bool KalmanFilter::predict( + const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q) +{ + if ( + x_.rows() != x_next.rows() || A.cols() != P_.rows() || Q.cols() != Q.rows() || + A.rows() != Q.cols()) { + return false; + } + x_ = x_next; + P_ = A * P_ * A.transpose() + Q; + return true; +} +bool KalmanFilter::predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A) +{ + return predict(x_next, A, Q_); +} + +bool KalmanFilter::predict( + const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, + const Eigen::MatrixXd & Q) +{ + if (A.cols() != x_.rows() || B.cols() != u.rows()) { + return false; + } + const Eigen::MatrixXd x_next = A * x_ + B * u; + return predict(x_next, A, Q); +} +bool KalmanFilter::predict(const Eigen::MatrixXd & u) { return predict(u, A_, B_, Q_); } + +bool KalmanFilter::update( + const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C, + const Eigen::MatrixXd & R) +{ + if ( + P_.cols() != C.cols() || R.rows() != R.cols() || R.rows() != C.rows() || + y.rows() != y_pred.rows() || y.rows() != C.rows()) { + return false; + } + const Eigen::MatrixXd PCT = P_ * C.transpose(); + const Eigen::MatrixXd K = PCT * ((R + C * PCT).inverse()); + + if (isnan(K.array()).any() || isinf(K.array()).any()) { + return false; + } + + x_ = x_ + K * (y - y_pred); + P_ = P_ - K * (C * P_); + return true; +} + +bool KalmanFilter::update( + const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R) +{ + if (C.cols() != x_.rows()) { + return false; + } + const Eigen::MatrixXd y_pred = C * x_; + return update(y, y_pred, C, R); +} +bool KalmanFilter::update(const Eigen::MatrixXd & y) { return update(y, C_, R_); } diff --git a/common/kalman_filter/src/time_delay_kalman_filter.cpp b/common/kalman_filter/src/time_delay_kalman_filter.cpp new file mode 100644 index 000000000000..3c06ef25150c --- /dev/null +++ b/common/kalman_filter/src/time_delay_kalman_filter.cpp @@ -0,0 +1,95 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 "kalman_filter/time_delay_kalman_filter.hpp" + +TimeDelayKalmanFilter::TimeDelayKalmanFilter() {} + +void TimeDelayKalmanFilter::init( + const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0, const int max_delay_step) +{ + max_delay_step_ = max_delay_step; + dim_x_ = x.rows(); + dim_x_ex_ = dim_x_ * max_delay_step; + + x_ = Eigen::MatrixXd::Zero(dim_x_ex_, 1); + P_ = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_); + + for (int i = 0; i < max_delay_step_; ++i) { + x_.block(i * dim_x_, 0, dim_x_, 1) = x; + P_.block(i * dim_x_, i * dim_x_, dim_x_, dim_x_) = P0; + } +} + +void TimeDelayKalmanFilter::getLatestX(Eigen::MatrixXd & x) { x = x_.block(0, 0, dim_x_, 1); } +void TimeDelayKalmanFilter::getLatestP(Eigen::MatrixXd & P) { P = P_.block(0, 0, dim_x_, dim_x_); } + +bool TimeDelayKalmanFilter::predictWithDelay( + const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q) +{ + /* + * time delay model: + * + * [A 0 0] [P11 P12 P13] [Q 0 0] + * A = [I 0 0], P = [P21 P22 P23], Q = [0 0 0] + * [0 I 0] [P31 P32 P33] [0 0 0] + * + * covariance calculation in prediction : P = A * P * A' + Q + * + * [A*P11*A'*+Q A*P11 A*P12] + * P = [ P11*A' P11 P12] + * [ P21*A' P21 P22] + */ + + const int d_dim_x = dim_x_ex_ - dim_x_; + + /* slide states in the time direction */ + Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, 1); + x_tmp.block(0, 0, dim_x_, 1) = x_next; + x_tmp.block(dim_x_, 0, d_dim_x, 1) = x_.block(0, 0, d_dim_x, 1); + x_ = x_tmp; + + /* update P with delayed measurement A matrix structure */ + Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_); + P_tmp.block(0, 0, dim_x_, dim_x_) = A * P_.block(0, 0, dim_x_, dim_x_) * A.transpose() + Q; + P_tmp.block(0, dim_x_, dim_x_, d_dim_x) = A * P_.block(0, 0, dim_x_, d_dim_x); + P_tmp.block(dim_x_, 0, d_dim_x, dim_x_) = P_.block(0, 0, d_dim_x, dim_x_) * A.transpose(); + P_tmp.block(dim_x_, dim_x_, d_dim_x, d_dim_x) = P_.block(0, 0, d_dim_x, d_dim_x); + P_ = P_tmp; + + return true; +} + +bool TimeDelayKalmanFilter::updateWithDelay( + const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R, + const int delay_step) +{ + if (delay_step >= max_delay_step_) { + std::cerr << "delay step is larger than max_delay_step. ignore update." << std::endl; + return false; + } + + const int dim_y = y.rows(); + + /* set measurement matrix */ + Eigen::MatrixXd C_ex = Eigen::MatrixXd::Zero(dim_y, dim_x_ex_); + C_ex.block(0, dim_x_ * delay_step, dim_y, dim_x_) = C; + + /* update */ + if (!update(y, C_ex, R)) { + return false; + } + + return true; +} diff --git a/common/osqp_interface/CMakeLists.txt b/common/osqp_interface/CMakeLists.txt new file mode 100644 index 000000000000..fa33d4b7aacd --- /dev/null +++ b/common/osqp_interface/CMakeLists.txt @@ -0,0 +1,78 @@ +# Copyright 2021 The Autoware Foundation +# +# 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. + +cmake_minimum_required(VERSION 3.5) + +project(osqp_interface) + +# require that dependencies from package.xml be available +find_package(ament_cmake_auto REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(osqp REQUIRED) +get_target_property(OSQP_INCLUDE_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES) + +ament_auto_find_build_dependencies(REQUIRED + ${${PROJECT_NAME}_BUILD_DEPENDS} + ${${PROJECT_NAME}_BUILDTOOL_DEPENDS} +) + +set(OSQP_INTERFACE_LIB_SRC + src/csc_matrix_conv.cpp + src/osqp_interface.cpp +) + +set(OSQP_INTERFACE_LIB_HEADERS + include/osqp_interface/csc_matrix_conv.hpp + include/osqp_interface/osqp_interface.hpp + include/osqp_interface/visibility_control.hpp +) + +# generate library +ament_auto_add_library(${PROJECT_NAME} SHARED + ${OSQP_INTERFACE_LIB_SRC} + ${OSQP_INTERFACE_LIB_HEADERS} +) +autoware_set_compile_options(${PROJECT_NAME}) +target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast -Wno-error=useless-cast) + +target_include_directories(osqp_interface PUBLIC "${OSQP_INCLUDE_DIR}") +ament_target_dependencies(osqp_interface + Eigen3 + osqp_vendor +) + +# crucial so downstream package builds because osqp_interface exposes osqp.hpp +ament_export_include_directories("${OSQP_INCLUDE_DIR}") +# crucial so the linking order is correct in a downstream package: libosqp_interface.a should come before libosqp.a +ament_export_libraries(osqp::osqp) + +# Testing +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + # Unit tests + set(TEST_SOURCES + test/test_osqp_interface.cpp + test/test_csc_matrix_conv.cpp + ) + set(TEST_OSQP_INTERFACE_EXE test_osqp_interface) + ament_add_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) + autoware_set_compile_options(${TEST_OSQP_INTERFACE_EXE}) + target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME}) +endif() + +# ament package generation and installing +ament_auto_package(INSTALL_TO_SHARE +) diff --git a/common/osqp_interface/design/osqp_interface-design.md b/common/osqp_interface/design/osqp_interface-design.md new file mode 100644 index 000000000000..5ed1c81a17e2 --- /dev/null +++ b/common/osqp_interface/design/osqp_interface-design.md @@ -0,0 +1,61 @@ +Interface for the OSQP library {#osqp_interface-package-design} +=========== + +This is the design document for the `osqp_interface` package. + + +# Purpose / Use cases + + +This packages provides a C++ interface for the [OSQP library](https://osqp.org/docs/solver/index.html). + + +# Design + + +The class `OSQPInterface` takes a problem formulation as Eigen matrices and vectors, converts these objects into +C-style Compressed-Column-Sparse matrices and dynamic arrays, loads the data into the OSQP workspace dataholder, and runs the optimizer. + +## Inputs / Outputs / API + + + The interface can be used in several ways: + + 1. Initialize the interface WITHOUT data. Load the problem formulation at the optimization call. + ``` + osqp_interface = OSQPInterface(); + osqp_interface.optimize(P, A, q, l, u); + ``` + + 2. Initialize the interface WITH data. + ``` + osqp_interface = OSQPInterface(P, A, q, l, u); + osqp_interface.optimize(); + ``` + + 3. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs. + ``` + osqp_interface = OSQPInterface(P, A, q, l, u); + osqp_interface.optimize(); + osqp.initializeProblem(P_new, A_new, q_new, l_new, u_new); + osqp_interface.optimize(); + ``` + +The optimization results are returned as a vector by the optimization function. +``` + std::tuple, std::vector> result = osqp_interface.optimize(); + std::vector param = std::get<0>(result); + double x_0 = param[0]; + double x_1 = param[1]; +``` + +# References / External links + +- OSQP library: https://osqp.org/ + +# Related issues + +- This package was introduced as a dependency of the MPC-based lateral controller: https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/issues/1057 diff --git a/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp b/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp new file mode 100644 index 000000000000..8f50bda214fd --- /dev/null +++ b/common/osqp_interface/include/osqp_interface/csc_matrix_conv.hpp @@ -0,0 +1,52 @@ +// Copyright 2021 The Autoware Foundation +// +// 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. + +#ifndef OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ +#define OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ + +#include + +#include "eigen3/Eigen/Core" +#include "osqp/glob_opts.h" // for 'c_int' type ('long' or 'long long') +#include "osqp_interface/visibility_control.hpp" + +namespace autoware +{ +namespace common +{ +namespace osqp +{ +/// \brief Compressed-Column-Sparse Matrix +struct OSQP_INTERFACE_PUBLIC CSC_Matrix +{ + /// Vector of non-zero values. Ex: [4,1,1,2] + std::vector m_vals; + /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') + std::vector m_row_idxs; + /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') + std::vector m_col_idxs; +}; + +/// \brief Calculate CSC matrix from Eigen matrix +OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat); +/// \brief Calculate upper trapezoidal CSC matrix from square Eigen matrix +OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat); +/// \brief Print the given CSC matrix to the standard output +OSQP_INTERFACE_PUBLIC void printCSCMatrix(const CSC_Matrix & csc_mat); + +} // namespace osqp +} // namespace common +} // namespace autoware + +#endif // OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ diff --git a/common/osqp_interface/include/osqp_interface/osqp_interface.hpp b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp new file mode 100644 index 000000000000..b0c6d2ea3cb5 --- /dev/null +++ b/common/osqp_interface/include/osqp_interface/osqp_interface.hpp @@ -0,0 +1,179 @@ +// Copyright 2021 The Autoware Foundation +// +// 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. + +#ifndef OSQP_INTERFACE__OSQP_INTERFACE_HPP_ +#define OSQP_INTERFACE__OSQP_INTERFACE_HPP_ + +#include "common/types.hpp" +#include "eigen3/Eigen/Core" +#include "osqp/osqp.h" +#include "osqp_interface/csc_matrix_conv.hpp" +#include "osqp_interface/visibility_control.hpp" + +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace common +{ +namespace osqp +{ +constexpr c_float INF = OSQP_INFTY; +using autoware::common::types::bool8_t; +using autoware::common::types::float64_t; + +/** + * Implementation of a native C++ interface for the OSQP solver. + * + */ +class OSQP_INTERFACE_PUBLIC OSQPInterface +{ +private: + OSQPWorkspace * m_work = nullptr; + std::unique_ptr m_settings; + std::unique_ptr m_data; + // store last work info since work is cleaned up at every execution to prevent memory leak. + OSQPInfo m_latest_work_info; + // Number of parameters to optimize + int64_t m_param_n; + // Flag to check if the current work exists + bool8_t m_work_initialized = false; + // Exitflag + int64_t m_exitflag; + + // Runs the solver on the stored problem. + std::tuple, std::vector, int64_t, int64_t> solve(); + +public: + /// \brief Constructor without problem formulation + explicit OSQPInterface( + const c_float eps_abs = std::numeric_limits::epsilon(), const bool8_t polish = true); + /// \brief Constructor with problem setup + /// \param P: (n,n) matrix defining relations between parameters. + /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. + /// \param q: (n) vector defining the linear cost of the problem. + /// \param l: (m) vector defining the lower bound problem constraint. + /// \param u: (m) vector defining the upper bound problem constraint. + /// \param eps_abs: Absolute convergence tolerance. + OSQPInterface( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u, const c_float eps_abs); + ~OSQPInterface(); + + /**************** + * OPTIMIZATION + ****************/ + /// \brief Solves the stored convec quadratic program (QP) problem using the OSQP solver. + // + /// \return The function returns a tuple containing the solution as two float vectors. + /// \return The first element of the tuple contains the 'primal' solution. + /// \return The second element contains the 'lagrange multiplier' solution. + /// \return The third element contains an integer with solver polish status information. + + /// \details How to use: + /// \details 1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem. + /// \details 2. Initialize the interface and set up the problem. + /// \details osqp_interface = OSQPInterface(P, A, q, l, u, 1e-6); + /// \details 3. Call the optimization function. + /// \details std::tuple, std::vector> result; + /// \details result = osqp_interface.optimize(); + /// \details 4. Access the optimized parameters. + /// \details std::vector param = std::get<0>(result); + /// \details float64_t x_0 = param[0]; + /// \details float64_t x_1 = param[1]; + std::tuple, std::vector, int64_t, int64_t> optimize(); + + /// \brief Solves convex quadratic programs (QPs) using the OSQP solver. + /// \return The function returns a tuple containing the solution as two float vectors. + /// \return The first element of the tuple contains the 'primal' solution. + /// \return The second element contains the 'lagrange multiplier' solution. + /// \return The third element contains an integer with solver polish status information. + /// \details How to use: + /// \details 1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem. + /// \details 2. Initialize the interface. + /// \details osqp_interface = OSQPInterface(1e-6); + /// \details 3. Call the optimization function with the problem formulation. + /// \details std::tuple, std::vector> result; + /// \details result = osqp_interface.optimize(P, A, q, l, u, 1e-6); + /// \details 4. Access the optimized parameters. + /// \details std::vector param = std::get<0>(result); + /// \details float64_t x_0 = param[0]; + /// \details float64_t x_1 = param[1]; + std::tuple, std::vector, int64_t, int64_t> optimize( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u); + + /// \brief Converts the input data and sets up the workspace object. + /// \param P (n,n) matrix defining relations between parameters. + /// \param A (m,n) matrix defining parameter constraints relative to the lower and upper bound. + /// \param q (n) vector defining the linear cost of the problem. + /// \param l (m) vector defining the lower bound problem constraint. + /// \param u (m) vector defining the upper bound problem constraint. + int64_t initializeProblem( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u); + + // Updates problem parameters while keeping solution in memory. + // + // Args: + // P_new: (n,n) matrix defining relations between parameters. + // A_new: (m,n) matrix defining parameter constraints relative to the lower and upper bound. + // q_new: (n) vector defining the linear cost of the problem. + // l_new: (m) vector defining the lower bound problem constraint. + // u_new: (m) vector defining the upper bound problem constraint. + void updateP(const Eigen::MatrixXd & P_new); + void updateA(const Eigen::MatrixXd & A_new); + void updateQ(const std::vector & q_new); + void updateL(const std::vector & l_new); + void updateU(const std::vector & u_new); + void updateBounds(const std::vector & l_new, const std::vector & u_new); + void updateEpsAbs(const double eps_abs); + void updateEpsRel(const double eps_rel); + void updateMaxIter(const int iter); + void updateVerbose(const bool verbose); + void updateRhoInterval(const int rho_interval); + void updateRho(const double rho); + void updateAlpha(const double alpha); + + /// \brief Get the number of iteration taken to solve the problem + inline int64_t getTakenIter() const { return static_cast(m_latest_work_info.iter); } + /// \brief Get the status message for the latest problem solved + inline std::string getStatusMessage() const + { + return static_cast(m_latest_work_info.status); + } + /// \brief Get the status value for the latest problem solved + inline int64_t getStatus() const { return static_cast(m_latest_work_info.status_val); } + /// \brief Get the status polish for the latest problem solved + inline int64_t getStatusPolish() const + { + return static_cast(m_latest_work_info.status_polish); + } + /// \brief Get the runtime of the latest problem solved + inline float64_t getRunTime() const { return m_latest_work_info.run_time; } + /// \brief Get the objective value the latest problem solved + inline float64_t getObjVal() const { return m_latest_work_info.obj_val; } + /// \brief Returns flag asserting interface condition (Healthy condition: 0). + inline int64_t getExitFlag() const { return m_exitflag; } +}; + +} // namespace osqp +} // namespace common +} // namespace autoware + +#endif // OSQP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/osqp_interface/include/osqp_interface/visibility_control.hpp b/common/osqp_interface/include/osqp_interface/visibility_control.hpp new file mode 100644 index 000000000000..816d3c0fff82 --- /dev/null +++ b/common/osqp_interface/include/osqp_interface/visibility_control.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 The Autoware Foundation +// +// 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. + +#ifndef OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ +#define OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) + #if defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS) + #define OSQP_INTERFACE_PUBLIC __declspec(dllexport) + #define OSQP_INTERFACE_LOCAL + #else // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS) + #define OSQP_INTERFACE_PUBLIC __declspec(dllimport) + #define OSQP_INTERFACE_LOCAL + #endif // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS) +#elif defined(__linux__) + #define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default"))) + #define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) + #define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default"))) + #define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden"))) +#else + #error "Unsupported Build Configuration" +#endif + +#endif // OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ diff --git a/common/osqp_interface/package.xml b/common/osqp_interface/package.xml new file mode 100644 index 000000000000..34bb8a5a195d --- /dev/null +++ b/common/osqp_interface/package.xml @@ -0,0 +1,27 @@ + + + + osqp_interface + 1.0.0 + Interface for the OSQP solver + Maxime CLEMENT + Apache 2.0 + + ament_cmake_auto + autoware_auto_cmake + + autoware_auto_common + eigen + osqp_vendor + rclcpp + rclcpp_components + + ament_cmake_gtest + + + eigen + + + ament_cmake + + diff --git a/common/osqp_interface/src/csc_matrix_conv.cpp b/common/osqp_interface/src/csc_matrix_conv.cpp new file mode 100644 index 000000000000..6dbe362ab277 --- /dev/null +++ b/common/osqp_interface/src/csc_matrix_conv.cpp @@ -0,0 +1,140 @@ +// Copyright 2021 The Autoware Foundation +// +// 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 +#include +#include + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/SparseCore" +#include "osqp_interface/csc_matrix_conv.hpp" + +namespace autoware +{ +namespace common +{ +namespace osqp +{ +CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) +{ + const size_t elem = static_cast(mat.nonZeros()); + const Eigen::Index rows = mat.rows(); + const Eigen::Index cols = mat.cols(); + + std::vector vals; + vals.reserve(elem); + std::vector row_idxs; + row_idxs.reserve(elem); + std::vector col_idxs; + col_idxs.reserve(elem); + + // Construct CSC matrix arrays + c_float val; + c_int elem_count = 0; + + col_idxs.push_back(0); + + for (Eigen::Index j = 0; j < cols; j++) { // col iteration + for (Eigen::Index i = 0; i < rows; i++) { // row iteration + // Get values of nonzero elements + val = mat(i, j); + if (std::fabs(val) < 1e-9) { + continue; + } + elem_count += 1; + + // Store values + vals.push_back(val); + row_idxs.push_back(i); + } + + col_idxs.push_back(elem_count); + } + + CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; + + return csc_matrix; +} + +CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) +{ + const size_t elem = static_cast(mat.nonZeros()); + const Eigen::Index rows = mat.rows(); + const Eigen::Index cols = mat.cols(); + + if (rows != cols) { + throw std::invalid_argument("Matrix must be square (n, n)"); + } + + std::vector vals; + vals.reserve(elem); + std::vector row_idxs; + row_idxs.reserve(elem); + std::vector col_idxs; + col_idxs.reserve(elem); + + // Construct CSC matrix arrays + c_float val; + Eigen::Index trap_last_idx = 0; + c_int elem_count = 0; + + col_idxs.push_back(0); + + for (Eigen::Index j = 0; j < cols; j++) { // col iteration + for (Eigen::Index i = 0; i <= trap_last_idx; i++) { // row iteration + // Get values of nonzero elements + val = mat(i, j); + if (std::fabs(val) < 1e-9) { + continue; + } + elem_count += 1; + + // Store values + vals.push_back(val); + row_idxs.push_back(i); + } + + col_idxs.push_back(elem_count); + trap_last_idx += 1; + } + + CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; + + return csc_matrix; +} + +void printCSCMatrix(const CSC_Matrix & csc_mat) +{ + std::cout << "["; + for (const c_float & val : csc_mat.m_vals) { + std::cout << val << ", "; + } + std::cout << "]\n"; + + std::cout << "["; + for (const c_int & row : csc_mat.m_row_idxs) { + std::cout << row << ", "; + } + std::cout << "]\n"; + + std::cout << "["; + for (const c_int & col : csc_mat.m_col_idxs) { + std::cout << col << ", "; + } + std::cout << "]\n"; +} + +} // namespace osqp +} // namespace common +} // namespace autoware diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/osqp_interface/src/osqp_interface.cpp new file mode 100644 index 000000000000..0d3e85fe6367 --- /dev/null +++ b/common/osqp_interface/src/osqp_interface.cpp @@ -0,0 +1,272 @@ +// Copyright 2021 The Autoware Foundation +// +// 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 "osqp_interface/osqp_interface.hpp" + +#include "osqp/osqp.h" +#include "osqp_interface/csc_matrix_conv.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace common +{ +namespace osqp +{ +OSQPInterface::OSQPInterface(const c_float eps_abs, const bool8_t polish) +{ + m_settings = std::make_unique(); + m_data = std::make_unique(); + + if (m_settings) { + osqp_set_default_settings(m_settings.get()); + m_settings->alpha = 1.6; // Change alpha parameter + m_settings->eps_rel = 1.0E-4; + m_settings->eps_abs = eps_abs; + m_settings->eps_prim_inf = 1.0E-4; + m_settings->eps_dual_inf = 1.0E-4; + m_settings->warm_start = true; + m_settings->max_iter = 4000; + m_settings->verbose = false; + m_settings->polish = polish; + } + m_exitflag = 0; +} + +OSQPInterface::OSQPInterface( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u, const c_float eps_abs) +: OSQPInterface(eps_abs) +{ + initializeProblem(P, A, q, l, u); +} + +void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) +{ + /* + // Transform 'P' into an 'upper trapezoidal matrix' + Eigen::MatrixXd P_trap = P_new.triangularView(); + // Transform 'P' into a sparse matrix and extract data as dynamic arrays + Eigen::SparseMatrix P_sparse = P_trap.sparseView(); + double *P_val_ptr = P_sparse.valuePtr(); + // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) + c_int P_elem_N = P_sparse.nonZeros(); + */ + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); + osqp_update_P(m_work, P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); +} + +void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) +{ + /* + // Transform 'A' into a sparse matrix and extract data as dynamic arrays + Eigen::SparseMatrix A_sparse = A_new.sparseView(); + double *A_val_ptr = A_sparse.valuePtr(); + // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) + c_int A_elem_N = A_sparse.nonZeros(); + */ + CSC_Matrix A_csc = calCSCMatrix(A_new); + osqp_update_A(m_work, A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); + return; +} + +void OSQPInterface::updateL(const std::vector & l_new) +{ + std::vector l_tmp(l_new.begin(), l_new.end()); + double * l_dyn = l_tmp.data(); + osqp_update_lower_bound(m_work, l_dyn); +} + +void OSQPInterface::updateU(const std::vector & u_new) +{ + std::vector u_tmp(u_new.begin(), u_new.end()); + double * u_dyn = u_tmp.data(); + osqp_update_upper_bound(m_work, u_dyn); +} + +void OSQPInterface::updateBounds( + const std::vector & l_new, const std::vector & u_new) +{ + std::vector l_tmp(l_new.begin(), l_new.end()); + std::vector u_tmp(u_new.begin(), u_new.end()); + double * l_dyn = l_tmp.data(); + double * u_dyn = u_tmp.data(); + osqp_update_bounds(m_work, l_dyn, u_dyn); +} + +void OSQPInterface::updateEpsAbs(const double eps_abs) +{ + m_settings->eps_abs = eps_abs; // for default setting + if (m_work_initialized) { + osqp_update_eps_abs(m_work, eps_abs); // for current work + } +} + +void OSQPInterface::updateEpsRel(const double eps_rel) +{ + m_settings->eps_rel = eps_rel; // for default setting + if (m_work_initialized) { + osqp_update_eps_rel(m_work, eps_rel); // for current work + } +} + +void OSQPInterface::updateMaxIter(const int max_iter) +{ + m_settings->max_iter = max_iter; // for default setting + if (m_work_initialized) { + osqp_update_max_iter(m_work, max_iter); // for current work + } +} + +void OSQPInterface::updateVerbose(const bool is_verbose) +{ + m_settings->verbose = is_verbose; // for default setting + if (m_work_initialized) { + osqp_update_verbose(m_work, is_verbose); // for current work + } +} + +void OSQPInterface::updateRhoInterval(const int rho_interval) +{ + m_settings->adaptive_rho_interval = rho_interval; // for default setting +} + +void OSQPInterface::updateRho(const double rho) +{ + m_settings->rho = rho; + if (m_work_initialized) { + osqp_update_rho(m_work, rho); + } +} + +void OSQPInterface::updateAlpha(const double alpha) +{ + m_settings->alpha = alpha; + if (m_work_initialized) { + osqp_update_alpha(m_work, alpha); + } +} + +int64_t OSQPInterface::initializeProblem( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) +{ + /******************* + * SET UP MATRICES + *******************/ + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + // Dynamic float arrays + std::vector q_tmp(q.begin(), q.end()); + std::vector l_tmp(l.begin(), l.end()); + std::vector u_tmp(u.begin(), u.end()); + float64_t * q_dyn = q_tmp.data(); + float64_t * l_dyn = l_tmp.data(); + float64_t * u_dyn = u_tmp.data(); + + /********************** + * OBJECTIVE FUNCTION + **********************/ + // Number of constraints + c_int constr_m = A.rows(); + // Number of parameters + m_param_n = P.rows(); + + /***************** + * POPULATE DATA + *****************/ + m_data->m = constr_m; + m_data->n = m_param_n; + m_data->P = csc_matrix( + m_data->n, m_data->n, static_cast(P_csc.m_vals.size()), P_csc.m_vals.data(), + P_csc.m_row_idxs.data(), P_csc.m_col_idxs.data()); + m_data->q = q_dyn; + m_data->A = csc_matrix( + m_data->m, m_data->n, static_cast(A_csc.m_vals.size()), A_csc.m_vals.data(), + A_csc.m_row_idxs.data(), A_csc.m_col_idxs.data()); + m_data->l = l_dyn; + m_data->u = u_dyn; + + // Setup workspace + m_exitflag = osqp_setup(&m_work, m_data.get(), m_settings.get()); + m_work_initialized = true; + + return m_exitflag; +} + +OSQPInterface::~OSQPInterface() +{ + // Cleanup dynamic OSQP memory + if (m_work) { + osqp_cleanup(m_work); + } +} + +std::tuple, std::vector, int64_t, int64_t> OSQPInterface::solve() +{ + // Solve Problem + osqp_solve(m_work); + + /******************** + * EXTRACT SOLUTION + ********************/ + float64_t * sol_x = m_work->solution->x; + float64_t * sol_y = m_work->solution->y; + std::vector sol_primal(sol_x, sol_x + m_param_n); + std::vector sol_lagrange_multiplier(sol_y, sol_y + m_param_n); + // Solver polish status + int64_t status_polish = m_work->info->status_polish; + // Solver solution status + int64_t status_solution = m_work->info->status_val; + // Result tuple + std::tuple, std::vector, int64_t, int64_t> result = + std::make_tuple(sol_primal, sol_lagrange_multiplier, status_polish, status_solution); + + m_latest_work_info = *(m_work->info); + + return result; +} + +std::tuple, std::vector, int64_t, int64_t> +OSQPInterface::optimize() +{ + // Run the solver on the stored problem representation. + std::tuple, std::vector, int64_t, int64_t> result = solve(); + return result; +} + +std::tuple, std::vector, int64_t, int64_t> +OSQPInterface::optimize( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) +{ + // Allocate memory for problem + initializeProblem(P, A, q, l, u); + + // Run the solver on the stored problem representation. + std::tuple, std::vector, int64_t, int64_t> result = solve(); + + return result; +} + +} // namespace osqp +} // namespace common +} // namespace autoware diff --git a/common/osqp_interface/test/test_csc_matrix_conv.cpp b/common/osqp_interface/test/test_csc_matrix_conv.cpp new file mode 100644 index 000000000000..5e627e6f2447 --- /dev/null +++ b/common/osqp_interface/test/test_csc_matrix_conv.cpp @@ -0,0 +1,185 @@ +// Copyright 2021 The Autoware Foundation +// +// 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 +#include +#include + +#include "eigen3/Eigen/Core" +#include "gtest/gtest.h" +#include "osqp_interface/csc_matrix_conv.hpp" + +TEST(TestCscMatrixConv, Nominal) { + using autoware::common::osqp::CSC_Matrix; + using autoware::common::osqp::calCSCMatrix; + + Eigen::MatrixXd rect1(1, 2); + rect1 << 0.0, 1.0; + + const CSC_Matrix rect_m1 = calCSCMatrix(rect1); + ASSERT_EQ(rect_m1.m_vals.size(), size_t(1)); + EXPECT_EQ(rect_m1.m_vals[0], 1.0); + ASSERT_EQ(rect_m1.m_row_idxs.size(), size_t(1)); + EXPECT_EQ(rect_m1.m_row_idxs[0], c_int(0)); + ASSERT_EQ(rect_m1.m_col_idxs.size(), size_t(3)); // nb of columns + 1 + EXPECT_EQ(rect_m1.m_col_idxs[0], c_int(0)); + EXPECT_EQ(rect_m1.m_col_idxs[1], c_int(0)); + EXPECT_EQ(rect_m1.m_col_idxs[2], c_int(1)); + + Eigen::MatrixXd rect2(2, 4); + rect2 << 1.0, 0.0, 3.0, 0.0, + 0.0, 6.0, 7.0, 0.0; + + const CSC_Matrix rect_m2 = calCSCMatrix(rect2); + ASSERT_EQ(rect_m2.m_vals.size(), size_t(4)); + EXPECT_EQ(rect_m2.m_vals[0], 1.0); + EXPECT_EQ(rect_m2.m_vals[1], 6.0); + EXPECT_EQ(rect_m2.m_vals[2], 3.0); + EXPECT_EQ(rect_m2.m_vals[3], 7.0); + ASSERT_EQ(rect_m2.m_row_idxs.size(), size_t(4)); + EXPECT_EQ(rect_m2.m_row_idxs[0], c_int(0)); + EXPECT_EQ(rect_m2.m_row_idxs[1], c_int(1)); + EXPECT_EQ(rect_m2.m_row_idxs[2], c_int(0)); + EXPECT_EQ(rect_m2.m_row_idxs[3], c_int(1)); + ASSERT_EQ(rect_m2.m_col_idxs.size(), size_t(5)); // nb of columns + 1 + EXPECT_EQ(rect_m2.m_col_idxs[0], c_int(0)); + EXPECT_EQ(rect_m2.m_col_idxs[1], c_int(1)); + EXPECT_EQ(rect_m2.m_col_idxs[2], c_int(2)); + EXPECT_EQ(rect_m2.m_col_idxs[3], c_int(4)); + EXPECT_EQ(rect_m2.m_col_idxs[4], c_int(4)); + + // Example from http://netlib.org/linalg/html_templates/node92.html + Eigen::MatrixXd square2(6, 6); + square2 << 10.0, 0.0, 0.0, 0.0, -2.0, 0.0, + 3.0, 9.0, 0.0, 0.0, 0.0, 3.0, + 0.0, 7.0, 8.0, 7.0, 0.0, 0.0, + 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, + 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, + 0.0, 4.0, 0.0, 0.0, 2.0, -1.0; + + const CSC_Matrix square_m2 = calCSCMatrix(square2); + ASSERT_EQ(square_m2.m_vals.size(), size_t(19)); + EXPECT_EQ(square_m2.m_vals[0], 10.0); + EXPECT_EQ(square_m2.m_vals[1], 3.0); + EXPECT_EQ(square_m2.m_vals[2], 3.0); + EXPECT_EQ(square_m2.m_vals[3], 9.0); + EXPECT_EQ(square_m2.m_vals[4], 7.0); + EXPECT_EQ(square_m2.m_vals[5], 8.0); + EXPECT_EQ(square_m2.m_vals[6], 4.0); + EXPECT_EQ(square_m2.m_vals[7], 8.0); + EXPECT_EQ(square_m2.m_vals[8], 8.0); + EXPECT_EQ(square_m2.m_vals[9], 7.0); + EXPECT_EQ(square_m2.m_vals[10], 7.0); + EXPECT_EQ(square_m2.m_vals[11], 9.0); + EXPECT_EQ(square_m2.m_vals[12], -2.0); + EXPECT_EQ(square_m2.m_vals[13], 5.0); + EXPECT_EQ(square_m2.m_vals[14], 9.0); + EXPECT_EQ(square_m2.m_vals[15], 2.0); + EXPECT_EQ(square_m2.m_vals[16], 3.0); + EXPECT_EQ(square_m2.m_vals[17], 13.0); + EXPECT_EQ(square_m2.m_vals[18], -1.0); + ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(19)); + EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); + EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); + EXPECT_EQ(square_m2.m_row_idxs[2], c_int(3)); + EXPECT_EQ(square_m2.m_row_idxs[3], c_int(1)); + EXPECT_EQ(square_m2.m_row_idxs[4], c_int(2)); + EXPECT_EQ(square_m2.m_row_idxs[5], c_int(4)); + EXPECT_EQ(square_m2.m_row_idxs[6], c_int(5)); + EXPECT_EQ(square_m2.m_row_idxs[7], c_int(2)); + EXPECT_EQ(square_m2.m_row_idxs[8], c_int(3)); + EXPECT_EQ(square_m2.m_row_idxs[9], c_int(2)); + EXPECT_EQ(square_m2.m_row_idxs[10], c_int(3)); + EXPECT_EQ(square_m2.m_row_idxs[11], c_int(4)); + EXPECT_EQ(square_m2.m_row_idxs[12], c_int(0)); + EXPECT_EQ(square_m2.m_row_idxs[13], c_int(3)); + EXPECT_EQ(square_m2.m_row_idxs[14], c_int(4)); + EXPECT_EQ(square_m2.m_row_idxs[15], c_int(5)); + EXPECT_EQ(square_m2.m_row_idxs[16], c_int(1)); + EXPECT_EQ(square_m2.m_row_idxs[17], c_int(4)); + EXPECT_EQ(square_m2.m_row_idxs[18], c_int(5)); + ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(7)); // nb of columns + 1 + EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); + EXPECT_EQ(square_m2.m_col_idxs[1], c_int(3)); + EXPECT_EQ(square_m2.m_col_idxs[2], c_int(7)); + EXPECT_EQ(square_m2.m_col_idxs[3], c_int(9)); + EXPECT_EQ(square_m2.m_col_idxs[4], c_int(12)); + EXPECT_EQ(square_m2.m_col_idxs[5], c_int(16)); + EXPECT_EQ(square_m2.m_col_idxs[6], c_int(19)); +} +TEST(TestCscMatrixConv, Trapezoidal) { + using autoware::common::osqp::CSC_Matrix; + using autoware::common::osqp::calCSCMatrixTrapezoidal; + + Eigen::MatrixXd square1(2, 2); + Eigen::MatrixXd square2(3, 3); + Eigen::MatrixXd rect1(1, 2); + square1 << 1.0, 2.0, + 2.0, 4.0; + square2 << 0.0, 2.0, 0.0, + 4.0, 5.0, 6.0, + 0.0, 0.0, 0.0; + rect1 << 0.0, 1.0; + + const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); + // Trapezoidal: skip the lower left triangle (2.0 in this example) + ASSERT_EQ(square_m1.m_vals.size(), size_t(3)); + EXPECT_EQ(square_m1.m_vals[0], 1.0); + EXPECT_EQ(square_m1.m_vals[1], 2.0); + EXPECT_EQ(square_m1.m_vals[2], 4.0); + ASSERT_EQ(square_m1.m_row_idxs.size(), size_t(3)); + EXPECT_EQ(square_m1.m_row_idxs[0], c_int(0)); + EXPECT_EQ(square_m1.m_row_idxs[1], c_int(0)); + EXPECT_EQ(square_m1.m_row_idxs[2], c_int(1)); + ASSERT_EQ(square_m1.m_col_idxs.size(), size_t(3)); + EXPECT_EQ(square_m1.m_col_idxs[0], c_int(0)); + EXPECT_EQ(square_m1.m_col_idxs[1], c_int(1)); + EXPECT_EQ(square_m1.m_col_idxs[2], c_int(3)); + + const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); + ASSERT_EQ(square_m2.m_vals.size(), size_t(3)); + EXPECT_EQ(square_m2.m_vals[0], 2.0); + EXPECT_EQ(square_m2.m_vals[1], 5.0); + EXPECT_EQ(square_m2.m_vals[2], 6.0); + ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(3)); + EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); + EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); + EXPECT_EQ(square_m2.m_row_idxs[2], c_int(1)); + ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(4)); + EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); + EXPECT_EQ(square_m2.m_col_idxs[1], c_int(0)); + EXPECT_EQ(square_m2.m_col_idxs[2], c_int(2)); + EXPECT_EQ(square_m2.m_col_idxs[3], c_int(3)); + + try { + const CSC_Matrix rect_m1 = calCSCMatrixTrapezoidal(rect1); + FAIL() << "calCSCMatrixTrapezoidal should fail with non-square inputs"; + } catch (const std::invalid_argument & e) { + EXPECT_EQ(e.what(), std::string("Matrix must be square (n, n)")); + } +} +TEST(TestCscMatrixConv, Print) { + using autoware::common::osqp::CSC_Matrix; + using autoware::common::osqp::printCSCMatrix; + using autoware::common::osqp::calCSCMatrix; + using autoware::common::osqp::calCSCMatrixTrapezoidal; + Eigen::MatrixXd square1(2, 2); + Eigen::MatrixXd rect1(1, 2); + square1 << 1.0, 2.0, 2.0, 4.0; + rect1 << 0.0, 1.0; + const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); + const CSC_Matrix rect_m1 = calCSCMatrix(rect1); + printCSCMatrix(square_m1); + printCSCMatrix(rect_m1); +} diff --git a/common/osqp_interface/test/test_osqp_interface.cpp b/common/osqp_interface/test/test_osqp_interface.cpp new file mode 100644 index 000000000000..5683023c7c3e --- /dev/null +++ b/common/osqp_interface/test/test_osqp_interface.cpp @@ -0,0 +1,91 @@ +// Copyright 2021 The Autoware Foundation +// +// 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 +#include + +#include "eigen3/Eigen/Core" +#include "gtest/gtest.h" +#include "osqp_interface/osqp_interface.hpp" + + +namespace +{ +using autoware::common::osqp::float64_t; +/// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py +// cppcheck-suppress syntaxError +TEST(TestOsqpInterface, BasicQp) { + auto check_result = + [](const std::tuple, std::vector, int, int> & result) { + EXPECT_EQ(std::get<2>(result), 1); + EXPECT_EQ(std::get<3>(result), 1); + ASSERT_EQ(std::get<0>(result).size(), size_t(2)); + ASSERT_EQ(std::get<1>(result).size(), size_t(2)); + EXPECT_DOUBLE_EQ(std::get<0>(result)[0], 0.3); + EXPECT_DOUBLE_EQ(std::get<0>(result)[1], 0.7); + EXPECT_DOUBLE_EQ(std::get<1>(result)[0], -2.9); + EXPECT_NEAR(std::get<1>(result)[1], 0.2, 1e-6); + }; + + { + // Define problem during optimization + autoware::common::osqp::OSQPInterface osqp; + Eigen::MatrixXd P(2, 2); + P << 4, 1, 1, 2; + Eigen::MatrixXd A(2, 4); + A << 1, 1, 1, 0, 0, 1, 0, 1; + std::vector q = {1.0, 1.0}; + std::vector l = {1.0, 0.0, 0.0, -autoware::common::osqp::INF}; + std::vector u = {1.0, 0.7, 0.7, autoware::common::osqp::INF}; + std::tuple, std::vector, int, int> result = osqp.optimize( + P, A, q, l, u); + check_result(result); + } + { + // Define problem during initialization + Eigen::MatrixXd P(2, 2); + P << 4, 1, 1, 2; + Eigen::MatrixXd A(2, 4); + A << 1, 1, 1, 0, 0, 1, 0, 1; + std::vector q = {1.0, 1.0}; + std::vector l = {1.0, 0.0, 0.0, -autoware::common::osqp::INF}; + std::vector u = {1.0, 0.7, 0.7, autoware::common::osqp::INF}; + autoware::common::osqp::OSQPInterface osqp(P, A, q, l, u, 1e-6); + std::tuple, std::vector, int, int> result = osqp.optimize(); + check_result(result); + } + { + std::tuple, std::vector, int, int> result; + // Dummy initial problem + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(2, 4); + std::vector q(2, 0.0); + std::vector l(4, 0.0); + std::vector u(4, 0.0); + autoware::common::osqp::OSQPInterface osqp(P, A, q, l, u, 1e-6); + osqp.optimize(); + // Redefine problem before optimization + Eigen::MatrixXd P_new(2, 2); + P_new << 4, 1, 1, 2; + Eigen::MatrixXd A_new(2, 4); + A_new << 1, 1, 1, 0, 0, 1, 0, 1; + std::vector q_new = {1.0, 1.0}; + std::vector l_new = {1.0, 0.0, 0.0, -autoware::common::osqp::INF}; + std::vector u_new = {1.0, 0.7, 0.7, autoware::common::osqp::INF}; + osqp.initializeProblem(P_new, A_new, q_new, l_new, u_new); + result = osqp.optimize(); + check_result(result); + } +} +} // namespace diff --git a/common/signal_processing/CMakeLists.txt b/common/signal_processing/CMakeLists.txt new file mode 100644 index 000000000000..563bb7d365af --- /dev/null +++ b/common/signal_processing/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.5) +project(signal_processing) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(lowpass_filter_1d SHARED + src/lowpass_filter_1d.cpp +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_add_gtest(test_signal_processing + test/src/lowpass_filter_1d_test.cpp + ) + target_link_libraries(test_signal_processing + lowpass_filter_1d + ) +endif() + +ament_auto_package() diff --git a/common/signal_processing/README.md b/common/signal_processing/README.md new file mode 100644 index 000000000000..cb566061a87f --- /dev/null +++ b/common/signal_processing/README.md @@ -0,0 +1,7 @@ +# signal_processing + +low-pass filter currently supports only the 1-D low pass filtering. + +## Assumptions / Known limits + +TBD. diff --git a/common/signal_processing/include/signal_processing/lowpass_filter_1d.hpp b/common/signal_processing/include/signal_processing/lowpass_filter_1d.hpp new file mode 100644 index 000000000000..2464adb2c0e9 --- /dev/null +++ b/common/signal_processing/include/signal_processing/lowpass_filter_1d.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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. + +#ifndef SIGNAL_PROCESSING__LOWPASS_FILTER_1D_HPP_ +#define SIGNAL_PROCESSING__LOWPASS_FILTER_1D_HPP_ + +/** + * @class First-order low-pass filter + * @brief filtering values + */ +class LowpassFilter1d +{ +private: + double x_; //!< @brief current filtered value + double gain_; //!< @brief gain value of first-order low-pass filter + +public: + LowpassFilter1d(const double x, const double gain); + + void reset(const double x); + + double getValue() const; + double filter(const double u); +}; + +#endif // SIGNAL_PROCESSING__LOWPASS_FILTER_1D_HPP_ diff --git a/common/signal_processing/package.xml b/common/signal_processing/package.xml new file mode 100644 index 000000000000..65455ece6b43 --- /dev/null +++ b/common/signal_processing/package.xml @@ -0,0 +1,19 @@ + + + signal_processing + 0.1.0 + The signal processing package + Takayuki Murooka + + Apache License 2.0 + + ament_cmake_auto + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/signal_processing/src/lowpass_filter_1d.cpp b/common/signal_processing/src/lowpass_filter_1d.cpp new file mode 100644 index 000000000000..17ab3228835d --- /dev/null +++ b/common/signal_processing/src/lowpass_filter_1d.cpp @@ -0,0 +1,28 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 "signal_processing/lowpass_filter_1d.hpp" + +LowpassFilter1d::LowpassFilter1d(const double x, const double gain) : x_(x), gain_(gain) {} + +void LowpassFilter1d::reset(const double x) { x_ = x; } + +double LowpassFilter1d::getValue() const { return x_; } + +double LowpassFilter1d::filter(const double u) +{ + const double ret = gain_ * x_ + (1.0 - gain_) * u; + x_ = ret; + return ret; +} diff --git a/common/signal_processing/test/src/lowpass_filter_1d_test.cpp b/common/signal_processing/test/src/lowpass_filter_1d_test.cpp new file mode 100644 index 000000000000..f76c83cfb8cf --- /dev/null +++ b/common/signal_processing/test/src/lowpass_filter_1d_test.cpp @@ -0,0 +1,45 @@ +// Copyright 2021 Tier IV, Inc. +// +// 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 "signal_processing/lowpass_filter_1d.hpp" + +#include + +constexpr double epsilon = 1e-6; + +TEST(lowpass_filter_1d, filter) +{ + LowpassFilter1d lowpass_filter_1d(0.0, 0.1); + + // initial state + EXPECT_NEAR(lowpass_filter_1d.getValue(), 0.0, epsilon); + + // random filter + EXPECT_NEAR(lowpass_filter_1d.filter(0.0), 0.0, epsilon); + EXPECT_NEAR(lowpass_filter_1d.filter(1.0), 0.9, epsilon); + EXPECT_NEAR(lowpass_filter_1d.filter(2.0), 1.89, epsilon); + EXPECT_NEAR(lowpass_filter_1d.getValue(), 1.89, epsilon); + + // reset + lowpass_filter_1d.reset(-1.1); + EXPECT_NEAR(lowpass_filter_1d.getValue(), -1.1, epsilon); + EXPECT_NEAR(lowpass_filter_1d.filter(0.0), -0.11, epsilon); + EXPECT_NEAR(lowpass_filter_1d.getValue(), -0.11, epsilon); +} + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}