MyEnigma

とある自律移動システムエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

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

目次

はじめに

たまに点列データから、

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

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

 

今回は、

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

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

 

Pythonコード

今回紹介する曲率の計算方法のPythonコードは、

下記で公開しています。

github.com

 

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

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

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

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

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

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

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

 

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

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

myenigma.hatenablog.com

数値的に計算されたそれぞれの点の曲率のリストを返す関数を作りました。

github.com

 

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

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

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

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

  

2. 方位差分による曲率計算

2つ目の手法は、点列データの方位情報がある場合に、

前後の曲率の差を二点の距離で割ることにより、

曲率を計算する方法です。

f:id:meison_amsl:20200812141829p:plain

こちらの計算は、下記の関数で計算しています。

github.com

 

3. 点列の二次微分を近似して計算する方法

3つ目の方法は、点列のデータから二次微分まで近似し、

曲率を計算する方法です。

f:id:meison_amsl:20200812142239p:plain

こちらの計算は、下記の関数で計算しています。

github.com

 

4. 4次のルンゲクッタで二次微分を近似する方法

最後の方法は、3つ目の方法と曲率の計算方法は同じですが、

二次微分を4次のルンゲクッタ法(下記の記事参照)

myenigma.hatenablog.com

で微分する方法です。

下記で、4次のルンゲクッタで二次微分を近似します。

f:id:meison_amsl:20200812140803p:plain

こちらの計算は、下記の関数で計算しています。

github.com

 

上記の方法の比較 

上記の手法の計算結果を評価するために、

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

myenigma.hatenablog.com

 

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

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

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

 

下記が比較結果です。

4つの手法とも、ほぼ同じように解析的に求めた曲率と同じような

計算結果になっています。

f:id:meison_amsl:20200812140637p:plain

 

下記の図は、曲率の比較のための

ズームアップしたものです。

ルンゲクッタと二次微分が若干異なる振る舞いをしていますが,

(バグかもしれません)

ほぼ同じようなカーブを描いています。

f:id:meison_amsl:20200812140649p:plain

 

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

 

MyEnigma Supporters

もしこの記事が参考になり、

ブログをサポートしたいと思われた方は、

こちらからよろしくお願いします。

myenigma.hatenablog.com