• Opencv实现的三次样条曲线(Cubic Spline)插值

    1.样条曲线简介

    样条曲线(Spline)本质是分段多项式实函数，在实数范围内有： S:[a,b]→R ，在区间 [a,b] 上包含 k 个子区间[ti−1,ti]，且有：

    a=t0

    对应每一段区间 i 的存在多项式： Pi:[ti−1,ti]→R，且满足于：

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

    其中， Pi(t) 多项式中最高次项的幂，视为样条的阶数或次数（Order of spline），根据子区间 [ti−1,ti] 的区间长度是否一致分为均匀（Uniform）样条和非均匀（Non-uniform）样条。

    满足了公式 (2) 的多项式有很多，为了保证曲线在 S 区间内具有据够的平滑度，一条n次样条，同时应具备处处连续且可微的性质：

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

    其中 i=1,…,k−1;j=0,…,n−1 。

    2.三次样条曲线

    2.1曲线条件

    按照上述的定义，给定节点：

    t:z:a=t0z0

    三次样条曲线满足三个条件：

    1. 在每段分段区间 [ti,ti+1],i=0,1,…,k−1 上， S(t)=Si(t) 都是一个三次多项式；
    2. 满足 S(ti)=zi,i=1,…,k−1 ;
    3. S(t) 的一阶导函数 S′(t) 和二阶导函数 S′′(t) 在区间 [a,b] 上都是连续的，从而曲线具有光滑性。

    则三次样条的方程可以写为：

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

    其中， ai,bi,ci,di 分别代表 n 个未知系数。

    • 曲线的连续性表示为：

    Si(ti)=zi,(6)

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

    其中 i=0,1,…,k−1 。

    • 曲线微分连续性：

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

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

    其中 i=0,1,…,k−2 。

    • 曲线的导函数表达式：

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

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

    令区间长度 hi=ti+1−ti ，则有：

    1. 由公式 (6) ，可得： ai=zi ；

    2. 由公式 (7) ，可得： ai+bihi+cih2i+dih3i=zi+1 ；

    3. 由公式 (8) ，可得：
      S′i(ti+1)=bi+2cihi+3dih2i ;
      S′i+1(ti+1)=bi+1 ；
      ⇒bi+2cihi+3dih2i−bi+1=0 ；

    4. 由公式 (9) ，可得：
      S′′i(ti+1)=2ci+6dihi ；
      S′′i+1(ti+1)=2ci+1 ；
      ⇒2ci+6dihi=2ci+1 ；

      设 mi=S′′i(xi)=2ci ，则：

      A. mi+6dihi−mi+1=0⇒
      di=mi+1−mi6hi ；

      B.将 ci,di 代入 zi+bihi+cih2i+dih3i=zi+1⇒
      bi=zi+1−zihi−hi2mi−hi6(mi+1−mi) ；

      C.将 bi,ci,di 代入 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端点条件

    在上述分析中，曲线段的两个端点 t0 和 tk 是不适用的，有一些常用的端点限制条件，这里只讲解自然边界。
    在自然边界下，首尾两端的二阶导函数满足 S′′=0 ，即 m0=0 和 mk=0 。

    3.三次样条插值类的实现

    头文件
    1. /*
    2.   *Cubic spline interpolation class.
    3.   *
    4. */
    5.  
    6. #ifndef CUBICSPLINEINTERPOLATION_H
    7. #pragma once
    8. #define CUBICSPLINEINTERPOLATION_H
    9.  
    10. #include <iostream>
    11. #include <vector>
    12. #include <math.h>
    13.  
    14. #include <opencv2/opencv.hpp>
    15.  
    16. using namespace std;
    17. using namespace cv;
    18.  
    19. /* Cubic spline interpolation coefficients */
    20. class CubicSplineCoeffs
    21. {
    22. public:
    23.     CubicSplineCoeffs( const int &count )
    24.     {
    25.         a = std::vector<double>(count);
    26.         b = std::vector<double>(count);
    27.         c = std::vector<double>(count);
    28.         d = std::vector<double>(count);
    29.     }
    30.     ~CubicSplineCoeffs()
    31.     {
    32.         std::vector<double>().swap(a);
    33.         std::vector<double>().swap(b);
    34.         std::vector<double>().swap(c);
    35.         std::vector<double>().swap(d);
    36.     }
    37.  
    38. public:
    39.     std::vector<double> a, b, c, d;
    40. };
    41.  
    42. enum CubicSplineMode
    43. {
    44.     CUBIC_NATURAL,    // Natural
    45.     CUBIC_CLAMPED,    // TODO: Clamped
    46.     CUBIC_NOT_A_KNOT  // TODO: Not a knot
    47. };
    48.  
    49. enum SplineFilterMode
    50. {
    51.     CUBIC_WITHOUT_FILTER, // without filter
    52.     CUBIC_MEDIAN_FILTER  // median filter
    53. };
    54.  
    55. /* Cubic spline interpolation */
    56. class CubicSplineInterpolation
    57. {
    58. public:
    59.     CubicSplineInterpolation() {}
    60.     ~CubicSplineInterpolation() {}
    61.  
    62. public:
    63.     /*
    64.         Calculate cubic spline coefficients.
    65.           - node list x (input_x);
    66.           - node list y (input_y);
    67.           - output coefficients (cubicCoeffs);
    68.           - ends mode (splineMode).
    69.     */
    70.     void calCubicSplineCoeffs( std::vector<double> &input_x,
    71.                 std::vector<double> &input_y, CubicSplineCoeffs *&cubicCoeffs,
    72.                 CubicSplineMode splineMode = CUBIC_NATURAL,
    73.                 SplineFilterMode filterMode = CUBIC_MEDIAN_FILTER );
    74.  
    75.     /*
    76.         Cubic spline interpolation for a list.
    77.           - input coefficients (cubicCoeffs);
    78.           - input node list x (input_x);
    79.           - output node list x (output_x);
    80.           - output node list y (output_y);
    81.           - interpolation step (interStep).
    82.     */
    83.     void cubicSplineInterpolation( CubicSplineCoeffs *&cubicCoeffs,
    84.                 std::vector<double> &input_x, std::vector<double> &output_x,
    85.                 std::vector<double> &output_y, const double interStep = 0.5 );
    86.  
    87.     /*
    88.         Cubic spline interpolation for a value.
    89.           - input coefficients (cubicCoeffs);
    90.           - input a value(x);
    91.           - output interpolation value(y);
    92.     */
    93.     void cubicSplineInterpolation2( CubicSplineCoeffs *&cubicCoeffs,
    94.             std::vector<double> input_x, double x, double &y );
    95.  
    96.     /*
    97.         calculate  tridiagonal matrices with Thomas Algorithm(TDMA) :
    98.  
    99.         example:
    100.         | b1 c1 0  0  0  0  |  |x1 |   |d1 |
    101.         | a2 b2 c2 0  0  0  |  |x2 |   |d2 |
    102.         | 0  a3 b3 c3 0  0  |  |x3 | = |d3 |
    103.         | ...         ...   |  |...|   |...|
    104.         | 0  0  0  0  an bn |  |xn |   |dn |
    105.  
    106.         Ci = ci/bi , i=1; ci / (bi - Ci-1 * ai) , i = 2, 3, ... n-1;
    107.         Di = di/bi , i=1; ( di - Di-1 * ai )/(bi - Ci-1 * ai) , i = 2, 3, ..., n-1
    108.  
    109.         xi = Di - Ci*xi+1 , i = n-1, n-2, 1;
    110.     */
    111.     bool caltridiagonalMatrices( cv::Mat_<double> &input_a,
    112.         cv::Mat_<double> &input_b, cv::Mat_<double> &input_c,
    113.         cv::Mat_<double> &input_d, cv::Mat_<double> &output_x );
    114.  
    115.     /* Calculate the curve index interpolation belongs to */
    116.     int calInterpolationIndex( double &pt, std::vector<double> &input_x );
    117.  
    118.     /* median filtering */
    119.     void cubicMedianFilter( std::vector<double> &input, const int filterSize = 5 );
    120.  
    121.     double cubicSort( std::vector<double> &input );
    122.     // double cubicNearestValue( std::vector );
    123. };
    124.  
    125. #endif // CUBICSPLINEINTERPOLATION_H
    实现文件（cpp）
    1. /*
    2.  * CubicSplineInterpolation.cpp
    3.  */
    5. #include "cubicsplineinterpolation.h"
    6.  
    7. void CubicSplineInterpolation::calCubicSplineCoeffs(
    8.     std::vector<double> &input_x,
    9.     std::vector<double> &input_y,
    10.     CubicSplineCoeffs *&cubicCoeffs,
    11.     CubicSplineMode splineMode /* = CUBIC_NATURAL */,
    12.     SplineFilterMode filterMode /*= CUBIC_MEDIAN_FILTER*/ )
    13. {
    14.     int sizeOfx = input_x.size();
    15.     int sizeOfy = input_y.size();
    16.  
    17.     if ( sizeOfx != sizeOfy )
    18.     {
    19.         std::cout << "Data input error!" << std::endl <<
    20.             "Location: CubicSplineInterpolation.cpp" <<
    21.             " -> calCubicSplineCoeffs()" << std::endl;
    22.  
    23.         return;
    24.     }
    25.  
    26.     /*
    27.         hi*mi + 2*(hi + hi+1)*mi+1 + hi+1*mi+2
    28.         =  6{ (yi+2 - yi+1)/hi+1 - (yi+1 - yi)/hi }
    29.  
    30.         so, ignore the both ends:
    31.         | -     -     -        0           ...             0     |  |m0 |
    32.         | h0 2(h0+h1) h1       0           ...             0     |  |m1 |
    33.         | 0     h1    2(h1+h2) h2 0        ...                   |  |m2 |
    34.         |         ...                      ...             0     |  |...|
    35.         | 0       ...           0 h(n-2) 2(h(n-2)+h(n-1)) h(n-1) |  |   |
    36.         | 0       ...                      ...             -     |  |mn |
    37.  
    38.     */
    40.     std::vector<double> copy_y = input_y;
    41.  
    42.     if ( filterMode == CUBIC_MEDIAN_FILTER )
    43.     {
    44.         cubicMedianFilter(copy_y, 5);
    45.     }
    46.  
    47.     const int count  = sizeOfx;
    48.     const int count1 = sizeOfx - 1;
    49.     const int count2 = sizeOfx - 2;
    50.     const int count3 = sizeOfx - 3;
    51.  
    52.     cubicCoeffs = new CubicSplineCoeffs( count1 );
    53.  
    54.     std::vector<double> step_h( count1, 0.0 );
    55.  
    56.     // for m matrix
    57.     cv::Mat_<double> m_a(1, count2, 0.0);
    58.     cv::Mat_<double> m_b(1, count2, 0.0);
    59.     cv::Mat_<double> m_c(1, count2, 0.0);
    60.     cv::Mat_<double> m_d(1, count2, 0.0);
    61.     cv::Mat_<double> m_part(1, count2, 0.0);
    62.  
    63.     cv::Mat_<double> m_all(1, count, 0.0);
    64.  
    65.     // initial step hi
    66.     for ( int idx=0; idx < count1; idx ++ )
    67.     {
    68.         step_h[idx] = input_x[idx+1] - input_x[idx];
    69.     }
    70.     // initial coefficients
    71.     for ( int idx=0; idx < count3; idx ++ )
    72.     {
    73.         m_a(idx) = step_h[idx];
    74.         m_b(idx) = 2 * (step_h[idx] + step_h[idx+1]);
    75.         m_c(idx) = step_h[idx+1];
    76.     }
    77.     // initial d
    78.     for ( int idx =0; idx < count3; idx ++ )
    79.     {
    80.         m_d(idx) = 6 * (
    81.             (copy_y[idx+2] - copy_y[idx+1]) / step_h[idx+1] -
    82.             (copy_y[idx+1] - copy_y[idx]) / step_h[idx] );
    83.     }
    84.  
    85.      //cv::Mat_<double> matOfm( count2,  )
    86.     bool isSucceed = caltridiagonalMatrices(m_a, m_b, m_c, m_d, m_part);
    87.     if ( !isSucceed )
    88.     {
    89.         std::cout<<"Calculate tridiagonal matrices failed!"<<std::endl<<
    90.             "Location: CubicSplineInterpolation.cpp -> " <<
    91.             "caltridiagonalMatrices()"<<std::endl;
    92.  
    93.         return;
    94.     }
    95.  
    96.     if ( splineMode == CUBIC_NATURAL )
    97.     {
    98.         m_all(0)      = 0.0;
    99.         m_all(count1) = 0.0;
    100.  
    101.         for ( int i=1; i<count1; i++ )
    102.         {
    103.             m_all(i) = m_part(i-1);
    104.         }
    105.  
    106.         for ( int i=0; i<count1; i++ )
    107.         {
    108.             cubicCoeffs->a[i] = copy_y[i];
    109.             cubicCoeffs->b[i] = ( copy_y[i+1] - copy_y[i] ) / step_h[i] -
    110.                 step_h[i]*( 2*m_all(i) + m_all(i+1) ) / 6;
    111.             cubicCoeffs->c[i] = m_all(i) / 2.0;
    112.             cubicCoeffs->d[i] = ( m_all(i+1) - m_all(i) ) / ( 6.0 * step_h[i] );
    113.         }
    114.     }
    115.     else
    116.     {
    117.         std::cout<<"Not define the interpolation mode!"<<std::endl;
    118.     }
    119. }
    120.  
    121. void CubicSplineInterpolation::cubicSplineInterpolation(
    122.     CubicSplineCoeffs *&cubicCoeffs,
    123.     std::vector<double> &input_x,
    124.     std::vector<double> &output_x,
    125.     std::vector<double> &output_y,
    126.     const double interStep )
    127. {
    128.     const int count = input_x.size();
    129.  
    130.     double low  = input_x[0];
    131.     double high = input_x[count-1];
    132.  
    133.     double interBegin = low;
    134.     for ( ; interBegin < high; interBegin += interStep )
    135.     {
    136.         int index = calInterpolationIndex(interBegin, input_x);
    137.         if ( index >= 0 )
    138.         {
    139.             double dertx = interBegin - input_x[index];
    140.             double y = cubicCoeffs->a[index] + cubicCoeffs->b[index] * dertx +
    141.                 cubicCoeffs->c[index] * dertx * dertx +
    142.                 cubicCoeffs->d[index] * dertx * dertx * dertx;
    143.             output_x.push_back(interBegin);
    144.             output_y.push_back(y);
    145.         }
    146.     }
    147. }
    148.  
    149. void CubicSplineInterpolation::cubicSplineInterpolation2(
    150.     CubicSplineCoeffs *&cubicCoeffs,
    151.     std::vector<double> input_x, double x, double &y)
    152. {
    153.     const int count = input_x.size();
    154.  
    155.     double low  = input_x[0];
    156.     double high = input_x[count-1];
    157.  
    158.     if ( x<low || x>high )
    159.     {
    160.         std::cout<<"The interpolation value is out of range!"<<std::endl;
    161.     }
    162.     else
    163.     {
    164.         int index = calInterpolationIndex(x, input_x);
    165.         if ( index >= 0 )
    166.         {
    167.             double dertx = x - input_x[index];
    168.             y = cubicCoeffs->a[index] + cubicCoeffs->b[index] * dertx +
    169.                 cubicCoeffs->c[index] * dertx * dertx +
    170.                 cubicCoeffs->d[index] * dertx * dertx * dertx;
    171.         }
    172.         else
    173.         {
    174.             std::cout<<"Can't find the interpolation range!"<<std::endl;
    175.         }
    176.     }
    177. }
    178.  
    179. bool CubicSplineInterpolation::caltridiagonalMatrices(
    180.     cv::Mat_<double> &input_a,
    181.     cv::Mat_<double> &input_b,
    182.     cv::Mat_<double> &input_c,
    183.     cv::Mat_<double> &input_d,
    184.     cv::Mat_<double> &output_x )
    185. {
    186.     int rows = input_a.rows;
    187.     int cols = input_a.cols;
    188.  
    189.     if ( ( rows == 1 && cols > rows ) ||
    190.         (cols == 1 && rows > cols ) )
    191.     {
    192.         const int count = ( rows > cols ? rows : cols ) - 1;
    193.  
    194.         output_x = cv::Mat_<double>::zeros(rows, cols);
    195.  
    196.         cv::Mat_<double> cCopy, dCopy;
    197.         input_c.copyTo(cCopy);
    198.         input_d.copyTo(dCopy);
    199.  
    200.         if ( input_b(0) != 0 )
    201.         {
    202.             cCopy(0) /= input_b(0);
    203.             dCopy(0) /= input_b(0);
    204.         }
    205.         else
    206.         {
    207.             return false;
    208.         }
    209.  
    210.         for ( int i=1; i < count; i++ )
    211.         {
    212.             double temp = input_b(i) - input_a(i) * cCopy(i-1);
    213.             if ( temp == 0.0 )
    214.             {
    215.                 return false;
    216.             }
    217.  
    218.             cCopy(i) /= temp;
    219.             dCopy(i) = ( dCopy(i) - dCopy(i-1)*input_a(i) ) / temp;
    220.         }
    221.  
    222.         output_x(count) = dCopy(count);
    223.         for ( int i=count-2; i > 0; i-- )
    224.         {
    225.             output_x(i) = dCopy(i) - cCopy(i)*output_x(i+1);
    226.         }
    227.         return true;
    228.     }
    229.     else
    230.     {
    231.         return false;
    232.     }
    233. }
    234.  
    235. int CubicSplineInterpolation::calInterpolationIndex(
    236.     double &pt, std::vector<double> &input_x )
    237. {
    238.     const int count = input_x.size()-1;
    239.     int index = -1;
    240.     for ( int i=0; i<count; i++ )
    241.     {
    242.         if ( pt > input_x[i] && pt <= input_x[i+1] )
    243.         {
    244.             index = i;
    245.             return index;
    246.         }
    247.     }
    248.     return index;
    249. }
    250.  
    251. void CubicSplineInterpolation::cubicMedianFilter(
    252.     std::vector<double> &input, const int filterSize /* = 5 */ )
    253. {
    254.     const int count = input.size();
    255.     for ( int i=filterSize/2; i<count-filterSize/2; i++ )
    256.     {
    257.         std::vector<double> temp(filterSize, 0.0);
    258.         for ( int j=0; j<filterSize; j++ )
    259.         {
    260.             temp[j] = input[i+j - filterSize/2];
    261.         }
    262.  
    263.         input[i] = cubicSort(temp);
    264.  
    265.         std::vector<double>().swap(temp);
    266.     }
    267.  
    268.     for ( int i=0; i<filterSize/2; i++ )
    269.     {
    270.         std::vector<double> temp(filterSize, 0.0);
    271.         for ( int j=0; j<filterSize; j++ )
    272.         {
    273.             temp[j] = input[j];
    274.         }
    275.  
    276.         input[i] = cubicSort(temp);
    277.         std::vector<double>().swap(temp);
    278.     }
    279.  
    280.     for ( int i=count-filterSize/2; i<count; i++ )
    281.     {
    282.         std::vector<double> temp(filterSize, 0.0);
    283.         for ( int j=0; j<filterSize; j++ )
    284.         {
    285.             temp[j] = input[j];
    286.         }
    287.  
    288.         input[i] = cubicSort(temp);
    289.         std::vector<double>().swap(temp);
    290.     }
    291. }
    292.  
    293. double CubicSplineInterpolation::cubicSort( std::vector<double> &input )
    294. {
    295.     int iCount = input.size();
    296.     for ( int j=0; j<iCount-1; j++ )
    297.     {
    298.         for ( int k=iCount-1; k>j; k-- )
    299.         {
    300.             if ( input[k-1] > input[k] )
    301.             {
    302.                 double tp  = input[k];
    303.                 input[k]   = input[k-1];
    304.                 input[k-1] = tp;
    305.             }
    306.         }
    307.     }
    308.     return input[iCount/2];
    309. }

  • 相关阅读:
    MySQL 执行查询流程
    IntelliJ IDEA 2022.2 正式发布：已完全支持 Spring 6 和 Spring Boot 3了吗？
    python反距离权重(IDW)插值站点到格点
    定语从句------六级
    CSS - 定位布局（position）
    文件操作系统调用接口、文件描述符的剖析、重定向的原理
    《Linux驱动：s3c2440 lcd 驱动分析--终结篇》
    故障分析 | MySQL 节点宕机分析一例
    兽用白油疫苗——博迈伦
    JSP page指令
  • 原文地址：https://blog.csdn.net/hulinhulin/article/details/134171633