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

MyEnigma

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

点列データから曲率を計算するPythonサンプルコード

Python

幾何学入門〈上〉 (ちくま学芸文庫)

幾何学入門〈上〉 (ちくま学芸文庫)

目次

はじめに

たまに点列データから、

その点列の滑らかさを評価するために、

近似的に曲率を計算したくなる時があります。

 

今回は、

いくつかの点群データから曲率を計算する方法の紹介と、

それらをPythonで実装したコードを公開したいと思います。

 

円フィッティングにより、曲率を計算する方法

点群から曲率を計算する方法は色々とありますが、

ある点の前後1点づつを使って、曲率を計算するのではなく、

曲率の計算結果を安定化するために、

より多くの点を使って曲率を計算したい場合は、

シンプルにそれらの点を円フィッティングし、

その半径の逆数を取ったほうが便利な場合があります。

 

そこで、点群のx-y座標と、曲率を計算する点数を与えると、

先日紹介した最小二乗法による円フィッティングを使って、

myenigma.hatenablog.com

数値的に計算されたそれぞれの点の曲率のリストを返す

関数を作りました。

#!/usr/bin/env python
# -*- coding:utf-8 -*-
#   Numeric Curvature Calculation Lib
#
#   author: Atsushi Sakai
from PyCubicSpline import PyCubicSpline

import numpy as np
import math

def CalcCurvature(x,y,npo=1):
    u"""
    Calc curvature
    x,y: x-y position list
    npo: the number of points using calculation curvature
    ex) npo=1: using 3 point
        npo=2: using 5 point
        npo=3: using 7 point
    """

    cv=[]

    ndata=len(x)

    for i in range(ndata):
        lind=i-npo
        hind=i+npo+1

        if lind<0:
            lind=0
        if hind>=ndata:
            hind=ndata
        #  print(lind,hind)

        xs=x[lind:hind]
        ys=y[lind:hind]
        #  print(xs,ys)
        (cxe,cye,re)=CircleFitting(xs,ys)
        #  print(re)

        if len(xs)>=3:
            # sign evalation 
            cind=int((len(xs)-1)/2.0)
            sign = (xs[0] - xs[cind]) * (ys[-1] - ys[cind]) - (ys[0] - ys[cind]) * (xs[-1] - xs[cind])

            # check straight line
            a = np.array([xs[0]-xs[cind],ys[0]-ys[cind]])
            b = np.array([xs[-1]-xs[cind],ys[-1]-ys[cind]])
            theta=math.degrees(math.acos(np.dot(a,b)/(np.linalg.norm(a)*np.linalg.norm(b))))
            #  print(theta)

            if theta==180.0:
                cv.append(0.0)#straight line
            elif sign>0:
                cv.append(1.0/-re)
            else:
                cv.append(1.0/re)
        else:
            cv.append(0.0)

    #  print(cv)
    return cv

def CircleFitting(x,y):
    u"""Circle Fitting with least squared
        input: point x-y positions  

        output  cxe x center position
                cye y center position
                re  radius of circle 

    """

    sumx  = sum(x)
    sumy  = sum(y)
    sumx2 = sum([ix ** 2 for ix in x])
    sumy2 = sum([iy ** 2 for iy in y])
    sumxy = sum([ix * iy for (ix,iy) in zip(x,y)])

    F = np.array([[sumx2,sumxy,sumx],
                  [sumxy,sumy2,sumy],
                  [sumx,sumy,len(x)]])

    G = np.array([[-sum([ix ** 3 + ix*iy **2 for (ix,iy) in zip(x,y)])],
                  [-sum([ix ** 2 *iy + iy **3 for (ix,iy) in zip(x,y)])],
                  [-sum([ix ** 2 + iy **2 for (ix,iy) in zip(x,y)])]])

    try:
        T=np.linalg.inv(F).dot(G)
    except:
        return (0,0,float("inf"))

    cxe=float(T[0]/-2)
    cye=float(T[1]/-2)
    #  print (cxe,cye,T)
    try:
        re=math.sqrt(cxe**2+cye**2-T[2])
    except:
        return (cxe,cye,float("inf"))
    return (cxe,cye,re)



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,3,0.01)
    ry=[spline.Calc(i) for i in rx]
    rc=[spline.CalcCurvature(i) for i in rx]

    nc=CalcCurvature(rx,ry,2)

    #  plt.plot(x,y,"xb")
    #  plt.plot(rx,ry,"-r")
    #  plt.axis("equal")
    plt.plot(rx,rc,"-b",label="True Curvature")
    plt.plot(rx,nc,"xr",label="Numeric Curvature")
    plt.grid(True)
    plt.legend(loc="lower right")
    plt.show()

 

上記のコードの注意点としては、

円フィッティングですと、曲率の方向がわからないので、

点列のベクトルの角度を別途計算し、曲率の符号を決めています。

また、入力点列が直線ですと、半径の計算が不安定になるので、

その部分にエラー処理を入れています。

 

上記のコードでは、計算結果を評価するために、

先日紹介した3次スプラインのコードを使っています。

myenigma.hatenablog.com

この記事で説明しているとおり、

3次スプラインは解析的に曲率を計算することができるので、

それを使って、答え合わせをしています。

  

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

f:id:meison_amsl:20161023042641p:plain

真の曲率データに対して、それなりの精度で曲率が計算できていることがわかります。

 

コードの公開場所

すべてのコードは下記で公開しています。

github.com

 

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

幾何学入門〈上〉 (ちくま学芸文庫)

幾何学入門〈上〉 (ちくま学芸文庫)