Cubic Spline interpolation implemented by Opencv

1. Introduction to spline curves

The spline (Spline) is essentially a piecewise polynomial real function. In the range of real numbers, there are: S:[a,b]→R, and the interval [a,b] contains k sub-intervals [ti?1,ti], And there are:

a=t0

The existence polynomial corresponding to each interval i: Pi:[ti?1,ti]→R, and satisfies:

S(t)=P1(t) , t0≤t

Among them, Pi(t) is the power of the highest degree term in the polynomial, which is regarded as the order or degree of the spline (Order of spline). It is divided into uniform (Uniform) according to whether the interval length of the sub-interval [ti?1,ti] is consistent. Splines and non-uniform splines.

There are many polynomials that satisfy formula (2). In order to ensure that the curve has sufficient smoothness in the S interval, an n-th degree spline should have the properties of being continuous and differentiable everywhere:

P(j)i(ti)=P(j)i + 1(ti);(3)

Among them i=1,…,k?1;j=0,…,n?1.

2. Cubic Spline

2.1 Curve Conditions

According to the above definition, given a node:

t:z:a=t0z0

Cubic splines satisfy three conditions:

1. On each piecewise interval [ti,ti + 1],i=0,1,…,k?1, S(t)=Si(t) is a cubic polynomial;
2. Satisfy S(ti)=zi,i=1,…,k?1;
3. The first-order derivative function S′(t) and the second-order derivative function S′′(t) of S(t) are both continuous in the interval [a,b], so the curve has smoothness.

Then the equation of the cubic spline can be written as:

Si(t)=ai + bi(t?ti) + ci(t?ti)2 + di(t?ti)3,(5)

Among them, ai, bi, ci, di respectively represent n unknown coefficients.

• The continuity of the curve is expressed as:

Si(ti)=zi,(6)

Si(ti + 1)=zi + 1,(7)

where i=0,1,…,k?1.

• Curve differential continuity:

S′i(ti + 1)=S′i + 1(ti + 1),(8)

S′′i(ti + 1)=S′′i + 1(ti + 1),(9)

where i=0,1,…,k?2 .

• The derivative function expression of the curve:

S′i=bi + 2ci(t?ti) + 3di(t?ti)2,(10)

S′′i(x)=2ci + 6di(t?ti),(11)

Let the interval length hi=ti + 1?ti, then there is:

1. According to formula (6), it can be obtained: ai=zi;

2. From formula (7), it can be obtained: ai + bihi + cih2i + dih3i=zi + 1;

3. From formula (8), we can get:
   S′i(ti + 1)=bi + 2cihi + 3dih2i;
   S′i + 1(ti + 1)=bi + 1;
   ?bi + 2cihi + 3dih2i?bi + 1=0;

4. From formula (9), we can get:
   S′′i(ti + 1)=2ci + 6dihi;
   S′′i + 1(ti + 1)=2ci + 1;
   ?2ci + 6dihi=2ci + 1;

   Assume mi=S′′i(xi)=2ci, then:

   A. mi + 6dihi?mi + 1=0?
   di=mi + 1?mi6hi;

   B. Substitute ci,di into zi + bihi + cih2i + dih3i=zi + 1?
   bi=zi + 1?zihi?hi2mi?hi6(mi + 1?mi);

   C. Substitute bi, ci, di into bi + 2cihi + 3dih2i=bi + 1?

   himi + 2(hi + hi + 1)mi + 1 + hi + 1mi + 2=6[zi + 2?zi + 1hi + 1?zi + 1?zihi].(12)

2.2 Endpoint Conditions

In the above analysis, the two endpoints t0 and tk of the curve segment are not applicable. There are some commonly used endpoint restrictions. Only natural boundaries are explained here.
Under the natural boundary, the second-order derivative functions at the first and last ends satisfy S′′=0, that is, m0=0 and mk=0.

3. Implementation of cubic spline interpolation class

Header file

/*
  *Cubic spline interpolation class.
  *
*/

#ifndef CUBICSPLINEINTERPOLATION_H
#pragma once
#define CUBICSPLINEINTERPOLATION_H

#include <iostream>
#include <vector>
#include <math.h>

#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

/* Cubic spline interpolation coefficients */
class CubicSplineCoeffs
{
public:
    CubicSplineCoeffs( const int & amp;count )
    {
        a = std::vector<double>(count);
        b = std::vector<double>(count);
        c = std::vector<double>(count);
        d = std::vector<double>(count);
    }
    ~CubicSplineCoeffs()
    {
        std::vector<double>().swap(a);
        std::vector<double>().swap(b);
        std::vector<double>().swap(c);
        std::vector<double>().swap(d);
    }

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

enum CubicSplineMode
{
    CUBIC_NATURAL, // Natural
    CUBIC_CLAMPED, // TODO: Clamped
    CUBIC_NOT_A_KNOT // TODO: Not a knot
};

enum SplineFilterMode
{
    CUBIC_WITHOUT_FILTER, // without filter
    CUBIC_MEDIAN_FILTER // median filter
};

/* Cubic spline interpolation */
class CubicSplineInterpolation
{
public:
    CubicSplineInterpolation() {}
    ~CubicSplineInterpolation() {}

public:
    /*
        Calculate cubic spline coefficients.
          - node list x (input_x);
          - node list y (input_y);
          - output coefficients (cubicCoeffs);
          - ends mode (splineMode).
    */
    void calCubicSplineCoeffs( std::vector<double> & amp;input_x,
                std::vector<double> & amp;input_y, CubicSplineCoeffs * & amp;cubicCoeffs,
                CubicSplineMode splineMode = CUBIC_NATURAL,
                SplineFilterMode filterMode = CUBIC_MEDIAN_FILTER );

    /*
        Cubic spline interpolation for a list.
          - input coefficients (cubicCoeffs);
          - input node list x (input_x);
          - output node list x (output_x);
          - output node list y (output_y);
          - interpolation step (interStep).
    */
    void cubicSplineInterpolation( CubicSplineCoeffs * & amp;cubicCoeffs,
                std::vector<double> & amp;input_x, std::vector<double> & amp;output_x,
                std::vector<double> & amp;output_y, const double interStep = 0.5 );

    /*
        Cubic spline interpolation for a value.
          - input coefficients (cubicCoeffs);
          - input a value(x);
          - output interpolation value(y);
    */
    void cubicSplineInterpolation2( CubicSplineCoeffs * & amp;cubicCoeffs,
            std::vector<double> input_x, double x, double & amp;y );

    /*
        calculate tridiagonal matrices with Thomas Algorithm(TDMA) :

        example:
        | b1 c1 0 0 0 0 | |x1 | |d1 |
        | a2 b2 c2 0 0 0 | |x2 | |d2 |
        | 0 a3 b3 c3 0 0 | |x3 | = |d3 |
        | ... ... | |...| |...|
        | 0 0 0 0 an bn | |xn | |dn |

        Ci = ci/bi , i=1; ci / (bi - Ci-1 * ai) , i = 2, 3, ... n-1;
        Di = di/bi , i=1; ( di - Di-1 * ai )/(bi - Ci-1 * ai) , i = 2, 3, ..., n-1

        xi = Di - Ci*xi + 1 , i = n-1, n-2, 1;
    */
    bool caltridiagonalMatrices( cv::Mat_<double> & amp;input_a,
        cv::Mat_<double> & amp;input_b, cv::Mat_<double> & amp;input_c,
        cv::Mat_<double> & amp;input_d, cv::Mat_<double> & amp;output_x );

    /* Calculate the curve index interpolation belongs to */
    int calInterpolationIndex( double & amp;pt, std::vector<double> & amp;input_x );

    /* median filtering */
    void cubicMedianFilter( std::vector<double> & amp;input, const int filterSize = 5);

    double cubicSort( std::vector<double> & amp;input );
    // double cubicNearestValue( std::vector );
};

#endif // CUBICSPLINEINTERPOLATION_H

Implementation file (cpp)

/*
 * CubicSplineInterpolation.cpp
 */

#include "cubicsplineinterpolation.h"

void CubicSplineInterpolation::calCubicSplineCoeffs(
    std::vector<double> & amp;input_x,
    std::vector<double> & amp;input_y,
    CubicSplineCoeffs * & amp;cubicCoeffs,
    CubicSplineMode splineMode /* = CUBIC_NATURAL */,
    SplineFilterMode filterMode /*= CUBIC_MEDIAN_FILTER*/ )
{
    int sizeOfx = input_x.size();
    int sizeOfy = input_y.size();

    if ( sizeOfx != sizeOfy )
    {
        std::cout << "Data input error!" << std::endl <<
            "Location: CubicSplineInterpolation.cpp" <<
            " -> calCubicSplineCoeffs()" << std::endl;

        return;
    }

    /*
        hi*mi + 2*(hi + hi + 1)*mi + 1 + hi + 1*mi + 2
        = 6{ (yi + 2 - yi + 1)/hi + 1 - (yi + 1 - yi)/hi }

        so, ignore the both ends:
        | - - - 0 ... 0 | |m0 |
        | h0 2(h0 + h1) h1 0 ... 0 | |m1 |
        | 0 h1 2(h1 + h2) h2 0 ... | |m2 |
        | ... ... 0 | |...|
        | 0 ... 0 h(n-2) 2(h(n-2) + h(n-1)) h(n-1) | | |
        | 0 ... ... - | |mn |

    */

    std::vector<double> copy_y = input_y;

    if ( filterMode == CUBIC_MEDIAN_FILTER )
    {
        cubicMedianFilter(copy_y, 5);
    }

    const int count = sizeOfx;
    const int count1 = sizeOfx - 1;
    const int count2 = sizeOfx - 2;
    const int count3 = sizeOfx - 3;

    cubicCoeffs = new CubicSplineCoeffs( count1 );

    std::vector<double> step_h( count1, 0.0 );

    // for m matrix
    cv::Mat_<double> m_a(1, count2, 0.0);
    cv::Mat_<double> m_b(1, count2, 0.0);
    cv::Mat_<double> m_c(1, count2, 0.0);
    cv::Mat_<double> m_d(1, count2, 0.0);
    cv::Mat_<double> m_part(1, count2, 0.0);

    cv::Mat_<double> m_all(1, count, 0.0);

    // initial step hi
    for (int idx=0; idx < count1; idx + + )
    {
        step_h[idx] = input_x[idx + 1] - input_x[idx];
    }
    // initial coefficients
    for (int idx=0; idx < count3; idx + + )
    {
        m_a(idx) = step_h[idx];
        m_b(idx) = 2 * (step_h[idx] + step_h[idx + 1]);
        m_c(idx) = step_h[idx + 1];
    }
    // initial d
    for (int idx =0; idx < count3; idx + + )
    {
        m_d(idx) = 6 * (
            (copy_y[idx + 2] - copy_y[idx + 1]) / step_h[idx + 1] -
            (copy_y[idx + 1] - copy_y[idx]) / step_h[idx] );
    }

     //cv::Mat_<double> matOfm( count2, )
    bool isSucceed = caltridiagonalMatrices(m_a, m_b, m_c, m_d, m_part);
    if (!isSucceed)
    {
        std::cout<<"Calculate tridiagonal matrices failed!"<<std::endl<<
            "Location: CubicSplineInterpolation.cpp -> " <<
            "caltridiagonalMatrices()"<<std::endl;

        return;
    }

    if ( splineMode == CUBIC_NATURAL )
    {
        m_all(0) = 0.0;
        m_all(count1) = 0.0;

        for ( int i=1; i<count1; i + + )
        {
            m_all(i) = m_part(i-1);
        }

        for ( int i=0; i<count1; i + + )
        {
            cubicCoeffs->a[i] = copy_y[i];
            cubicCoeffs->b[i] = ( copy_y[i + 1] - copy_y[i] ) / step_h[i] -
                step_h[i]*( 2*m_all(i) + m_all(i + 1) ) / 6;
            cubicCoeffs->c[i] = m_all(i) / 2.0;
            cubicCoeffs->d[i] = ( m_all(i + 1) - m_all(i) ) / ( 6.0 * step_h[i] );
        }
    }
    else
    {
        std::cout<<"Not define the interpolation mode!"<<std::endl;
    }
}

void CubicSplineInterpolation::cubicSplineInterpolation(
    CubicSplineCoeffs * & amp;cubicCoeffs,
    std::vector<double> & amp;input_x,
    std::vector<double> & amp;output_x,
    std::vector<double> & amp;output_y,
    const double interStep )
{
    const int count = input_x.size();

    double low = input_x[0];
    double high = input_x[count-1];

    double interBegin = low;
    for ( ; interBegin < high; interBegin + = interStep )
    {
        int index = calInterpolationIndex(interBegin, input_x);
        if ( index >= 0 )
        {
            double dertx = interBegin - input_x[index];
            double y = cubicCoeffs->a[index] + cubicCoeffs->b[index] * dertx +
                cubicCoeffs->c[index] * dertx * dertx +
                cubicCoeffs->d[index] * dertx * dertx * dertx;
            output_x.push_back(interBegin);
            output_y.push_back(y);
        }
    }
}

void CubicSplineInterpolation::cubicSplineInterpolation2(
    CubicSplineCoeffs * & amp;cubicCoeffs,
    std::vector<double> input_x, double x, double & amp;y)
{
    const int count = input_x.size();

    double low = input_x[0];
    double high = input_x[count-1];

    if ( x<low || x>high )
    {
        std::cout<<"The interpolation value is out of range!"<<std::endl;
    }
    else
    {
        int index = calInterpolationIndex(x, input_x);
        if ( index >= 0 )
        {
            double dertx = x - input_x[index];
            y = cubicCoeffs->a[index] + cubicCoeffs->b[index] * dertx +
                cubicCoeffs->c[index] * dertx * dertx +
                cubicCoeffs->d[index] * dertx * dertx * dertx;
        }
        else
        {
            std::cout<<"Can't find the interpolation range!"<<std::endl;
        }
    }
}

bool CubicSplineInterpolation::caltridiagonalMatrices(
    cv::Mat_<double> & amp;input_a,
    cv::Mat_<double> & amp;input_b,
    cv::Mat_<double> & amp;input_c,
    cv::Mat_<double> & amp;input_d,
    cv::Mat_<double> & amp;output_x )
{
    int rows = input_a.rows;
    int cols = input_a.cols;

    if ( ( rows == 1 & amp; & amp; cols > rows ) ||
        (cols == 1 & amp; & amp; rows > cols ) )
    {
        const int count = ( rows > cols ? rows : cols ) - 1;

        output_x = cv::Mat_<double>::zeros(rows, cols);

        cv::Mat_<double> cCopy, dCopy;
        input_c.copyTo(cCopy);
        input_d.copyTo(dCopy);

        if ( input_b(0) != 0 )
        {
            cCopy(0) /= input_b(0);
            dCopy(0) /= input_b(0);
        }
        else
        {
            return false;
        }

        for ( int i=1; i < count; i + + )
        {
            double temp = input_b(i) - input_a(i) * cCopy(i-1);
            if(temp==0.0)
            {
                return false;
            }

            cCopy(i) /= temp;
            dCopy(i) = ( dCopy(i) - dCopy(i-1)*input_a(i) ) / temp;
        }

        output_x(count) = dCopy(count);
        for (int i=count-2; i > 0; i--)
        {
            output_x(i) = dCopy(i) - cCopy(i)*output_x(i + 1);
        }
        return true;
    }
    else
    {
        return false;
    }
}

int CubicSplineInterpolation::calInterpolationIndex(
    double & amp;pt, std::vector<double> & amp;input_x )
{
    const int count = input_x.size()-1;
    int index = -1;
    for ( int i=0; i<count; i + + )
    {
        if ( pt > input_x[i] & amp; & pt <= input_x[i + 1] )
        {
            index = i;
            return index;
        }
    }
    return index;
}

void CubicSplineInterpolation::cubicMedianFilter(
    std::vector<double> & amp;input, const int filterSize /* = 5 */ )
{
    const int count = input.size();
    for ( int i=filterSize/2; i<count-filterSize/2; i + + )
    {
        std::vector<double> temp(filterSize, 0.0);
        for ( int j=0; j<filterSize; j + + )
        {
            temp[j] = input[i + j - filterSize/2];
        }

        input[i] = cubicSort(temp);

        std::vector<double>().swap(temp);
    }

    for ( int i=0; i<filterSize/2; i + + )
    {
        std::vector<double> temp(filterSize, 0.0);
        for ( int j=0; j<filterSize; j + + )
        {
            temp[j] = input[j];
        }

        input[i] = cubicSort(temp);
        std::vector<double>().swap(temp);
    }

    for (int i=count-filterSize/2; i<count; i + + )
    {
        std::vector<double> temp(filterSize, 0.0);
        for ( int j=0; j<filterSize; j + + )
        {
            temp[j] = input[j];
        }

        input[i] = cubicSort(temp);
        std::vector<double>().swap(temp);
    }
}

double CubicSplineInterpolation::cubicSort( std::vector<double> & amp;input )
{
    int iCount = input.size();
    for (int j=0; j<iCount-1; j + + )
    {
        for ( int k=iCount-1; k>j; k-- )
        {
            if ( input[k-1] > input[k] )
            {
                double tp = input[k];
                input[k] = input[k-1];
                input[k-1] = tp;
            }
        }
    }
    return input[iCount/2];
}

The knowledge points of the article match the official knowledge files, and you can further learn related knowledge. OpenCV skill tree Home page Overview 23830 people are learning the system