Scipyのsplprepによるスプライン補間について

matplotlibのキャンバス上に手入力で制御点を打ち，その点をスプライン補間しています。スプライン曲線の算出にはScipyのsplprepを用いています。しかし打ち込む点が多くなると，曲線が滑らかさを失い、がたつきがある線になってしまいます。原因はなんでしょうか。
また、算出した曲線の曲率を算出したいのですが、近接した3点を取る方法がわかりません。ご教授願います。よろしくお願いします。

python
1import matplotlib.pyplot as plt
2import numpy as np
3from scipy import interpolate
4
5
6def main():
7    fig = plt.figure(figsize=(11.0, 5.0))
8    ClickAddPoints(fig)
9
10
11class ClickAddPoints:
12    def __init__(self, fig):
13        self.fig = fig
14        self.ax1 = fig.add_subplot(121)
15        # ax2に曲率を表示
16        self.ax2 = fig.add_subplot(122)
17        self.ax1.figure.canvas.mpl_connect('button_press_event', self.on_click)
18        self.ax1.figure.canvas.mpl_connect('key_press_event', self.on_key)
19        self.x = []
20        self.y = []
21        self.r = []
22        self.s = []
23        self.x2 = []
24        self.y2 = []
25        self.update_plot()
26        self.div = 1000  # スプラインの分割数。多いほどなめらかに描画。
27        plt.show()
28
29    def calc_spline(self, x, y, point, deg):
30        tck, u = interpolate.splprep([x, y], k=deg, s=0)
31        u = np.linspace(0, 1, num=point, endpoint=True)
32        spline = interpolate.splev(u, tck, der=0)
33        return spline[0], spline[1]
34
35    def cal_curveture(self, x1, y1, x2, y2, x3, y3):
36        r = 1/(abs(x1*y2-x2*y1+x2*y3-x3*y2+x3*y1-x1*y3)/(
37            ((x1-x2)**2+(y1-y2)**2)*((x2-x3)**2+(y2-y3)**2)*((x3-x1)**2+(y3-y1)**2))**(1/2))
38        return r
39
40    def cal_curve_length(self, x1, y1, x2, y2):
41        s = np.sqrt((x2-x1)**2+(y2-y1)**2)
42        return s
43
44    def cal_clothoid(self, T, dt, V, R, _P0x, _P0y, _dθ):
45        #クロソイドの計算
46        v = (V*1000)/3600
47        L = v*T
48        A = np.sqrt(L*R)
49        dL = v*dt
50        P0x = _P0x
51        P0y = _P0y
52        Lt = 0
53        dθ = _dθ
54        dLt = dL
55        A2 = A**2
56        tsteps = np.arange(0, T, dt)
57
58        for i in tsteps:
59            x2 = P0x
60            y2 = P0y
61
62            self.x2.append(x2)
63            self.y2.append(y2)
64
65            Lt = Lt + dLt
66            Rt = A2 / Lt
67            k = 1 / Rt
68            dθ = dθ + dLt / Rt
69            dx = dL * np.cos(dθ)
70            dy = dL * np.sin(dθ)
71            P1x = P0x + dx
72            P1y = P0y + dy
73            P0x = P1x
74            P0y = P1y
75
76        return self.x2, self.y2
77
78    def update_plot(self):
79        self.ax1.set_xticks(np.linspace(0, 10, 5))
80        self.ax1.set_yticks(np.linspace(0, 10, 5))
81        self.ax1.set_aspect('equal')
82        self.ax1.figure.canvas.draw()
83        self.ax2.set_ylim(0, 40)
84        self.ax2.figure.canvas.draw()
85
86    def on_key(self, event):
87        # qキーで終了
88        if event.key == 'q':
89            print('Finish')
90            return
91
92        # wキーで全削除
93        if event.key == 'w':
94            self.x.clear()
95            self.y.clear()
96            self.redraw()
97            print('All Clear')
98
99        # pキーでスプライン
100        if event.key == 'p':
101            print('spline')
102
103        # cキーでクロソイド
104        elif event.key == 'c':
105            print('clothoid')
106
107        # tキーで直線
108        elif event.key == 't':
109            print('straight line')
110
111        else:
112            return
113
114    def on_click(self, event):
115        if event.button == 1:
116            # コントロール点を追加
117            if event.inaxes is not self.ax1:
118                return
119            # 過去の座標と同一座標をクリックした場合は抜ける
120            if event.xdata in self.x and event.ydata in self.y:
121                return
122            self.x.append(event.xdata)
123            self.y.append(event.ydata)
124            self.redraw()
125            print('Added   no.{} point at [{} {}]'.format(len(self.x), self.x[-1], self.y[-1]))
126        elif event.button == 3:
127            # 直近のコントロール点を削除
128            if len(self.x) > 0:
129                self.x.pop()
130                self.y.pop()
131                self.redraw()
132                print('Removed no.{0} point'.format(len(self.x)+1))
133
134    def redraw(self):
135        self.ax1.cla()
136        self.ax2.cla()
137        count = len(self.x)
138        if count > 0:
139            # クリック点の描画
140            self.ax1.plot(self.x, self.y, 'ro')
141        if count <= 1:
142            self.update_plot()
143            return
144        if count == 2:
145            deg = 1
146        elif count == 3:
147            deg = 2
148        elif count > 3:
149            deg = 3
150        else:
151            return
152
153        x1, y1 = self.calc_spline(self.x, self.y, self.div, deg)
154        #曲線長
155        if len(self.x) >= 2:
156            self.s = [self.cal_curve_length(x1[i],y1[i],
157                                            x1[i+1],y1[i+1])
158                      for i in range(0, len(x1)-1)]
159
160        # 曲率データの更新
161        if len(self.x) > 2:
162            self.r = [self.cal_curveture(x1[i-1], y1[i-1],
163                                         x1[i],   y1[i],
164                                         x1[i+1], y1[i+1])
165                      for i in range(1, len(x1)-1)]
166
167        # 線の描画
168        self.ax1.plot(x1, y1)
169        self.ax2.plot(self.r)
170        self.update_plot()
171
172
173main()

行動規範の内容に同意します

回答1件

ベストアンサー

曲線が滑らかさを失い、がたつきがある線になってしまいます。原因はなんでしょうか。

np.linspace(0, 1, num, endpoint)
のうち、numパラメータは曲線の分割数に対応しています。（曲線をnum-1個の直線に分割）
numが大きければ大きいほど、なめらかになります。
たとえばnum=3とすると、最初からガタガタの線になります。
質問元のコードだと300なので、100個以上コントロールポイントを追加したあたりからガタつきが目立ってくるのかもしれません。
下記コードでは、__init__内で、div属性を分割数として設定しています。（1000個に分割）

近接した3点を取る方法がわかりません。

x1, y1 = self.calc_spline(self.x, self.y, self.div, deg)
のx1、y1はそれぞれ曲線を構成するdiv個に分割された点のx座標配列、y座標配列となっているので、
この配列から3点を取り出し、曲率を計算しているcal_curvetureに渡せばOKです。
コード中の

        # 曲率データの更新
        if len(self.x) > 2:
            self.k = [self.cal_curveture(x1[i-1], y1[i-1],
                                         x1[i],   y1[i],
                                         x1[i+1], y1[i+1]) 
                      for i in range(1, len(x1)-1)]

の部分です。

全体コードは以下：

import matplotlib.pyplot as plt
import numpy as np
from scipy import interpolate


def main():
    fig = plt.figure(figsize=(11.0, 5.0))
    ClickAddPoints(fig)


class ClickAddPoints:
    def __init__(self, fig):
        self.fig = fig
        self.ax1 = fig.add_subplot(121)
        # ax2に曲率を表示
        self.ax2 = fig.add_subplot(122)
        self.ax1.figure.canvas.mpl_connect('button_press_event', self.on_click)
        self.ax1.figure.canvas.mpl_connect('key_press_event', self.on_key)
        self.x = []
        self.y = []
        self.k = []
        self.update_plot()
        self.div = 1000  # スプラインの分割数。多いほどなめらかに描画。
        plt.show()

    def calc_spline(self, x, y, point, deg):
        tck, u = interpolate.splprep([x, y], k=deg, s=0)
        u = np.linspace(0, 1, num=point, endpoint=True)
        spline = interpolate.splev(u, tck, der=0)
        return spline[0], spline[1]

    def cal_curveture(self, x1, y1, x2, y2, x3, y3):
        k = abs(x1*y2-x2*y1+x2*y3-x3*y2+x3*y1-x1*y3)/(
            ((x1-x2)**2+(y1-y2)**2)*((x2-x3)**2+(y2-y3)**2)*((x3-x1)**2+(y3-y1)**2))**(1/2)
        return k

    def update_plot(self):
        self.ax1.set_xticks(np.linspace(0, 6, 6))
        self.ax1.set_yticks(np.linspace(0, 6, 6))
        self.ax1.set_aspect('equal')
        self.ax1.figure.canvas.draw()
        self.ax2.figure.canvas.draw()

    def on_click(self, event):
        if event.button == 1:
            # コントロール点を追加
            if event.inaxes is not self.ax1:
                return
            # 過去の座標と同一座標をクリックした場合は抜ける
            if event.xdata in self.x and event.ydata in self.y:
                return
            self.x.append(event.xdata)
            self.y.append(event.ydata)
            self.redraw()
            print('Added   no.{} point at [{} {}]'.format(len(self.x), self.x[-1], self.y[-1]))
        elif event.button == 3:
            # 直近のコントロール点を削除
            if len(self.x) > 0:
                self.x.pop()
                self.y.pop()
                self.redraw()
                print('Removed no.{0} point'.format(len(self.x)+1))

    def redraw(self):
        self.ax1.cla()
        self.ax2.cla()
        count = len(self.x)
        if count > 0:
            # クリック点の描画
            self.ax1.plot(self.x, self.y, 'ro')
        if count <= 1:
            self.update_plot()
            return
        if count == 2:
            deg = 1
        elif count == 3:
            deg = 2
        elif count > 3:
            deg = 3
        else:
            return

        x1, y1 = self.calc_spline(self.x, self.y, self.div, deg)
        # 曲率データの更新
        if len(self.x) > 2:
            self.k = [self.cal_curveture(x1[i-1], y1[i-1],
                                         x1[i],   y1[i],
                                         x1[i+1], y1[i+1]) 
                      for i in range(1, len(x1)-1)]
        # 線の描画
        self.ax1.plot(x1, y1)
        self.ax2.plot(self.k)
        self.update_plot()

    def on_key(self, event):
        # qキーで終了
        if event.key == 'q':
            print('Finish')
            return

        # wキーで全削除
        if event.key == 'w':
            self.x.clear()
            self.y.clear()
            self.redraw()
            print('All Clear')


main()

投稿2020/07/01 14:07

編集2020/07/01 14:18

kotori_a

総合スコア687

takes.it.easy

2020/07/02 06:41

Pキーでスプライン曲線(手入力)、Cキーで計算された別の曲線(パラメータ入力)を描くように条件分岐させるにはどのようにすればよいでしょうか。

kotori_a

2020/07/02 07:37 編集

曲線の手入力/パラメータ入力とは、pythonコードでいうとどういう内容でしょうか？

takes.it.easy

2020/07/02 08:46

手入力はマウスでクリックし、点を追加する場合です。コードではdef on_clickのところでしょうか。パラメータ入力は必要なパラメータを入力し、計算結果を返して点を追加する場合です。コードではdef cal_clothoidがその部分になります。

kotori_a

2020/07/02 09:46

クロソイド曲線と、クリックして生成したスプラインの間に何も関連はないという理解でよろしいでしょうか。 cキーを押したら、cal_clothoidの引数T, dt, V, R, _P0x, _P0y, _dθを全てinput等で手入力するモードに切り替わり、引数手入力完了後、左のax1に描かれたスプライン曲線をクリアして新たにクロソイド曲線を描画するということでしょうか？

takes.it.easy

2020/07/02 10:16

cキーを押したら、cal_clothoidの引数を全てinput等で手入力するモードに切り替わるところはそのような理解で問題ありません。 cキーでクロソイドモードに切り替わり、生成された曲線を、それまでのスプライン曲線に曲率の連続性を保ちながら接続するには、それぞれの接線方向を揃える必要があるのでしょうか。もしそうであれば、スプライン曲線とクロゾイド曲線の間には全く関連がないとは言えません。

kotori_a

2020/07/02 12:00 編集

当方Clothoid曲線に対する知見がなく、検索しても、似たようなソースを記載しているサイトを見つけることができませんでした。 cal_clothoidの引数　T, dt, V, R, _P0x, _P0y, _dθ　それぞれの内容について教えてください。たとえば、「P0x＝クロソイド曲線の始点のx座標、_P0y,＝同y座標、dt：分割間隔（0.01 等）　dΘ分割角（30等）～」といった具合です。（カッコ内の数字は適当です）

takes.it.easy

2020/07/03 03:14

クロソイド曲線は速度一定で走行する車両のハンドルを一定の角速度で回したときの車両の軌跡を表します。車速と操舵角速度がパラメータとして反映されているのが特徴です。一般的に直線から円弧、または円弧から直線へ移行する際の緩和曲線として使われます。以下cal_clithoidの引数の説明になります。 R：接続したい円弧の半径。単位：m (R=5) T：接続したい円弧に到達するまでのハンドルを回すのにかかる時間。単位：ｓ (2s) dt：Tの分割間隔(0.01) V：車速。単位：km/h　(5km/h) _P0x：クロソイド曲線の始点のx座標 _P0y：同y座標 dθ：初期角度（方位？）。スプライン曲線終点の方位を入力することで曲率連続な接続が可能と思われる。単位：radian

kotori_a

2020/07/03 17:25

スプライン終点の2点からarctanを用いて角度を計算し、その方向のクロソイドを生成するように組みました。ソースは長いので別ページに記載しました。 https://gist.github.com/taizan-hokuto/6b54285d71943ef8afc61a521cc81fe0 なお、RuntimeWarning: invalid value encountered in double_scalarsが発生することがありますが。この原因については今のところ追求しきれていません。（おそらく曲線長の計算で、丸めが発生しているため？）これ以上はさらに数学的な知識が必要となり私の手に負えなさそうです。

takes.it.easy

2020/07/04 04:08

ありがとうございます。最後に一つよろしいでしょうか。モード切替をした際に，それまでに描画した線はそのまま残したいのですが，どのようにしたらできますでしょうか。たとえばスプライン曲線を何点か描画した後，直線モードに移行すると，それまでのスプライン曲線が直線になってしまうのですが，スプライン曲線はスプライン曲線で残したいのです。よろしくお願いします。

kotori_a

2024/04/22 12:14 編集

・tキーによる直線重ね合わせ表示／非表示を切り替えられるようにしました。・直線の場合に右側プロットで曲率がゼロ以外になるのを修正・ RuntimeWarning: invalid value encountered in double_scalars エラーを修正しました。

takes.it.easy

2020/07/04 12:12

丁寧な回答からその後の要望への対応もありがとうございました。とても勉強になりました。

行動規範の内容に同意します