読者です 読者をやめる 読者になる 読者になる

MyEnigma

とあるエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

3次スプライン補間の概要とC++,Pythonサンプルコード

Cによるスプライン関数―データ解析 CG 微分方程式

Cによるスプライン関数―データ解析 CG 微分方程式

 

目次

はじめに

3次スプライン補間は、

計算がそこまで複雑ではなく、

また二次微分までの連続性が担保されているため、

様々な用途に利用されています。

 

今回の記事では、

この3次スプライン曲線の概要と、

3次スプライン曲線を作成する

C++, Pythonのサンプルコードを作成したので、

公開したいと思います。

 

3次スプライン補間とは?

3次スプライン補間は下記の図のように、

複数のサンプル点が与えられた時に、

そのサンプル点の間を3次の多項式で近似し、

滑らかに補間する手法です。

f:id:meison_amsl:20161011044036p:plain

各データの間を区切り、

各区間においてそれぞれ別の3次多項式で近似する部分が特徴的です。

 

3次多項式なので、各データ区間は下記の式で近似されます。

f:id:meison_amsl:20161011045421p:plain

但し、それぞれの区間によって、多項式のパラメータは変わるので、

x_j< x <x_j+1となります。

上記の式の通り一つの区間に対して、4つの未知パラメータ(a,b,c,d)があるので、

データ数をNとすると、4Nの未知パラメータを決定することで補間が可能になります。

 

一般的に、上記の4Nの未知パラメータを推定するために、

下記の5つの条件を用います。

これらの5つの条件が、3次スプライン曲線を特徴付けています。

条件1

f:id:meison_amsl:20161011051939p:plain

こちらは簡単ですね。

それぞれの区間の多項式はxを入力として、yを出力としています。 

条件2

f:id:meison_amsl:20161011052214p:plain

こちらは各区間の境界の連続性条件です。

各区間の境界はかならず繋がるように連続性が決められます。

条件3

f:id:meison_amsl:20161011052530p:plain

上記は一次微分の連続性条件です。

これにより、各区間の境界は傾きまでを含めて同じように連結されます。

条件4

f:id:meison_amsl:20161011053637p:plain

上記は二次微分の連続性条件です。

これにより、傾きの変化率まで含めて、

滑らかに連結されるため、

非常に滑らかに補間をすることができます。

条件5

f:id:meison_amsl:20161011053858p:plain

こちらの条件は、始点と終点の二次微分の境界条件です。

この始点と終点の境界条件には、いくつか流派があり、

上記のように、曲線の始点と終点の二次微分が0である境界条件を

使うものをNatural Cubic Spline(自然3次スプライン)と呼ぶようです。

しかし、この方式を利用する場合が多いため、

この方式を単に3次スプラインと呼んでいる場合も多い気がします。

 

3次スプライン補間を手計算+pythonで解く

上記の3次スプライン補間のパラメータを求める方法はいくつかあるのですが、

まず初めに3次スプライン補間の理解を深めるため、

下記の記事を参考にして

手計算とpythonで解いてみようと思います。

 

上記の記事の通り、

4Nのパラメータを解くには、4N個の方程式が必要です。

まず、区間一つに対して、両端はかならずサンプリング点を通ることから、

それぞれの区間に対して2つの方程式を立てることができ、

その結果、2Nの方程式を立てることができます。

 

続いて、上記の条件3と条件4により、

2つの区間を挟むサンプリング点の分だけ、

それぞれ方程式を立てることができるので、

2N-2の方程式を立てることができます。

(始点と終点は方程式を立てることができません。)

 

最後に条件5により、始点と終点で一つづつ方程式を立てられるので、

すべてを合わせると4Nの方程式を作ることができ、

これでパラメータを計算することができます。

 

それぞれの方程式を先程の記事のように、

線形方程式にし、あとは逆行列と行列演算で解くだけです。

下記がそのコードになります。

 

#! /usr/bin/python 
# -*- coding: utf-8 -*- 
u""" 
author Atsushi Sakai
"""
import matplotlib.pyplot as plt
import numpy as np

def CubicFunc(x,p):
    y=p[0]*(x**3)+p[1]*(x**2)+p[2]*x+p[3]
    return y

#input
x=[1,2,3,4]
y=[2.7,6,5,6.5]

A=np.array([[1,1,1,1, 0,0,0,0,0,0,0,0],
            [8,4,2,1, 0,0,0,0,0,0,0,0],
            [0,0,0,0, 8,4,2,1,0,0,0,0],
            [0,0,0,0,27,9,3,1,0,0,0,0],
            [0,0,0,0,0,0,0,0,27,9,3,1],
            [0,0,0,0,0,0,0,0,64,16,4,1],
            [12,4,1,0,-12,-4,-1,0,0,0,0,0],
            [0,0,0,0,27,6,1,0,-27,-6,-1,0],
            [12,2,0,0,-12,-2,0,0,0,0,0,0],
            [0,0,0,0,18,2,0,0,-18,-2,0,0],
            [6,2,0,0,0,0,0,0,0,0,0,0],
            [0,0,0,0,0,0,0,0,24,2,0,0],
            ])

b=np.array([2.7,6,6,5,5,6.5,0,0,0,0,0,0])

xs=np.linalg.solve(A,b)

fx,fy=[],[]
for i in range(3):
    xt=np.arange(x[i],x[i+1]+0.1,0.1)
    p=xs[4*i:4*(i+1)]
    yt=[CubicFunc(xi,p) for xi in xt]
    fx.extend(xt)
    fy.extend(yt)

plt.plot(x,y,"xb")
plt.plot(fx,fy,"-r")
plt.grid(True)
plt.axis("equal")
plt.show()

上記のコードでxsというベクトルが、

求めたいパラメータベクトルであり、

その後のコードでそのパラメータを元に補間データを復元しています。

上記のコードを実行すると下記のようなグラフが表示されます。

f:id:meison_amsl:20161011082354p:plain

 

ちゃんと滑らかに補間できていることが確認できました。

 

入力データ数が不定な場合の3次Spline補間

前述のような、手計算の方法だと、

入力データの数が変わると行列のサイズなどが変更になり、

コードを修正しないといけないので、

ツールとしてはあまり使い勝手がよくありません。

 

そこで、媒介変数を使った

下記の記事の手法を使うことで、

入力データ数が可変でも

同じコードで3次スプライン補間をすることができます。

 

上記の手法をPythonとC++で実装してみました。

 

Pythonサンプルコード

Pythonでのコードは下記になります。

#! /usr/bin/python 
# -*- coding: utf-8 -*- 
u""" 
Cubic Spline library
author Atsushi Sakai
license: MIT
"""
import math
import numpy as np

class Spline:
    u"""
    Cubic Spline class
    usage:
        spline=Spline(y)
        rx=np.arange(0,4,0.1)
        ry=[spline.calc(i) for i in rx]
    """

    def __init__(self, y):
        self.a, self.b, self.c, self.d, self.w = [], [], [], [], []

        self.a = [iy for iy in y]

        for i in range(len(self.a)):
            if i == 0:
                self.c.append(0)
            elif i == (len(self.a) - 1):
                self.c.append(0)
            else:
                self.c.append(3.0 * (self.a[i - 1] - 2.0 * self.a[i] + self.a[i + 1]))

        for i in range(len(self.a)):
            if i == 0:
                self.w.append(0)
            elif i == (len(self.a) - 1):
                pass
            else:
                tmp = 4.0 - self.w[i - 1]
                self.c[i] = (self.c[i] - self.c[i - 1]) / tmp
                self.w.append(1.0 / tmp)

        i = len(self.a) - 1
        while(i > 0):
            i -= 1
            self.c[i] = self.c[i] - self.c[i + 1] * self.w[i]

        for i in range(len(self.a)):
            if i == (len(self.a) - 1):
                self.b.append(0)
                self.d.append(0)
            else:
                self.d.append((self.c[i + 1] - self.c[i]) / 3.0)
                self.b.append(self.a[i + 1] - self.a[i] - self.c[i] - self.d[i])

    def calc(self, t):
        u"""
        Calc position
        """

        j = int(math.floor(t))
        if(j < 0):
            j = 0
        elif(j >= len(self.a)):
            j = len(self.a) - 1

        dt = t - j
        result = self.a[j] + (self.b[j] + (self.c[j] + self.d[j] * dt) * dt) * dt
        return result

    def calcd(self, t):
        u"""
        Calc first derivative
        """

        j = int(math.floor(t))
        if(j < 0):
            j = 0
        elif(j >= len(self.a)):
            j = len(self.a) - 1

        dt = t - j
        result = self.b[j] + 2.0 * self.c[j] * dt + 3.0 * self.d[j] * dt * dt
        return result

    def calcdd(self, t):
        u"""
        Calc second derivative
        """
        j = int(math.floor(t))
        if(j < 0):
            j = 0
        elif(j >= len(self.a)):
            j = len(self.a) - 1

        dt = t - j
        result = 2.0 * self.c[j] + 6.0 * self.d[j] * dt
    return result


if __name__ == '__main__':
    import matplotlib.pyplot as plt
    #input
    x=[0,1,2,3]
    y=[2.7,6,5,6.5]

    # 3d spline interporation
    spline=PyCubicSpline(y)
    rx=np.arange(0,4,0.1)
    ry=[spline.Calc(i) for i in rx]

    plt.plot(x,y,"xb")
    plt.plot(rx,ry,"-r")
    plt.grid(True)
    plt.axis("equal")
  plt.show()

上記のコードを実行すると、

下記のグラフを得られます。

f:id:meison_amsl:20161011141000p:plain

きちんと補間できていることがわかりました。

 

しかし、この手法だとx方向のデータのサンプリングを

等間隔にする必要があります。

等間隔ではなく、かつ入力数に汎用的に

3次元補間する手法は今のこと見つまりませんでした、

知っている方がいましたら、教えてくださると助かります。

 

C++サンプルコード

続いて、

上記のPythonコードをC++に移植しました。

下記が3次元スプライン補間用のヘッダライブラリです。

/**
 *  @brief Cubic Spline header library
 *
 *  @author Atsushi Sakai
 *
 **/

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

using namespace std;

/**
 *  @brief Cubic Spline header library
 *
 *  @usage 
 *    vector<double> sy{2.7,6,5,6.5};
 *    CppCubicSpline cppCubicSpline(sy);
 *    vector<double> rx;
 *    vector<double> ry;
 *    for(double i=0.0;i<=3.2;i+=0.1){
 *       rx.push_back(i);
 *       ry.push_back(cppCubicSpline.Calc(i));
 *    }
 */
class CppCubicSpline{
  public:
    CppCubicSpline(const vector<double> &y){

      InitParameter(y);

    }

    double Calc(double t){
        int j=int(floor(t));
        if(j<0){
            j=0;
        }
        else if(j>=a_.size()){
            j=(a_.size()-1);
        }

        double dt=t-j;
        double result=a_[j]+(b_[j]+(c_[j]+d_[j]*dt)*dt)*dt;
        return result;
    }

  private:
    vector<double> a_;
    vector<double> b_;
    vector<double> c_;
    vector<double> d_;
    vector<double> w_;

    void InitParameter(const vector<double> &y){
      int ndata=y.size()-1;

      for(int i=0;i<=ndata;i++){
        a_.push_back(y[i]);
      }

      for(int i=0;i<ndata;i++){
        if(i==0){
          c_.push_back(0.0);
        }
        else if(i==ndata){
          c_.push_back(0.0);
        }
        else{
          c_.push_back(3.0*(a_[i-1]-2.0*a_[i]+a_[i+1]));
        }
      }

      for(int i=0;i<ndata;i++){
        if(i==0){
          w_.push_back(0.0);
        }
        else{
          double tmp=4.0-w_[i-1];
          c_[i]=(c_[i]-c_[i-1])/tmp;
          w_.push_back(1.0/tmp);
        }
      }

      for(int i=(ndata-1);i>0;i--){
        c_[i]=c_[i]-c_[i+1]*w_[i];
      }

      for(int i=0;i<=ndata;i++){
        if(i==ndata){
          d_.push_back(0.0);
          b_.push_back(0.0);
        }
        else{
          d_.push_back((c_[i+1]-c_[i])/3.0);
          b_.push_back(a_[i+1]-a_[i]-c_[i]-d_[i]);
        }
      }

    }

};

 

このクラスを使って、

こんな感じでスプライン補間をすることができます。

 

#include<iostream>
#include<vector>
#include"CppCubicSpline.h"
#include"matplotlibcpp.h"

namespace plt=matplotlibcpp;

using namespace std;

int main(void){
  cout<<"cpp spline sample"<<endl;
  vector<double> sx{0,1,2,3};
  vector<double> sy{2.7,6,5,6.5};

  CppCubicSpline cppCubicSpline(sy);
  vector<double> rx;
  vector<double> ry;
  for(double i=0.0;i<=3.2;i+=0.1){
    rx.push_back(i);
    ry.push_back(cppCubicSpline.Calc(i));
  }

  plt::named_plot("Truth",sx,sy, "xb");
  plt::named_plot("interporation",rx,ry, "-r");
  plt::legend();
  plt::axis("equal");
  plt::grid(true);
  plt::show();
}

上記のコードを実行すると、

下記のようなグラフが得られます。

f:id:meison_amsl:20161012073111p:plain

また、c++コードのグラフ作成には、

下記のツールを使いました。

myenigma.hatenablog.com

 

3次スプラインにおける曲率の計算方法

下記のWikipediaの記事にある通り、

ある方程式f(x)の各xにおける曲率は、

下記の式で計算することができます。

f:id:meison_amsl:20161014071730p:plain

 

3次スプラインは3次多項式ですので、

先程の式における一次微分と二次微分は下記のようになります。

f:id:meison_amsl:20161014072329p:plain

f:id:meison_amsl:20161014072457p:plain

 

あとは上記の式を先程の曲率の式に入れれば、

各点における曲率が計算できます。

 

ためしに、

先程の3次スプライン補間のデータの曲率を計算してみました。

下記の関数を追加することで、補間点を計算するCalc関数と同じように、

媒介変数tで指定した位置の曲率を取得することができます。

def CalcCurvature(self,t):
   j=int(math.floor(t))
      if(j<0):
       j=0
      elif(j>=len(self.a)):
        j=len(self.a)-1

      dt=t-j

      df=self.b[j]+2.0*self.c[j]*dt+3.0*self.d[j]*dt*dt
      ddf=2.0*self.c[j]+6.0*self.d[j]*dt
      k=ddf/((1+df**2)**1.5)

      result=k
   return result

 

下記が、その曲率の計算結果です。

f:id:meison_amsl:20161014073032p:plain

上記の補間結果を比べると、

ちゃんとカーブのきつい部分の曲率が大きくなっていることがわかります。

また曲率も3次スプラインの特徴通り、

滑らかに繋がっていることがわかります。

 

x-y座標系における点群のスプライン補間

二次元の平面上の点群をスプライン補間して、

滑らかな補間を実施したい場合、

スプライン補間の入力を各入力点間の距離にし、

x座標とy座標をそれぞれスプライン補間することで、

なめらかなコースを作ることができます。

f:id:meison_amsl:20161121083242p:plain

f:id:meison_amsl:20161121083252p:plain

 

方位の計算方法

幾何学的関係より、各点の方位θは下記の式で計算できます。

f:id:meison_amsl:20161122083132p:plain

xとyの微分値は前述の式より計算できるので、

各点のおける方位が下記のようなコードで実現できます。

def calc_yaw(s, sx, sy):
    u"""
    calc yaw of x-y spline curve
    """
    dx = sx.calcd(s)
    dy = sy.calcd(s)
    yaw = math.atan2(dy, dx)
    return yaw

 

下記は直線と円弧で構成されるコースにおいて、

方位の推定を実施したものです。

真値に近い方位が、

スプライン補間の結果から計算できていることがわかります。

f:id:meison_amsl:20161122083833p:plain

 

曲率の計算方法

このコースにおける曲率を計算したい場合は、

下記の二次元曲線の曲率の定義と、

f:id:meison_amsl:20161121084053p:plain

xとyのそれぞれの1次微分値と2次微分値(前述の式参照)を使うことで、

計算できます。

(元の曲率が不連続な部分で振動してしまっていますが)

f:id:meison_amsl:20161121084143p:plain

 

曲率を計算するコードはこちらになります。

def calc_xy_curvature(s, sx, sy):
    u"""
    Calc curvature x-y spline curve
    """
    dx = sx.calcd(s)
    ddx = sx.calcdd(s)
    dy = sy.calcd(s)
    ddy = sy.calcdd(s)
    k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2)
  return k

 

コードの保管場所

上記のコードはすべて下記のリポジトリで公開しました。

github.com

 

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

Cによるスプライン関数―データ解析 CG 微分方程式

Cによるスプライン関数―データ解析 CG 微分方程式