robotics : yyRW5

Spline Interpolation

The mathematics and programming for interpolating three dimensional polynomials of degree 1, 3 and 5.

Jan 25, 2024
Markdown
Updated Feb 3, 2024

Introduction

A spline is a function which is defined piecewise by polynomials that interpolate through a set of points. The degree \(d\) of the polynomials determines the overall continuity \(C^{d-1}\) of the spline. When working in three dimensions, the set of points are three dimensional vectors. Each sub-function in the spline is a three dimensional vector function where each coordinate \(x\), \(y\) and \(z\) has a corresponding polynomial of degree \(d\).

A set of \(n\) points needs \(n-1\) sub-functions in order to create a spline which interpolates through all of the points. The degrees of splines which will be covered here are \(d \in \{1, 3, 5\}\). The main reason to why these three is simple. For \(d=1\) the spline is essentially drawing straight lines between each point which has its specific usecase. For \(d=3\) you get a sufficiently smooth curve that can be used as the default interpolation degree. You might in some cases want more smoothness which is why \(d=5\) is implemented, more than \(5\) and numerical instabilities might occur. All of the degrees are odd since even degrees would cause the amount of constraints to be unbalanced. This will become more as you read the post.

As usual, all code presented here is in ANSI C, this post will have a lot more hidden functions such as vector operations. Visit the github repository github.com/fullnitrous/arm for more details. All relevant code is found under /src/core/.

Parameterization

The first step before calculating the coefficients for each polynomial for each sub-function, is to parameterize the points. Parameterization meaning to assign a value \(t_i\) to each point \(\vec{p_i}\). That way you have intervals \([t_i, t_{i+1}]\) which correspond to one sub-function.

For \(n\) three-dimensional points.

\[ P = \{\vec{p_0}, \space ... \space ,\vec{p_{n-1}}\} \]

Where.

\[ \vec{p_i} = \begin{bmatrix} p_{i,x} \\ p_{i,y} \\ p_{i,z} \end{bmatrix} \]

Then the parameter values.

\[ T = \{t_0, \space, ..., \space t_{n-1}\} \]

Are calculated with the normalized chord length between each point.

\[ \begin{aligned} S &= \sum_{i=0}^{n-1} | \vec{p_i} - \vec{p_{i-1}} | \\ t_0 &= 0 \\ t_{n-1} &= 1 \\ t_i &= \frac{|\vec{p_i} - \vec{p_{i-1}}|}{S} + t_{i-1}, \quad i = 1, \space ..., n-2 \end{aligned} \]

Note that this is not the only way to parameterize the points, but it is the method chosen. The parameterization step is required for all degrees.

Programming the parameterization function is simple. The function intrpl_parameterize requires the n points p. The parameter outputs are stored in the array u.

Note just for clarity — the parameter values \(t_i\) always start at \(0\) and end at \(1\).

Linear Spline

The linear spline interpolation function, or in other words, a spline with polynomials of degree \(1\), looks like the following.

\[ L(t) = \begin{cases} l_0(t) & t \in [t_0, t_1] \\ \vdots & \vdots \\ l_{n-2}(t) & t \in (t_{n-2}, t_{n-1}] \end{cases} \]

The function requires that \(n \geq 2\), where each sub-function looks like the following.

\[ l_i(t) = \begin{bmatrix} a_{i,x} \\ a_{i,y} \\ a_{i,z} \\ \end{bmatrix}t + \begin{bmatrix} b_{i,x} \\ b_{i,y} \\ b_{i,z} \\ \end{bmatrix} \]

I decided to make one type ppoly_t which covers all three types of splines. The struct has the array of coefficients c whichs structure depends on what type of spline it is. The array u is simply the parameter values and n is the length of the arrays.

For each of the types of splines, there is an initialization function which returns the corresponding ppoly_t;

The initialization functions do not call malloc or anything that would require a syscall. Therefore there are macros that define how much memory each array requires. In the case of a linear spline there is only one.

For the coefficient array. The parameter array is always just an array of doubles with length n.

Interpolation

The cofficients \(\vec{a_i}\) and \(\vec{b_i}\) are defined by solving the equation system that follows from the positional constraints for each sub-function.

\[ \begin{aligned} l_i(t_i) &= \vec{p_i} \\ l_i(t_{i+1}) &= \vec{p_{i+1}} \end{aligned} \quad i = 0, \space ... ,\space n-2 \]

Since the equations are not dependent on eachother, the coefficients can be calculated directly by.

\[ \begin{aligned} \vec{a_{i}} &= \frac{\vec{p_{i+1}} - \vec{p_{i}}}{t_{i+1}-t_i} \\ \vec{b_i} &= \vec{p_i} - \vec{a_i}t_i \end{aligned} \quad i = 0, \space ... \space, n-2 \]

Linear spline interpolation through 12 points, x(t)=t, y(t)=sin(3t), z(t)=cos(3t), t \in [0, \pi].
Linear spline interpolation through 12 points, \(x(t)=t\), \(y(t)=sin(3t)\), \(z(t)=cos(3t)\), \(t \in [0, \pi]\).

Evaluation

The evaluation function eval_ppoly takes in the function f which in this case is a a ppoly_t initialized as a linear spline. And of course the parameter t.

This is a wrapper function for further generalization which will be required later on. The function first calculates which index in the cofficient array to use k via the eval_findinterval function.

This function is essentially just binary search on the array u with length n searching for parameter t. Binary search is possible since the parameter array is strictly increasing therefore ordered.

Once the index k has been found, the eval_ppoly returns the return value of the actual evaluation function __eval_poly which requires the function f index k and parameter t.

The return value of eval_ppoly1 and __eval_ppoly1 is not just the \((x, y, z)\) values but the first and second derivative as well. In addition to that, the magnitude of the first derivative.

These values will be used by other algorithms which I will cover in later posts.

However, there is a __eval_ppoly_fast function.

Designed to execute as fast as possible, when only the position is required. The usecase for this is on the embedded side of things.

Cubic Spline

The cubic spline interpolation function looks like the following.

\[ C(t) = \begin{cases} c_0(t) & t \in [t_0, t_1] \\ \vdots & \vdots \\ c_{n-2}(t) & t \in (t_{n-2}, t_{n-1}] \end{cases} \]

The function requires that \(n \geq 2\), where each sub-function looks like the following.

\[ c_i(t) = \begin{bmatrix} a_{i,x} \\ a_{i,y} \\ a_{i,z} \\ \end{bmatrix}t^3 + \begin{bmatrix} b_{i,x} \\ b_{i,y} \\ b_{i,z} \\ \end{bmatrix}t^2 + \begin{bmatrix} c_{i,x} \\ c_{i,y} \\ c_{i,z} \\ \end{bmatrix}t + \begin{bmatrix} d_{i,x} \\ d_{i,y} \\ d_{i,z} \\ \end{bmatrix} \]

Interpolation

The coefficients \(\vec{a_i}\), \(\vec{b_i}\), \(\vec{c_i}\) and \(\vec{d_i}\) are calculated by solving the equation system that follows from the positional, first and second derivative equivalence constraints.

\[ \begin{aligned} c_i(t_i) &= \vec{p_i} \\ c_i(t_{i+1}) &= \vec{p_{i+1}} \end{aligned} \quad i = 0, \space ... \space n-2 \]

\[ \begin{aligned} c'_i(t_{i+1}) &= c'_{i+1}(t_{i+1}) \\ c''_i(t_{i+1}) &= c''_{i+1}(t_{i+1}) \end{aligned} \quad i = 0, \space ... \space n-3 \]

In addition to the positional, first and second derivative constraints, two more equations are required to form a linear system of equations. In other words, the spline still has some degrees of freedom after the previous constraints. What you wish to further constrain with is an arbitrary choice, however usually, you set the second derivative of the first and last sub-functions to zero in order for the spline to be considered a “natural spline”.

\[ \begin{aligned} c''_0(t_0) &= \vec{0} \\ c''_{n-2}(t_{n-1}) &= \vec{0} \end{aligned} \]

The constraints for interpolating a cubic spline are dependent on eachother since the derivatives of the sub-functions are the same where they connect. Therefore, one must solve a linear system of equations.

Since the splines will interpolate through thee-dimensional points, the equation system needs to be solved for three different right hand sides. If one were to use Gauss-Jordan elimination then three computations of complexity \(\mathcal{O}(n^3)\) would be required. However, with LU decomposition, one decomposition step of complexity \(\mathcal{O}(n^3)\) followed by three \(\mathcal{O}(n^2)\) for solving each right hand side would be required. Essentially, LU decomposition is faster when you solve the same equation system for multiple right hand sides.

The algorithm for LU decomposition is a modified version of the algorithm found in the article “An efficient implementation of LU decomposition in C”, the article can be found at ScienceDirect.

Firstly, a structure containing the context for the decomposition is required along side a helper macro APPROACHES_ZERO.

The struct has size n of the matrix, pivot diagonal p of length n, equation system matrix m of size n*n and right hand side b of length n.

The function mat_lud decomposes matrix m and saves the decomposed matrix back to m.

Once the matrix has been decomposed, the function mat_lusolve can be used by setting the b right hand side in the context then calling the function to get the answers saved back into b.

The following macros define the sizes for the coefficient array and the arrays in luctx_t.

These are used to allocate the required memory then initialize ppoly_t and luctx_t. The initialization for ppoly_t is the same as for the linear spline, however luctx_t has its own initialization function eval_ppoly_luctx.

Once the structs are initialized, the function intrpl_ppoly3 is used to interpolate though the points p of length n. Optionally the initial and final velocity v0 and v1 can be set. For the spline to be “natural” these parameters are just set to NULL. The function also requires the decomposition context ctx and saves the spline to ppoly.

The macro VEC3GET gets the \((x, y, z)\) components in a vec3_t based on index.

The intrpl_ppoly3 function depends on yet another function intrpl_spline_eqsys which generates the equation system to be solved. This function needs the parameter array u, degree d, number of points n and saves the equation system to matrix matrix. The return value is the size of the matrix.

Cubic spline interpolation through 12 points, x(t)=t, y(t)=sin(3t), z(t)=cos(3t), t \in [0, \pi]. The spline is “natural”.
Cubic spline interpolation through 12 points, \(x(t)=t\), \(y(t)=sin(3t)\), \(z(t)=cos(3t)\), \(t \in [0, \pi]\). The spline is “natural”.
Cubic spline interpolation through 12 points, x(t)=t, y(t)=sin(3t), z(t)=cos(3t), t \in [0, \pi]. With initial and final derivatives set to (0, 0, -30).
Cubic spline interpolation through 12 points, \(x(t)=t\), \(y(t)=sin(3t)\), \(z(t)=cos(3t)\), \(t \in [0, \pi]\). With initial and final derivatives set to \((0, 0, -30)\).

Evaluation

The family of functions for evaluation is pretty much the same as for the linear spline, just with more coefficients.

Quintic Spline

The quintic spline interpolation function looks like the following.

\[ Q(t) = \begin{cases} q_0(t) & t \in [t_0, t_1] \\ \vdots & \vdots \\ q_{n-2}(t) & t \in (t_{n-2}, t_{n-1}] \end{cases} \]

The function requires that \(n \geq 2\), where each sub-function looks like the following.

\[ q_i(t) = \begin{bmatrix} a_{i,x} \\ a_{i,y} \\ a_{i,z} \\ \end{bmatrix}t^5 + \begin{bmatrix} b_{i,x} \\ b_{i,y} \\ b_{i,z} \\ \end{bmatrix}t^4 + \begin{bmatrix} c_{i,x} \\ c_{i,y} \\ c_{i,z} \\ \end{bmatrix}t^3 + \begin{bmatrix} d_{i,x} \\ d_{i,y} \\ d_{i,z} \\ \end{bmatrix}t^2 + \begin{bmatrix} e_{i,x} \\ e_{i,y} \\ e_{i,z} \\ \end{bmatrix}t + \begin{bmatrix} f_{i,x} \\ f_{i,y} \\ f_{i,z} \\ \end{bmatrix} \]

Interpolation

The cofficients \(\vec{a_i}\), \(\vec{b_i}\), \(\vec{c_i}\), \(\vec{d_i}\), \(\vec{e_i}\) and \(\vec{f_i}\) are calculated by solving the equation system that follows from the positional, first, second and third derivative equivalence constraints.

\[ \begin{aligned} q_i(t_i) &= \vec{p_i} \\ q_i(t_{i+1}) &= \vec{p_{i+1}} \end{aligned} \quad i = 0, \space ... \space , n-2 \]

\[ \begin{aligned} q'_i(t_{i+1}) &= q'_{i+1}(t_{i+1}) \\ q''_i(t_{i+1}) &= q''_{i+1}(t_{i+1}) \\ q'''_i(t_{i+1}) &= q''_{i+1}(t_{i+1}) \\ q''''_i(t_{i+1}) &= q''''_{i+1}(t_{i+1}) \\ \end{aligned} \quad i = 0, \space ... \space n-3 \]

As with cubic splines, a couple more equations are required to fully constrain everything such that a linear system of equation is formed. In the case of a quintic spline, four more equations are required. Unless something else, the second and third derivatives are set to zero.

\[ \begin{aligned} q''_0(t_0) &= \vec{0} \\ q'''_0(t_0) &= \vec{0} \\ q''_{n-2}(t_{n-1}) &= \vec{0} \\ q'''_{n-2}(t_{n-1}) &= \vec{0} \end{aligned} \]

In terms of code, interpolating a quintic spline is pretty much identical to the cubic spline. Just with the option to set the initial and final acceleration a0 and a1 due to it having more degrees of freedom. The lengths of the arrays for the coefficient array and luctx_t are also different.

int intrpl_ppoly5(vec3_t* p, int n, vec3_t* v0, vec3_t* v1, vec3_t* a0, vec3_t* a1, luctx_t* ctx, ppoly_t* ppoly) {
    vec3_t* c;  
    double* u;
    int i, sz, component;
    
    if(n < 2) { return 0; }

    c = ppoly->c;
    u = ppoly->u;

    intrpl_parameterize(p, n, u); 

    sz = intrpl_spline_eqsys(u, 5, n, ctx->m);
    ctx->n = sz; 

    if(v0 == NULL) {
        ctx->m[sz*(sz-4) + 0] = 60*u[0]*u[0];
        ctx->m[sz*(sz-4) + 1] = 24*u[0];
        ctx->m[sz*(sz-4) + 2] = 6;
    } else {
        ctx->m[sz*(sz-4) + 0] = 5*u[0]*u[0]*u[0]*u[0];
        ctx->m[sz*(sz-4) + 1] = 4*u[0]*u[0]*u[0];
        ctx->m[sz*(sz-4) + 2] = 3*u[0]*u[0];
        ctx->m[sz*(sz-4) + 3] = 2*u[0];
        ctx->m[sz*(sz-4) + 4] = 1;
    }   

    ctx->m[sz*(sz-3) + 0] = 20*u[0]*u[0]*u[0];
    ctx->m[sz*(sz-3) + 1] = 12*u[0]*u[0];
    ctx->m[sz*(sz-3) + 2] = 6*u[0];
    ctx->m[sz*(sz-3) + 3] = 2;

    if(v1 == NULL) {
        ctx->m[sz*(sz-2) + sz-6] = 60*u[n-1]*u[n-1];
        ctx->m[sz*(sz-2) + sz-5] = 24*u[n-1];
        ctx->m[sz*(sz-2) + sz-4] = 6;
    } else {
        ctx->m[sz*(sz-2) + sz-6] = 5*u[n-1]*u[n-1]*u[n-1]*u[n-1];
        ctx->m[sz*(sz-2) + sz-5] = 4*u[n-1]*u[n-1]*u[n-1];
        ctx->m[sz*(sz-2) + sz-4] = 3*u[n-1]*u[n-1];
        ctx->m[sz*(sz-2) + sz-3] = 2*u[n-1];
        ctx->m[sz*(sz-2) + sz-2] = 1;
    }   

    ctx->m[sz*(sz-1) + sz-6] = 20*u[n-1]*u[n-1]*u[n-1];
    ctx->m[sz*(sz-1) + sz-5] = 12*u[n-1]*u[n-1];
    ctx->m[sz*(sz-1) + sz-4] = 6*u[n-1];
    ctx->m[sz*(sz-1) + sz-3] = 2;

    if(!mat_lud(ctx)) { return 0; }    

    for(component = 0; component < 3; component++) {
        for(i = 0; i < sz; i++) { ctx->b[i] = 0.0; }
        for(i = 0; i < n-1; i++) {
            ctx->b[2*i + 0] = *VEC3GET(p+i+0, component);
            ctx->b[2*i + 1] = *VEC3GET(p+i+1, component);
        }
    
        if(v0 != NULL) { ctx->b[sz-4] = *VEC3GET(v0, component); }
        if(a0 != NULL) { ctx->b[sz-3] = *VEC3GET(a0, component); }
        if(v1 != NULL) { ctx->b[sz-2] = *VEC3GET(v1, component); }
        if(a1 != NULL) { ctx->b[sz-1] = *VEC3GET(a1, component); }

        mat_lusolve(ctx);

        for(i = 0; i < sz; i++) {
            *VEC3GET(c+i, component) = ctx->b[i];
        }
    }   

    return 1;
}
Quintic spline interpolation through 12 points, x(t)=t, y(t)=sin(3t), z(t)=cos(3t), t \in [0, \pi]
Quintic spline interpolation through 12 points, \(x(t)=t\), \(y(t)=sin(3t)\), \(z(t)=cos(3t)\), \(t \in [0, \pi]\)
Quintic spline interpolation through 12 points, x(t)=t, y(t)=sin(3t), z(t)=cos(3t), t \in [0, \pi]. With initial and final derivatives set to (0, 0, -30).
Quintic spline interpolation through 12 points, \(x(t)=t\), \(y(t)=sin(3t)\), \(z(t)=cos(3t)\), \(t \in [0, \pi]\). With initial and final derivatives set to \((0, 0, -30)\).

Evaluation

Once again, the family of functions for evaluation is pretty much the same as for the linear and cubic spline, but just with more coefficients.