diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index 33cf69e013..5b3f4b8f81 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -1,4 +1,6 @@ *:*/test/* +missingInclude missingIncludeSystem unmatchedSuppression +unusedFunction diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index dd724dae7d..52a1890950 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,6 +1,8 @@ ### Automatically generated from package.xml ### common/autoware_geography_utils/** anh.nguyen.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners common/autoware_node/** mfc@autoware.org @autowarefoundation/autoware-core-global-codeowners +common/autoware_point_types/** cynthia.liu@autocore.ai david.wong@tier4.jp max.schmeller@tier4.jp @autowarefoundation/autoware-core-global-codeowners demos/autoware_test_node/** mfc@autoware.org @autowarefoundation/autoware-core-global-codeowners ### Copied from .github/CODEOWNERS-manual ### +.github/** @autowarefoundation/autoware-core-global-codeowners diff --git a/.github/CODEOWNERS-manual b/.github/CODEOWNERS-manual index e69de29bb2..db2211f777 100644 --- a/.github/CODEOWNERS-manual +++ b/.github/CODEOWNERS-manual @@ -0,0 +1 @@ +.github/** @autowarefoundation/autoware-core-global-codeowners diff --git a/.github/workflows/build-and-test-daily.yaml b/.github/workflows/build-and-test-daily.yaml index a79bbd6236..6fdd27c0f6 100644 --- a/.github/workflows/build-and-test-daily.yaml +++ b/.github/workflows/build-and-test-daily.yaml @@ -44,7 +44,7 @@ jobs: - name: Upload coverage to CodeCov if: ${{ steps.test.outputs.coverage-report-files != '' }} - uses: codecov/codecov-action@v4 + uses: codecov/codecov-action@v5 with: files: ${{ steps.test.outputs.coverage-report-files }} fail_ci_if_error: false diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index e937514742..3392fb502e 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -17,14 +17,14 @@ env: CXX: /usr/lib/ccache/g++ jobs: - make-sure-label-is-present: - uses: autowarefoundation/autoware-github-actions/.github/workflows/make-sure-label-is-present.yaml@v1 + require-label: + uses: autowarefoundation/autoware-github-actions/.github/workflows/require-label.yaml@v1 with: - label: tag:run-build-and-test-differential + label: run:build-and-test-differential build-and-test-differential: - needs: make-sure-label-is-present - if: ${{ needs.make-sure-label-is-present.outputs.result == 'true' }} + needs: require-label + if: ${{ needs.require-label.outputs.result == 'true' }} runs-on: ubuntu-24.04 container: ghcr.io/autowarefoundation/autoware:core-devel steps: diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 52bdc7830a..3e7a734c4f 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -95,7 +95,7 @@ jobs: - name: Upload coverage to CodeCov if: ${{ steps.test.outputs.coverage-report-files != '' }} - uses: codecov/codecov-action@v4 + uses: codecov/codecov-action@v5 with: files: ${{ steps.test.outputs.coverage-report-files }} fail_ci_if_error: false diff --git a/.github/workflows/bump-version-pr.yaml b/.github/workflows/bump-version-pr.yaml new file mode 100644 index 0000000000..5f3a931e90 --- /dev/null +++ b/.github/workflows/bump-version-pr.yaml @@ -0,0 +1,80 @@ +name: bump-version-pr + +on: + workflow_dispatch: + inputs: + version_part: + description: Which part of the version number to bump? + required: true + default: minor + type: choice + options: + - major + - minor + - patch + +jobs: + bump-version-pr: + runs-on: ubuntu-22.04 + steps: + - name: Check out repository + uses: actions/checkout@v4 + with: + ref: humble + fetch-depth: 0 + + - name: Generate token + id: generate-token + uses: actions/create-github-app-token@v1 + with: + app-id: ${{ secrets.APP_ID }} + private-key: ${{ secrets.PRIVATE_KEY }} + + - name: Set git config + uses: autowarefoundation/autoware-github-actions/set-git-config@v1 + with: + token: ${{ steps.generate-token.outputs.token }} + + - name: Setup Python 3.x + uses: actions/setup-python@v5 + with: + python-version: 3.x + + - name: Install dependencies + run: pip3 install -U catkin_tools + shell: bash + + - name: Bump version from humble branch + id: bump-version-from-humble-branch + run: | + git checkout -b tmp/bot/bump_version_base + git fetch origin main + git merge origin/main + catkin_generate_changelog -y + git add * + git commit -m "update CHANGELOG.rst" + catkin_prepare_release -y --bump ${{ inputs.version_part }} --no-push + version=$(git describe --tags) + echo "version=${version}" >> $GITHUB_OUTPUT + shell: bash + + - name: Create target branch + run: | + git checkout origin/main + git checkout -b chore/bot/bump_version + git merge tmp/bot/bump_version_base + git push origin chore/bot/bump_version --force + shell: bash + + - name: Create PR + id: create-pr + run: > + gh + pr + create + --base=main + --body="Bump version to ${{ steps.bump-version-from-humble-branch.outputs.version }}" + --title="chore: bump version to ${{ steps.bump-version-from-humble-branch.outputs.version }}" + --head=chore/bot/bump_version + env: + GH_TOKEN: ${{ steps.generate-token.outputs.token }} diff --git a/.github/workflows/clang-tidy-pr-comments-manually.yaml b/.github/workflows/clang-tidy-pr-comments-manually.yaml index 747c62cdcd..2c05e2b26f 100644 --- a/.github/workflows/clang-tidy-pr-comments-manually.yaml +++ b/.github/workflows/clang-tidy-pr-comments-manually.yaml @@ -15,7 +15,7 @@ jobs: runs-on: ubuntu-22.04 steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Download analysis results run: | @@ -40,7 +40,7 @@ jobs: - name: Check out PR head if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: repository: ${{ steps.set-variables.outputs.pr-head-repo }} ref: ${{ steps.set-variables.outputs.pr-head-ref }} diff --git a/.github/workflows/clang-tidy-pr-comments.yaml b/.github/workflows/clang-tidy-pr-comments.yaml index c8df6a62c9..2e9510fc3d 100644 --- a/.github/workflows/clang-tidy-pr-comments.yaml +++ b/.github/workflows/clang-tidy-pr-comments.yaml @@ -17,7 +17,7 @@ jobs: runs-on: ubuntu-22.04 steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Download analysis results run: | @@ -41,7 +41,7 @@ jobs: - name: Check out PR head if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: repository: ${{ steps.set-variables.outputs.pr-head-repo }} ref: ${{ steps.set-variables.outputs.pr-head-ref }} diff --git a/.github/workflows/cppcheck-weekly.yaml b/.github/workflows/cppcheck-weekly.yaml index c75a72e1fd..7fc5c72308 100644 --- a/.github/workflows/cppcheck-weekly.yaml +++ b/.github/workflows/cppcheck-weekly.yaml @@ -11,7 +11,7 @@ jobs: steps: - name: Checkout code - uses: actions/checkout@v2 + uses: actions/checkout@v4 # cppcheck from apt does not yet support --check-level args, and thus install from snap - name: Install Cppcheck from snap diff --git a/.github/workflows/delete-closed-pr-docs.yaml b/.github/workflows/delete-closed-pr-docs.yaml index b8ff4f6d14..86301c06a9 100644 --- a/.github/workflows/delete-closed-pr-docs.yaml +++ b/.github/workflows/delete-closed-pr-docs.yaml @@ -14,7 +14,7 @@ jobs: runs-on: ubuntu-22.04 steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index 47009a25e6..3af7a51d36 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -34,7 +34,7 @@ jobs: runs-on: ubuntu-22.04 steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: fetch-depth: 0 ref: ${{ github.event.pull_request.head.sha }} diff --git a/common/autoware_kalman_filter/CMakeLists.txt b/common/autoware_kalman_filter/CMakeLists.txt new file mode 100644 index 0000000000..076d2d3cad --- /dev/null +++ b/common/autoware_kalman_filter/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_kalman_filter) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +include_directories( + SYSTEM + ${EIGEN3_INCLUDE_DIR} +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/kalman_filter.cpp + src/time_delay_kalman_filter.cpp + include/autoware/kalman_filter/kalman_filter.hpp + include/autoware/kalman_filter/time_delay_kalman_filter.hpp +) + +if(BUILD_TESTING) + file(GLOB_RECURSE test_files test/*.cpp) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) + + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) +endif() + +ament_auto_package() diff --git a/common/autoware_kalman_filter/README.md b/common/autoware_kalman_filter/README.md new file mode 100644 index 0000000000..daa0066f16 --- /dev/null +++ b/common/autoware_kalman_filter/README.md @@ -0,0 +1,288 @@ +# kalman_filter + +## Overview + +This package contains the kalman filter with time delay and the calculation of the kalman filter. + +## Design + +The Kalman filter is a recursive algorithm used to estimate the state of a dynamic system. The Time Delay Kalman filter is based on the standard Kalman filter and takes into account possible time delays in the measured values. + +### Standard Kalman Filter + +#### System Model + +Assume that the system can be represented by the following linear discrete model: + +$$ +x_{k} = A x_{k-1} + B u_{k} \\ +y_{k} = C x_{k-1} +$$ + +where, + +- $x_k$ is the state vector at time $k$. +- $u_k$ is the control input vector at time $k$. +- $y_k$ is the measurement vector at time $k$. +- $A$ is the state transition matrix. +- $B$ is the control input matrix. +- $C$ is the measurement matrix. + +#### Prediction Step + +The prediction step consists of updating the state and covariance matrices: + +$$ +x_{k|k-1} = A x_{k-1|k-1} + B u_{k} \\ +P_{k|k-1} = A P_{k-1|k-1} A^{T} + Q +$$ + +where, + +- $x_{k|k-1}$ is the priori state estimate. +- $P_{k|k-1}$ is the priori covariance matrix. + +#### Update Step + +When the measurement value \( y_k \) is received, the update steps are as follows: + +$$ +K_k = P_{k|k-1} C^{T} (C P_{k|k-1} C^{T} + R)^{-1} \\ +x_{k|k} = x_{k|k-1} + K_k (y_{k} - C x_{k|k-1}) \\ +P_{k|k} = (I - K_k C) P_{k|k-1} +$$ + +where, + +- $K_k$ is the Kalman gain. +- $x_{k|k}$ is the posterior state estimate. +- $P_{k|k}$ is the posterior covariance matrix. + +### Extension to Time Delay Kalman Filter + +For the Time Delay Kalman filter, it is assumed that there may be a maximum delay of step ($d$) in the measured value. To handle this delay, we extend the state vector to: + +$$ +(x_{k})_e = \begin{bmatrix} +x_k \\ +x_{k-1} \\ +\vdots \\ +x_{k-d+1} +\end{bmatrix} +$$ + +The corresponding state transition matrix ($A_e$) and process noise covariance matrix ($Q_e$) are also expanded: + +$$ +A_e = \begin{bmatrix} +A & 0 & 0 & \cdots & 0 \\ +I & 0 & 0 & \cdots & 0 \\ +0 & I & 0 & \cdots & 0 \\ +\vdots & \vdots & \vdots & \ddots & \vdots \\ +0 & 0 & 0 & \cdots & 0 +\end{bmatrix}, \quad +Q_e = \begin{bmatrix} +Q & 0 & 0 & \cdots & 0 \\ +0 & 0 & 0 & \cdots & 0 \\ +0 & 0 & 0 & \cdots & 0 \\ +\vdots & \vdots & \vdots & \ddots & \vdots \\ +0 & 0 & 0 & \cdots & 0 +\end{bmatrix} +$$ + +#### Prediction Step + +The prediction step consists of updating the extended state and covariance matrices. + +Update extension status: + +$$ +(x_{k|k-1})_e = \begin{bmatrix} +A & 0 & 0 & \cdots & 0 \\ +I & 0 & 0 & \cdots & 0 \\ +0 & I & 0 & \cdots & 0 \\ +\vdots & \vdots & \vdots & \ddots & \vdots \\ +0 & 0 & 0 & \cdots & 0 +\end{bmatrix} +\begin{bmatrix} +x_{k-1|k-1} \\ +x_{k-2|k-1} \\ +\vdots \\ +x_{k-d|k-1} +\end{bmatrix} +$$ + +Update extended covariance matrix: + +$$ +(P_{k|k-1})_e = \begin{bmatrix} +A & 0 & 0 & \cdots & 0 \\ +I & 0 & 0 & \cdots & 0 \\ +0 & I & 0 & \cdots & 0 \\ +\vdots & \vdots & \vdots & \ddots & \vdots \\ +0 & 0 & 0 & \cdots & 0 +\end{bmatrix} +\begin{bmatrix} +P_{k-1|k-1}^{(1)} & P_{k-1|k-1}^{(1,2)} & \cdots & P_{k-1|k-1}^{(1,d)} \\ +P_{k-1|k-1}^{(2,1)} & P_{k-1|k-1}^{(2)} & \cdots & P_{k-1|k-1}^{(2,d)} \\ +\vdots & \vdots & \ddots & \vdots \\ +P_{k-1|k-1}^{(d,1)} & P_{k-1|k-1}^{(d,2)} & \cdots & P_{k-1|k-1}^{(d)} +\end{bmatrix} +\begin{bmatrix} + A^T & I & 0 & \cdots & 0 \\ + 0 & 0 & I & \cdots & 0 \\ + 0 & 0 & 0 & \cdots & 0 \\ + \vdots & \vdots & \vdots & \ddots & \vdots \\ + 0 & 0 & 0 & \cdots & 0 + \end{bmatrix} + + \begin{bmatrix} + Q & 0 & 0 & \cdots & 0 \\ + 0 & 0 & 0 & \cdots & 0 \\ + 0 & 0 & 0 & \cdots & 0 \\ + \vdots & \vdots & \vdots & \ddots & \vdots \\ + 0 & 0 & 0 & \cdots & 0 + \end{bmatrix} +$$ + +$\Longrightarrow$ + +$$ +(P_{k|k-1})_e = \begin{bmatrix} A P_{k-1|k-1}^{(1)} A^T + Q & A P_{k-1|k-1}^{(1,2)} & \cdots & A P_{k-1|k-1}^{(1,d)} \\ P_{k-1|k-1}^{(2,1)} A^T & P_{k-1|k-1}^{(2)} & \cdots & P_{k-1|k-1}^{(2,d)} \\ \vdots & \vdots & \ddots & \vdots \\ P_{k-1|k-1}^{(d,1)} A^T & P_{k-1|k-1}^{(d,2)} & \cdots & P_{k-1|k-1}^{(d)} \end{bmatrix} +$$ + +where, + +- $(x_{k|k-1})_e$ is the priori extended state estimate. +- $(P_{k|k-1})_e$ is the priori extended covariance matrix. + +#### Update Step + +When receiving the measurement value ( $y_{k}$ ) with a delay of ( $ds$ ), the update steps are as follows: + +Update kalman gain: + +$$ +K_k = \begin{bmatrix} +P_{k|k-1}^{(1)} C^T \\ +P_{k|k-1}^{(2)} C^T \\ +\vdots \\ +P_{k|k-1}^{(ds)} C^T \\ +\vdots \\ +P_{k|k-1}^{(d)} C^T +\end{bmatrix} +(C P_{k|k-1}^{(ds)} C^T + R)^{-1} +$$ + +Update extension status: + +$$ +(x_{k|k})_e = \begin{bmatrix} +x_{k|k-1} \\ +x_{k-1|k-1} \\ +\vdots \\ +x_{k-d+1|k-1} +\end{bmatrix} + +\begin{bmatrix} +K_k^{(1)} \\ +K_k^{(2)} \\ +\vdots \\ +K_k^{(ds)} \\ +\vdots \\ +K_k^{(d)} +\end{bmatrix} (y_k - C x_{k-ds|k-1}) +$$ + +Update extended covariance matrix: + +$$ + (P_{k|k})_e = \left(I - + \begin{bmatrix} + K_k^{(1)} C \\ + K_k^{(2)} C \\ + \vdots \\ + K_k^{(ds)} C \\ + \vdots \\ + K_k^{(d)} C + \end{bmatrix}\right) + \begin{bmatrix} + P_{k|k-1}^{(1)} & P_{k|k-1}^{(1,2)} & \cdots & P_{k|k-1}^{(1,d)} \\ + P_{k|k-1}^{(2,1)} & P_{k|k-1}^{(2)} & \cdots & P_{k|k-1}^{(2,d)} \\ + \vdots & \vdots & \ddots & \vdots \\ + P_{k|k-1}^{(d,1)} & P_{k|k-1}^{(d,2)} & \cdots & P_{k|k-1}^{(d)} + \end{bmatrix} +$$ + +where, + +- $K_k$ is the Kalman gain. +- $(x_{k|k})_e$ is the posterior extended state estimate. +- $(P_{k|k})_e$ is the posterior extended covariance matrix. +- $C$ is the measurement matrix, which only applies to the delayed state part. + +## Example Usage + +This section describes Example Usage of KalmanFilter. + +- Initialization + +```cpp +#include "autoware/kalman_filter/kalman_filter.hpp" + +// Define system parameters +int dim_x = 2; // state vector dimensions +int dim_y = 1; // measure vector dimensions + +// Initial state +Eigen::MatrixXd x0 = Eigen::MatrixXd::Zero(dim_x, 1); +x0 << 0.0, 0.0; + +// Initial covariance matrix +Eigen::MatrixXd P0 = Eigen::MatrixXd::Identity(dim_x, dim_x); +P0 *= 100.0; + +// Define state transition matrix +Eigen::MatrixXd A = Eigen::MatrixXd::Identity(dim_x, dim_x); +A(0, 1) = 1.0; + +// Define measurement matrix +Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x); +C(0, 0) = 1.0; + +// Define process noise covariance matrix +Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(dim_x, dim_x); +Q *= 0.01; + +// Define measurement noise covariance matrix +Eigen::MatrixXd R = Eigen::MatrixXd::Identity(dim_y, dim_y); +R *= 1.0; + +// Initialize Kalman filter +autoware::kalman_filter::KalmanFilter kf; +kf.init(x0, P0); +``` + +- Predict step + +```cpp +const Eigen::MatrixXd x_next = A * x0; +kf.predict(x_next, A, Q); +``` + +- Update step + +```cpp +// Measured value +Eigen::MatrixXd y = Eigen::MatrixXd::Zero(dim_y, 1); +kf.update(y, C, R); +``` + +- Get the current estimated state and covariance matrix + +```cpp +Eigen::MatrixXd x_curr = kf.getX(); +Eigen::MatrixXd P_curr = kf.getP(); +``` + +## Assumptions / Known limits + +- Delay Step Check: Ensure that the `delay_step` provided during the update does not exceed the maximum delay steps set during initialization. diff --git a/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp new file mode 100644 index 0000000000..74db04f6e8 --- /dev/null +++ b/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp @@ -0,0 +1,214 @@ +// 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 AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ +#define AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ + +#include +#include + +namespace autoware::kalman_filter +{ + +/** + * @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) const; + + /** + * @brief get current kalman filter covariance + * @param P kalman filter covariance + */ + void getP(Eigen::MatrixXd & P) const; + + /** + * @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) const; + + /** + * @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 +}; +} // namespace autoware::kalman_filter +#endif // AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ diff --git a/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp new file mode 100644 index 0000000000..80375b7579 --- /dev/null +++ b/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp @@ -0,0 +1,89 @@ +// 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 AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ +#define AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ + +#include "autoware/kalman_filter/kalman_filter.hpp" + +#include +#include + +#include + +namespace autoware::kalman_filter +{ +/** + * @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 + */ + Eigen::MatrixXd getLatestX() const; + + /** + * @brief get latest time estimation covariance + */ + Eigen::MatrixXd getLatestP() const; + + /** + * @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 +}; +} // namespace autoware::kalman_filter +#endif // AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ diff --git a/common/autoware_kalman_filter/package.xml b/common/autoware_kalman_filter/package.xml new file mode 100644 index 0000000000..d43810a0ea --- /dev/null +++ b/common/autoware_kalman_filter/package.xml @@ -0,0 +1,28 @@ + + + + autoware_kalman_filter + 0.0.0 + The kalman filter package + Yukihiro Saito + Takeshi Ishita + Koji Minoda + Apache License 2.0 + + Takamasa Horibe + + ament_cmake_auto + autoware_cmake + + eigen + eigen3_cmake_module + + ament_cmake_cppcheck + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_kalman_filter/src/kalman_filter.cpp b/common/autoware_kalman_filter/src/kalman_filter.cpp new file mode 100644 index 0000000000..bbd963675f --- /dev/null +++ b/common/autoware_kalman_filter/src/kalman_filter.cpp @@ -0,0 +1,161 @@ +// 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 "autoware/kalman_filter/kalman_filter.hpp" + +namespace autoware::kalman_filter +{ +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) const +{ + x = x_; +} +void KalmanFilter::getP(Eigen::MatrixXd & P) const +{ + P = P_; +} +double KalmanFilter::getXelement(unsigned int i) const +{ + 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_); +} +} // namespace autoware::kalman_filter diff --git a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp new file mode 100644 index 0000000000..4d1dd8f33b --- /dev/null +++ b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp @@ -0,0 +1,109 @@ +// 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 "autoware/kalman_filter/time_delay_kalman_filter.hpp" + +#include + +namespace autoware::kalman_filter +{ +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; + } +} + +Eigen::MatrixXd TimeDelayKalmanFilter::getLatestX() const +{ + return x_.block(0, 0, dim_x_, 1); +} + +Eigen::MatrixXd TimeDelayKalmanFilter::getLatestP() const +{ + return 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; +} +} // namespace autoware::kalman_filter diff --git a/common/autoware_kalman_filter/test/test_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_kalman_filter.cpp new file mode 100644 index 0000000000..34e23ef9d0 --- /dev/null +++ b/common/autoware_kalman_filter/test/test_kalman_filter.cpp @@ -0,0 +1,96 @@ +// Copyright 2023 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 "autoware/kalman_filter/kalman_filter.hpp" + +#include + +using autoware::kalman_filter::KalmanFilter; + +TEST(kalman_filter, kf) +{ + KalmanFilter kf_; + + Eigen::MatrixXd x_t(2, 1); + x_t << 1, 2; + + Eigen::MatrixXd P_t(2, 2); + P_t << 1, 0, 0, 1; + + Eigen::MatrixXd Q_t(2, 2); + Q_t << 0.01, 0, 0, 0.01; + + Eigen::MatrixXd R_t(2, 2); + R_t << 0.09, 0, 0, 0.09; + + Eigen::MatrixXd C_t(2, 2); + C_t << 1, 0, 0, 1; + + Eigen::MatrixXd A_t(2, 2); + A_t << 1, 0, 0, 1; + + Eigen::MatrixXd B_t(2, 2); + B_t << 1, 0, 0, 1; + + // Initialize the filter and check if initialization was successful + EXPECT_TRUE(kf_.init(x_t, A_t, B_t, C_t, Q_t, R_t, P_t)); + + // Perform prediction + Eigen::MatrixXd u_t(2, 1); + u_t << 0.1, 0.1; + EXPECT_TRUE(kf_.predict(u_t)); + + // Check the updated state and covariance matrix + Eigen::MatrixXd x_predict_expected = A_t * x_t + B_t * u_t; + Eigen::MatrixXd P_predict_expected = A_t * P_t * A_t.transpose() + Q_t; + + Eigen::MatrixXd x_predict; + kf_.getX(x_predict); + Eigen::MatrixXd P_predict; + kf_.getP(P_predict); + + EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); + EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); + + // Perform update + Eigen::MatrixXd y_t(2, 1); + y_t << 1.05, 2.05; + EXPECT_TRUE(kf_.update(y_t)); + + // Check the updated state and covariance matrix + const Eigen::MatrixXd PCT_t = P_predict_expected * C_t.transpose(); + const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_t * PCT_t).inverse()); + const Eigen::MatrixXd y_pred = C_t * x_predict_expected; + Eigen::MatrixXd x_update_expected = x_predict_expected + K_t * (y_t - y_pred); + Eigen::MatrixXd P_update_expected = P_predict_expected - K_t * (C_t * P_predict_expected); + + Eigen::MatrixXd x_update; + kf_.getX(x_update); + Eigen::MatrixXd P_update; + kf_.getP(P_update); + + EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); + EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); + EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); + EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); +} + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + bool result = RUN_ALL_TESTS(); + return result; +} diff --git a/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp new file mode 100644 index 0000000000..50c22fae12 --- /dev/null +++ b/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp @@ -0,0 +1,123 @@ +// Copyright 2023 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 "autoware/kalman_filter/time_delay_kalman_filter.hpp" + +#include + +using autoware::kalman_filter::TimeDelayKalmanFilter; + +TEST(time_delay_kalman_filter, td_kf) +{ + TimeDelayKalmanFilter td_kf_; + + Eigen::MatrixXd x_t(3, 1); + x_t << 1.0, 2.0, 3.0; + Eigen::MatrixXd P_t(3, 3); + P_t << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.3; + const int max_delay_step = 5; + const int dim_x = x_t.rows(); + const int dim_x_ex = dim_x * max_delay_step; + // Initialize the filter + td_kf_.init(x_t, P_t, max_delay_step); + + // Check if initialization was successful + Eigen::MatrixXd x_init = td_kf_.getLatestX(); + Eigen::MatrixXd P_init = td_kf_.getLatestP(); + Eigen::MatrixXd x_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, 1); + Eigen::MatrixXd P_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); + for (int i = 0; i < max_delay_step; ++i) { + x_ex_t.block(i * dim_x, 0, dim_x, 1) = x_t; + P_ex_t.block(i * dim_x, i * dim_x, dim_x, dim_x) = P_t; + } + + EXPECT_EQ(x_init.rows(), 3); + EXPECT_EQ(x_init.cols(), 1); + EXPECT_EQ(P_init.rows(), 3); + EXPECT_EQ(P_init.cols(), 3); + EXPECT_NEAR(x_init(0, 0), x_t(0, 0), 1e-5); + EXPECT_NEAR(x_init(1, 0), x_t(1, 0), 1e-5); + EXPECT_NEAR(x_init(2, 0), x_t(2, 0), 1e-5); + EXPECT_NEAR(P_init(0, 0), P_t(0, 0), 1e-5); + EXPECT_NEAR(P_init(1, 1), P_t(1, 1), 1e-5); + EXPECT_NEAR(P_init(2, 2), P_t(2, 2), 1e-5); + + // Define prediction parameters + Eigen::MatrixXd A_t(3, 3); + A_t << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0; + Eigen::MatrixXd Q_t(3, 3); + Q_t << 0.01, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.03; + Eigen::MatrixXd x_next(3, 1); + x_next << 2.0, 4.0, 6.0; + + // Perform prediction + EXPECT_TRUE(td_kf_.predictWithDelay(x_next, A_t, Q_t)); + + // Check the prediction state and covariance matrix + Eigen::MatrixXd x_predict = td_kf_.getLatestX(); + Eigen::MatrixXd P_predict = td_kf_.getLatestP(); + Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex, 1); + x_tmp.block(0, 0, dim_x, 1) = A_t * x_t; + x_tmp.block(dim_x, 0, dim_x_ex - dim_x, 1) = x_ex_t.block(0, 0, dim_x_ex - dim_x, 1); + x_ex_t = x_tmp; + Eigen::MatrixXd x_predict_expected = x_ex_t.block(0, 0, dim_x, 1); + Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); + P_tmp.block(0, 0, dim_x, dim_x) = A_t * P_ex_t.block(0, 0, dim_x, dim_x) * A_t.transpose() + Q_t; + P_tmp.block(0, dim_x, dim_x, dim_x_ex - dim_x) = + A_t * P_ex_t.block(0, 0, dim_x, dim_x_ex - dim_x); + P_tmp.block(dim_x, 0, dim_x_ex - dim_x, dim_x) = + P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x) * A_t.transpose(); + P_tmp.block(dim_x, dim_x, dim_x_ex - dim_x, dim_x_ex - dim_x) = + P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x_ex - dim_x); + P_ex_t = P_tmp; + Eigen::MatrixXd P_predict_expected = P_ex_t.block(0, 0, dim_x, dim_x); + EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); + EXPECT_NEAR(x_predict(2, 0), x_predict_expected(2, 0), 1e-5); + EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); + EXPECT_NEAR(P_predict(2, 2), P_predict_expected(2, 2), 1e-5); + + // Define update parameters + Eigen::MatrixXd C_t(3, 3); + C_t << 0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5; + Eigen::MatrixXd R_t(3, 3); + R_t << 0.001, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.003; + Eigen::MatrixXd y_t(3, 1); + y_t << 1.05, 2.05, 3.05; + const int delay_step = 2; // Choose an appropriate delay step + const int dim_y = y_t.rows(); + + // Perform update + EXPECT_TRUE(td_kf_.updateWithDelay(y_t, C_t, R_t, delay_step)); + + // Check the updated state and covariance matrix + Eigen::MatrixXd x_update = td_kf_.getLatestX(); + Eigen::MatrixXd P_update = td_kf_.getLatestP(); + + Eigen::MatrixXd C_ex_t = Eigen::MatrixXd::Zero(dim_y, dim_x_ex); + const Eigen::MatrixXd PCT_t = P_ex_t * C_ex_t.transpose(); + const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_ex_t * PCT_t).inverse()); + const Eigen::MatrixXd y_pred = C_ex_t * x_ex_t; + x_ex_t = x_ex_t + K_t * (y_t - y_pred); + P_ex_t = P_ex_t - K_t * (C_ex_t * P_ex_t); + Eigen::MatrixXd x_update_expected = x_ex_t.block(0, 0, dim_x, 1); + Eigen::MatrixXd P_update_expected = P_ex_t.block(0, 0, dim_x, dim_x); + EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); + EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); + EXPECT_NEAR(x_update(2, 0), x_update_expected(2, 0), 1e-5); + EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); + EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); + EXPECT_NEAR(P_update(2, 2), P_update_expected(2, 2), 1e-5); +} diff --git a/common/autoware_osqp_interface/CMakeLists.txt b/common/autoware_osqp_interface/CMakeLists.txt new file mode 100644 index 0000000000..b770af659e --- /dev/null +++ b/common/autoware_osqp_interface/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_osqp_interface) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Eigen3 REQUIRED) + +# after find_package(osqp_vendor) in ament_auto_find_build_dependencies +find_package(osqp REQUIRED) +get_target_property(OSQP_INCLUDE_SUB_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES) +get_filename_component(OSQP_INCLUDE_DIR ${OSQP_INCLUDE_SUB_DIR} PATH) + +set(OSQP_INTERFACE_LIB_SRC + src/csc_matrix_conv.cpp + src/osqp_interface.cpp +) + +set(OSQP_INTERFACE_LIB_HEADERS + include/autoware/osqp_interface/csc_matrix_conv.hpp + include/autoware/osqp_interface/osqp_interface.hpp + include/autoware/osqp_interface/visibility_control.hpp +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + ${OSQP_INTERFACE_LIB_SRC} + ${OSQP_INTERFACE_LIB_HEADERS} +) +target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast) + +target_include_directories(${PROJECT_NAME} + SYSTEM PUBLIC + "${OSQP_INCLUDE_DIR}" + "${EIGEN3_INCLUDE_DIR}" +) + +ament_target_dependencies(${PROJECT_NAME} + Eigen3 + osqp_vendor +) + +# crucial so downstream package builds because autoware_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) + +if(BUILD_TESTING) + set(TEST_SOURCES + test/test_osqp_interface.cpp + test/test_csc_matrix_conv.cpp + ) + set(TEST_OSQP_INTERFACE_EXE test_osqp_interface) + ament_add_ros_isolated_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) + target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME}) +endif() + +ament_auto_package(INSTALL_TO_SHARE +) diff --git a/common/autoware_osqp_interface/design/osqp_interface-design.md b/common/autoware_osqp_interface/design/osqp_interface-design.md new file mode 100644 index 0000000000..887a4b4d9a --- /dev/null +++ b/common/autoware_osqp_interface/design/osqp_interface-design.md @@ -0,0 +1,70 @@ +# Interface for the OSQP library + +This is the design document for the `autoware_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. + + ```cpp + osqp_interface = OSQPInterface(); + osqp_interface.optimize(P, A, q, l, u); + ``` + +2. Initialize the interface WITH data. + + ```cpp + osqp_interface = OSQPInterface(P, A, q, l, u); + osqp_interface.optimize(); + ``` + +3. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs. + + ```cpp + 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. + + ```cpp + 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: + +## Related issues + + diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp new file mode 100644 index 0000000000..8c21553152 --- /dev/null +++ b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp @@ -0,0 +1,47 @@ +// 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 AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ +#define AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ + +#include "autoware/osqp_interface/visibility_control.hpp" +#include "osqp/glob_opts.h" // for 'c_int' type ('long' or 'long long') + +#include + +#include + +namespace autoware::osqp_interface +{ +/// \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 autoware::osqp_interface + +#endif // AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp new file mode 100644 index 0000000000..ff3013bd61 --- /dev/null +++ b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp @@ -0,0 +1,194 @@ +// 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 AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ +#define AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ + +#include "autoware/osqp_interface/csc_matrix_conv.hpp" +#include "autoware/osqp_interface/visibility_control.hpp" +#include "osqp/osqp.h" + +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::osqp_interface +{ +constexpr c_float INF = 1e30; + +/** + * Implementation of a native C++ interface for the OSQP solver. + * + */ +class OSQP_INTERFACE_PUBLIC OSQPInterface +{ +private: + std::unique_ptr> m_work; + 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 + bool m_work_initialized = false; + // Exitflag + int64_t m_exitflag; + + // Runs the solver on the stored problem. + std::tuple, std::vector, int64_t, int64_t, int64_t> solve(); + + static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; + +public: + /// \brief Constructor without problem formulation + explicit OSQPInterface( + const c_float eps_abs = std::numeric_limits::epsilon(), const bool 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( + const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, + const std::vector & l, const std::vector & u, const c_float eps_abs); + ~OSQPInterface(); + + /**************** + * OPTIMIZATION + ****************/ + /// \brief Solves the stored convex 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 double x_0 = param[0]; + /// \details double x_1 = param[1]; + std::tuple, std::vector, int64_t, 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 double x_0 = param[0]; + /// \details double x_1 = param[1]; + std::tuple, std::vector, int64_t, 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); + int64_t initializeProblem( + CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, + const std::vector & u); + + // Setter functions for warm start + bool setWarmStart( + const std::vector & primal_variables, const std::vector & dual_variables); + bool setPrimalVariables(const std::vector & primal_variables); + bool setDualVariables(const std::vector & dual_variables); + + // 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 updateCscP(const CSC_Matrix & P_csc); + void updateA(const Eigen::MatrixXd & A_new); + void updateCscA(const CSC_Matrix & A_csc); + 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); + void updateScaling(const int scaling); + void updatePolish(const bool polish); + void updatePolishRefinementIteration(const int polish_refine_iter); + void updateCheckTermination(const int check_termination); + + /// \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 double getRunTime() const { return m_latest_work_info.run_time; } + /// \brief Get the objective value the latest problem solved + inline double 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; } + + void logUnsolvedStatus(const std::string & prefix_message = "") const; +}; + +} // namespace autoware::osqp_interface + +#endif // AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp new file mode 100644 index 0000000000..a6cdaa8a0a --- /dev/null +++ b/common/autoware_osqp_interface/include/autoware/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 AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE__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 // AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_osqp_interface/package.xml b/common/autoware_osqp_interface/package.xml new file mode 100644 index 0000000000..0b9f539481 --- /dev/null +++ b/common/autoware_osqp_interface/package.xml @@ -0,0 +1,29 @@ + + + + autoware_osqp_interface + 0.0.0 + Interface for the OSQP solver + Maxime CLEMENT + Takayuki Murooka + Fumiya Watanabe + Satoshi Ota + Apache 2.0 + + ament_cmake_auto + autoware_cmake + + eigen + osqp_vendor + rclcpp + rclcpp_components + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/common/autoware_osqp_interface/src/csc_matrix_conv.cpp b/common/autoware_osqp_interface/src/csc_matrix_conv.cpp new file mode 100644 index 0000000000..c6e1a0a42d --- /dev/null +++ b/common/autoware_osqp_interface/src/csc_matrix_conv.cpp @@ -0,0 +1,135 @@ +// 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 "autoware/osqp_interface/csc_matrix_conv.hpp" + +#include +#include + +#include +#include +#include + +namespace autoware::osqp_interface +{ +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 autoware::osqp_interface diff --git a/common/autoware_osqp_interface/src/osqp_interface.cpp b/common/autoware_osqp_interface/src/osqp_interface.cpp new file mode 100644 index 0000000000..ceeae62622 --- /dev/null +++ b/common/autoware_osqp_interface/src/osqp_interface.cpp @@ -0,0 +1,435 @@ +// 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 "autoware/osqp_interface/osqp_interface.hpp" + +#include "autoware/osqp_interface/csc_matrix_conv.hpp" +#include "osqp/osqp.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::osqp_interface +{ +OSQPInterface::OSQPInterface(const c_float eps_abs, const bool polish) +: m_work{nullptr, OSQPWorkspaceDeleter} +{ + 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); +} + +OSQPInterface::OSQPInterface( + const CSC_Matrix & P, const CSC_Matrix & 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); +} + +OSQPInterface::~OSQPInterface() +{ + if (m_data->P) free(m_data->P); + if (m_data->A) free(m_data->A); +} + +void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept +{ + if (ptr != nullptr) { + osqp_cleanup(ptr); + } +} + +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.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); +} + +void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) +{ + osqp_update_P( + m_work.get(), 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.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); + return; +} + +void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) +{ + osqp_update_A( + m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); +} + +void OSQPInterface::updateQ(const std::vector & q_new) +{ + std::vector q_tmp(q_new.begin(), q_new.end()); + double * q_dyn = q_tmp.data(); + osqp_update_lin_cost(m_work.get(), q_dyn); +} + +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.get(), 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.get(), 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.get(), 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.get(), 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.get(), 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.get(), 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.get(), 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.get(), rho); + } +} + +void OSQPInterface::updateAlpha(const double alpha) +{ + m_settings->alpha = alpha; + if (m_work_initialized) { + osqp_update_alpha(m_work.get(), alpha); + } +} + +void OSQPInterface::updateScaling(const int scaling) +{ + m_settings->scaling = scaling; +} + +void OSQPInterface::updatePolish(const bool polish) +{ + m_settings->polish = polish; + if (m_work_initialized) { + osqp_update_polish(m_work.get(), polish); + } +} + +void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter) +{ + if (polish_refine_iter < 0) { + std::cerr << "Polish refinement iterations must be positive" << std::endl; + return; + } + + m_settings->polish_refine_iter = polish_refine_iter; + if (m_work_initialized) { + osqp_update_polish_refine_iter(m_work.get(), polish_refine_iter); + } +} + +void OSQPInterface::updateCheckTermination(const int check_termination) +{ + if (check_termination < 0) { + std::cerr << "Check termination must be positive" << std::endl; + return; + } + + m_settings->check_termination = check_termination; + if (m_work_initialized) { + osqp_update_check_termination(m_work.get(), check_termination); + } +} + +bool OSQPInterface::setWarmStart( + const std::vector & primal_variables, const std::vector & dual_variables) +{ + return setPrimalVariables(primal_variables) && setDualVariables(dual_variables); +} + +bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) +{ + if (!m_work_initialized) { + return false; + } + + const auto result = osqp_warm_start_x(m_work.get(), primal_variables.data()); + if (result != 0) { + std::cerr << "Failed to set primal variables for warm start" << std::endl; + return false; + } + + return true; +} + +bool OSQPInterface::setDualVariables(const std::vector & dual_variables) +{ + if (!m_work_initialized) { + return false; + } + + const auto result = osqp_warm_start_y(m_work.get(), dual_variables.data()); + if (result != 0) { + std::cerr << "Failed to set dual variables for warm start" << std::endl; + return false; + } + + return true; +} + +int64_t OSQPInterface::initializeProblem( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) +{ + // check if arguments are valid + std::stringstream ss; + if (P.rows() != P.cols()) { + ss << "P.rows() and P.cols() are not the same. P.rows() = " << P.rows() + << ", P.cols() = " << P.cols(); + throw std::invalid_argument(ss.str()); + } + if (P.rows() != static_cast(q.size())) { + ss << "P.rows() and q.size() are not the same. P.rows() = " << P.rows() + << ", q.size() = " << q.size(); + throw std::invalid_argument(ss.str()); + } + if (P.rows() != A.cols()) { + ss << "P.rows() and A.cols() are not the same. P.rows() = " << P.rows() + << ", A.cols() = " << A.cols(); + throw std::invalid_argument(ss.str()); + } + if (A.rows() != static_cast(l.size())) { + ss << "A.rows() and l.size() are not the same. A.rows() = " << A.rows() + << ", l.size() = " << l.size(); + throw std::invalid_argument(ss.str()); + } + if (A.rows() != static_cast(u.size())) { + ss << "A.rows() and u.size() are not the same. A.rows() = " << A.rows() + << ", u.size() = " << u.size(); + throw std::invalid_argument(ss.str()); + } + + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + return initializeProblem(P_csc, A_csc, q, l, u); +} + +int64_t OSQPInterface::initializeProblem( + CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector & q, const std::vector & l, + const std::vector & u) +{ + // 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()); + double * q_dyn = q_tmp.data(); + double * l_dyn = l_tmp.data(); + double * u_dyn = u_tmp.data(); + + /********************** + * OBJECTIVE FUNCTION + **********************/ + m_param_n = static_cast(q.size()); + m_data->m = static_cast(l.size()); + + /***************** + * POPULATE DATA + *****************/ + m_data->n = m_param_n; + if (m_data->P) free(m_data->P); + 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; + if (m_data->A) free(m_data->A); + 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 + OSQPWorkspace * workspace; + m_exitflag = osqp_setup(&workspace, m_data.get(), m_settings.get()); + m_work.reset(workspace); + m_work_initialized = true; + + return m_exitflag; +} + +std::tuple, std::vector, int64_t, int64_t, int64_t> +OSQPInterface::solve() +{ + // Solve Problem + osqp_solve(m_work.get()); + + /******************** + * EXTRACT SOLUTION + ********************/ + double * sol_x = m_work->solution->x; + double * 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_data->m); + + int64_t status_polish = m_work->info->status_polish; + int64_t status_solution = m_work->info->status_val; + int64_t status_iteration = m_work->info->iter; + + // Result tuple + std::tuple, std::vector, int64_t, int64_t, int64_t> result = + std::make_tuple( + sol_primal, sol_lagrange_multiplier, status_polish, status_solution, status_iteration); + + m_latest_work_info = *(m_work->info); + + return result; +} + +std::tuple, std::vector, int64_t, int64_t, int64_t> +OSQPInterface::optimize() +{ + // Run the solver on the stored problem representation. + std::tuple, std::vector, int64_t, int64_t, int64_t> result = solve(); + return result; +} + +std::tuple, std::vector, int64_t, 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, int64_t> result = solve(); + + m_work.reset(); + m_work_initialized = false; + + return result; +} + +void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const +{ + const int status = getStatus(); + if (status == 1) { + // No need to log since optimization was solved. + return; + } + + // create message + std::string output_message = ""; + if (prefix_message != "") { + output_message = prefix_message + " "; + } + + const auto status_message = getStatusMessage(); + output_message += "Optimization failed due to " + status_message; + + // log with warning + RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), "%s", output_message.c_str()); +} +} // namespace autoware::osqp_interface diff --git a/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp b/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp new file mode 100644 index 0000000000..a63f979a96 --- /dev/null +++ b/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp @@ -0,0 +1,181 @@ +// 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 "autoware/osqp_interface/csc_matrix_conv.hpp" +#include "gtest/gtest.h" + +#include + +#include +#include +#include + +TEST(TestCscMatrixConv, Nominal) +{ + using autoware::osqp_interface::calCSCMatrix; + using autoware::osqp_interface::CSC_Matrix; + + 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::osqp_interface::calCSCMatrixTrapezoidal; + using autoware::osqp_interface::CSC_Matrix; + + 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::osqp_interface::calCSCMatrix; + using autoware::osqp_interface::calCSCMatrixTrapezoidal; + using autoware::osqp_interface::CSC_Matrix; + using autoware::osqp_interface::printCSCMatrix; + 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/autoware_osqp_interface/test/test_osqp_interface.cpp b/common/autoware_osqp_interface/test/test_osqp_interface.cpp new file mode 100644 index 0000000000..f65b07e6d7 --- /dev/null +++ b/common/autoware_osqp_interface/test/test_osqp_interface.cpp @@ -0,0 +1,164 @@ +// 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 "autoware/osqp_interface/osqp_interface.hpp" +#include "gtest/gtest.h" + +#include + +#include +#include +#include + +namespace +{ +// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py +// +// min 1/2 * x' * P * x + q' * x +// s.t. lb <= A * x <= ub +// +// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] +// [1, 2] [1] [1, 0] [ 0] [0.7] +// [0, 1] [ 0] [0.7] +// [0, 1] [-inf] [inf] +// +// The optimal solution is +// x = [0.3, 0.7]' +// y = [-2.9, 0.0, 0.2, 0.0]` +// obj = 1.88 + +TEST(TestOsqpInterface, BasicQp) +{ + using autoware::osqp_interface::calCSCMatrix; + using autoware::osqp_interface::calCSCMatrixTrapezoidal; + using autoware::osqp_interface::CSC_Matrix; + + auto check_result = + [](const std::tuple, std::vector, int, int, int> & result) { + EXPECT_EQ(std::get<2>(result), 1); // polish succeeded + EXPECT_EQ(std::get<3>(result), 1); // solution succeeded + + static const auto ep = 1.0e-8; + + const auto prime_val = std::get<0>(result); + ASSERT_EQ(prime_val.size(), size_t(2)); + EXPECT_NEAR(prime_val[0], 0.3, ep); + EXPECT_NEAR(prime_val[1], 0.7, ep); + + const auto dual_val = std::get<1>(result); + ASSERT_EQ(dual_val.size(), size_t(4)); + EXPECT_NEAR(dual_val[0], -2.9, ep); + EXPECT_NEAR(dual_val[1], 0.0, ep); + EXPECT_NEAR(dual_val[2], 0.2, ep); + EXPECT_NEAR(dual_val[3], 0.0, ep); + }; + + const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); + const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); + const std::vector q = {1.0, 1.0}; + const std::vector l = {1.0, 0.0, 0.0, -autoware::osqp_interface::INF}; + const std::vector u = {1.0, 0.7, 0.7, autoware::osqp_interface::INF}; + + { + // Define problem during optimization + autoware::osqp_interface::OSQPInterface osqp; + std::tuple, std::vector, int, int, int> result = + osqp.optimize(P, A, q, l, u); + check_result(result); + } + + { + // Define problem during initialization + autoware::osqp_interface::OSQPInterface osqp(P, A, q, l, u, 1e-6); + std::tuple, std::vector, int, int, int> result = osqp.optimize(); + check_result(result); + } + + { + std::tuple, std::vector, int, int, int> result; + // Dummy initial problem + Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); + autoware::osqp_interface::OSQPInterface osqp(P_ini, A_ini, q_ini, l_ini, u_ini, 1e-6); + osqp.optimize(); + + // Redefine problem before optimization + osqp.initializeProblem(P, A, q, l, u); + result = osqp.optimize(); + check_result(result); + } + + { + // Define problem during initialization with csc matrix + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + autoware::osqp_interface::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6); + std::tuple, std::vector, int, int, int> result = osqp.optimize(); + check_result(result); + } + + { + std::tuple, std::vector, int, int, int> result; + // Dummy initial problem with csc matrix + CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); + CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); + autoware::osqp_interface::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); + osqp.optimize(); + + // Redefine problem before optimization + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + osqp.initializeProblem(P_csc, A_csc, q, l, u); + result = osqp.optimize(); + check_result(result); + } + + // add warm startup + { + std::tuple, std::vector, int, int, int> result; + // Dummy initial problem with csc matrix + CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); + CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); + autoware::osqp_interface::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); + osqp.optimize(); + + // Redefine problem before optimization + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + osqp.initializeProblem(P_csc, A_csc, q, l, u); + result = osqp.optimize(); + check_result(result); + + osqp.updateCheckTermination(1); + const auto primal_val = std::get<0>(result); + const auto dual_val = std::get<1>(result); + for (size_t i = 0; i < primal_val.size(); ++i) { + std::cerr << primal_val.at(i) << std::endl; + } + osqp.setWarmStart(primal_val, dual_val); + result = osqp.optimize(); + check_result(result); + EXPECT_EQ(osqp.getTakenIter(), 1); + } +} +} // namespace diff --git a/common/autoware_point_types/CMakeLists.txt b/common/autoware_point_types/CMakeLists.txt new file mode 100644 index 0000000000..c149f79dab --- /dev/null +++ b/common/autoware_point_types/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_point_types) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +include_directories( + include + SYSTEM + ${PCL_INCLUDE_DIRS} +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_autoware_point_types + test/test_point_types.cpp + ) + target_include_directories(test_autoware_point_types + PRIVATE include + ) + ament_target_dependencies(test_autoware_point_types + point_cloud_msg_wrapper + ) +endif() + +ament_auto_package() diff --git a/common/autoware_point_types/README.md b/common/autoware_point_types/README.md new file mode 100644 index 0000000000..5a14d68f4e --- /dev/null +++ b/common/autoware_point_types/README.md @@ -0,0 +1,68 @@ +# Autoware Point Types + +## Overview + +This package provides a variety of structures to represent different types of point cloud data, mainly used for point cloud processing and analysis. + +## Design + +### Point cloud data type definition + +`autoware_point_types` defines multiple structures (such as PointXYZI, PointXYZIRC, PointXYZIRADRT, PointXYZIRCAEDT), each structure contains different attributes to adapt to different application scenarios. + +- `autoware::point_types::PointXYZI`: Point type with intensity information. +- `autoware::point_types::PointXYZIRC`: Extended PointXYZI, adds return_type and channel information. +- `autoware::point_types::PointXYZIRADRT`: Extended PointXYZI, adds ring, azimuth, distance, return_type and time_stamp information. +- `autoware::point_types::PointXYZIRCAEDT`: Similar to PointXYZIRADRT, but adds elevation information and uses `std::uint32_t` as the data type for time_stamp. + +### Operator overload + +Each structure overloads the `==` operator, allowing users to easily compare whether two points are equal, which is very useful for deduplication and matching of point cloud data. + +### Field generators + +The field generator is implemented using macro definitions and std::tuple, which simplifies the serialization and deserialization process of point cloud messages and improves the reusability and readability of the code. + +### Registration mechanism + +Register custom point cloud structures into the PCL library through the macro `POINT_CLOUD_REGISTER_POINT_STRUCT`, so that these structures can be directly integrated with other functions of the PCL library. + +## Usage + +- Create a point cloud object of PointXYZIRC type + +```cpp +#include "autoware/point_types/types.hpp" + +int main(){ + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + + for (int i = 0; i < 5; ++i) { + autoware::point_types::PointXYZIRC point; + point.x = static_cast(i * 0.1); + point.y = static_cast(i * 0.2); + point.z = static_cast(i * 0.3); + point.intensity = static_cast(i * 10); + point.return_type = autoware::point_types::ReturnType::SINGLE_STRONGEST; + point.channel = static_cast(i); + + cloud->points.push_back(point); + } + cloud->width = cloud->points.size(); + cloud->height = 1; + + return 0; +} +``` + +- Convert ROS message to point cloud of PointXYZIRC type + +```cpp +ExampleNode::points_callback(const PointCloud2::ConstSharedPtr & points_msg_ptr) +{ + pcl::PointCloud::Ptr points_ptr( + new pcl::PointCloud); + + pcl::fromROSMsg(*points_msg_ptr, *points_ptr); +} +``` diff --git a/common/autoware_point_types/include/autoware/point_types/types.hpp b/common/autoware_point_types/include/autoware/point_types/types.hpp new file mode 100644 index 0000000000..0fde62222e --- /dev/null +++ b/common/autoware_point_types/include/autoware/point_types/types.hpp @@ -0,0 +1,188 @@ +// 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 AUTOWARE__POINT_TYPES__TYPES_HPP_ +#define AUTOWARE__POINT_TYPES__TYPES_HPP_ + +#include + +#include + +#include +#include + +namespace autoware::point_types +{ +template +bool float_eq(const T a, const T b, const T eps = 10e-6) +{ + return std::fabs(a - b) < eps; +} + +struct PointXYZI +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + float intensity{0.0F}; + friend bool operator==(const PointXYZI & p1, const PointXYZI & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && + float_eq(p1.z, p2.z) && float_eq(p1.intensity, p2.intensity); + } +}; + +enum ReturnType : uint8_t { + INVALID = 0, + SINGLE_STRONGEST, + SINGLE_LAST, + DUAL_STRONGEST_FIRST, + DUAL_STRONGEST_LAST, + DUAL_WEAK_FIRST, + DUAL_WEAK_LAST, + DUAL_ONLY, +}; + +struct PointXYZIRC +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + std::uint8_t intensity{0U}; + std::uint8_t return_type{0U}; + std::uint16_t channel{0U}; + + friend bool operator==(const PointXYZIRC & p1, const PointXYZIRC & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && + float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && + p1.return_type == p2.return_type && p1.channel == p2.channel; + } +}; + +struct PointXYZIRADRT +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + float intensity{0.0F}; + uint16_t ring{0U}; + float azimuth{0.0F}; + float distance{0.0F}; + uint8_t return_type{0U}; + double time_stamp{0.0}; + friend bool operator==(const PointXYZIRADRT & p1, const PointXYZIRADRT & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && + float_eq(p1.z, p2.z) && float_eq(p1.intensity, p2.intensity) && + p1.ring == p2.ring && float_eq(p1.azimuth, p2.azimuth) && + float_eq(p1.distance, p2.distance) && p1.return_type == p2.return_type && + float_eq(p1.time_stamp, p2.time_stamp); + } +}; + +struct PointXYZIRCAEDT +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + std::uint8_t intensity{0U}; + std::uint8_t return_type{0U}; + std::uint16_t channel{0U}; + float azimuth{0.0F}; + float elevation{0.0F}; + float distance{0.0F}; + std::uint32_t time_stamp{0U}; + + friend bool operator==(const PointXYZIRCAEDT & p1, const PointXYZIRCAEDT & p2) noexcept + { + return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && + float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && + p1.return_type == p2.return_type && p1.channel == p2.channel && + float_eq(p1.azimuth, p2.azimuth) && float_eq(p1.distance, p2.distance) && + p1.time_stamp == p2.time_stamp; + } +}; + +enum class PointXYZIIndex { X, Y, Z, Intensity }; +enum class PointXYZIRCIndex { X, Y, Z, Intensity, ReturnType, Channel }; +enum class PointXYZIRADRTIndex { + X, + Y, + Z, + Intensity, + Ring, + Azimuth, + Distance, + ReturnType, + TimeStamp +}; +enum class PointXYZIRCAEDTIndex { + X, + Y, + Z, + Intensity, + ReturnType, + Channel, + Azimuth, + Elevation, + Distance, + TimeStamp +}; + +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(elevation); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(distance); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(return_type); +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(time_stamp); + +LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(channel); + +using PointXYZIRCGenerator = std::tuple< + point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, + point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, + field_return_type_generator, field_channel_generator>; + +using PointXYZIRADRTGenerator = std::tuple< + point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, + point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, + point_cloud_msg_wrapper::field_ring_generator, field_azimuth_generator, field_distance_generator, + field_return_type_generator, field_time_stamp_generator>; + +using PointXYZIRCAEDTGenerator = std::tuple< + point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, + point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, + field_return_type_generator, field_channel_generator, field_azimuth_generator, + field_elevation_generator, field_distance_generator, field_time_stamp_generator>; + +} // namespace autoware::point_types + +POINT_CLOUD_REGISTER_POINT_STRUCT( + autoware::point_types::PointXYZIRC, + (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( + std::uint8_t, return_type, return_type)(std::uint16_t, channel, channel)) + +POINT_CLOUD_REGISTER_POINT_STRUCT( + autoware::point_types::PointXYZIRADRT, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)( + float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)( + double, time_stamp, time_stamp)) + +POINT_CLOUD_REGISTER_POINT_STRUCT( + autoware::point_types::PointXYZIRCAEDT, + (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( + std::uint8_t, return_type, + return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)( + float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp)) +#endif // AUTOWARE__POINT_TYPES__TYPES_HPP_ diff --git a/common/autoware_point_types/package.xml b/common/autoware_point_types/package.xml new file mode 100644 index 0000000000..73db182947 --- /dev/null +++ b/common/autoware_point_types/package.xml @@ -0,0 +1,35 @@ + + + + autoware_point_types + 0.0.0 + The point types definition to use point_cloud_msg_wrapper + David Wong + Max Schmeller + Cynthia Liu + Apache License 2.0 + + ament_cmake_core + ament_cmake_export_dependencies + ament_cmake_test + autoware_cmake + + ament_cmake_core + ament_cmake_test + + ament_cmake_copyright + ament_cmake_cppcheck + ament_cmake_lint_cmake + ament_cmake_xmllint + pcl_ros + point_cloud_msg_wrapper + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + point_cloud_msg_wrapper + + + ament_cmake + + diff --git a/common/autoware_point_types/test/test_point_types.cpp b/common/autoware_point_types/test/test_point_types.cpp new file mode 100644 index 0000000000..08696a9948 --- /dev/null +++ b/common/autoware_point_types/test/test_point_types.cpp @@ -0,0 +1,66 @@ +// 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 "autoware/point_types/types.hpp" + +#include + +#include + +TEST(PointEquality, PointXYZI) +{ + using autoware::point_types::PointXYZI; + + PointXYZI pt0{0, 1, 2, 3}; + PointXYZI pt1{0, 1, 2, 3}; + EXPECT_EQ(pt0, pt1); +} + +TEST(PointEquality, PointXYZIRADRT) +{ + using autoware::point_types::PointXYZIRADRT; + + PointXYZIRADRT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8}; + PointXYZIRADRT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8}; + EXPECT_EQ(pt0, pt1); +} + +TEST(PointEquality, PointXYZIRCAEDT) +{ + using autoware::point_types::PointXYZIRCAEDT; + + PointXYZIRCAEDT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; + PointXYZIRCAEDT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; + EXPECT_EQ(pt0, pt1); +} + +TEST(PointEquality, FloatEq) +{ + // test template + EXPECT_TRUE(autoware::point_types::float_eq(1, 1)); + EXPECT_TRUE(autoware::point_types::float_eq(1, 1)); + + // test floating point error + EXPECT_TRUE(autoware::point_types::float_eq(1, 1 + std::numeric_limits::epsilon())); + + // test difference of sign + EXPECT_FALSE(autoware::point_types::float_eq(2, -2)); + EXPECT_FALSE(autoware::point_types::float_eq(-2, 2)); + + // small value difference + EXPECT_FALSE(autoware::point_types::float_eq(2, 2 + 10e-6)); + + // expect same value if epsilon is larger than difference + EXPECT_TRUE(autoware::point_types::float_eq(2, 2 + 10e-6, 10e-5)); +} diff --git a/common/autoware_qp_interface/CMakeLists.txt b/common/autoware_qp_interface/CMakeLists.txt new file mode 100644 index 0000000000..1df75d2d71 --- /dev/null +++ b/common/autoware_qp_interface/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_qp_interface) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Eigen3 REQUIRED) +find_package(proxsuite REQUIRED) + +# after find_package(osqp_vendor) in ament_auto_find_build_dependencies +find_package(osqp REQUIRED) +get_target_property(OSQP_INCLUDE_SUB_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES) +get_filename_component(OSQP_INCLUDE_DIR ${OSQP_INCLUDE_SUB_DIR} PATH) + +set(QP_INTERFACE_LIB_SRC + src/qp_interface.cpp + src/osqp_interface.cpp + src/osqp_csc_matrix_conv.cpp + src/proxqp_interface.cpp +) + +set(QP_INTERFACE_LIB_HEADERS + include/autoware/qp_interface/qp_interface.hpp + include/autoware/qp_interface/osqp_interface.hpp + include/autoware/qp_interface/osqp_csc_matrix_conv.hpp + include/autoware/qp_interface/proxqp_interface.hpp +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + ${QP_INTERFACE_LIB_SRC} + ${QP_INTERFACE_LIB_HEADERS} +) +target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast) + +target_include_directories(${PROJECT_NAME} + SYSTEM PUBLIC + "${OSQP_INCLUDE_DIR}" + "${EIGEN3_INCLUDE_DIR}" +) + +ament_target_dependencies(${PROJECT_NAME} + Eigen3 + osqp_vendor + proxsuite +) + +# 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) + +if(BUILD_TESTING) + set(TEST_SOURCES + test/test_osqp_interface.cpp + test/test_csc_matrix_conv.cpp + test/test_proxqp_interface.cpp + ) + set(TEST_OSQP_INTERFACE_EXE test_osqp_interface) + ament_add_ros_isolated_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) + target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME}) +endif() + +ament_auto_package(INSTALL_TO_SHARE +) diff --git a/common/autoware_qp_interface/design/qp_interface-design.md b/common/autoware_qp_interface/design/qp_interface-design.md new file mode 100644 index 0000000000..edc7fa9845 --- /dev/null +++ b/common/autoware_qp_interface/design/qp_interface-design.md @@ -0,0 +1,48 @@ +# Interface for QP solvers + +This is the design document for the `autoware_qp_interface` package. + +## Purpose / Use cases + +This packages provides a C++ interface for QP solvers. +Currently, supported QP solvers are + +- [OSQP library](https://osqp.org/docs/solver/index.html) + +## Design + +The class `QPInterface` 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 QP workspace dataholder, and runs the optimizer. + +## Inputs / Outputs / API + +The interface can be used in several ways: + +1. Initialize the interface, and load the problem formulation at the optimization call. + + ```cpp + QPInterface qp_interface; + qp_interface.optimize(P, A, q, l, u); + ``` + +2. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs. + + ```cpp + QPInterface qp_interface(true); + qp_interface.optimize(P, A, q, l, u); + qp_interface.optimize(P_new, A_new, q_new, l_new, u_new); + ``` + + The optimization results are returned as a vector by the optimization function. + + ```cpp + const auto solution = qp_interface.optimize(); + double x_0 = solution[0]; + double x_1 = solution[1]; + ``` + +## References / External links + +- OSQP library: + +## Related issues diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp new file mode 100644 index 0000000000..9575d9d18c --- /dev/null +++ b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp @@ -0,0 +1,46 @@ +// Copyright 2023 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 AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ +#define AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ + +#include "osqp/glob_opts.h" + +#include + +#include + +namespace autoware::qp_interface +{ +/// \brief Compressed-Column-Sparse Matrix +struct CSC_Matrix +{ + /// Vector of non-zero values. Ex: [4,1,1,2] + std::vector vals_; + /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') + std::vector row_idxs_; + /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') + std::vector col_idxs_; +}; + +/// \brief Calculate CSC matrix from Eigen matrix +CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat); +/// \brief Calculate upper trapezoidal CSC matrix from square Eigen matrix +CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat); +/// \brief Print the given CSC matrix to the standard output +void printCSCMatrix(const CSC_Matrix & csc_mat); + +} // namespace autoware::qp_interface + +#endif // AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp new file mode 100644 index 0000000000..a5777dd545 --- /dev/null +++ b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp @@ -0,0 +1,147 @@ +// Copyright 2023 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 AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ +#define AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ + +#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" +#include "autoware/qp_interface/qp_interface.hpp" +#include "osqp/osqp.h" + +#include +#include +#include +#include + +namespace autoware::qp_interface +{ +constexpr c_float OSQP_INF = 1e30; + +class OSQPInterface : public QPInterface +{ +public: + /// \brief Constructor without problem formulation + OSQPInterface( + const bool enable_warm_start = false, const int max_iteration = 20000, + const c_float eps_abs = std::numeric_limits::epsilon(), + const c_float eps_rel = std::numeric_limits::epsilon(), const bool polish = true, + const bool verbose = false); + /// \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 bool enable_warm_start = false, + const c_float eps_abs = std::numeric_limits::epsilon()); + OSQPInterface( + const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, + const std::vector & l, const std::vector & u, + const bool enable_warm_start = false, + const c_float eps_abs = std::numeric_limits::epsilon()); + ~OSQPInterface(); + + static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; + + std::vector optimize( + CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, + const std::vector & u); + + int getIterationNumber() const override; + bool isSolved() const override; + std::string getStatus() const override; + + int getPolishStatus() const; + std::vector getDualSolution() const; + + void updateEpsAbs(const double eps_abs) override; + void updateEpsRel(const double eps_rel) override; + void updateVerbose(const bool verbose) override; + + // 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 updateCscP(const CSC_Matrix & P_csc); + void updateA(const Eigen::MatrixXd & A_new); + void updateCscA(const CSC_Matrix & A_csc); + 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 updateMaxIter(const int iter); + void updateRhoInterval(const int rho_interval); + void updateRho(const double rho); + void updateAlpha(const double alpha); + void updateScaling(const int scaling); + void updatePolish(const bool polish); + void updatePolishRefinementIteration(const int polish_refine_iter); + void updateCheckTermination(const int check_termination); + + /// \brief Get the number of iteration taken to solve the problem + inline int64_t getTakenIter() const { return static_cast(latest_work_info_.iter); } + /// \brief Get the status message for the latest problem solved + inline std::string getStatusMessage() const + { + return static_cast(latest_work_info_.status); + } + /// \brief Get the runtime of the latest problem solved + inline double getRunTime() const { return latest_work_info_.run_time; } + /// \brief Get the objective value the latest problem solved + inline double getObjVal() const { return latest_work_info_.obj_val; } + /// \brief Returns flag asserting interface condition (Healthy condition: 0). + inline int64_t getExitFlag() const { return exitflag_; } + + // Setter functions for warm start + bool setWarmStart( + const std::vector & primal_variables, const std::vector & dual_variables); + bool setPrimalVariables(const std::vector & primal_variables); + bool setDualVariables(const std::vector & dual_variables); + +private: + std::unique_ptr> work_; + std::unique_ptr settings_; + std::unique_ptr data_; + // store last work info since work is cleaned up at every execution to prevent memory leak. + OSQPInfo latest_work_info_; + // Number of parameters to optimize + int64_t param_n_; + // Flag to check if the current work exists + bool work__initialized = false; + // Exitflag + int64_t exitflag_; + + void initializeProblemImpl( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) override; + + void initializeCSCProblemImpl( + CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, + const std::vector & u); + + std::vector optimizeImpl() override; +}; +} // namespace autoware::qp_interface + +#endif // AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp new file mode 100644 index 0000000000..324da4b18c --- /dev/null +++ b/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp @@ -0,0 +1,57 @@ +// Copyright 2023 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 AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ +#define AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ + +#include "autoware/qp_interface/qp_interface.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace autoware::qp_interface +{ +class ProxQPInterface : public QPInterface +{ +public: + explicit ProxQPInterface( + const bool enable_warm_start, const int max_iteration, const double eps_abs, + const double eps_rel, const bool verbose = false); + + int getIterationNumber() const override; + bool isSolved() const override; + std::string getStatus() const override; + + void updateEpsAbs(const double eps_abs) override; + void updateEpsRel(const double eps_rel) override; + void updateVerbose(const bool verbose) override; + +private: + proxsuite::proxqp::Settings settings_{}; + std::shared_ptr> qp_ptr_{nullptr}; + + void initializeProblemImpl( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) override; + + std::vector optimizeImpl() override; +}; +} // namespace autoware::qp_interface + +#endif // AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp new file mode 100644 index 0000000000..be3c598512 --- /dev/null +++ b/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp @@ -0,0 +1,61 @@ +// Copyright 2023 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 AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ +#define AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ + +#include + +#include +#include +#include + +namespace autoware::qp_interface +{ +class QPInterface +{ +public: + explicit QPInterface(const bool enable_warm_start) : enable_warm_start_(enable_warm_start) {} + + std::vector optimize( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u); + + virtual bool isSolved() const = 0; + virtual int getIterationNumber() const = 0; + virtual std::string getStatus() const = 0; + + virtual void updateEpsAbs([[maybe_unused]] const double eps_abs) = 0; + virtual void updateEpsRel([[maybe_unused]] const double eps_rel) = 0; + virtual void updateVerbose([[maybe_unused]] const bool verbose) {} + +protected: + bool enable_warm_start_{false}; + + void initializeProblem( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u); + + virtual void initializeProblemImpl( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) = 0; + + virtual std::vector optimizeImpl() = 0; + + std::optional variables_num_{std::nullopt}; + std::optional constraints_num_{std::nullopt}; +}; +} // namespace autoware::qp_interface + +#endif // AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/package.xml b/common/autoware_qp_interface/package.xml new file mode 100644 index 0000000000..c1bf5b06d5 --- /dev/null +++ b/common/autoware_qp_interface/package.xml @@ -0,0 +1,30 @@ + + + + autoware_qp_interface + 0.0.0 + Interface for the QP solvers + Takayuki Murooka + Fumiya Watanabe + Maxime CLEMENT + Satoshi Ota + Apache 2.0 + + ament_cmake_auto + autoware_cmake + + eigen + osqp_vendor + proxsuite + rclcpp + rclcpp_components + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp b/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp new file mode 100644 index 0000000000..15314a9e4a --- /dev/null +++ b/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp @@ -0,0 +1,134 @@ +// Copyright 2023 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 "autoware/qp_interface/osqp_csc_matrix_conv.hpp" + +#include +#include + +#include +#include +#include + +namespace autoware::qp_interface +{ +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.vals_) { + std::cout << val << ", "; + } + std::cout << "]\n"; + + std::cout << "["; + for (const c_int & row : csc_mat.row_idxs_) { + std::cout << row << ", "; + } + std::cout << "]\n"; + + std::cout << "["; + for (const c_int & col : csc_mat.col_idxs_) { + std::cout << col << ", "; + } + std::cout << "]\n"; +} +} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/osqp_interface.cpp b/common/autoware_qp_interface/src/osqp_interface.cpp new file mode 100644 index 0000000000..fbb8e71f4c --- /dev/null +++ b/common/autoware_qp_interface/src/osqp_interface.cpp @@ -0,0 +1,394 @@ +// Copyright 2023 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 "autoware/qp_interface/osqp_interface.hpp" + +#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::qp_interface +{ +OSQPInterface::OSQPInterface( + const bool enable_warm_start, const int max_iteration, const c_float eps_abs, + const c_float eps_rel, const bool polish, const bool verbose) +: QPInterface(enable_warm_start), work_{nullptr, OSQPWorkspaceDeleter} +{ + settings_ = std::make_unique(); + data_ = std::make_unique(); + + if (settings_) { + osqp_set_default_settings(settings_.get()); + settings_->alpha = 1.6; // Change alpha parameter + settings_->eps_rel = eps_rel; + settings_->eps_abs = eps_abs; + settings_->eps_prim_inf = 1.0E-4; + settings_->eps_dual_inf = 1.0E-4; + settings_->warm_start = enable_warm_start; + settings_->max_iter = max_iteration; + settings_->verbose = verbose; + settings_->polish = polish; + } + 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 bool enable_warm_start, + const c_float eps_abs) +: OSQPInterface(enable_warm_start, eps_abs) +{ + initializeProblem(P, A, q, l, u); +} + +OSQPInterface::OSQPInterface( + const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, + const std::vector & l, const std::vector & u, const bool enable_warm_start, + const c_float eps_abs) +: OSQPInterface(enable_warm_start, eps_abs) +{ + initializeCSCProblemImpl(P, A, q, l, u); +} + +OSQPInterface::~OSQPInterface() +{ + if (data_->P) free(data_->P); + if (data_->A) free(data_->A); +} + +void OSQPInterface::initializeProblemImpl( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) +{ + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + initializeCSCProblemImpl(P_csc, A_csc, q, l, u); +} + +void OSQPInterface::initializeCSCProblemImpl( + CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector & q, const std::vector & l, + const std::vector & u) +{ + // 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()); + double * q_dyn = q_tmp.data(); + double * l_dyn = l_tmp.data(); + double * u_dyn = u_tmp.data(); + + /********************** + * OBJECTIVE FUNCTION + **********************/ + param_n_ = static_cast(q.size()); + data_->m = static_cast(l.size()); + + /***************** + * POPULATE DATA + *****************/ + data_->n = param_n_; + if (data_->P) free(data_->P); + data_->P = csc_matrix( + data_->n, data_->n, static_cast(P_csc.vals_.size()), P_csc.vals_.data(), + P_csc.row_idxs_.data(), P_csc.col_idxs_.data()); + data_->q = q_dyn; + if (data_->A) free(data_->A); + data_->A = csc_matrix( + data_->m, data_->n, static_cast(A_csc.vals_.size()), A_csc.vals_.data(), + A_csc.row_idxs_.data(), A_csc.col_idxs_.data()); + data_->l = l_dyn; + data_->u = u_dyn; + + // Setup workspace + OSQPWorkspace * workspace; + exitflag_ = osqp_setup(&workspace, data_.get(), settings_.get()); + work_.reset(workspace); + work__initialized = true; +} + +void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept +{ + if (ptr != nullptr) { + osqp_cleanup(ptr); + } +} + +void OSQPInterface::updateEpsAbs(const double eps_abs) +{ + settings_->eps_abs = eps_abs; // for default setting + if (work__initialized) { + osqp_update_eps_abs(work_.get(), eps_abs); // for current work + } +} + +void OSQPInterface::updateEpsRel(const double eps_rel) +{ + settings_->eps_rel = eps_rel; // for default setting + if (work__initialized) { + osqp_update_eps_rel(work_.get(), eps_rel); // for current work + } +} + +void OSQPInterface::updateMaxIter(const int max_iter) +{ + settings_->max_iter = max_iter; // for default setting + if (work__initialized) { + osqp_update_max_iter(work_.get(), max_iter); // for current work + } +} + +void OSQPInterface::updateVerbose(const bool is_verbose) +{ + settings_->verbose = is_verbose; // for default setting + if (work__initialized) { + osqp_update_verbose(work_.get(), is_verbose); // for current work + } +} + +void OSQPInterface::updateRhoInterval(const int rho_interval) +{ + settings_->adaptive_rho_interval = rho_interval; // for default setting +} + +void OSQPInterface::updateRho(const double rho) +{ + settings_->rho = rho; + if (work__initialized) { + osqp_update_rho(work_.get(), rho); + } +} + +void OSQPInterface::updateAlpha(const double alpha) +{ + settings_->alpha = alpha; + if (work__initialized) { + osqp_update_alpha(work_.get(), alpha); + } +} + +void OSQPInterface::updateScaling(const int scaling) +{ + settings_->scaling = scaling; +} + +void OSQPInterface::updatePolish(const bool polish) +{ + settings_->polish = polish; + if (work__initialized) { + osqp_update_polish(work_.get(), polish); + } +} + +void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter) +{ + if (polish_refine_iter < 0) { + std::cerr << "Polish refinement iterations must be positive" << std::endl; + return; + } + + settings_->polish_refine_iter = polish_refine_iter; + if (work__initialized) { + osqp_update_polish_refine_iter(work_.get(), polish_refine_iter); + } +} + +void OSQPInterface::updateCheckTermination(const int check_termination) +{ + if (check_termination < 0) { + std::cerr << "Check termination must be positive" << std::endl; + return; + } + + settings_->check_termination = check_termination; + if (work__initialized) { + osqp_update_check_termination(work_.get(), check_termination); + } +} + +bool OSQPInterface::setWarmStart( + const std::vector & primal_variables, const std::vector & dual_variables) +{ + return setPrimalVariables(primal_variables) && setDualVariables(dual_variables); +} + +bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) +{ + if (!work__initialized) { + return false; + } + + const auto result = osqp_warm_start_x(work_.get(), primal_variables.data()); + if (result != 0) { + std::cerr << "Failed to set primal variables for warm start" << std::endl; + return false; + } + + return true; +} + +bool OSQPInterface::setDualVariables(const std::vector & dual_variables) +{ + if (!work__initialized) { + return false; + } + + const auto result = osqp_warm_start_y(work_.get(), dual_variables.data()); + if (result != 0) { + std::cerr << "Failed to set dual variables for warm start" << std::endl; + return false; + } + + return true; +} + +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(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); +} + +void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) +{ + osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.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(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); + return; +} + +void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) +{ + osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); +} + +void OSQPInterface::updateQ(const std::vector & q_new) +{ + std::vector q_tmp(q_new.begin(), q_new.end()); + double * q_dyn = q_tmp.data(); + osqp_update_lin_cost(work_.get(), q_dyn); +} + +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(work_.get(), 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(work_.get(), 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(work_.get(), l_dyn, u_dyn); +} + +int OSQPInterface::getIterationNumber() const +{ + return work_->info->iter; +} + +std::string OSQPInterface::getStatus() const +{ + return "OSQP_SOLVED"; +} + +bool OSQPInterface::isSolved() const +{ + return latest_work_info_.status_val == 1; +} + +int OSQPInterface::getPolishStatus() const +{ + return static_cast(latest_work_info_.status_polish); +} + +std::vector OSQPInterface::getDualSolution() const +{ + double * sol_y = work_->solution->y; + std::vector dual_solution(sol_y, sol_y + data_->m); + return dual_solution; +} + +std::vector OSQPInterface::optimizeImpl() +{ + osqp_solve(work_.get()); + + double * sol_x = work_->solution->x; + std::vector sol_primal(sol_x, sol_x + param_n_); + + latest_work_info_ = *(work_->info); + + if (!enable_warm_start_) { + work_.reset(); + work__initialized = false; + } + + return sol_primal; +} + +std::vector OSQPInterface::optimize( + CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, + const std::vector & u) +{ + initializeCSCProblemImpl(P, A, q, l, u); + const auto result = optimizeImpl(); + + // show polish status if not successful + const int status_polish = static_cast(latest_work_info_.status_polish); + if (status_polish != 1) { + const auto msg = status_polish == 0 ? "unperformed" + : status_polish == -1 ? "unsuccessful" + : "unknown"; + std::cerr << "osqp polish process failed : " << msg << ". The result may be inaccurate" + << std::endl; + } + + return result; +} + +} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/proxqp_interface.cpp b/common/autoware_qp_interface/src/proxqp_interface.cpp new file mode 100644 index 0000000000..a0ebbf0db0 --- /dev/null +++ b/common/autoware_qp_interface/src/proxqp_interface.cpp @@ -0,0 +1,157 @@ +// Copyright 2023 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 "autoware/qp_interface/proxqp_interface.hpp" + +#include +#include +#include + +namespace autoware::qp_interface +{ +using proxsuite::proxqp::QPSolverOutput; + +ProxQPInterface::ProxQPInterface( + const bool enable_warm_start, const int max_iteration, const double eps_abs, const double eps_rel, + const bool verbose) +: QPInterface(enable_warm_start) +{ + settings_.max_iter = max_iteration; + settings_.eps_abs = eps_abs; + settings_.eps_rel = eps_rel; + settings_.verbose = verbose; +} + +void ProxQPInterface::initializeProblemImpl( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) +{ + const size_t variables_num = q.size(); + const size_t constraints_num = l.size(); + + const bool enable_warm_start = [&]() { + if ( + !enable_warm_start_ // Warm start is designated + || !qp_ptr_ // QP pointer is initialized + // The number of variables is the same as the previous one. + || !variables_num_ || + *variables_num_ != variables_num + // The number of constraints is the same as the previous one + || !constraints_num_ || *constraints_num_ != constraints_num) { + return false; + } + return true; + }(); + + if (!enable_warm_start) { + qp_ptr_ = std::make_shared>( + variables_num, 0, constraints_num); + } + + settings_.initial_guess = + enable_warm_start ? proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT + : proxsuite::proxqp::InitialGuessStatus::NO_INITIAL_GUESS; + + qp_ptr_->settings = settings_; + + Eigen::SparseMatrix P_sparse(variables_num, constraints_num); + P_sparse = P.sparseView(); + + // NOTE: const std vector cannot be converted to eigen vector + std::vector non_const_q = q; + Eigen::VectorXd eigen_q = + Eigen::Map(non_const_q.data(), non_const_q.size()); + std::vector l_std_vec = l; + Eigen::VectorXd eigen_l = + Eigen::Map(l_std_vec.data(), l_std_vec.size()); + std::vector u_std_vec = u; + Eigen::VectorXd eigen_u = + Eigen::Map(u_std_vec.data(), u_std_vec.size()); + + if (enable_warm_start) { + qp_ptr_->update( + P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); + } else { + qp_ptr_->init( + P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); + } +} + +void ProxQPInterface::updateEpsAbs(const double eps_abs) +{ + settings_.eps_abs = eps_abs; +} + +void ProxQPInterface::updateEpsRel(const double eps_rel) +{ + settings_.eps_rel = eps_rel; +} + +void ProxQPInterface::updateVerbose(const bool is_verbose) +{ + settings_.verbose = is_verbose; +} + +bool ProxQPInterface::isSolved() const +{ + if (qp_ptr_) { + return qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED; + } + return false; +} + +int ProxQPInterface::getIterationNumber() const +{ + if (qp_ptr_) { + return qp_ptr_->results.info.iter; + } + return 0; +} + +std::string ProxQPInterface::getStatus() const +{ + if (qp_ptr_) { + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED) { + return "PROXQP_SOLVED"; + } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_MAX_ITER_REACHED) { + return "PROXQP_MAX_ITER_REACHED"; + } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { + return "PROXQP_PRIMAL_INFEASIBLE"; + } + // if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE) { + // return "PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE"; + // } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_DUAL_INFEASIBLE) { + return "PROXQP_DUAL_INFEASIBLE"; + } + if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_NOT_RUN) { + return "PROXQP_NOT_RUN"; + } + } + return "None"; +} + +std::vector ProxQPInterface::optimizeImpl() +{ + qp_ptr_->solve(); + + std::vector result; + for (Eigen::Index i = 0; i < qp_ptr_->results.x.size(); ++i) { + result.push_back(qp_ptr_->results.x[i]); + } + return result; +} +} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/qp_interface.cpp b/common/autoware_qp_interface/src/qp_interface.cpp new file mode 100644 index 0000000000..f01e57772d --- /dev/null +++ b/common/autoware_qp_interface/src/qp_interface.cpp @@ -0,0 +1,70 @@ +// Copyright 2023 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 "autoware/qp_interface/qp_interface.hpp" + +#include +#include +#include + +namespace autoware::qp_interface +{ +void QPInterface::initializeProblem( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) +{ + // check if arguments are valid + std::stringstream ss; + if (P.rows() != P.cols()) { + ss << "P.rows() and P.cols() are not the same. P.rows() = " << P.rows() + << ", P.cols() = " << P.cols(); + throw std::invalid_argument(ss.str()); + } + if (P.rows() != static_cast(q.size())) { + ss << "P.rows() and q.size() are not the same. P.rows() = " << P.rows() + << ", q.size() = " << q.size(); + throw std::invalid_argument(ss.str()); + } + if (P.rows() != A.cols()) { + ss << "P.rows() and A.cols() are not the same. P.rows() = " << P.rows() + << ", A.cols() = " << A.cols(); + throw std::invalid_argument(ss.str()); + } + if (A.rows() != static_cast(l.size())) { + ss << "A.rows() and l.size() are not the same. A.rows() = " << A.rows() + << ", l.size() = " << l.size(); + throw std::invalid_argument(ss.str()); + } + if (A.rows() != static_cast(u.size())) { + ss << "A.rows() and u.size() are not the same. A.rows() = " << A.rows() + << ", u.size() = " << u.size(); + throw std::invalid_argument(ss.str()); + } + + initializeProblemImpl(P, A, q, l, u); + + variables_num_ = q.size(); + constraints_num_ = l.size(); +} + +std::vector QPInterface::optimize( + const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, + const std::vector & l, const std::vector & u) +{ + initializeProblem(P, A, q, l, u); + const auto result = optimizeImpl(); + + return result; +} +} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp b/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp new file mode 100644 index 0000000000..1f377a1a24 --- /dev/null +++ b/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp @@ -0,0 +1,181 @@ +// Copyright 2023 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 "autoware/qp_interface/osqp_csc_matrix_conv.hpp" +#include "gtest/gtest.h" + +#include + +#include +#include +#include + +TEST(TestCscMatrixConv, Nominal) +{ + using autoware::qp_interface::calCSCMatrix; + using autoware::qp_interface::CSC_Matrix; + + Eigen::MatrixXd rect1(1, 2); + rect1 << 0.0, 1.0; + + const CSC_Matrix rect_m1 = calCSCMatrix(rect1); + ASSERT_EQ(rect_m1.vals_.size(), size_t(1)); + EXPECT_EQ(rect_m1.vals_[0], 1.0); + ASSERT_EQ(rect_m1.row_idxs_.size(), size_t(1)); + EXPECT_EQ(rect_m1.row_idxs_[0], c_int(0)); + ASSERT_EQ(rect_m1.col_idxs_.size(), size_t(3)); // nb of columns + 1 + EXPECT_EQ(rect_m1.col_idxs_[0], c_int(0)); + EXPECT_EQ(rect_m1.col_idxs_[1], c_int(0)); + EXPECT_EQ(rect_m1.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.vals_.size(), size_t(4)); + EXPECT_EQ(rect_m2.vals_[0], 1.0); + EXPECT_EQ(rect_m2.vals_[1], 6.0); + EXPECT_EQ(rect_m2.vals_[2], 3.0); + EXPECT_EQ(rect_m2.vals_[3], 7.0); + ASSERT_EQ(rect_m2.row_idxs_.size(), size_t(4)); + EXPECT_EQ(rect_m2.row_idxs_[0], c_int(0)); + EXPECT_EQ(rect_m2.row_idxs_[1], c_int(1)); + EXPECT_EQ(rect_m2.row_idxs_[2], c_int(0)); + EXPECT_EQ(rect_m2.row_idxs_[3], c_int(1)); + ASSERT_EQ(rect_m2.col_idxs_.size(), size_t(5)); // nb of columns + 1 + EXPECT_EQ(rect_m2.col_idxs_[0], c_int(0)); + EXPECT_EQ(rect_m2.col_idxs_[1], c_int(1)); + EXPECT_EQ(rect_m2.col_idxs_[2], c_int(2)); + EXPECT_EQ(rect_m2.col_idxs_[3], c_int(4)); + EXPECT_EQ(rect_m2.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.vals_.size(), size_t(19)); + EXPECT_EQ(square_m2.vals_[0], 10.0); + EXPECT_EQ(square_m2.vals_[1], 3.0); + EXPECT_EQ(square_m2.vals_[2], 3.0); + EXPECT_EQ(square_m2.vals_[3], 9.0); + EXPECT_EQ(square_m2.vals_[4], 7.0); + EXPECT_EQ(square_m2.vals_[5], 8.0); + EXPECT_EQ(square_m2.vals_[6], 4.0); + EXPECT_EQ(square_m2.vals_[7], 8.0); + EXPECT_EQ(square_m2.vals_[8], 8.0); + EXPECT_EQ(square_m2.vals_[9], 7.0); + EXPECT_EQ(square_m2.vals_[10], 7.0); + EXPECT_EQ(square_m2.vals_[11], 9.0); + EXPECT_EQ(square_m2.vals_[12], -2.0); + EXPECT_EQ(square_m2.vals_[13], 5.0); + EXPECT_EQ(square_m2.vals_[14], 9.0); + EXPECT_EQ(square_m2.vals_[15], 2.0); + EXPECT_EQ(square_m2.vals_[16], 3.0); + EXPECT_EQ(square_m2.vals_[17], 13.0); + EXPECT_EQ(square_m2.vals_[18], -1.0); + ASSERT_EQ(square_m2.row_idxs_.size(), size_t(19)); + EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[2], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[3], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[4], c_int(2)); + EXPECT_EQ(square_m2.row_idxs_[5], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[6], c_int(5)); + EXPECT_EQ(square_m2.row_idxs_[7], c_int(2)); + EXPECT_EQ(square_m2.row_idxs_[8], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[9], c_int(2)); + EXPECT_EQ(square_m2.row_idxs_[10], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[11], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[12], c_int(0)); + EXPECT_EQ(square_m2.row_idxs_[13], c_int(3)); + EXPECT_EQ(square_m2.row_idxs_[14], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[15], c_int(5)); + EXPECT_EQ(square_m2.row_idxs_[16], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[17], c_int(4)); + EXPECT_EQ(square_m2.row_idxs_[18], c_int(5)); + ASSERT_EQ(square_m2.col_idxs_.size(), size_t(7)); // nb of columns + 1 + EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.col_idxs_[1], c_int(3)); + EXPECT_EQ(square_m2.col_idxs_[2], c_int(7)); + EXPECT_EQ(square_m2.col_idxs_[3], c_int(9)); + EXPECT_EQ(square_m2.col_idxs_[4], c_int(12)); + EXPECT_EQ(square_m2.col_idxs_[5], c_int(16)); + EXPECT_EQ(square_m2.col_idxs_[6], c_int(19)); +} +TEST(TestCscMatrixConv, Trapezoidal) +{ + using autoware::qp_interface::calCSCMatrixTrapezoidal; + using autoware::qp_interface::CSC_Matrix; + + 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.vals_.size(), size_t(3)); + EXPECT_EQ(square_m1.vals_[0], 1.0); + EXPECT_EQ(square_m1.vals_[1], 2.0); + EXPECT_EQ(square_m1.vals_[2], 4.0); + ASSERT_EQ(square_m1.row_idxs_.size(), size_t(3)); + EXPECT_EQ(square_m1.row_idxs_[0], c_int(0)); + EXPECT_EQ(square_m1.row_idxs_[1], c_int(0)); + EXPECT_EQ(square_m1.row_idxs_[2], c_int(1)); + ASSERT_EQ(square_m1.col_idxs_.size(), size_t(3)); + EXPECT_EQ(square_m1.col_idxs_[0], c_int(0)); + EXPECT_EQ(square_m1.col_idxs_[1], c_int(1)); + EXPECT_EQ(square_m1.col_idxs_[2], c_int(3)); + + const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); + ASSERT_EQ(square_m2.vals_.size(), size_t(3)); + EXPECT_EQ(square_m2.vals_[0], 2.0); + EXPECT_EQ(square_m2.vals_[1], 5.0); + EXPECT_EQ(square_m2.vals_[2], 6.0); + ASSERT_EQ(square_m2.row_idxs_.size(), size_t(3)); + EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); + EXPECT_EQ(square_m2.row_idxs_[2], c_int(1)); + ASSERT_EQ(square_m2.col_idxs_.size(), size_t(4)); + EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); + EXPECT_EQ(square_m2.col_idxs_[1], c_int(0)); + EXPECT_EQ(square_m2.col_idxs_[2], c_int(2)); + EXPECT_EQ(square_m2.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::qp_interface::calCSCMatrix; + using autoware::qp_interface::calCSCMatrixTrapezoidal; + using autoware::qp_interface::CSC_Matrix; + using autoware::qp_interface::printCSCMatrix; + 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/autoware_qp_interface/test/test_osqp_interface.cpp b/common/autoware_qp_interface/test/test_osqp_interface.cpp new file mode 100644 index 0000000000..f97cc2888a --- /dev/null +++ b/common/autoware_qp_interface/test/test_osqp_interface.cpp @@ -0,0 +1,170 @@ +// Copyright 2023 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 "autoware/qp_interface/osqp_interface.hpp" +#include "gtest/gtest.h" + +#include + +#include +#include +#include +#include + +namespace +{ +// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py +// +// min 1/2 * x' * P * x + q' * x +// s.t. lb <= A * x <= ub +// +// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] +// [1, 2] [1] [1, 0] [ 0] [0.7] +// [0, 1] [ 0] [0.7] +// [0, 1] [-inf] [inf] +// +// The optimal solution is +// x = [0.3, 0.7]' +// y = [-2.9, 0.0, 0.2, 0.0]` +// obj = 1.88 + +TEST(TestOsqpInterface, BasicQp) +{ + using autoware::qp_interface::calCSCMatrix; + using autoware::qp_interface::calCSCMatrixTrapezoidal; + using autoware::qp_interface::CSC_Matrix; + + auto check_result = + [](const auto & solution, const std::string & status, const int polish_status) { + EXPECT_EQ(status, "OSQP_SOLVED"); + EXPECT_EQ(polish_status, 1); + + static const auto ep = 1.0e-8; + + ASSERT_EQ(solution.size(), size_t(2)); + EXPECT_NEAR(solution[0], 0.3, ep); + EXPECT_NEAR(solution[1], 0.7, ep); + }; + + const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); + const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); + const std::vector q = {1.0, 1.0}; + const std::vector l = {1.0, 0.0, 0.0, -autoware::qp_interface::OSQP_INF}; + const std::vector u = {1.0, 0.7, 0.7, autoware::qp_interface::OSQP_INF}; + + { + // Define problem during optimization + autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); + const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); + const auto status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, status, polish_status); + } + + { + // Define problem during initialization + autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); + const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); + const auto status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, status, polish_status); + } + + { + std::tuple, std::vector, int, int, int> result; + // Dummy initial problem + Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); + autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); + osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini); + } + + { + // Define problem during initialization with csc matrix + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); + + const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); + const auto status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, status, polish_status); + } + + { + std::tuple, std::vector, int, int, int> result; + // Dummy initial problem with csc matrix + CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); + CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); + autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); + osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); + + // Redefine problem before optimization + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + + const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); + const auto status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, status, polish_status); + } + + // add warm startup + { + // Dummy initial problem with csc matrix + CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); + CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); + std::vector q_ini(2, 0.0); + std::vector l_ini(4, 0.0); + std::vector u_ini(4, 0.0); + autoware::qp_interface::OSQPInterface osqp(true, 4000, 1e-6); // enable warm start + osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); + + // Redefine problem before optimization + CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); + CSC_Matrix A_csc = calCSCMatrix(A); + { + const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); + const auto status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, status, polish_status); + + osqp.updateCheckTermination(1); + const auto primal_val = solution; + const auto dual_val = osqp.getDualSolution(); + for (size_t i = 0; i < primal_val.size(); ++i) { + std::cerr << primal_val.at(i) << std::endl; + } + osqp.setWarmStart(primal_val, dual_val); + } + + { + const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); + const auto status = osqp.getStatus(); + const auto polish_status = osqp.getPolishStatus(); + check_result(solution, status, polish_status); + } + + // NOTE: This should be true, but currently optimize function reset the workspace, which + // disables warm start. + // EXPECT_EQ(osqp.getTakenIter(), 1); + } +} +} // namespace diff --git a/common/autoware_qp_interface/test/test_proxqp_interface.cpp b/common/autoware_qp_interface/test/test_proxqp_interface.cpp new file mode 100644 index 0000000000..e47af10c7a --- /dev/null +++ b/common/autoware_qp_interface/test/test_proxqp_interface.cpp @@ -0,0 +1,85 @@ +// Copyright 2023 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 "autoware/qp_interface/proxqp_interface.hpp" +#include "gtest/gtest.h" + +#include + +#include +#include +#include +#include + +namespace +{ +// Problem taken from +// https://github.com/proxqp/proxqp/blob/master/tests/basic_qp/generate_problem.py +// +// min 1/2 * x' * P * x + q' * x +// s.t. lb <= A * x <= ub +// +// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] +// [1, 2] [1] [1, 0] [ 0] [0.7] +// [0, 1] [ 0] [0.7] +// [0, 1] [-inf] [inf] +// +// The optimal solution is +// x = [0.3, 0.7]' +// y = [-2.9, 0.0, 0.2, 0.0]` +// obj = 1.88 + +TEST(TestProxqpInterface, BasicQp) +{ + auto check_result = [](const auto & solution, const std::string & status) { + EXPECT_EQ(status, "PROXQP_SOLVED"); + + static const auto ep = 1.0e-8; + ASSERT_EQ(solution.size(), size_t(2)); + EXPECT_NEAR(solution[0], 0.3, ep); + EXPECT_NEAR(solution[1], 0.7, ep); + }; + + const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); + const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); + const std::vector q = {1.0, 1.0}; + const std::vector l = {1.0, 0.0, 0.0, -std::numeric_limits::max()}; + const std::vector u = {1.0, 0.7, 0.7, std::numeric_limits::max()}; + + { + // Define problem during optimization + autoware::qp_interface::ProxQPInterface proxqp(false, 4000, 1e-9, 1e-9, false); + const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); + const auto status = proxqp.getStatus(); + check_result(solution, status); + } + + { + // Define problem during optimization with warm start + autoware::qp_interface::ProxQPInterface proxqp(true, 4000, 1e-9, 1e-9, false); + { + const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); + const auto status = proxqp.getStatus(); + check_result(solution, status); + EXPECT_NE(proxqp.getIterationNumber(), 1); + } + { + const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); + const auto status = proxqp.getStatus(); + check_result(solution, status); + EXPECT_EQ(proxqp.getIterationNumber(), 0); + } + } +} +} // namespace diff --git a/localization/autoware_localization_util/CMakeLists.txt b/localization/autoware_localization_util/CMakeLists.txt new file mode 100644 index 0000000000..61428d4a7f --- /dev/null +++ b/localization/autoware_localization_util/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_localization_util) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +include_directories( + include +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/util_func.cpp + src/smart_pose_buffer.cpp + src/tree_structured_parzen_estimator.cpp + src/covariance_ellipse.cpp +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_smart_pose_buffer + test/test_smart_pose_buffer.cpp + src/smart_pose_buffer.cpp + ) + + ament_auto_add_gtest(test_tpe + test/test_tpe.cpp + src/tree_structured_parzen_estimator.cpp + ) +endif() + +ament_auto_package( + INSTALL_TO_SHARE +) diff --git a/localization/autoware_localization_util/README.md b/localization/autoware_localization_util/README.md new file mode 100644 index 0000000000..b4404dc158 --- /dev/null +++ b/localization/autoware_localization_util/README.md @@ -0,0 +1,149 @@ +# autoware_localization_util + +## Overview + +`autoware_localization_util` is a collection of localization utility packages. It contains 5 individual library that used by autoware localization nodes. + +- `covariance_ellipse` 2d covariance visualization wrapper. +- `smart_pose_buffer` pose buffer management library which contains interpolate and data validation. +- `tree_structured_parzen_estimator` A Tree Structured Parzen Estimator library. +- `util_func` A tool library which contains several function for localization nodes. + +## Design + +- `covariance_ellipse` Translate `geometry_msgs::msg::PoseWithCovariance` message into ellipse visual marker to demonstrate covariance distribution. +- `smart_pose_buffer` A buffer library which implements pose message buffering, pose interpolate and pose validation. +- `tree_structured_parzen_estimator` A Probability Estimator library that includes estimator and log likelihood ratio calculation. +- `util_func` Tool function collection. + +## Usage + +### covariance_ellipse + +Include header file to use: + +```cpp +#include "autoware/localization_util/covariance_ellipse.hpp" +``` + +calculate ellipse and visualize + +```cpp +autoware::localization_util::Ellipse ellipse_ = autoware::localization_util::calculate_xy_ellipse(input_msg->pose, scale_); + + const auto ellipse_marker = autoware::localization_util::create_ellipse_marker( + ellipse_, input_msg->header, input_msg->pose); +``` + +### smart_pose_buffer + +buffer init + +```cpp +#include "autoware/localization_util/smart_pose_buffer.hpp" + +using autoware::localization_util::SmartPoseBuffer; + +std::unique_ptr initial_pose_buffer_; +initial_pose_buffer_ = std::make_unique( + this->get_logger(), param_.validation.initial_pose_timeout_sec, + param_.validation.initial_pose_distance_tolerance_m); +``` + +interpolate and pop out old pose message + +```cpp +std::optional interpolation_result_opt = +initial_pose_buffer_->interpolate(sensor_ros_time); + +... + +initial_pose_buffer_->pop_old(sensor_ros_time); +const SmartPoseBuffer::InterpolateResult & interpolation_result = +interpolation_result_opt.value(); +``` + +clear buffer + +```cpp +initial_pose_buffer_->clear(); +``` + +### tree_structured_parzen_estimator + +init the estimator. +n_startup_trials -- The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search. + +```cpp +#include "autoware/localization_util/tree_structured_parzen_estimator.hpp" + +using autoware::localization_util::TreeStructuredParzenEstimator; + +TreeStructuredParzenEstimator tpe( +TreeStructuredParzenEstimator::Direction::MAXIMIZE, +param_.initial_pose_estimation.n_startup_trials, sample_mean, sample_stddev); +``` + +get estimation result + +```cpp +const TreeStructuredParzenEstimator::Input input = tpe.get_next_input(); +``` + +add new data to the estimator + +```cpp +TreeStructuredParzenEstimator::Input result(6); + result[0] = pose.position.x; + result[1] = pose.position.y; + result[2] = pose.position.z; + result[3] = rpy.x; + result[4] = rpy.y; + result[5] = rpy.z; +tpe.add_trial(TreeStructuredParzenEstimator::Trial{result, ndt_result.transform_probability}); +``` + +### util_func + +include header file to use + +```cpp +#include "autoware/localization_util/util_func.hpp" + +using autoware::localization_util::exchange_color_crc; +using autoware::localization_util::matrix4f_to_pose; +using autoware::localization_util::point_to_vector3d; +using autoware::localization_util::pose_to_matrix4f; +``` + +list of useful function + +```cpp +std_msgs::msg::ColorRGBA exchange_color_crc(double x); +double calc_diff_for_radian(const double lhs_rad, const double rhs_rad); +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose); +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose); +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); +geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( + const double r_rad, const double p_rad, const double y_rad); +geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( + const double r_deg, const double p_deg, const double y_deg); +geometry_msgs::msg::Twist calc_twist( + const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b); +geometry_msgs::msg::PoseStamped interpolate_pose( + const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, + const rclcpp::Time & time_stamp); +geometry_msgs::msg::PoseStamped interpolate_pose( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp); +Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose); +Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose); +geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix); +Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos); +template +T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform);double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); + +void output_pose_with_cov_to_log( + const rclcpp::Logger & logger, const std::string & prefix, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov); +``` diff --git a/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp b/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp new file mode 100644 index 0000000000..68d92e181c --- /dev/null +++ b/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp @@ -0,0 +1,46 @@ +// Copyright 2024 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 AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ + +#include + +#include +#include + +#include + +namespace autoware::localization_util +{ + +struct Ellipse +{ + double long_radius; + double short_radius; + double yaw; + Eigen::Matrix2d P; + double size_lateral_direction; +}; + +Ellipse calculate_xy_ellipse( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale); + +visualization_msgs::msg::Marker create_ellipse_marker( + const Ellipse & ellipse, const std_msgs::msg::Header & header, + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); + +} // namespace autoware::localization_util + +#endif // AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp b/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp new file mode 100644 index 0000000000..bda6ff19f2 --- /dev/null +++ b/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp @@ -0,0 +1,26 @@ +// 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. + +#ifndef AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ + +#include + +namespace autoware::localization_util +{ +using Matrix6d = Eigen::Matrix; +using RowMatrixXd = Eigen::Matrix; +} // namespace autoware::localization_util + +#endif // AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp b/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp new file mode 100644 index 0000000000..8c10506c36 --- /dev/null +++ b/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp @@ -0,0 +1,71 @@ +// Copyright 2015-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 AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ + +#include "autoware/localization_util/util_func.hpp" + +#include + +#include + +#include + +namespace autoware::localization_util +{ +class SmartPoseBuffer +{ +private: + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + +public: + struct InterpolateResult + { + PoseWithCovarianceStamped old_pose; + PoseWithCovarianceStamped new_pose; + PoseWithCovarianceStamped interpolated_pose; + }; + + SmartPoseBuffer() = delete; + SmartPoseBuffer( + const rclcpp::Logger & logger, const double & pose_timeout_sec, + const double & pose_distance_tolerance_meters); + + std::optional interpolate(const rclcpp::Time & target_ros_time); + + void push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr); + + void pop_old(const rclcpp::Time & target_ros_time); + + void clear(); + +private: + rclcpp::Logger logger_; + std::deque pose_buffer_; + std::mutex mutex_; // This mutex is for pose_buffer_ + + const double pose_timeout_sec_; + const double pose_distance_tolerance_meters_; + + [[nodiscard]] bool validate_time_stamp_difference( + const rclcpp::Time & target_time, const rclcpp::Time & reference_time, + const double time_tolerance_sec) const; + [[nodiscard]] bool validate_position_difference( + const geometry_msgs::msg::Point & target_point, + const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; +}; +} // namespace autoware::localization_util + +#endif // AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp b/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp new file mode 100644 index 0000000000..ddf7625c20 --- /dev/null +++ b/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp @@ -0,0 +1,87 @@ +// Copyright 2023 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 AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ + +/* +A implementation of tree-structured parzen estimator (TPE) +See below pdf for the TPE algorithm detail. +https://papers.nips.cc/paper_files/paper/2011/file/86e8f7ab32cfd12577bc2619bc635690-Paper.pdf + +Optuna is also used as a reference for implementation. +https://github.com/optuna/optuna +*/ + +#include +#include +#include + +namespace autoware::localization_util +{ +class TreeStructuredParzenEstimator +{ +public: + using Input = std::vector; + using Score = double; + struct Trial + { + Input input; + Score score; + }; + + enum Direction { + MINIMIZE = 0, + MAXIMIZE = 1, + }; + + enum Index { + TRANS_X = 0, + TRANS_Y = 1, + TRANS_Z = 2, + ANGLE_X = 3, + ANGLE_Y = 4, + ANGLE_Z = 5, + INDEX_NUM = 6, + }; + + TreeStructuredParzenEstimator() = delete; + TreeStructuredParzenEstimator( + const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, + std::vector sample_stddev); + void add_trial(const Trial & trial); + [[nodiscard]] Input get_next_input() const; + +private: + static constexpr double max_good_rate = 0.10; + static constexpr int64_t n_ei_candidates = 100; + + static std::mt19937_64 engine; + + [[nodiscard]] double compute_log_likelihood_ratio(const Input & input) const; + [[nodiscard]] static double log_gaussian_pdf( + const Input & input, const Input & mu, const Input & sigma); + + std::vector trials_; + int64_t above_num_; + const Direction direction_; + const int64_t n_startup_trials_; + const int64_t input_dimension_; + const std::vector sample_mean_; + const std::vector sample_stddev_; + Input base_stddev_; +}; +} // namespace autoware::localization_util + +#endif // AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp b/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp new file mode 100644 index 0000000000..bef9968f34 --- /dev/null +++ b/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp @@ -0,0 +1,88 @@ +// Copyright 2015-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 AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ +#define AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ + +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +#include +#include +#include +#include +#include +#include + +namespace autoware::localization_util +{ +// ref by http://takacity.blog.fc2.com/blog-entry-69.html +std_msgs::msg::ColorRGBA exchange_color_crc(double x); + +double calc_diff_for_radian(const double lhs_rad, const double rhs_rad); + +// x: roll, y: pitch, z: yaw +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose); +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose); +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); + +geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( + const double r_rad, const double p_rad, const double y_rad); +geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( + const double r_deg, const double p_deg, const double y_deg); + +geometry_msgs::msg::Twist calc_twist( + const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b); + +geometry_msgs::msg::PoseStamped interpolate_pose( + const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, + const rclcpp::Time & time_stamp); + +geometry_msgs::msg::PoseStamped interpolate_pose( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp); + +Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose); +Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose); +geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix); +Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos); + +template +T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform) +{ + T output; + tf2::doTransform(input, output, transform); + return output; +} + +double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); + +void output_pose_with_cov_to_log( + const rclcpp::Logger & logger, const std::string & prefix, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov); + +} // namespace autoware::localization_util + +#endif // AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ diff --git a/localization/autoware_localization_util/package.xml b/localization/autoware_localization_util/package.xml new file mode 100644 index 0000000000..d7059022f2 --- /dev/null +++ b/localization/autoware_localization_util/package.xml @@ -0,0 +1,36 @@ + + + + autoware_localization_util + 0.0.0 + The autoware_localization_util package + Xingang Liu + Yamato Ando + Masahiro Sakamoto + Shintaro Sakoda + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Ryu Yamamoto + Apache License 2.0 + Yamato Ando + + ament_cmake_auto + autoware_cmake + + diagnostic_msgs + geometry_msgs + rclcpp + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + visualization_msgs + + ament_cmake_cppcheck + ament_lint_auto + + + ament_cmake + + diff --git a/localization/autoware_localization_util/src/covariance_ellipse.cpp b/localization/autoware_localization_util/src/covariance_ellipse.cpp new file mode 100644 index 0000000000..847f89e0da --- /dev/null +++ b/localization/autoware_localization_util/src/covariance_ellipse.cpp @@ -0,0 +1,92 @@ +// Copyright 2024 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 "autoware/localization_util/covariance_ellipse.hpp" + +#include + +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +namespace autoware::localization_util +{ + +Ellipse calculate_xy_ellipse( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale) +{ + // input geometry_msgs::PoseWithCovariance contain 6x6 matrix + Eigen::Matrix2d xy_covariance; + const auto cov = pose_with_covariance.covariance; + xy_covariance(0, 0) = cov[0 * 6 + 0]; + xy_covariance(0, 1) = cov[0 * 6 + 1]; + xy_covariance(1, 0) = cov[1 * 6 + 0]; + xy_covariance(1, 1) = cov[1 * 6 + 1]; + + Eigen::SelfAdjointEigenSolver eigensolver(xy_covariance); + + Ellipse ellipse; + + // eigen values and vectors are sorted in ascending order + ellipse.long_radius = scale * std::sqrt(eigensolver.eigenvalues()(1)); + ellipse.short_radius = scale * std::sqrt(eigensolver.eigenvalues()(0)); + + // principal component vector + const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1); + ellipse.yaw = std::atan2(pc_vector.y(), pc_vector.x()); + + // ellipse size along lateral direction (body-frame) + ellipse.P = xy_covariance; + const double yaw_vehicle = tf2::getYaw(pose_with_covariance.pose.orientation); + const Eigen::Matrix2d & p_inv = ellipse.P.inverse(); + Eigen::MatrixXd e(2, 1); + e(0, 0) = std::cos(yaw_vehicle); + e(1, 0) = std::sin(yaw_vehicle); + const double d = std::sqrt((e.transpose() * p_inv * e)(0, 0) / p_inv.determinant()); + ellipse.size_lateral_direction = scale * d; + + return ellipse; +} + +visualization_msgs::msg::Marker create_ellipse_marker( + const Ellipse & ellipse, const std_msgs::msg::Header & header, + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) +{ + tf2::Quaternion quat; + quat.setEuler(0, 0, ellipse.yaw); + + const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0); + const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0); + visualization_msgs::msg::Marker marker; + marker.header = header; + marker.ns = "error_ellipse"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = pose_with_covariance.pose; + marker.pose.orientation = tf2::toMsg(quat); + marker.scale.x = ellipse_long_radius * 2; + marker.scale.y = ellipse_short_radius * 2; + marker.scale.z = 0.01; + marker.color.a = 0.1; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + return marker; +} + +} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/smart_pose_buffer.cpp b/localization/autoware_localization_util/src/smart_pose_buffer.cpp new file mode 100644 index 0000000000..3b529d68cf --- /dev/null +++ b/localization/autoware_localization_util/src/smart_pose_buffer.cpp @@ -0,0 +1,158 @@ +// Copyright 2023- 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 "autoware/localization_util/smart_pose_buffer.hpp" + +namespace autoware::localization_util +{ +SmartPoseBuffer::SmartPoseBuffer( + const rclcpp::Logger & logger, const double & pose_timeout_sec, + const double & pose_distance_tolerance_meters) +: logger_(logger), + pose_timeout_sec_(pose_timeout_sec), + pose_distance_tolerance_meters_(pose_distance_tolerance_meters) +{ +} + +std::optional SmartPoseBuffer::interpolate( + const rclcpp::Time & target_ros_time) +{ + InterpolateResult result; + + { + std::lock_guard lock(mutex_); + + if (pose_buffer_.size() < 2) { + RCLCPP_INFO(logger_, "pose_buffer_.size() < 2"); + return std::nullopt; + } + + const rclcpp::Time time_first = pose_buffer_.front()->header.stamp; + const rclcpp::Time time_last = pose_buffer_.back()->header.stamp; + + if (target_ros_time < time_first) { + RCLCPP_INFO(logger_, "Mismatch between pose timestamp and current timestamp"); + return std::nullopt; + } + + // [time_last < target_ros_time] is acceptable here. + // It is possible that the target_ros_time (often sensor timestamp) is newer than the latest + // timestamp of buffered pose (often EKF). + // However, if the timestamp difference is too large, + // it will later be rejected by validate_time_stamp_difference. + + // get the nearest poses + result.old_pose = *pose_buffer_.front(); + for (const PoseWithCovarianceStamped::ConstSharedPtr & pose_cov_msg_ptr : pose_buffer_) { + result.new_pose = *pose_cov_msg_ptr; + const rclcpp::Time pose_time_stamp = result.new_pose.header.stamp; + if (pose_time_stamp > target_ros_time) { + break; + } + result.old_pose = *pose_cov_msg_ptr; + } + } + + // check the time stamp + const bool is_old_pose_valid = validate_time_stamp_difference( + result.old_pose.header.stamp, target_ros_time, pose_timeout_sec_); + const bool is_new_pose_valid = validate_time_stamp_difference( + result.new_pose.header.stamp, target_ros_time, pose_timeout_sec_); + + // check the position jumping (ex. immediately after the initial pose estimation) + const bool is_pose_diff_valid = validate_position_difference( + result.old_pose.pose.pose.position, result.new_pose.pose.pose.position, + pose_distance_tolerance_meters_); + + // all validations must be true + if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { + return std::nullopt; + } + + // interpolate the pose + const geometry_msgs::msg::PoseStamped interpolated_pose_msg = + interpolate_pose(result.old_pose, result.new_pose, target_ros_time); + result.interpolated_pose.header = interpolated_pose_msg.header; + result.interpolated_pose.pose.pose = interpolated_pose_msg.pose; + // This does not interpolate the covariance. + // interpolated_pose.pose.covariance is always set to old_pose.covariance + result.interpolated_pose.pose.covariance = result.old_pose.pose.covariance; + return result; +} + +void SmartPoseBuffer::push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr) +{ + std::lock_guard lock(mutex_); + if (!pose_buffer_.empty()) { + // Check for non-chronological timestamp order + // This situation can arise when replaying a rosbag multiple times + const rclcpp::Time end_time = pose_buffer_.back()->header.stamp; + const rclcpp::Time msg_time = pose_msg_ptr->header.stamp; + if (msg_time < end_time) { + // Clear the buffer if timestamps are reversed to maintain chronological order + pose_buffer_.clear(); + } + } + pose_buffer_.push_back(pose_msg_ptr); +} + +void SmartPoseBuffer::pop_old(const rclcpp::Time & target_ros_time) +{ + std::lock_guard lock(mutex_); + while (!pose_buffer_.empty()) { + if (rclcpp::Time(pose_buffer_.front()->header.stamp) >= target_ros_time) { + break; + } + pose_buffer_.pop_front(); + } +} + +void SmartPoseBuffer::clear() +{ + std::lock_guard lock(mutex_); + pose_buffer_.clear(); +} + +bool SmartPoseBuffer::validate_time_stamp_difference( + const rclcpp::Time & target_time, const rclcpp::Time & reference_time, + const double time_tolerance_sec) const +{ + const double dt = std::abs((target_time - reference_time).seconds()); + const bool success = dt < time_tolerance_sec; + if (!success) { + RCLCPP_WARN( + logger_, + "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " + "difference is %lf[sec] (the tolerance is %lf[sec]).", + reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); + } + return success; +} + +bool SmartPoseBuffer::validate_position_difference( + const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, + const double distance_tolerance_m_) const +{ + const double distance = norm(target_point, reference_point); + const bool success = distance < distance_tolerance_m_; + if (!success) { + RCLCPP_WARN( + logger_, + "Validation error. The distance from reference position to target position is %lf[m] (the " + "tolerance is %lf[m]).", + distance, distance_tolerance_m_); + } + return success; +} +} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp new file mode 100644 index 0000000000..e9f0318d1e --- /dev/null +++ b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp @@ -0,0 +1,185 @@ +// Copyright 2023 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 "autoware/localization_util/tree_structured_parzen_estimator.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::localization_util +{ +// random number generator +std::mt19937_64 TreeStructuredParzenEstimator::engine(0); + +TreeStructuredParzenEstimator::TreeStructuredParzenEstimator( + const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, + std::vector sample_stddev) +: above_num_(0), + direction_(direction), + n_startup_trials_(n_startup_trials), + input_dimension_(INDEX_NUM), + sample_mean_(std::move(sample_mean)), + sample_stddev_(std::move(sample_stddev)) +{ + if (sample_mean_.size() != ANGLE_Z) { + std::cerr << "sample_mean size is invalid" << std::endl; + throw std::runtime_error("sample_mean size is invalid"); + } + if (sample_stddev_.size() != ANGLE_Z) { + std::cerr << "sample_stddev size is invalid" << std::endl; + throw std::runtime_error("sample_stddev size is invalid"); + } + // base_stddev_ is defined as the stable convergence range of ndt_scan_matcher. + base_stddev_.resize(input_dimension_); + base_stddev_[TRANS_X] = 0.25; // [m] + base_stddev_[TRANS_Y] = 0.25; // [m] + base_stddev_[TRANS_Z] = 0.25; // [m] + base_stddev_[ANGLE_X] = 1.0 / 180.0 * M_PI; // [rad] + base_stddev_[ANGLE_Y] = 1.0 / 180.0 * M_PI; // [rad] + base_stddev_[ANGLE_Z] = 2.5 / 180.0 * M_PI; // [rad] +} + +void TreeStructuredParzenEstimator::add_trial(const Trial & trial) +{ + trials_.push_back(trial); + std::sort(trials_.begin(), trials_.end(), [this](const Trial & lhs, const Trial & rhs) { + return (direction_ == Direction::MAXIMIZE ? lhs.score > rhs.score : lhs.score < rhs.score); + }); + above_num_ = std::min( + {static_cast(10), + static_cast(static_cast(trials_.size()) * max_good_rate)}); +} + +TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_input() const +{ + std::normal_distribution dist_normal_trans_x( + sample_mean_[TRANS_X], sample_stddev_[TRANS_X]); + std::normal_distribution dist_normal_trans_y( + sample_mean_[TRANS_Y], sample_stddev_[TRANS_Y]); + std::normal_distribution dist_normal_trans_z( + sample_mean_[TRANS_Z], sample_stddev_[TRANS_Z]); + std::normal_distribution dist_normal_angle_x( + sample_mean_[ANGLE_X], sample_stddev_[ANGLE_X]); + std::normal_distribution dist_normal_angle_y( + sample_mean_[ANGLE_Y], sample_stddev_[ANGLE_Y]); + std::uniform_real_distribution dist_uniform_angle_z(-M_PI, M_PI); + + if (static_cast(trials_.size()) < n_startup_trials_ || above_num_ == 0) { + // Random sampling based on prior until the number of trials reaches `n_startup_trials_`. + Input input(input_dimension_); + input[TRANS_X] = dist_normal_trans_x(engine); + input[TRANS_Y] = dist_normal_trans_y(engine); + input[TRANS_Z] = dist_normal_trans_z(engine); + input[ANGLE_X] = dist_normal_angle_x(engine); + input[ANGLE_Y] = dist_normal_angle_y(engine); + input[ANGLE_Z] = dist_uniform_angle_z(engine); + return input; + } + + Input best_input; + double best_log_likelihood_ratio = std::numeric_limits::lowest(); + for (int64_t i = 0; i < n_ei_candidates; i++) { + Input input(input_dimension_); + input[TRANS_X] = dist_normal_trans_x(engine); + input[TRANS_Y] = dist_normal_trans_y(engine); + input[TRANS_Z] = dist_normal_trans_z(engine); + input[ANGLE_X] = dist_normal_angle_x(engine); + input[ANGLE_Y] = dist_normal_angle_y(engine); + input[ANGLE_Z] = dist_uniform_angle_z(engine); + const double log_likelihood_ratio = compute_log_likelihood_ratio(input); + if (log_likelihood_ratio > best_log_likelihood_ratio) { + best_log_likelihood_ratio = log_likelihood_ratio; + best_input = input; + } + } + return best_input; +} + +double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & input) const +{ + const auto n = static_cast(trials_.size()); + + // The above KDE and the below KDE are calculated respectively, and the ratio is the criteria to + // select best sample. + std::vector above_logs; + std::vector below_logs; + + for (int64_t i = 0; i < n; i++) { + const double log_p = log_gaussian_pdf(input, trials_[i].input, base_stddev_); + if (i < above_num_) { + const double w = 1.0 / static_cast(above_num_); + const double log_w = std::log(w); + above_logs.push_back(log_p + log_w); + } else { + const double w = 1.0 / static_cast(n - above_num_); + const double log_w = std::log(w); + below_logs.push_back(log_p + log_w); + } + } + + auto log_sum_exp = [](const std::vector & log_vec) { + const double max = *std::max_element(log_vec.begin(), log_vec.end()); + double sum = std::accumulate( + log_vec.begin(), log_vec.end(), 0.0, + [max](double total, double log_v) { return total + std::exp(log_v - max); }); + return max + std::log(sum); + }; + + const double above = log_sum_exp(above_logs); + const double below = log_sum_exp(below_logs); + + // Multiply by a constant so that the score near the "below sample" becomes lower. + // cspell:disable-line TODO(Shintaro Sakoda): It's theoretically incorrect, consider it again + // later. + const double r = above - below * 5.0; + return r; +} + +double TreeStructuredParzenEstimator::log_gaussian_pdf( + const Input & input, const Input & mu, const Input & sigma) +{ + const double log_2pi = std::log(2.0 * M_PI); + auto log_gaussian_pdf_1d = [&](const double diff, const double sigma) { + return -0.5 * log_2pi - std::log(sigma) - (diff * diff) / (2.0 * sigma * sigma); + }; + + const auto n = static_cast(input.size()); + + double result = 0.0; + for (int64_t i = 0; i < n; i++) { + double diff = input[i] - mu[i]; + if (i == ANGLE_Z) { + // Normalize the loop variable to [-pi, pi) + while (diff >= M_PI) { + diff -= 2 * M_PI; + } + while (diff < -M_PI) { + diff += 2 * M_PI; + } + } + // Experimentally, it is better to consider only trans_xy and yaw, so ignore trans_z, angle_x, + // angle_y. + if (i == TRANS_Z || i == ANGLE_X || i == ANGLE_Y) { + continue; + } + result += log_gaussian_pdf_1d(diff, sigma[i]); + } + return result; +} +} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/util_func.cpp b/localization/autoware_localization_util/src/util_func.cpp new file mode 100644 index 0000000000..17187a8d73 --- /dev/null +++ b/localization/autoware_localization_util/src/util_func.cpp @@ -0,0 +1,257 @@ +// Copyright 2015-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 "autoware/localization_util/util_func.hpp" + +#include "autoware/localization_util/matrix_type.hpp" + +#include +#include + +namespace autoware::localization_util +{ +// ref by http://takacity.blog.fc2.com/blog-entry-69.html +std_msgs::msg::ColorRGBA exchange_color_crc(double x) +{ + std_msgs::msg::ColorRGBA color; + + x = std::max(x, 0.0); + x = std::min(x, 0.9999); + + if (x <= 0.25) { + color.b = 1.0; + color.g = static_cast(std::sin(x * 2.0 * M_PI)); + color.r = 0; + } else if (x <= 0.5) { + color.b = static_cast(std::sin(x * 2 * M_PI)); + color.g = 1.0; + color.r = 0; + } else if (x <= 0.75) { + color.b = 0; + color.g = 1.0; + color.r = static_cast(-std::sin(x * 2.0 * M_PI)); + } else { + color.b = 0; + color.g = static_cast(-std::sin(x * 2.0 * M_PI)); + color.r = 1.0; + } + color.a = 0.999; + return color; +} + +double calc_diff_for_radian(const double lhs_rad, const double rhs_rad) +{ + double diff_rad = lhs_rad - rhs_rad; + if (diff_rad > M_PI) { + diff_rad = diff_rad - 2 * M_PI; + } else if (diff_rad < -M_PI) { + diff_rad = diff_rad + 2 * M_PI; + } + return diff_rad; +} + +Eigen::Map make_eigen_covariance(const std::array & covariance) +{ + return {covariance.data(), 6, 6}; +} + +// x: roll, y: pitch, z: yaw +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Vector3 rpy; + tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); + tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); + return rpy; +} + +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose) +{ + return get_rpy(pose.pose); +} + +geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +{ + return get_rpy(pose.pose.pose); +} + +geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( + const double r_rad, const double p_rad, const double y_rad) +{ + tf2::Quaternion q; + q.setRPY(r_rad, p_rad, y_rad); + geometry_msgs::msg::Quaternion quaternion_msg; + quaternion_msg.x = q.x(); + quaternion_msg.y = q.y(); + quaternion_msg.z = q.z(); + quaternion_msg.w = q.w(); + return quaternion_msg; +} + +geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( + const double r_deg, const double p_deg, const double y_deg) +{ + const double r_rad = r_deg * M_PI / 180.0; + const double p_rad = p_deg * M_PI / 180.0; + const double y_rad = y_deg * M_PI / 180.0; + return rpy_rad_to_quaternion(r_rad, p_rad, y_rad); +} + +geometry_msgs::msg::Twist calc_twist( + const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b) +{ + const rclcpp::Duration dt = rclcpp::Time(pose_b.header.stamp) - rclcpp::Time(pose_a.header.stamp); + const double dt_s = dt.seconds(); + + if (dt_s == 0) { + return geometry_msgs::msg::Twist(); + } + + const auto pose_a_rpy = get_rpy(pose_a); + const auto pose_b_rpy = get_rpy(pose_b); + + geometry_msgs::msg::Vector3 diff_xyz; + geometry_msgs::msg::Vector3 diff_rpy; + + diff_xyz.x = pose_b.pose.position.x - pose_a.pose.position.x; + diff_xyz.y = pose_b.pose.position.y - pose_a.pose.position.y; + diff_xyz.z = pose_b.pose.position.z - pose_a.pose.position.z; + diff_rpy.x = calc_diff_for_radian(pose_b_rpy.x, pose_a_rpy.x); + diff_rpy.y = calc_diff_for_radian(pose_b_rpy.y, pose_a_rpy.y); + diff_rpy.z = calc_diff_for_radian(pose_b_rpy.z, pose_a_rpy.z); + + geometry_msgs::msg::Twist twist; + twist.linear.x = diff_xyz.x / dt_s; + twist.linear.y = diff_xyz.y / dt_s; + twist.linear.z = diff_xyz.z / dt_s; + twist.angular.x = diff_rpy.x / dt_s; + twist.angular.y = diff_rpy.y / dt_s; + twist.angular.z = diff_rpy.z / dt_s; + + return twist; +} + +geometry_msgs::msg::PoseStamped interpolate_pose( + const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, + const rclcpp::Time & time_stamp) +{ + const rclcpp::Time pose_a_time_stamp = pose_a.header.stamp; + const rclcpp::Time pose_b_time_stamp = pose_b.header.stamp; + if ( + (pose_a_time_stamp.seconds() == 0.0) || (pose_b_time_stamp.seconds() == 0.0) || + (time_stamp.seconds() == 0.0)) { + return geometry_msgs::msg::PoseStamped(); + } + + const auto twist = calc_twist(pose_a, pose_b); + const double dt = (time_stamp - pose_a_time_stamp).seconds(); + + const auto pose_a_rpy = get_rpy(pose_a); + + geometry_msgs::msg::Vector3 xyz; + geometry_msgs::msg::Vector3 rpy; + xyz.x = pose_a.pose.position.x + twist.linear.x * dt; + xyz.y = pose_a.pose.position.y + twist.linear.y * dt; + xyz.z = pose_a.pose.position.z + twist.linear.z * dt; + rpy.x = pose_a_rpy.x + twist.angular.x * dt; + rpy.y = pose_a_rpy.y + twist.angular.y * dt; + rpy.z = pose_a_rpy.z + twist.angular.z * dt; + + tf2::Quaternion tf_quaternion; + tf_quaternion.setRPY(rpy.x, rpy.y, rpy.z); + + geometry_msgs::msg::PoseStamped pose; + pose.header = pose_a.header; + pose.header.stamp = time_stamp; + pose.pose.position.x = xyz.x; + pose.pose.position.y = xyz.y; + pose.pose.position.z = xyz.z; + pose.pose.orientation = tf2::toMsg(tf_quaternion); + return pose; +} + +geometry_msgs::msg::PoseStamped interpolate_pose( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp) +{ + geometry_msgs::msg::PoseStamped tmp_pose_a; + tmp_pose_a.header = pose_a.header; + tmp_pose_a.pose = pose_a.pose.pose; + + geometry_msgs::msg::PoseStamped tmp_pose_b; + tmp_pose_b.header = pose_b.header; + tmp_pose_b.pose = pose_b.pose.pose; + + return interpolate_pose(tmp_pose_a, tmp_pose_b, time_stamp); +} + +Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose) +{ + Eigen::Affine3d eigen_pose; + tf2::fromMsg(ros_pose, eigen_pose); + return eigen_pose; +} + +Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose) +{ + Eigen::Affine3d eigen_pose_affine = pose_to_affine3d(ros_pose); + Eigen::Matrix4f eigen_pose_matrix = eigen_pose_affine.matrix().cast(); + return eigen_pose_matrix; +} + +Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos) +{ + Eigen::Vector3d eigen_pos; + eigen_pos.x() = ros_pos.x; + eigen_pos.y() = ros_pos.y; + eigen_pos.z() = ros_pos.z; + return eigen_pos; +} + +geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix) +{ + Eigen::Affine3d eigen_pose_affine; + eigen_pose_affine.matrix() = eigen_pose_matrix.cast(); + geometry_msgs::msg::Pose ros_pose = tf2::toMsg(eigen_pose_affine); + return ros_pose; +} + +double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + const double dx = p1.x - p2.x; + const double dy = p1.y - p2.y; + const double dz = p1.z - p2.z; + return std::sqrt(dx * dx + dy * dy + dz * dz); +} + +void output_pose_with_cov_to_log( + const rclcpp::Logger & logger, const std::string & prefix, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov) +{ + const Eigen::Map covariance = + make_eigen_covariance(pose_with_cov.pose.covariance); + const geometry_msgs::msg::Pose pose = pose_with_cov.pose.pose; + geometry_msgs::msg::Vector3 rpy = get_rpy(pose); + rpy.x = rpy.x * 180.0 / M_PI; + rpy.y = rpy.y * 180.0 / M_PI; + rpy.z = rpy.z * 180.0 / M_PI; + + RCLCPP_DEBUG_STREAM( + logger, std::fixed << prefix << "," << pose.position.x << "," << pose.position.y << "," + << pose.position.z << "," << pose.orientation.x << "," << pose.orientation.y + << "," << pose.orientation.z << "," << pose.orientation.w << "," << rpy.x + << "," << rpy.y << "," << rpy.z << "," << covariance(0, 0) << "," + << covariance(1, 1) << "," << covariance(2, 2) << "," << covariance(3, 3) + << "," << covariance(4, 4) << "," << covariance(5, 5)); +} +} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp b/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp new file mode 100644 index 0000000000..d55555682b --- /dev/null +++ b/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp @@ -0,0 +1,109 @@ +// Copyright 2023- 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 "autoware/localization_util/smart_pose_buffer.hpp" +#include "autoware/localization_util/util_func.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; +using SmartPoseBuffer = autoware::localization_util::SmartPoseBuffer; + +bool compare_pose( + const PoseWithCovarianceStamped & pose_a, const PoseWithCovarianceStamped & pose_b) +{ + return pose_a.header.stamp == pose_b.header.stamp && + pose_a.header.frame_id == pose_b.header.frame_id && + pose_a.pose.covariance == pose_b.pose.covariance && + pose_a.pose.pose.position.x == pose_b.pose.pose.position.x && + pose_a.pose.pose.position.y == pose_b.pose.pose.position.y && + pose_a.pose.pose.position.z == pose_b.pose.pose.position.z && + pose_a.pose.pose.orientation.x == pose_b.pose.pose.orientation.x && + pose_a.pose.pose.orientation.y == pose_b.pose.pose.orientation.y && + pose_a.pose.pose.orientation.z == pose_b.pose.pose.orientation.z && + pose_a.pose.pose.orientation.w == pose_b.pose.pose.orientation.w; +} + +TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT +{ + rclcpp::Logger logger = rclcpp::get_logger("test_logger"); + const double pose_timeout_sec = 10.0; + const double pose_distance_tolerance_meters = 100.0; + SmartPoseBuffer smart_pose_buffer(logger, pose_timeout_sec, pose_distance_tolerance_meters); + + // first data + PoseWithCovarianceStamped::SharedPtr old_pose_ptr = std::make_shared(); + old_pose_ptr->header.stamp.sec = 10; + old_pose_ptr->header.stamp.nanosec = 0; + old_pose_ptr->pose.pose.position.x = 10.0; + old_pose_ptr->pose.pose.position.y = 20.0; + old_pose_ptr->pose.pose.position.z = 0.0; + old_pose_ptr->pose.pose.orientation = + autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 0.0); + smart_pose_buffer.push_back(old_pose_ptr); + + // second data + PoseWithCovarianceStamped::SharedPtr new_pose_ptr = std::make_shared(); + new_pose_ptr->header.stamp.sec = 20; + new_pose_ptr->header.stamp.nanosec = 0; + new_pose_ptr->pose.pose.position.x = 20.0; + new_pose_ptr->pose.pose.position.y = 40.0; + new_pose_ptr->pose.pose.position.z = 0.0; + new_pose_ptr->pose.pose.orientation = + autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 90.0); + smart_pose_buffer.push_back(new_pose_ptr); + + // interpolate + builtin_interfaces::msg::Time target_ros_time_msg; + target_ros_time_msg.sec = 15; + target_ros_time_msg.nanosec = 0; + const std::optional & interpolate_result = + smart_pose_buffer.interpolate(target_ros_time_msg); + ASSERT_TRUE(interpolate_result.has_value()); + const SmartPoseBuffer::InterpolateResult result = interpolate_result.value(); + + // check old + EXPECT_TRUE(compare_pose(result.old_pose, *old_pose_ptr)); + + // check new + EXPECT_TRUE(compare_pose(result.new_pose, *new_pose_ptr)); + + // check interpolated + EXPECT_EQ(result.interpolated_pose.header.stamp.sec, 15); + EXPECT_EQ(result.interpolated_pose.header.stamp.nanosec, static_cast(0)); + EXPECT_EQ(result.interpolated_pose.pose.pose.position.x, 15.0); + EXPECT_EQ(result.interpolated_pose.pose.pose.position.y, 30.0); + EXPECT_EQ(result.interpolated_pose.pose.pose.position.z, 0.0); + const auto rpy = autoware::localization_util::get_rpy(result.interpolated_pose.pose.pose); + EXPECT_NEAR(rpy.x * 180 / M_PI, 0.0, 1e-6); + EXPECT_NEAR(rpy.y * 180 / M_PI, 0.0, 1e-6); + EXPECT_NEAR(rpy.z * 180 / M_PI, 45.0, 1e-6); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/localization/autoware_localization_util/test/test_tpe.cpp b/localization/autoware_localization_util/test/test_tpe.cpp new file mode 100644 index 0000000000..2d71a38524 --- /dev/null +++ b/localization/autoware_localization_util/test/test_tpe.cpp @@ -0,0 +1,75 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "autoware/localization_util/tree_structured_parzen_estimator.hpp" + +#include + +#include +#include +#include +#include +#include + +using TreeStructuredParzenEstimator = autoware::localization_util::TreeStructuredParzenEstimator; + +TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function) +{ + auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { + double value = 0.0; + const auto n = static_cast(input.size()); + for (int64_t i = 0; i < n; i++) { + const double v = input[i] * 10; + value += v * v; + } + return value; + }; + + constexpr int64_t k_outer_trials_num = 20; + constexpr int64_t k_inner_trials_num = 200; + std::cout << std::fixed; + std::vector mean_scores; + std::vector sample_mean(5, 0.0); + std::vector sample_stddev{1.0, 1.0, 0.1, 0.1, 0.1}; + + for (const int64_t n_startup_trials : {k_inner_trials_num, k_inner_trials_num / 2}) { + const std::string method = ((n_startup_trials == k_inner_trials_num) ? "Random" : "TPE"); + + std::vector scores; + for (int64_t i = 0; i < k_outer_trials_num; i++) { + double best_score = std::numeric_limits::lowest(); + TreeStructuredParzenEstimator estimator( + TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials, sample_mean, + sample_stddev); + for (int64_t trial = 0; trial < k_inner_trials_num; trial++) { + const TreeStructuredParzenEstimator::Input input = estimator.get_next_input(); + const double score = -sphere_function(input); + estimator.add_trial({input, score}); + best_score = std::max(best_score, score); + } + scores.push_back(best_score); + } + + const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); + const double mean = sum / static_cast(scores.size()); + mean_scores.push_back(mean); + double sq_sum = std::accumulate( + scores.begin(), scores.end(), 0.0, + [mean](double total, double score) { return total + (score - mean) * (score - mean); }); + const double stddev = std::sqrt(sq_sum / static_cast(scores.size())); + + std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; + } + ASSERT_LT(mean_scores[0], mean_scores[1]); +}