曲线平滑算法Spline(自然样条曲线)

1 . Spline 原理

数学模型定义如下,给定个 $n+1$ 结点 $P_{i} = (x_{i},y_{i}) (i = 0 , 1, ..., n)$ ,定义如下分段曲线:

$S(x) = \begin{cases} S_{1} (x)& \text{ if }x_{0} \leq x \leq x_{1} \\ S_{2} (x) & \text{ if }x_{1} \leq x \leq x_{2} \\ \vdots \\ S_{n} (x) & \text{ if }x_{n-1} \leq x \leq x_{n} \\ \end{cases}$ (1)

其中每一个分段函数都是一个三次多项式:

$S_{i}(x) = a_{i} + b_{i}(x - x_{i}) + c_{i}(x - x_{i}) ^{2} + d_{i}(x - x_{i}) ^{3}$ (2)

在结点处满足以下述边界条件:

$S_{i}(x_{i}) = S_{i+1}(x_{i}) = y_{i}&\\ S^{'}_{i}(x_{i}) = S^{'}_{i+1}(x_{i}) \\ S^{''}_{i}(x_{i}) = S^{''}_{i+1}(x_{i})$ (3)

由于每一段曲线都是三次多项式,那么其二阶微分是关于 x 的一次函数 , 可表示如下:

$S^{'}_{i}(x) = z_{i} + (x - x_{i} )(z_{i+1} - z_{i})\rac/h_{i}$ (4)

其中，

$h_{i} = x_{i+1} - x_{i} \\ z_{i} = S^{'}_{i}(x_{i})\\ z_{i+1} =S^{'}_{i+1}(x_{i+1})$ (5)

扫描二维码关注公众号，回复： 9893342 查看本文章

将(4)式所有index的微分方程列成矩阵的形式:

$\begin{bmatrix} v_{1} &h_{1} & & & & \\ h_{1}&v_{2} &h_{2} & & & \\ &h_{2} &v_{3} &h_{3}& & \\ & &\ddots & \ddots & \ddots & \\ & & & \ddots &\ddots & h_{n-2}\\ & & & &h_{n-2} & h_{n-1} \end{bmatrix} \begin{bmatrix} z_{1}\\ z_{2} \\ z_{3} \\ \ddots \\ z_{n-2} \\ z_{n-1} \end{bmatrix} =\begin{bmatrix} u_{1}\\ u_{2}\\ u_{3}\\ \ddots \\ u_{n-2}\\ u_{n-1} \end{bmatrix}$ (6)

其中，

$b_{i} = \frac{1}{h_{i}}(y_{i+1} - y_{i})\\ v_{i} = 2(h_{i-1} + h_{i})\\ u_{i} = 6(b_{i} - b_{i-1})\\ z_{0}=z_{n}=0$ (7)

则每段三次自然样条曲线的表达式为:

$S_{i}(x) = \frac{z_{i+1}}{6h_{i}}(x-x_{i})^{3} + \frac{z_{i}}{6h_{i}}(x_{i+1}-x)^{3} + (\frac{y_{i+1}}{h_{i}}- \frac{z_{i+1}}{6}h_{i})(x-x_{i})+(\frac{y_{i}}{h_{i}}- \frac{h_{i}}{6})(x_{i+1}-x)$ (8)

2. 代码:

头文件(.h)

#include <algorithm>
#include <cassert>
#include <cstdio>
#include <vector>
class band_matrix {
 private:
  std::vector<std::vector<double>> m_upper;  //// upper band
  std::vector<std::vector<double>> m_lower;  //// lower band
 public:
  band_matrix(){};                         //// constructor
  band_matrix(int dim, int n_u, int n_l);  //// constructor
  ~band_matrix(){};                        //// destructor
  void resize(int dim, int n_u, int n_l);  //// init with dim,n_u,n_l
  int dim() const;                         //// matrix dimension
  int num_upper() const { return m_upper.size() - 1; }
  int num_lower() const { return m_lower.size() - 1; }
  //// access operator
  double& operator()(int i, int j);       //// write
  double operator()(int i, int j) const;  //// read
  //// we can store an additional diagonal (in m_lower)
  double& saved_diag(int i);
  double saved_diag(int i) const;
  void lu_decompose();
  std::vector<double> r_solve(const std::vector<double>& b) const;
  std::vector<double> l_solve(const std::vector<double>& b) const;
  std::vector<double> lu_solve(const std::vector<double>& b,
                               bool is_lu_decomposed = false);
};

//// spline interpolation
class spline {
 public:
  enum bd_type { first_deriv = 1, second_deriv = 2 };

 private:
  std::vector<double> m_x, m_y;  //// x,y coordinates of points
  //// interpolation parameters
  //// f(x) = a*(x-x_i)^3 + b*(x-x_i)^2 + c*(x-x_i) + y_i
  std::vector<double> m_a, m_b, m_c;  //// spline coefficients
  double m_b0, m_c0;                  //// for left extrapol
  bd_type m_left, m_right;
  double m_left_value, m_right_value;
  bool m_force_linear_extrapolation;

 public:
  //// set default boundary condition to be zero curvature at both ends
  spline()
      : m_left(second_deriv),
        m_right(second_deriv),
        m_left_value(0.0),
        m_right_value(0.0),
        m_force_linear_extrapolation(false) {
    ;
  }

  //// optional, but if called it has to come be before set_points()
  void set_boundary(bd_type left, double left_value, bd_type right,
                    double right_value,
                    bool force_linear_extrapolation = false);
  void set_points(const std::vector<double>& x, const std::vector<double>& y,
                  bool cubic_spline = true);
  double operator()(double x) const;
  double cal_first_deriv(double x) const;
  double cal_second_deriv(double x) const;
};

源文件(.cpp)

#include "spline.h"

band_matrix::band_matrix(int dim, int n_u, int n_l) { resize(dim, n_u, n_l); }
void band_matrix::resize(int dim, int n_u, int n_l) {
  assert(dim > 0);
  assert(n_u >= 0);
  assert(n_l >= 0);
  m_upper.resize(n_u + 1);
  m_lower.resize(n_l + 1);
  for (size_t i = 0; i < m_upper.size(); i++) {
    m_upper[i].resize(dim);
  }
  for (size_t i = 0; i < m_lower.size(); i++) {
    m_lower[i].resize(dim);
  }
}
int band_matrix::dim() const {
  if (m_upper.size() > 0) {
    return m_upper[0].size();
  } else {
    return 0;
  }
}
//// defines the new operator (), so that we can access the elements
//// by A(i,j), index going from i=0,...,dim()-1
double& band_matrix::operator()(int i, int j) {
  int k = j - i;  //// what band is the entry
  assert((i >= 0) && (i < dim()) && (j >= 0) && (j < dim()));
  assert((-num_lower() <= k) && (k <= num_upper()));
  //// k=0 -> diogonal, k<0 lower left part, k>0 upper right part
  if (k >= 0)
    return m_upper[k][i];
  else
    return m_lower[-k][i];
}
double band_matrix::operator()(int i, int j) const {
  int k = j - i;
  assert((i >= 0) && (i < dim()) && (j >= 0) && (j < dim()));
  assert((-num_lower() <= k) && (k <= num_upper()));
  //// k=0 -> diagonal, k<0 lower left part, k>0 upper right part
  if (k >= 0)
    return m_upper[k][i];
  else
    return m_lower[-k][i];
}
//// second diag (used in LU decomposition), saved in m_lower
double band_matrix::saved_diag(int i) const {
  assert((i >= 0) && (i < dim()));
  return m_lower[0][i];
}
double& band_matrix::saved_diag(int i) {
  assert((i >= 0) && (i < dim()));
  return m_lower[0][i];
}

//// LR-Decomposition of a band matrix
void band_matrix::lu_decompose() {
  int i_max, j_max;
  int j_min;
  double x;

  //// preconditioning
  //// normalize column i so that a_ii=1
  for (int i = 0; i < this->dim(); i++) {
    assert(this->operator()(i, i) != 0.0);
    this->saved_diag(i) = 1.0 / this->operator()(i, i);
    j_min = std::max(0, i - this->num_lower());
    j_max = std::min(this->dim() - 1, i + this->num_upper());
    for (int j = j_min; j <= j_max; j++) {
      this->operator()(i, j) *= this->saved_diag(i);
    }
    this->operator()(i, i) = 1.0;  //// prevents rounding errors
  }

  //// Gauss LR-Decomposition
  for (int k = 0; k < this->dim(); k++) {
    i_max = std::min(this->dim() - 1,
                     k + this->num_lower());  //// num_lower not a mistake!
    for (int i = k + 1; i <= i_max; i++) {
      assert(this->operator()(k, k) != 0.0);
      x = -this->operator()(i, k) / this->operator()(k, k);
      this->operator()(i, k) = -x;  //// assembly part of L
      j_max = std::min(this->dim() - 1, k + this->num_upper());
      for (int j = k + 1; j <= j_max; j++) {
        //// assembly part of R
        this->operator()(i, j) =
            this->operator()(i, j) + x * this->operator()(k, j);
      }
    }
  }
}
//// solves Ly=b
std::vector<double> band_matrix::l_solve(const std::vector<double>& b) const {
  assert(this->dim() == (int)b.size());
  std::vector<double> x(this->dim());
  int j_start;
  double sum;
  for (int i = 0; i < this->dim(); i++) {
    sum = 0;
    j_start = std::max(0, i - this->num_lower());
    for (int j = j_start; j < i; j++) sum += this->operator()(i, j) * x[j];
    x[i] = (b[i] * this->saved_diag(i)) - sum;
  }
  return x;
}
//// solves Rx=y
std::vector<double> band_matrix::r_solve(const std::vector<double>& b) const {
  assert(this->dim() == (int)b.size());
  std::vector<double> x(this->dim());
  int j_stop;
  double sum;
  for (int i = this->dim() - 1; i >= 0; i--) {
    sum = 0;
    j_stop = std::min(this->dim() - 1, i + this->num_upper());
    for (int j = i + 1; j <= j_stop; j++) sum += this->operator()(i, j) * x[j];
    x[i] = (b[i] - sum) / this->operator()(i, i);
  }
  return x;
}

std::vector<double> band_matrix::lu_solve(const std::vector<double>& b,
                                          bool is_lu_decomposed) {
  assert(this->dim() == (int)b.size());
  std::vector<double> x, y;
  if (is_lu_decomposed == false) {
    this->lu_decompose();
  }
  y = this->l_solve(b);
  x = this->r_solve(y);
  return x;
}

//// spline implementation
void spline::set_boundary(spline::bd_type left, double left_value,
                          spline::bd_type right, double right_value,
                          bool force_linear_extrapolation) {
  assert(m_x.size() == 0);  //// set_points() must not have happened yet
  m_left = left;
  m_right = right;
  m_left_value = left_value;
  m_right_value = right_value;
  m_force_linear_extrapolation = force_linear_extrapolation;
}

void spline::set_points(const std::vector<double>& x,
                        const std::vector<double>& y, bool cubic_spline) {
  assert(x.size() == y.size());
  assert(x.size() > 2);
  m_x = x;
  m_y = y;
  int n = x.size();
  // TODO: maybe sort x and y, rather than returning an error
  for (int i = 0; i < n - 1; i++) {
    assert(m_x[i] < m_x[i + 1]);
  }

  if (cubic_spline == true) {  //// cubic spline interpolation
    //// setting up the matrix and right hand side of the equation system
    //// for the parameters b[]
    band_matrix A(n, 1, 1);
    std::vector<double> rhs(n);
    for (int i = 1; i < n - 1; i++) {
      A(i, i - 1) = 1.0 / 3.0 * (x[i] - x[i - 1]);
      A(i, i) = 2.0 / 3.0 * (x[i + 1] - x[i - 1]);
      A(i, i + 1) = 1.0 / 3.0 * (x[i + 1] - x[i]);
      rhs[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) -
               (y[i] - y[i - 1]) / (x[i] - x[i - 1]);
    }
    //// boundary conditions
    if (m_left == spline::second_deriv) {
      //// 2*b[0] = f''
      A(0, 0) = 2.0;
      A(0, 1) = 0.0;
      rhs[0] = m_left_value;
    } else if (m_left == spline::first_deriv) {
      //// c[0] = f', needs to be re-expressed in terms of b:
      //// (2b[0]+b[1])(x[1]-x[0]) = 3 ((y[1]-y[0])/(x[1]-x[0]) - f')
      A(0, 0) = 2.0 * (x[1] - x[0]);
      A(0, 1) = 1.0 * (x[1] - x[0]);
      rhs[0] = 3.0 * ((y[1] - y[0]) / (x[1] - x[0]) - m_left_value);
    } else {
      assert(false);
    }
    if (m_right == spline::second_deriv) {
      //// 2*b[n-1] = f''
      A(n - 1, n - 1) = 2.0;
      A(n - 1, n - 2) = 0.0;
      rhs[n - 1] = m_right_value;
    } else if (m_right == spline::first_deriv) {
      //// c[n-1] = f', needs to be re-expressed in terms of b:
      //// (b[n-2]+2b[n-1])(x[n-1]-x[n-2])
      //// = 3 (f' - (y[n-1]-y[n-2])/(x[n-1]-x[n-2]))
      A(n - 1, n - 1) = 2.0 * (x[n - 1] - x[n - 2]);
      A(n - 1, n - 2) = 1.0 * (x[n - 1] - x[n - 2]);
      rhs[n - 1] =
          3.0 * (m_right_value - (y[n - 1] - y[n - 2]) / (x[n - 1] - x[n - 2]));
    } else {
      assert(false);
    }

    //// solve the equation system to obtain the parameters b[]
    m_b = A.lu_solve(rhs);

    //// calculate parameters a[] and c[] based on b[]
    m_a.resize(n);
    m_c.resize(n);
    for (int i = 0; i < n - 1; i++) {
      m_a[i] = 1.0 / 3.0 * (m_b[i + 1] - m_b[i]) / (x[i + 1] - x[i]);
      m_c[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) -
               1.0 / 3.0 * (2.0 * m_b[i] + m_b[i + 1]) * (x[i + 1] - x[i]);
    }
  } else {  //// linear interpolation
    m_a.resize(n);
    m_b.resize(n);
    m_c.resize(n);
    for (int i = 0; i < n - 1; i++) {
      m_a[i] = 0.0;
      m_b[i] = 0.0;
      m_c[i] = (m_y[i + 1] - m_y[i]) / (m_x[i + 1] - m_x[i]);
    }
  }

  //// for left extrapolation coefficients
  m_b0 = (m_force_linear_extrapolation == false) ? m_b[0] : 0.0;
  m_c0 = m_c[0];

  //// for the right extrapolation coefficients
  //// f_{n-1}(x) = b*(x-x_{n-1})^2 + c*(x-x_{n-1}) + y_{n-1}
  double h = x[n - 1] - x[n - 2];
  //// m_b[n-1] is determined by the boundary condition
  m_a[n - 1] = 0.0;
  m_c[n - 1] = 3.0 * m_a[n - 2] * h * h + 2.0 * m_b[n - 2] * h +
               m_c[n - 2];  //// = f'_{n-2}(x_{n-1})
  if (m_force_linear_extrapolation == true) m_b[n - 1] = 0.0;
}

double spline::operator()(double x) const {
  size_t n = m_x.size();
  //// find the closest point m_x[idx] < x, idx=0 even if x<m_x[0]
  std::vector<double>::const_iterator it;
  it = std::lower_bound(m_x.begin(), m_x.end(), x);
  int idx = std::max(int(it - m_x.begin()) - 1, 0);

  double h = x - m_x[idx];
  double interpol;
  if (x < m_x[0]) {
    //// extrapolation to the left
    interpol = (m_b0 * h + m_c0) * h + m_y[0];
  } else if (x > m_x[n - 1]) {
    //// extrapolation to the right
    interpol = (m_b[n - 1] * h + m_c[n - 1]) * h + m_y[n - 1];
  } else {
    //// interpolation
    interpol = ((m_a[idx] * h + m_b[idx]) * h + m_c[idx]) * h + m_y[idx];
  }
  return interpol;
}

double spline::cal_first_deriv(double x) const {
  size_t n = m_x.size();
  //// find the closest point m_x[idx] < x, idx=0 even if x<m_x[0]
  std::vector<double>::const_iterator it;
  it = std::lower_bound(m_x.begin(), m_x.end(), x);
  int idx = std::max(int(it - m_x.begin()) - 1, 0);

  double h = x - m_x[idx];
  double interpol;
  if (x < m_x[0]) {
    //// extrapolation to the left
    interpol = m_c0;
  } else if (x > m_x[n - 1]) {
    //// extrapolation to the right
    interpol = m_c[n - 1];
  } else {
    //// interpolation
    interpol = 3 * m_a[idx] * h * h + 2 * m_b[idx] * h + m_c[idx];
  }
  return interpol;
}

double spline::cal_second_deriv(double x) const {
  size_t n = m_x.size();
  //// find the closest point m_x[idx] < x, idx=0 even if x<m_x[0]
  std::vector<double>::const_iterator it;
  it = std::lower_bound(m_x.begin(), m_x.end(), x);
  int idx = std::max(int(it - m_x.begin()) - 1, 0);

  double h = x - m_x[idx];
  double interpol;
  if (x < m_x[0]) {
    //// extrapolation to the left
    interpol = 2 * m_b0;
  } else if (x > m_x[n - 1]) {
    //// extrapolation to the right
    interpol = 2 * m_b[n - 1];
  } else {
    //// interpolation
    interpol = 6 * m_a[idx] * h + 2 * m_b[idx];
  }
  return interpol;
}

3. 示例

#include <cstdio>
#include <cstdlib>
#include <vector>
#include "spline.h"

int main(int argc, char** argv) {

   std::vector<double> X(5), Y(5);
   X[0]=0.1; X[1]=0.4; X[2]=1.2; X[3]=1.8; X[4]=2.0;
   Y[0]=0.1; Y[1]=0.7; Y[2]=0.6; Y[3]=1.1; Y[4]=0.9;

   spline s;
   s.set_points(X,Y);    // currently it is required that X is already sorted

   double x=1.5;

   printf("spline at %f is %f\n", x, s(x));

   return EXIT_SUCCESS;
}

fool_i'm

发布了4 篇原创文章 · 获赞 2 · 访问量 819

私信关注

曲线平滑算法Spline(自然样条曲线)

猜你喜欢