Bezier曲線を用いて経路生成を行いたい。

Bezier曲線を用いて経路生成を行おうとしています。コードを実行すると、キャンバスが出てきます。左画面上を左クリックすると1点追加，右クリックで直近の1点削除です。3点追加されると、Bezier曲線が表示されるようにしたいのですが、上手くいきません。Bezier曲線を計算する関数から値が戻ってきているか不明です。Bezier曲線を連続で接続したいです。ある区間の終点とその次の区間の始点が一致し、さらに1階微分まで一致させたいです。
Bezier曲線は各区間の始点(P0)と終点(P2)を通り、その間の点を制御点(P1)とする2次Bezier曲線です。よろしくお願い致します。

Python
1import matplotlib.pyplot as plt
2import numpy as np
3from scipy import interpolate
4
5def main():
6    fig = plt.figure(figsize=(11.0, 5.0))
7    ClickAddPoints(fig)
8
9class ClickAddPoints:
10    def __init__(self, fig):
11        self.fig = fig
12        self.ax1 = fig.add_subplot(121)
13        # ax2に曲率を表示
14        self.ax2 = fig.add_subplot(122)
15        self.ax1.figure.canvas.mpl_connect('button_press_event', self.on_click)
16        self.ax1.figure.canvas.mpl_connect('key_press_event', self.on_key)
17        self.x = []
18        self.y = []
19        self.r = []
20        self.update_plot()
21        plt.show()
22
23    def calc_bezier(self, x1, y1, x2, y2, x3, y3):
24
25        P0x = 0
26        P0y = 0
27        bez_x = []
28        bez_y = []
29        dt = 100
30        tstep = np.arange(0, 1, dt)
31        for i in tstep:
32            x = P0x
33            y = P0y
34
35            bez_x.append(x)
36            bez_y.append(y)
37
38            #ベジェ計算
39            x = (x3 - 2*x2 + x1)*i**2 + 2*(x2 - x1)*i + x1
40            y = (y3 - 2*y2 + y1)*i**2 + 2*(y2 - y1)*i + y1
41            #1階微分
42            dx = 2*(x3 - 2*x2 + x1)*i + 2*(x2 - x1)
43            dy = 2*(y3 - 2*y2 + y1)*i + 2*(y2 - y1)
44            #2階微分
45            ddx = 2*(x3 - 2*x2 + x1)
46            ddy = 2*(y3 - 2*y2 + y1)
47
48            P1x = P0x + x
49            P1y = P0y + y
50            P0x = P1x
51            P0y = P1y
52
53        return bez_x, bez_y
54
55    def update_plot(self):
56        self.ax1.set_xticks(np.linspace(0, 6, 6))
57        self.ax1.set_yticks(np.linspace(0, 6, 6))
58        self.ax1.set_aspect('equal')
59        self.ax1.figure.canvas.draw()
60        self.ax2.set_ylim(0, 40)
61        self.ax2.figure.canvas.draw()
62
63    def on_click(self, event):
64        if event.button == 1:
65            # コントロール点を追加
66            if event.inaxes is not self.ax1:
67                return
68            # 過去の座標と同一座標をクリックした場合は抜ける
69            if event.xdata in self.x and event.ydata in self.y:
70                return
71            self.x.append(event.xdata)
72            self.y.append(event.ydata)
73            self.redraw()
74            print('Added   no.{} point at [{} {}]'.format(len(self.x), self.x[-1], self.y[-1]))
75        elif event.button == 3:
76            # 直近のコントロール点を削除
77            if len(self.x) > 0:
78                self.x.pop()
79                self.y.pop()
80                self.redraw()
81                print('Removed no.{0} point'.format(len(self.x)+1))
82
83    def redraw(self):
84        self.ax1.cla()
85        self.ax2.cla()
86        count = len(self.x)
87        if count > 0:
88            # クリック点の描画
89            self.ax1.plot(self.x, self.y, 'ro')
90
91        elif count >= 3:
92            x1, y1 = [self.calc_bezier(self.x[i-1], self.y[i-1], self.x[i], self.y[i], self.x[i+1], self.y[i+1])for i in range(1, len(self.x)-1, 2)]
93            print(x1)
94            self.ax1.plot(self.x, self.y, 'ro')
95            self.ax1.plot(x1, y1)
96            print()
97
98        else:
99            return
100
101        # 線の描画
102
103        self.ax2.plot(self.r)
104        self.update_plot()
105
106    def on_key(self, event):
107        # qキーで終了
108        if event.key == 'q':
109            print('Finish')
110            return
111
112        # wキーで全削除
113        if event.key == 'w':
114            self.x.clear()
115            self.y.clear()
116            self.redraw()
117            print('All Clear')
118
119main()

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

回答1件

ベストアンサー

一階微分を一致させる云々の前に、とりあえずベジェ曲線を描くことを目指しました。

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.update_plot()
23        plt.show()
24
25    def calc_bezier(self, x1, y1, x2, y2, x3, y3):
26        tstep = np.linspace(0, 1, 100)
27        bez_x = (1-tstep)**2*x1+2*tstep*(1-tstep)*x2+tstep**2*x3
28        bez_y = (1-tstep)**2*y1+2*tstep*(1-tstep)*y2+tstep**2*y3
29        return bez_x, bez_y
30
31    def update_plot(self):
32        self.ax1.set_xticks(np.linspace(0, 6, 6))
33        self.ax1.set_yticks(np.linspace(0, 6, 6))
34        self.ax1.set_aspect('equal')
35        self.ax1.figure.canvas.draw()
36        self.ax2.set_ylim(0, 40)
37        self.ax2.figure.canvas.draw()
38
39    def on_click(self, event):
40        if event.button == 1:
41            # コントロール点を追加
42            if event.inaxes is not self.ax1:
43                return
44            # 過去の座標と同一座標をクリックした場合は抜ける
45            if event.xdata in self.x and event.ydata in self.y:
46                return
47            self.x.append(event.xdata)
48            self.y.append(event.ydata)
49            self.redraw()
50            print('Added   no.{} point at [{} {}]'.format(
51                len(self.x), self.x[-1], self.y[-1]))
52        elif event.button == 3:
53            # 直近のコントロール点を削除
54            if len(self.x) > 0:
55                self.x.pop()
56                self.y.pop()
57                self.redraw()
58                print('Removed no.{0} point'.format(len(self.x)+1))
59
60    def redraw(self):
61        self.ax1.cla()
62        self.ax2.cla()
63        count = len(self.x)
64        if 0 < count < 3:
65            # クリック点の描画
66            self.ax1.plot(self.x, self.y, 'ro')
67
68        elif count >= 3:
69            x1 = []
70            y1 = []
71            for i in range(1, len(self.x)-1, 2):
72                x1temp, y1temp = self.calc_bezier(self.x[i-1], self.y[i-1], self.x[i], self.y[i],
73                                                  self.x[i+1], self.y[i+1])
74                x1.extend(x1temp)
75                y1.extend(y1temp)
76            self.ax1.plot(self.x, self.y, 'ro')
77            self.ax1.plot(x1, y1)
78        else:
79            return
80
81        # 線の描画
82
83        self.ax2.plot(self.r)
84        self.update_plot()
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
100main()
101

投稿2020/07/20 13:05

Penpen7

総合スコア698

takes.it.easy

2020/07/21 02:32

回答ありがとうございます。 calc_bezier関数内でtstepを計算式に直接代入していますが、これによってどのような計算が行われるのでしょうか。一度プログラムを回すと、tstep内のすべての数値を代入して計算し、それぞれ算出された数値をbezier_x, bezier_yに格納しているのでしょうか。また、クリックによって点を追加するような手法を用いていると、P0の終点とP1の始点の一階微分を一致させるようなことは難しいのでしょうか。

Penpen7

2020/07/21 04:31 編集

> tstep内のすべての数値を代入して計算し、それぞれ算出された数値をbezier_x, bezier_yに格納しているのでしょうか。その通りです。numpy配列の機能を使っています。 https://www.headboost.jp/python-numpy-array-calculations/ 基本的にはpythonのfor文は実行速度が遅いため、できる限りnumpyの機能を使用してfor文を減らすといいです。 > また、クリックによって点を追加するような手法を用いていると、P0の終点とP1の始点の一階微分を一致させるようなことは難しいのでしょうか。制御点によって概形が決定されるので、ユーザー側ではなくプログラムの方で適切に制御点を定めれば可能だと思います。その場合、始点と終点をユーザー側に求める形になるでしょう。

Penpen7

2020/07/21 06:00 編集

https://postd.cc/bezier-curves/ の図をみるとおり, P0とP2を始点終点とする二次ベジェ曲線の場合, P0における傾きは直線P0P1の傾きであり、P2における傾きは直線P1P2の傾きに一致します。 P2において接線が存在して、新しくP2, P3, P4を始点, 制御点, 終点としてP2において滑らかなベジェ曲線を描きたい場合、P2の接線上にP3を配置すればいいのではないでしょうか。(ただし直線のどこにP3を置くかは任意性があるため他の方法で決める必要がある)

takes.it.easy

2020/07/22 02:56

理解しました。丁寧な回答、解説ありがとうございます。

Penpen7

2020/07/22 03:59

なのでやることとしては、最初の3点はユーザー側が自由に決めてしまっても良いです。以降はユーザーは終点を必ず指定することになります。(下に述べるように一直線上にあるというルールを守るようにするなら、制御点もユーザー側で決めて良い) 次にユーザーがキャンバス上をクリックしたとすると、その点はP4とし、新しいベジェ曲線の終点としておきます。P3は制御点となりますが、この時にP1P2P3が一直線上になるようにP3を配置します。そうすると、ベジェ曲線P0P2P4は滑らかに繋がることになりますね。ただしP3は直線P1P2上にありさえすればいいので、直線上のどこに置くかは自由に決めてもいいわけです。直線上のどこに置くかはプログラム上で一定のルールを決めておいて自動的に決めるか、もしくはユーザー側に決定権を委ねるかどちらかでしょう。この繰り返しでベジェ曲線を滑らかに繋げることができると思います。

takes.it.easy

2020/07/22 09:21

なるほどですね。いまP3の制御点の扱いについて考えています。いまのところ、ルールを決めて自動的に配置し、その後、微調整を行いたい場合は点をドラッグして直線上を動かす、というものです。ルールは算出された曲率半径の最小値が指定した数値以上になること。疑問なのは点をドラッグして動かす場合，P1とP2の接線上のみを動くようにできるかどうかです。しかし，ベジェ曲線の曲率を考慮しながら曲線を描く場合，二階微分可能な3次ベジェ曲線を使用すべきなのでしょうか。2次ベジェ曲線で曲率を考慮することは難しいでしょうか。

Penpen7

2020/07/23 10:10

ある直線上で点を動かすというのは、UIの工夫でなんとかなるんじゃないかと思います。 (matplotlibでそれが達成可能かはわかりませんが) 二次ベジェ曲線においては制御点のx座標とy座標の2つ自由度があると思います。滑らかに繋げるというので、1自由度、曲線のある点での曲率をある値に決めてやるともう1つ自由度を消費すると思うので、答えを言うと、2次ベジェ曲線においても曲率を考慮することは可能なのではないでしょうか。

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