feat(interpolation): add functions for flexible usage (non-static spline interpolation)

[takayuki5168]

Related Issue(required)

#350

Description(required)

• implemented non-static spline interpolation with class
  example 1

SplineInterpolation1d spline;
spline.calcSplineCoefficients(base_keys, base_values);  // memorize pre-interpolation result
internally
const auto interpolation_result1 = spline.getSplineInterpolatedValues(base_keys,
query_keys1);
const auto interpolation_result2 = spline.getSplineInterpolatedValues(base_keys,
query_keys2);

example 2

SplineInterpolationPoint spline;
spline.calcSplineCoefficients(base_keys, base_values);  // memorize pre-interpolation result internally
const auto interpolation_result1 = spline.getSplineInterpolatedPoints(base_keys,
query_keys1);
const auto interpolation_result2 = spline.getSplineInterpolatedPoints(base_keys,
query_keys2);
const auto yaw_interpolation_result = spline.getSplineInterpolatedYaws(base_keys,
query_keys1);

FYI, conventional usage (calculate spline interpolation every slerp execution)

const auto interpolation_result1 = slerp(base_keys, base_values, query_keys1);
const auto interpolation_result2 = slerp(base_keys, base_values, query_keys2);

• added test for new functions
• minor refactoring

Review Procedure(required)

run test

Related PR(optional)

Pre-Review Checklist for the PR Author

PR Author should check the checkboxes below when creating the PR.

• Read commit-guidelines
• Assign PR to reviewer

If you are adding new package following items are required:

• Documentation with description of the package is available
• A sample launch file and parameter file are available if the package contains executable nodes

Checklist for the PR Reviewer

Reviewers should check the checkboxes below before approval.

• Commits are properly organized and messages are according to the guideline
• PR title describes the changes

Post-Review Checklist for the PR Author

PR Author should check the checkboxes below before merging.

• All open points are addressed and tracked via issues or tickets

CI Checks

• Build and test for PR / build-and-test-pr: Required to pass before the merge.
• Build and test for PR / clang-tidy-pr: NOT required to pass before the merge. It is up to the reviewer(s). Found false positives? See the [guidelines][clang-tidy-guidelines].
• Check spelling: NOT required to pass before the merge. It is up to the reviewer(s). See here if you want to add some words to the spell check dictionary.

[takayuki5168]

refactor: validateInput -> validateKeys and validateKeysAndValues

[codecov]

Codecov Report

Merging #352 (0f42e6d) into tier4/proposal (a9c47ef) will increase coverage by 1.63%.
The diff coverage is 40.00%.

@@                Coverage Diff                 @@
##           tier4/proposal     #352      +/-   ##
==================================================
+ Coverage            9.30%   10.93%   +1.63%     
==================================================
  Files                 701      181     -520     
  Lines               48911    18237   -30674     
  Branches             5807     2672    -3135     
==================================================
- Hits                 4549     1995    -2554     
+ Misses              40551    14325   -26226     
+ Partials             3811     1917    -1894     

Impacted Files	Coverage Δ
...ion/include/interpolation/spline_interpolation.hpp	28.57% <ø> (ø)
...nterpolation/test/src/test_interpolation_utils.cpp	20.40% <18.18%> (+4.12%)	⬆️
...terpolation/test/src/test_spline_interpolation.cpp	25.39% <21.95%> (-6.43%)	⬇️
common/interpolation/src/spline_interpolation.cpp	59.39% <60.25%> (-1.62%)	⬇️
...tion/include/interpolation/interpolation_utils.hpp	58.62% <63.63%> (+2.09%)	⬆️
common/interpolation/src/linear_interpolation.cpp	64.28% <100.00%> (+2.74%)	⬆️
..._planner/src/scene_module/occlusion_spot/debug.cpp	0.00% <0.00%> (ø)
...sion_spot/scene_occlusion_spot_in_private_road.cpp	0.00% <0.00%> (ø)
...nning_simulator/simple_planning_simulator_core.cpp
...nterface/src/pacmod_interface/pacmod_interface.cpp
... and 518 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update a9c47ef...0f42e6d. Read the comment docs.

[takayuki5168]

1-dimensional spline interpolation

[takayuki5168]

2-dimensional point (z is ignored) spline interpolation
Yaw can be calculated by spline interpolation derivative.

[takayuki5168]

@rej55 @taikitanaka3 @TakaHoribe This is ready for review. 🙏 (This is a part of obstacle_avoidance_planner improvement)

[kenji-miyake]

The title could be changed like feat(interpolation): add functions for ....

[taikitanaka3]

LG

1: [==========] Running 7 tests from 3 test suites.
1: [----------] Global test environment set-up.
1: [----------] 4 tests from interpolation_utils
1: [ RUN      ] interpolation_utils.isIncreasing
1: [       OK ] interpolation_utils.isIncreasing (0 ms)
1: [ RUN      ] interpolation_utils.isNotDecreasing
1: [       OK ] interpolation_utils.isNotDecreasing (0 ms)
1: [ RUN      ] interpolation_utils.validateKeys
1: [       OK ] interpolation_utils.validateKeys (0 ms)
1: [ RUN      ] interpolation_utils.validateKeysAndValues
1: [       OK ] interpolation_utils.validateKeysAndValues (0 ms)
1: [----------] 4 tests from interpolation_utils (0 ms total)
1: 
1: [----------] 2 tests from linear_interpolation
1: [ RUN      ] linear_interpolation.lerp_scalar
1: [       OK ] linear_interpolation.lerp_scalar (0 ms)
1: [ RUN      ] linear_interpolation.lerp_vector
1: [       OK ] linear_interpolation.lerp_vector (0 ms)
1: [----------] 2 tests from linear_interpolation (0 ms total)
1: 
1: [----------] 1 test from spline_interpolation
1: [ RUN      ] spline_interpolation.slerp
1: [       OK ] spline_interpolation.slerp (0 ms)
1: [----------] 1 test from spline_interpolation (0 ms total)
1: 
1: [----------] Global test environment tear-down
1: [==========] 7 tests from 3 test suites ran. (0 ms total)
1: [  PASSED  ] 7 tests.

@rej55 can you check calculation details?

[rej55]

@takayuki5168
Thank you for your contribution!
I think the functions for yaw calculations should not be in the spline interpolation package because it contains a specific structure that it is for the trajectory calculation.
In my opinion, the functions for yaw calculations should be in, for example, tier4_autoware_utils instead of the spline interpolation package.

If there are any reasons to include these functions, could you tell us the reasons?
How do you think?

[takayuki5168]

(@rej55 Let's discuss in a thread)

Thank you. It's a good point.

For static spline interpolation (this function slerpYawFromPoints), I'm ok to move elsewhere like tier4_autoware_utils as you pointed.

For non-static spline interpolation, I implemented SplineInterpolation1d and SplineInterpolationPoint2d classes.
The second class includes yaw calculation (getSplineInterpolatedYaw). This member function is an option in this class. I mean, SplineInterpolationPoint2d is mainly for interpolating and calculating x and y, and as an option, you can get yaw angle as well (it is calculated from derivatives of x and y).
So it's not weird that SplineInterpolationPoint2d, which has getSplineInterpolatedYaw, is in interpolation package, IMO.

[TakaHoribe]

I feel it is a little weird having SplineInterpolation"Point2d" here as well since it depends on the geometric configuration.

It is better to create SplineInterpolation"Path2d" instead and manage it in a separate file. (The interface could be the same as the "Point2d".)

[takayuki5168]

(@taikitanaka3 Related to this comment #352 (comment))

I removed throwing out of range here since it is hard to assure that base_s_vec_.at(idx) + s is within interpolation or not on the application side.

[takayuki5168]

FYI, previous code is

autoware.universe/common/interpolation/src/spline_interpolation.cpp

Lines 331 to 337 in f73fe11

	double whole_s = base_s_vec_.at(idx) + s;
	if (whole_s < base_s_vec_.front()) {
	throw std::out_of_range("s [m] forward from idx'th base point is out of base points.");
	}
	if (whole_s > base_s_vec_.back()) {
	throw std::out_of_range("s [m] forward from idx'th base point is out of base points.");
	}

[TakaHoribe]

I don't get the function. Please add a description of the usage.

[TakaHoribe]

I feel it is a little weird having SplineInterpolation"Point2d" here as well since it depends on the geometric configuration.

It is better to create SplineInterpolation"Path2d" instead and manage it in a separate file. (The interface could be the same as the "Point2d".)