Skip to content

Commit

Permalink
feat : add common math (#26)
Browse files Browse the repository at this point in the history 
* release v0.4.0

* Add spline interpolation library (#705)

* Feature/intersection use spline interpolation library (#710)

* Use spline_interpolation module

* Remove debug messages

* Fix/spline interpolation in intersection module (#726)

* Remove duplicating sample points

* Change isValidInput

* Apply clang-format

* Fix convergence check in PCG

* update osqp interface (#730)

* remove ROS1 packages temporarily

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add sample ros2 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* remove ROS1 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Revert "remove ROS1 packages temporarily"

This reverts commit 7127cf7.

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add COLCON_IGNORE to ros1 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Port osqp-interface to ros2 (#45)

It doesn't use any ros functionality so the source code is unchanged, only `package.xml` and `CMakeLists.txt` needed to
be modified. But osqp is added through a vendor package created at https://github.com/tier4/osqp_vendor

To test this locally, eigen needs to be installed in the environment with

    sudo apt install libeigen3-dev

* Port spline interpolation to ros2. (#65)

* Fix osqp_interface dependencies (#66)

* [osqp_interface] fix eigen3 dependencies

fixes #63

* Fix link order in consumers of osqp_interface

* Update logger name for SOR class (#87)

Signed-off-by: Servando German Serrano <servando.german.serrano@linaro.org>

* Rename h files to hpp (#142)

* Change includes

* Rename files

* Adjustments to make things compile

* Other packages

* Adjust copyright notice on 532 out of 699 source files (#143)

* Use quotes for includes where appropriate (#144)

* Use quotes for includes where appropriate

* Fix lint tests

* Make tests pass hopefully

* Run uncrustify on the entire Pilot.Auto codebase (#151)

* Run uncrustify on the entire Pilot.Auto codebase

* Exclude open PRs

* ROS2 Linting: osqp_interface (#202)

* Add linters

* Fix clang-tidy missing header error

* ROS2 Linting: spline_interpolation (#203)

* Add linters and fix indentation in package.xml

* Address PR comments:
 - Convert indents to spaces

* Use two spaces for indent

Co-authored-by: Esteve Fernandez <esteve@apache.org>

* Fix out-of-range bug of spline_interpolation (#917) (#273)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* Ros2 v0.8.0 osqp interface (#271)

* restore file name for v0.8.0 update

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix typos in common modules (#914)

* fix typos in common modules

* minor fix (lowercasing)

* revert changes in PathPoint.msg

* Fix/osqp interface api error (#969)

* [osqp_interface] fix update setting api

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* [osqp_interface] fix format

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Revert "restore file name for v0.8.0 update"

This reverts commit b767c308d492c6f754f9aa45ab9521f209ad9c9c.

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Ros2 v0.8.0 ekf localizer (#270)

* restore file name for v0.8.0 update

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* fix typos in localization (#890)

* move kalman filter to lib package (#1141)

* move kalman filter to lib package

* add kalman_filter dir

* Revert "restore file name for v0.8.0 update"

This reverts commit 485111da0aba91eeddda77e1e3b6b3f517373163.

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* Feature/ekf tracker (#1158) (#381)

* Feature/ekf tracker (#1158)

* change to ekf tracker

* visualize covariance

* cosmetic change

* cosmetic change

* change param

* add multi model ekf tracker (#1165)

* add multi model ekf tracker

* cosmetic change

* cosmetic change

* add iou filter

* change correct license

* change correct license

* cosmetic change

* bug fix

* cosmetic change

* check clockwise

* change param

* bug fix

* cosmetic change

* add comment

* add enum

* cosmetic change

* cosmetic change

* apply format

* apply format

* fix config name

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* [multi_object_tracker] apply ament_uncrustify

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* [multi_object_tracker] fix lint errors

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* fix include brackets

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Fix typo in common module (#433)

* Unify Apache-2.0 license name (#1242)

* Make control modules components (#1262)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Make planning modules components (#1263)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Porting small fix (#1288)

* Delete unused code (#1183)

* Fix control topic name of closest_velocity_checker.py (#1174)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add comments for livox tag (#1188)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Clear return value (#1193)

* Change tracker model of unknown object (#1204)

* treat polygon points as relative (#1205)

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* hotfix: reference velocity in consideration of vehicle gear (#1213)

* fix reference velocity for vehicle gear

* add initialization

* revert

* add comment

* change max area param (#1218)

* Fix an identical code for different branches (#1230)

* Update livox_tag_filter.launch.xml

* Fixup

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: shin <8327162+0x126@users.noreply.github.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix osqp interface double free problem (#1327)

* Use smart pointers for members

* Add test code

* Fix typo

* Fix test code

* Change variable name

* Fix for lint test

* Fix include

* Fix header and CMakeLists.txt

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>

* add new updater function to osqp_interface (#1433) (#1483)

* add new updater function

* modify spell

Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com>

* Add pre-commit (#1560)

* add pre-commit

* add pre-commit-config

* add additional settings for private repository

* use default pre-commit-config

* update pre-commit setting

* Ignore whitespace for line breaks in markdown

* Update .github/workflows/pre-commit.yml

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* exclude svg

* remove pretty-format-json

* add double-quote-string-fixer

* consider COLCON_IGNORE file when seaching modified package

* format file

* pre-commit fixes

* Update pre-commit.yml

* Update .pre-commit-config.yaml

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: pre-commit <pre-commit@example.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* Fix -Wunused-parameter (#1836)

* Fix -Wunused-parameter

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix mistake

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* fix spell

* Fix lint issues

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore flake8 warnings

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>

* Fix bugprone-fold-init-type in spline_interpolation (#1816)

* Fix bugprone-fold-init-type

* Fix modernize-use-override

* Fix cppcoreguidelines-pro-type-member-init

* Fix modernize-loop-convert

* add sort-package-xml hook in pre-commit (#1881)

* add sort xml hook in pre-commit

* change retval to exit_status

* rename

* add prettier plugin-xml

* use early return

* add license note

* add tier4 license

* restore prettier

* change license order

* move local hooks to public repo

* move prettier-xml to pre-commit-hooks-ros

* update version for bug-fix

* apply pre-commit

* remove spline_interpolation, and create interpolation package that ha… (#2114)

* remove spline_interpolation and add interpolation package

* fix bugs and add simple test

* modify application for interpolation package

* add some tests and fix ci test

* add some tests

* add -Werror

* move functions to anonymous

* fix reviews

* fix typo

* fix review and lint

* add tests

* resolve TODOs

* add README.md

* Update common/math/interpolation/test/src/test_interpolation_utils.cpp

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* Update common/math/interpolation/test/src/test_interpolation_utils.cpp

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* Update common/math/interpolation/test/src/test_interpolation_utils.cpp

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* fix README.md

* dealt with reviews

* update README.md

* update README.md

* Update common/math/interpolation/README.md

* fix some bugs

* fix typo

* update README.md

* Update common/math/interpolation/README.md

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* fix test

* Update common/math/interpolation/README.md

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* fix cdots

* fix README.md

* Update common/math/interpolation/README.md

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* Feature/lpf in common ros2 (#1628)

* add first order lowpass filter (#1505)

* add first order lowpass filter

* fix bugs, and add tests

* update copyright

* apply clang-format

* add another constructor

* add another reset member function

* update

* add test

* update test

* update test

* rename lowpass filter to signal processing

* Porting lpf1d to ros2

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>

* [osqp interface] size check invalid argument error ros2 (#1870)

* throw invalid_argument when osqp matrix/vector are invalid

* add test for invalid_argumetn

* Change formatter to clang-format and black (#2332)

* Revert "Temporarily comment out pre-commit hooks"

This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3.

* Replace ament_lint_common with autoware_lint_common

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove ament_cmake_uncrustify and ament_clang_format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply Black

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix for cpplint

* Fix include double quotes to angle brackets

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add COLCON_IGNORE (#500)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* remove COLCON_IGNORE (#506)

Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>

* Remove COLCON_IGNORE from interpolation (#516)

* delete COLCON IGNORE in signal processing package(#537)

* Delet colcon ignore (#534)

* Back port .auto control packages (#571)

* Implement Lateral and Longitudinal Control Muxer

* [#570] Porting wf_simulator

* [#1189] Deactivate flaky test in 'trajectory_follower_nodes'

* [#1189] Fix flacky test in 'trajectory_follower_nodes/latlon_muxer'

* [#1057] Add osqp_interface package

* [#1057] Add library code for MPC-based lateral control

* [#1271] Use std::abs instead of abs

* [#1057] Implement Lateral Controller for Cargo ODD

* [#1246] Resolve "Test case names currently use snake_case but should be CamelCase"

* [#1325] Deactivate flaky smoke test in 'trajectory_follower_nodes'

* [#1058] Add library code of longitudinal controller

* Fix build error for trajectory follower

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix build error for trajectory follower nodes

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* [#1272] Add AckermannControlCommand support to simple_planning_simulator

* [#1058] Add Longitudinal Controller node

* [#1058] Rename velocity_controller -> longitudinal_controller

* [#1058] Update CMakeLists.txt for the longitudinal_controller_node

* [#1058] Add smoke test python launch file

* [#1058] Use LowPassFilter1d from trajectory_follower

* [#1058] Use autoware_auto_msgs

* [#1058] Changes for .auto (debug msg tmp fix, common func, tf listener)

* [#1058] Remove unused parameters

* [#1058] Fix ros test

* [#1058] Rm default params from declare_parameters + use autoware types

* [#1058] Use default param file to setup NodeOptions in the ros test

* [#1058] Fix docstring

* [#1058] Replace receiving a Twist with a VehicleKinematicState

* [#1058] Change class variables format to m_ prefix

* [#1058] Fix plugin name of LongitudinalController in CMakeLists.txt

* [#1058] Fix copyright dates

* [#1058] Reorder includes

* [#1058] Add some tests (~89% coverage without disabling flaky tests)

* [#1058] Add more tests (90+% coverage without disabling flaky tests)

* [#1058] Use Float32MultiArrayDiagnostic message for debug and slope

* [#1058] Calculate wheel_base value from vehicle parameters

* [#1058] Cleanup redundant logger setting in tests

* [#1058] Set ROS_DOMAIN_ID when running tests to prevent CI failures

* [#1058] Remove TF listener and use published vehicle state instead

* [#1058] Change smoke tests to use autoware_testing

* [#1058] Add plotjuggler cfg for both lateral and longitudinal control

* [#1058] Improve design documents

* [#1058] Disable flaky test

* [#1058] Properly transform vehicle state in longitudinal node

* [#1058] Fix TF buffer of lateral controller

* [#1058] Tuning of lateral controller for LGSVL

* [#1058] Fix formating

* [#1058] Fix /tf_static sub to be transient_local

* [#1058] Fix yaw recalculation of reverse trajs in the lateral controller

* modify trajectory_follower for galactic build

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* [#1379] Update trajectory_follower

* [#1379] Update simple_planning_simulator

* [#1379] Update trajectory_follower_nodes

* apply trajectory msg modification in control

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* move directory

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remote control/trajectory_follower level dorectpry

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove .iv trajectory follower

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* use .auto trajectory_follower

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove .iv simple_planning_simulator & osqp_interface

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* use .iv simple_planning_simulator & osqp_interface

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add tmp_autoware_auto_dependencies

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* tmporally add autoware_auto_msgs

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* apply .auto message split

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix build depend

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix packages using osqp

* fix autoware_auto_geometry

* ignore lint of some packages

* ignore ament_lint of some packages

* ignore lint/pre-commit of trajectory_follower_nodes

* disable unit tests of some packages

Co-authored-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Co-authored-by: Joshua Whitley <josh.whitley@autoware.org>
Co-authored-by: Igor Bogoslavskyi <igor.bogoslavskyi@gmail.com>
Co-authored-by: MIURA Yasuyuki <kokosabu@gmail.com>
Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>
Co-authored-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* add readme signal processing (#587)

* add readme signal processing

* better fix

* add readme in kalman_filter (#633)

* ci(pre-commit): autofix

Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp>
Co-authored-by: Frederik Beaujean <72439809+fred-apex-ai@users.noreply.github.com>
Co-authored-by: Yunus Emre Çalışkan <yunus.ec@gmail.com>
Co-authored-by: Servando <43142004+sgermanserrano@users.noreply.github.com>
Co-authored-by: Nikolai Morin <nnmmgit@gmail.com>
Co-authored-by: Jilada Eccleston <jilada.eccleston@gmail.com>
Co-authored-by: Jilada Eccleston <jilada.eccleston@tier4.jp>
Co-authored-by: Esteve Fernandez <esteve@apache.org>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
Co-authored-by: Keisuke Shima <keisuke.shima@tier4.jp>
Co-authored-by: shin <8327162+0x126@users.noreply.github.com>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>
Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com>
Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
Co-authored-by: pre-commit <pre-commit@example.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>
Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com>
Co-authored-by: Yohei Mishina <66298900+YoheiMishina@users.noreply.github.com>
Co-authored-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Co-authored-by: Joshua Whitley <josh.whitley@autoware.org>
Co-authored-by: Igor Bogoslavskyi <igor.bogoslavskyi@gmail.com>
Co-authored-by: MIURA Yasuyuki <kokosabu@gmail.com>
Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@taikitanaka3 @mitsudome-r
@rej55 @k0suke-murakami @fred-apex-ai @cvasfi @sgermanserrano @nnmm @jilaada @esteve @wep21 @kenji-miyake @kmiya @TakaHoribe @yukkysaito @KeisukeShima @0x126 @h-ohta @tkimura4 @purewater0901 @takayuki5168 @kyoichi-sugahara @YoheiMishina @maxime-clem @niosus @kokosabu @1222-takeshi @pre-commit-ci
36 people authored Dec 3, 2021
1 parent 5ef7648 commit 52b809b
Show file tree
Hide file tree
Showing 36 changed files with 2,720 additions and 1 deletion.
2 changes: 1 addition & 1 deletion .pre-commit-config.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,4 +83,4 @@ repos:
args: ["--quiet"]
exclude: ".cu"

exclude: ".svg|common/autoware_auto_perception_rviz_plugin"
exclude: ".svg|control/trajectory_follower|control/trajectory_follower_nodes|common/autoware_auto_dependencies|common/autoware_auto_perception_rviz_plugin|common/osqp_interface|simulator/simple_planning_simulator|planning/freespace_planner|planning/astar_search|planning/costmap_generator"
36 changes: 36 additions & 0 deletions common/interpolation/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
cmake_minimum_required(VERSION 3.5)
project(interpolation)

### Compile options
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic -Werror)
endif()

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

ament_auto_add_library(interpolation SHARED
src/linear_interpolation.cpp
src/spline_interpolation.cpp
)

# Test
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()

find_package(ament_cmake_gtest REQUIRED)

file(GLOB_RECURSE test_files test/**/*.cpp)

ament_add_gtest(test_interpolation ${test_files})

target_link_libraries(test_interpolation
interpolation
)
endif()

ament_auto_package()
109 changes: 109 additions & 0 deletions common/interpolation/README.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,109 @@
# Interpolation package

This package supplies linear and spline interpolation functions.

## Linear Interpolation

`lerp(src_val, dst_val, ratio)` (for scalar interpolation) interpolates `src_val` and `dst_val` with `ratio`.
This will be replaced with `std::lerp(src_val, dst_val, ratio)` in `C++20`.

`lerp(base_keys, base_values, query_keys)` (for vector interpolation) applies linear regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`.
Then it calculates interpolated values on y-axis for `query_keys` on x-axis.

## Spline Interpolation

`slerp(base_keys, base_values, query_keys)` (for vector interpolation) applies spline regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`.
Then it calculates interpolated values on y-axis for `query_keys` on x-axis.

### Evaluation of calculation cost

We evaluated calculation cost of spline interpolation for 100 points, and adopted the best one which is tridiagonal matrix algorithm.
Methods except for tridiagonal matrix algorithm exists in `spline_interpolation` package, which has been removed from Autoware.

| Method | Calculation time |
| --------------------------------- | ---------------- |
| Tridiagonal Matrix Algorithm | 0.007 [ms] |
| Preconditioned Conjugate Gradient | 0.024 [ms] |
| Successive Over-Relaxation | 0.074 [ms] |

### Spline Interpolation Algorithm

Assuming that the size of `base_keys` ($x_i$) and `base_values` ($y_i$) are $N + 1$, we aim to calculate spline interpolation with the following equation to interpolate between $y_i$ and $y_{i+1}$.

$$
Y_i(x) = a_i (x - x_i)^3 + b_i (x - x_i)^2 + c_i (x - x_i) + d_i \ \ \ (i = 0, \dots, N-1)
$$

Constraints on spline interpolation are as follows.
The number of constraints is $4N$, which is equal to the number of variables of spline interpolation.

$$
\begin{align}
Y_i (x_i) & = y_i \ \ \ (i = 0, \dots, N-1) \\
Y_i (x_{i+1}) & = y_{i+1} \ \ \ (i = 0, \dots, N-1) \\
Y'_i (x_{i+1}) & = Y'_{i+1} (x_{i+1}) \ \ \ (i = 0, \dots, N-2) \\
Y''_i (x_{i+1}) & = Y''_{i+1} (x_{i+1}) \ \ \ (i = 0, \dots, N-2) \\
Y''_0 (x_0) & = 0 \\
Y''_{N-1} (x_N) & = 0
\end{align}
$$

According to [this article](https://www.mk-mode.com/rails/docs/INTERPOLATION_SPLINE.pdf), spline interpolation is formulated as the following linear equation.

$$
\begin{align}
\begin{pmatrix}
2(h_0 + h_1) & h_1 \\
h_0 & 2 (h_1 + h_2) & h_2 & & O \\
& & & \ddots \\
O & & & & h_{N-2} & 2 (h_{N-2} + h_{N-1})
\end{pmatrix}
\begin{pmatrix}
v_1 \\ v_2 \\ v_3 \\ \vdots \\ v_{N-1}
\end{pmatrix}=
\begin{pmatrix}
w_1 \\ w_2 \\ w_3 \\ \vdots \\ w_{N-1}
\end{pmatrix}
\end{align}
$$

where

$$
\begin{align}
h_i & = x_{i+1} - x_i \ \ \ (i = 0, \dots, N-1) \\
w_i & = 6 \left(\frac{y_{i+1} - y_{i+1}}{h_i} - \frac{y_i - y_{i-1}}{h_{i-1}}\right) \ \ \ (i = 1, \dots, N-1)
\end{align}
$$

The coefficient matrix of this linear equation is tridiagonal matrix. Therefore, it can be solve with tridiagonal matrix algorithm, which can solve linear equations without gradient descent methods.

Solving this linear equation with tridiagonal matrix algorithm, we can calculate coefficients of spline interpolation as follows.

$$
\begin{align}
a_i & = \frac{v_{i+1} - v_i}{6 (x_{i+1} - x_i)} \ \ \ (i = 0, \dots, N-1) \\
b_i & = \frac{v_i}{2} \ \ \ (i = 0, \dots, N-1) \\
c_i & = \frac{y_{i+1} - y_i}{x_{i+1} - x_i} - \frac{1}{6}(x_{i+1} - x_i)(2 v_i + v_{i+1}) \ \ \ (i = 0, \dots, N-1) \\
d_i & = y_i \ \ \ (i = 0, \dots, N-1)
\end{align}
$$

### Tridiagonal Matrix Algorithm

We solve tridiagonal linear equation according to [this article](https://mathlang.hatenablog.com/entry/2018/10/14/232741) where variables of linear equation are expressed as follows in the implementation.

$$
\begin{align}
\begin{pmatrix}
b_0 & c_0 & & \\
a_0 & b_1 & c_2 & O \\
& & \ddots \\
O & & a_{N-2} & b_{N-1}
\end{pmatrix}
x =
\begin{pmatrix}
d_0 \\ d_2 \\ d_3 \\ \vdots \\ d_{N-1}
\end{pmatrix}
\end{align}
$$
87 changes: 87 additions & 0 deletions common/interpolation/include/interpolation/interpolation_utils.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,87 @@
// Copyright 2021 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef INTERPOLATION__INTERPOLATION_UTILS_HPP_
#define INTERPOLATION__INTERPOLATION_UTILS_HPP_

#include <array>
#include <stdexcept>
#include <vector>

namespace interpolation_utils
{
inline bool isIncreasing(const std::vector<double> & x)
{
if (x.empty()) {
throw std::invalid_argument("Points is empty.");
}

for (size_t i = 0; i < x.size() - 1; ++i) {
if (x.at(i) >= x.at(i + 1)) {
return false;
}
}

return true;
}

inline bool isNotDecreasing(const std::vector<double> & x)
{
if (x.empty()) {
throw std::invalid_argument("Points is empty.");
}

for (size_t i = 0; i < x.size() - 1; ++i) {
if (x.at(i) > x.at(i + 1)) {
return false;
}
}

return true;
}

inline void validateInput(
const std::vector<double> & base_keys, const std::vector<double> & base_values,
const std::vector<double> & query_keys)
{
// when vectors are empty
if (base_keys.empty() || base_values.empty() || query_keys.empty()) {
throw std::invalid_argument("Points is empty.");
}

// when size of vectors are less than 2
if (base_keys.size() < 2 || base_values.size() < 2) {
throw std::invalid_argument(
"The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()) +
", base_values.size() = " + std::to_string(base_values.size()));
}

// when indices are not sorted
if (!isIncreasing(base_keys) || !isNotDecreasing(query_keys)) {
throw std::invalid_argument("Either base_keys or query_keys is not sorted.");
}

// when query_keys is out of base_keys (This function does not allow exterior division.)
if (query_keys.front() < base_keys.front() || base_keys.back() < query_keys.back()) {
throw std::invalid_argument("query_keys is out of base_keys");
}

// when sizes of indices and values are not same
if (base_keys.size() != base_values.size()) {
throw std::invalid_argument("The size of base_keys and base_values are not the same.");
}
}
} // namespace interpolation_utils

#endif // INTERPOLATION__INTERPOLATION_UTILS_HPP_
31 changes: 31 additions & 0 deletions common/interpolation/include/interpolation/linear_interpolation.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
// Copyright 2021 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef INTERPOLATION__LINEAR_INTERPOLATION_HPP_
#define INTERPOLATION__LINEAR_INTERPOLATION_HPP_

#include "interpolation/interpolation_utils.hpp"

#include <vector>

namespace interpolation
{
double lerp(const double src_val, const double dst_val, const double ratio);

std::vector<double> lerp(
const std::vector<double> & base_keys, const std::vector<double> & base_values,
const std::vector<double> & query_keys);
} // namespace interpolation

#endif // INTERPOLATION__LINEAR_INTERPOLATION_HPP_
48 changes: 48 additions & 0 deletions common/interpolation/include/interpolation/spline_interpolation.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
// Copyright 2021 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef INTERPOLATION__SPLINE_INTERPOLATION_HPP_
#define INTERPOLATION__SPLINE_INTERPOLATION_HPP_

#include <algorithm>
#include <cmath>
#include <iostream>
#include <numeric>
#include <vector>

namespace interpolation
{
// NOTE: X(s) = a_i (s - s_i)^3 + b_i (s - s_i)^2 + c_i (s - s_i) + d_i : (i = 0, 1, ... N-1)
struct MultiSplineCoef
{
explicit MultiSplineCoef(const size_t num_spline)
{
a.resize(num_spline);
b.resize(num_spline);
c.resize(num_spline);
d.resize(num_spline);
}

std::vector<double> a;
std::vector<double> b;
std::vector<double> c;
std::vector<double> d;
};

std::vector<double> slerp(
const std::vector<double> & base_keys, const std::vector<double> & base_values,
const std::vector<double> & query_keys);
} // namespace interpolation

#endif // INTERPOLATION__SPLINE_INTERPOLATION_HPP_
18 changes: 18 additions & 0 deletions common/interpolation/package.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>interpolation</name>
<version>0.1.0</version>
<description>The spline interpolation package</description>
<maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
<maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
<license>Apache License 2.0</license>
<buildtool_depend>ament_cmake_auto</buildtool_depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
52 changes: 52 additions & 0 deletions common/interpolation/src/linear_interpolation.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
// Copyright 2021 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "interpolation/linear_interpolation.hpp"

#include <vector>

namespace interpolation
{
double lerp(const double src_val, const double dst_val, const double ratio)
{
return src_val + (dst_val - src_val) * ratio;
}

std::vector<double> lerp(
const std::vector<double> & base_keys, const std::vector<double> & base_values,
const std::vector<double> & query_keys)
{
// throw exception for invalid arguments
interpolation_utils::validateInput(base_keys, base_values, query_keys);

// calculate linear interpolation
std::vector<double> query_values;
size_t key_index = 0;
for (const auto query_key : query_keys) {
while (base_keys.at(key_index + 1) < query_key) {
++key_index;
}

const double src_val = base_values.at(key_index);
const double dst_val = base_values.at(key_index + 1);
const double ratio = (query_key - base_keys.at(key_index)) /
(base_keys.at(key_index + 1) - base_keys.at(key_index));

const double interpolated_val = lerp(src_val, dst_val, ratio);
query_values.push_back(interpolated_val);
}

return query_values;
}
} // namespace interpolation
Loading

0 comments on commit 52b809b

Please sign in to comment.