python 路径平滑贝塞尔曲线优化
Posted 怪皮蛇皮怪
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了python 路径平滑贝塞尔曲线优化相关的知识,希望对你有一定的参考价值。
python 路径平滑(3)贝塞尔曲线优化
python 路径优化(1)多项式曲线优化
python 路径平滑(2)指定函数曲线优化
python 路径平滑(3)贝塞尔曲线优化
贝塞尔曲线图示
这篇博文里有贝塞尔曲线的动态绘制方法,辅助图可以帮助理解贝塞尔曲线
大佬代码实现
我接下来的代码基本上是大量参考(chao)大佬的代码
import matplotlib.pyplot as plt
import numpy as np
import math
import copy
class BezierSmooth():
# 输入控制点,Points是一个array,num是控制点间的插补个数
def __init__(self,route_x,route_y,route_theta,InterpolationNum=1000):
Points=[]
# for i in range(len(route_x)):
# Points.append([route_x[i],route_y[i],route_theta[i]])
for i in range(len(route_x)):
Points.append([route_x[i],route_y[i]])
Points=np.array(Points)
self.demension=Points.shape[1] # 点的维度
self.order=Points.shape[0]-1 # 贝塞尔阶数=控制点个数-1
self.num=InterpolationNum # 相邻控制点的插补个数
self.pointsNum=Points.shape[0] # 控制点的个数
self.Points=Points
def smooth(self,method=1):
ans=self.getBezierPoints(method)
new_route_x=[]
new_route_y=[]
new_route_theta=[]
if ans.shape[1]==2:
for x,y in ans:
new_route_x.append(x)
new_route_y.append(y)
if ans.shape[1]==3:
for x,y,theta in ans:
new_route_x.append(x)
new_route_y.append(y)
new_route_theta.append(theta)
return new_route_x,new_route_y,new_route_theta
# 获取Bezeir所有插补点
def getBezierPoints(self,method):
if method==0:
return self.DigitalAlgo()
if method==1:
return self.DeCasteljauAlgo()
# 数值解法
def DigitalAlgo(self):
PB=np.zeros((self.pointsNum,self.demension)) # 求和前各项
pis =[] # 插补点
for u in np.arange(0,1+1/self.num,1/self.num):
for i in range(0,self.pointsNum):
PB[i]=(math.factorial(self.order)/(math.factorial(i)*math.factorial(self.order-i)))*(u**i)*(1-u)**(self.order-i)*self.Points[i]
pi=sum(PB).tolist() #求和得到一个插补点
pis.append(pi)
return np.array(pis)
# 德卡斯特里奥解法
def DeCasteljauAlgo(self):
pis =[] # 插补点
for u in np.arange(0,1+1/self.num,1/self.num):
Att=copy.deepcopy(self.Points)
for i in np.arange(0,self.order):
for j in np.arange(0,self.order-i):
Att[j]=(1.0-u)*Att[j]+u*Att[j+1]
pis.append(Att[0].tolist())
return np.array(pis)
测试效果
from bezier_smooth import BezierSmooth
import matplotlib.pyplot as plt
import numpy as np
def new_test():
bezier_smooth=BezierSmooth(x,y,theta)
new_route_x,new_route_y,theta=bezier_smooth.smooth()
new_route_theta=theta
for i in range(len(new_route_x)):
print(new_route_x[i],new_route_y[i],new_route_theta[i]*180/math.pi)
plot1=plt.plot(x, y, '*',label='original values')
plot2=plt.plot(new_route_x, new_route_y, 'r',label='polyfit values')
plt.xlabel('x axis')
plt.ylabel('y axis')
plt.legend(loc=4) # 指定legend的位置,读者可以自己help它的用法
plt.title('polyfitting')
plt.show()
if __name__ == '__main__':
new_test()
以上是关于python 路径平滑贝塞尔曲线优化的主要内容,如果未能解决你的问题,请参考以下文章