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 arebase_keys and whose y values are base_values. Then it calculates interpolated values on y-axis for query_keys on x-axis.

Spline Interpolation

spline(base_keys, base_values, query_keys) (for vector interpolation) applies spline regression to each two continuous points whose x values arebase_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, 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 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 \

File truncated at 100 lines see the full file

Changelog for package autoware_interpolation

1.1.0 (2025-05-01)

• refactor(interpolation): use [autoware_utils_*]{.title-ref} instead of [autoware_utils]{.title-ref} (#382) feat(interpolation): use split autoware utils

• fix(autoware_path_optimizer): incorrect application of input velocity due to badly mapping output trajectory to input trajectory (#355)

  • changes to avoid improper mapping

  * Update common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>> ---------Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>>

• Contributors: Arjun Jagdish Ram, Takagi, Isamu

1.9.0 (2026-06-24)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base

• perf(autoware_interpolation): scalar spline-eval overloads remove per-knot vector allocations (#1127)

  * perf(autoware_interpolation): scalar spline-eval overloads remove per-knot vector allocations Add scalar getSplineInterpolatedValue / getSplineInterpolatedDiffValue / getSplineInterpolatedQuadDiffValue overloads to SplineInterpolation and route the per-knot loops in SplineInterpolationPoints2d (point, yaw, curvature, updateCurvatureSpline, extendLinearlyForward, projectPointOntoSpline) through them. Previously each per-knot evaluation built a freshly heap-allocated single-element std::vector ({s}) and received another single-element std::vector back, costing O(N) allocations to evaluate an N-knot spline. The new scalar overloads evaluate a single key in place with no allocation; the existing vector overloads now delegate to them, removing the duplicated Horner/derivative arithmetic. Also drop the dead validated_query_keys copies in the three vector getters: the cropped result of validateKeys() was never read (the loops iterate the original query_keys), so the call is kept only for its throwing precondition side effect and the unused full-length copy of query_keys is no longer allocated. Add reserve() to getSplineInterpolatedYaws and the extendLinearlyForward extension vectors. Behavior-preserving: the scalar overloads reproduce the exact per-key arithmetic (get_index + Horner) of the vector loop bodies, and all per-knot call sites already clamp the key into the knot range before evaluating, so dropping the per-call validateKeys() (which never threw for in-range keys) changes nothing. The public getSplineInterpolatedPointAt entry point keeps the vector overload to preserve its validateKeys() out-of-range precondition. Public API change is additive only. Refs: autowarefoundation/autoware_core#1096

  • perf(autoware_interpolation): address review feedback

  - Guard scalar spline-eval overloads against un-built (default-constructed) splines: get_index() now throws std::runtime_error when base_keys_ has fewer than 2 knots, preventing std::clamp(lo>hi) UB and out-of-bounds reads. Add a unit test covering the un-built-spline path.

  - Restore validateKeys() endpoint cropping in the vector getters: iterate the validated (cropped) query keys instead of the raw query_keys, so near-boundary floating-point queries are clamped as before the refactor.

  - Reserve extended_s to target_n_knots in extendLinearlyForward() to avoid reallocations while appending extended knots. Refs: autowarefoundation/autoware_core#1096 ---------

• Contributors: Yutaka Kondo, github-actions

1.8.0 (2026-05-01)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base
• refactor(autoware_core): add USE_SCOPED_HEADER_INSTALL_DIR to common and testing packages (#967) Co-authored-by: github-actions <<github-actions@github.com>> Co-authored-by: Junya Sasaki <<j2sasaki1990@gmail.com>>
• fix(autoware_interpolation): fix bugprone-narrowing-conversions warnings (#935) Co-authored-by: Junya Sasaki <<j2sasaki1990@gmail.com>>
• Contributors: NorahXiong, Vishal Chauhan, github-actions

1.7.0 (2026-02-14)

• Merge remote-tracking branch 'origin/main' into humble

File truncated at 100 lines see the full file