# CRS曲线拟合：centripetal Catmull-Rom spline

### CRS曲线拟合

import numpy, sys, math

# 首尾相接以保证闭合曲线，论文中有方法
def CatmullRomSpline(P0, P1, P2, P3, nPoints=100):
"""
P0, P1, P2, and P3 should be (x,y,z) point triples that define the Catmull-Rom spline.
nPoints is the number of points to include in this curve segment.
"""
# Convert the points to numpy so that we can do array multiplication
P0, P1, P2, P3 = map(numpy.array, [P0, P1, P2, P3])

# Calculate t0 to t4
alpha = 0.5
def tj(ti, Pi, Pj):
return sum([(i-j)**2 for i,j in zip(Pi,Pj)])**alpha + ti

t0 = 0
t1 = tj(t0, P0, P1)
t2 = tj(t1, P1, P2)
t3 = tj(t2, P2, P3)

# Only calculate points between P1 and P2
t = numpy.linspace(t1,t2,nPoints)

# Reshape so that we can multiply by the points P0 to P3
# and get a point for each value of t.
t = t.reshape(len(t),1)
A1 = (t1-t)/(t1-t0)*P0 + (t-t0)/(t1-t0)*P1
A2 = (t2-t)/(t2-t1)*P1 + (t-t1)/(t2-t1)*P2
A3 = (t3-t)/(t3-t2)*P2 + (t-t2)/(t3-t2)*P3
if CatmullRomSpline.toDebug:
print(t)
print(A1)
print(A2)
print(A3)
B1 = (t2-t)/(t2-t0)*A1 + (t-t0)/(t2-t0)*A2
B2 = (t3-t)/(t3-t1)*A2 + (t-t1)/(t3-t1)*A3

C  = (t2-t)/(t2-t1)*B1 + (t-t1)/(t2-t1)*B2
return C

CatmullRomSpline.toDebug = False

def CatmullRomChain(P,nP=100):
"""
Calculate Catmull Rom for a chain of points and return the combined curve.
"""
sz = len(P)

# The curve C will contain an array of (x,y,z) points.
C = []
for i in range(sz-3):
c = CatmullRomSpline(P[i], P[i+1], P[i+2], P[i+3], nP)
C.extend(c)
return C

""""Read a point list file, and retun a list of 3D coordinates."""

plist = []
try:
f = open(fname, 'r')
except IOError:
print ('CatmullRom: Cannot open file %s for reading' % fname)
raise
for line in f:
tempwords = [ t.strip('\n') for t in line.split(',') ]
if len(tempwords) == 3:
try:
plist.append ([float(tempwords[0]), float(tempwords[1]), math.degrees(float(tempwords[2]))])
except:
print ('Número Inválido: %s\n' % tempwords)
continue

f.close()
return plist

def main(argv=None):
"""
Uses matplotlib for drawing a curve.
"""

import pylab as plt

if argv is None:
argv = sys.argv

# Define a set of points for curve to go through
if len(argv) > 1:
else:
# 一系列控制点
# Points = [[0,1.5],[2,2],[3,1],[4,0.5],[5,1],[6,2],[7,3]]
Points = [[79,  353], [87, 358], [126, 372], [77, 398], [21, 426], [5, 421], [5, 414], [0, 404], [2, 349], [9, 346],
[26, 344], [47, 346], [59, 349]]

# plt.plot(Points[0], Points[:, 1])
x = [Points[i][0] for i in range(len(Points))]
y = [Points[i][1] for i in range(len(Points))]
plt.plot(x, y)

# Turn debugging ON
CatmullRomSpline.toDebug = False

# Calculate the Catmull-Rom splines through the points
c = CatmullRomChain(Points, nP=1024)

# Convert the Catmull-Rom curve points into x and y arrays and plot
x, y = zip(*c)
plt.plot(x, y, 'r')

# Plot the control points
px, py = zip(*Points)
plt.plot(px,py,'or')

plt.show()

# https://matplotlib.org/mpl_toolkits/mplot3d/tutorial.html
# import matplotlib as mpl
# from mpl_toolkits.mplot3d import Axes3D
# import numpy as np
# import matplotlib.pyplot as plt
#
# mpl.rcParams['legend.fontsize'] = 10
#
# fig = plt.figure()
# ax = fig.gca(projection='3d')
# #theta = np.linspace(-4 * np.pi, 4 * np.pi, 100)
# #z = np.linspace(-2, 2, 100)
# #r = z**2 + 1
# #x = r * np.sin(theta)
# #y = r * np.cos(theta)
# ax.plot(x, y, z, label='spline curve')
# ax.legend()
#
# plt.show()

if __name__=="__main__":
sys.exit(main())

posted @ 2020-03-20 18:58  pigcv  阅读(985)  评论(0编辑  收藏  举报