def omega2thetadot(t, o):
mat1 = Matrix([[1, sin(t[0])*tan(t[1]), cos(t[0])*tan(t[1])],
[0, cos(t[0]), -sin(t[0])],
[0, sin(t[0])/cos(t[1]), cos(t[0])/cos(t[1])]])
#mat1 = [[0, -sin(t[0]), cos(t[0])*cos(t[1])], [0, cos(t[0]), sin(t[0])*cos(t[1])], [1, 0, -sin(t[1])]]
return mat1*o
def thetadot2omega(tD, t):
mat1 = Matrix([[1, 0, -sin(t[1])], [0, cos(t[0]), cos(t[1])*sin(t[0])], [0, -sin(t[0]), cos(t[1])*cos(t[0])]])
#mat2 = [[0, -sin(t[0]), cos(t[0])*sin(t[1])], [0, cos(t[0]), sin(t[0])*cos(t[1])], [1, 0, -sin(t[1])]]
print "thetaDot"
print tD
print mat1
print mat1*tD
return mat1*tD
def composeAffineMatrix(theta):
alpha = theta[0]
rot = np.array( [[cos(alpha), -sin(alpha)],
[sin(alpha), cos(alpha)]] )
s_sk = np.array( [[theta[1], theta[4]],
[theta[3], theta[2]]] )
trans = np.array( [theta[5], theta[6]] )
rot_scale = np.dot(s_sk, rot)
res = np.column_stack((rot_scale, trans))
res = np.vstack((res, [0, 0, 1]))
return res
def helixHeteroscedasticDiags():
#Parameterise a helix (no noise)
fig5 = plt.figure()
t = arange(-1,1,0.0005)
x = map(lambda x: x + gauss(0,0.001 + 0.001*sin(2*pi*x)**2), (1 - t*t)*sin(4*pi*t))
y = map(lambda x: x + gauss(0,0.001 + 0.001*sin(2*pi*x)**2), (1 - t*t)*cos(4*pi*t))
z = map(lambda x: x + gauss(0,0.001 + 0.001*sin(2*pi*x)**2), t)
line = array(zip(x,y,z))
lpc = LPCImpl(h = 0.1, t0 = 0.1, mult = 1, it = 500, scaled = False, cross = False)
lpc_curve = lpc.lpc(X=line)
ax = Axes3D(fig5)
ax.set_title('helixHeteroscedastic')
curve = lpc_curve[0]['save_xd']
ax.scatter(x,y,z, c = 'red')
ax.plot(curve[:,0],curve[:,1],curve[:,2])
saveToPdf(fig5, '/tmp/helixHeteroscedastic.pdf')
residuals_calc = LPCResiduals(line, tube_radius = 0.2, k = 20)
residual_diags = residuals_calc.getPathResidualDiags(lpc_curve[0])
fig6 = plt.figure()
#plt.plot(lpc_curve[0]['lamb'][1:], residual_diags['line_seg_num_NN'], drawstyle = 'step', linestyle = '--')
plt.plot(lpc_curve[0]['lamb'][1:], residual_diags['line_seg_mean_NN'])
plt.plot(lpc_curve[0]['lamb'][1:], residual_diags['line_seg_std_NN'])
saveToPdf(fig6, '/tmp/helixHeteroscedasticPathResiduals.pdf')
coverage_graph = residuals_calc.getCoverageGraph(lpc_curve[0], arange(0.01, .052, 0.01))
fig7 = plt.figure()
plt.plot(coverage_graph[0],coverage_graph[1])
saveToPdf(fig7, '/tmp/helixHeteroscedasticCoverage.pdf')
residual_graph = residuals_calc.getGlobalResiduals(lpc_curve[0])
fig8 = plt.figure()
plt.plot(residual_graph[0], residual_graph[1])
saveToPdf(fig8, '/tmp/helixHeteroscedasticResiduals.pdf')
fig9 = plt.figure()
plt.plot(range(len(lpc_curve[0]['lamb'])), lpc_curve[0]['lamb'])
saveToPdf(fig9, '/tmp/helixHeteroscedasticPathLength.pdf')
def helixHeteroscedasticCrossingDemo():
#Parameterise a helix (no noise)
fig5 = plt.figure()
t = arange(-1,1,0.001)
x = map(lambda x: x + gauss(0,0.01 + 0.05*sin(8*pi*x)), (1 - t*t)*sin(4*pi*t))
y = map(lambda x: x + gauss(0,0.01 + 0.05*sin(8*pi*x)), (1 - t*t)*cos(4*pi*t))
z = map(lambda x: x + gauss(0,0.01 + 0.05*sin(8*pi*x)), t)
line = array(zip(x,y,z))
lpc = LPCImpl(h = 0.15, t0 = 0.1, mult = 2, it = 500, scaled = False)
lpc_curve = lpc.lpc(line)
ax = Axes3D(fig5)
ax.set_title('helixHeteroscedasticWithCrossing')
curve = lpc_curve[0]['save_xd']
ax.scatter(x,y,z, c = 'red')
ax.plot(curve[:,0],curve[:,1],curve[:,2])
saveToPdf(fig5, '/tmp/helixHeteroscedasticWithCrossing.pdf')
lpc.set_in_dict('cross', False, '_lpcParameters')
fig6 = plt.figure()
lpc_curve = lpc.lpc(X=line)
ax = Axes3D(fig6)
ax.set_title('helixHeteroscedasticWithoutCrossing')
curve = lpc_curve[0]['save_xd']
ax.scatter(x,y,z, c = 'red')
ax.plot(curve[:,0],curve[:,1],curve[:,2])
saveToPdf(fig6, '/tmp/helixHeteroscedasticWithoutCrossing.pdf')
def transformPoint(p, theta):
alpha = theta[0]
rot = np.array( [[cos(alpha), -sin(alpha)],
[sin(alpha), cos(alpha)]] )
s_sk = np.array( [[theta[1], theta[4]],
[theta[3], theta[2]]] )
trans = np.array( [theta[5], theta[6]] )
#Scaling and skewing
ans = np.dot(s_sk, p)
#Rotation
ans = np.dot(rot, ans)
#Translation
ans = ans - trans
#print ans
return ans
def rotate(self, angle, x, y, z): c = cos(angle) s = sin(angle) mat = numpy.matrix([ [x**2*(1-c)+c, x*y*(1-c)-z*s, x*z*(1-c)+y*s, 0], [y*x*(1-c)+z*s, y**2*(1-c)+c, y*z*(1-c)-x*s, 0], [x*z*(1-c)-y*s, y*z*(1-c)+x*s, z**2*(1-c)+c, 0], [0, 0, 0, 1], ]); self.mat_ = numpy.dot(self.mat_, mat);
def transform(self, positions, R_i=None, t_i=None, s_i=None, flip=False):
"""
Return subclusters with (randomly) rotated translated and scaled
positions. If R_i, s_i or t_i is given then that part of transformation
is not random.
"""
for sub in positions:
t = t_i or rand(2) * 10
s = s_i or rand() * 2
if R_i is None:
th = 2 * pi * rand()
# ccw
R = array([[cos(th), -sin(th)], [sin(th), cos(th)]])
else:
R = R_i
if flip:
#TODO: make R with flip
pass
for node, pos in sub.items():
sub[node] = concatenate((dot(dot(s, R), pos[:2]) + t, [nan]))
def transform(self, positions, R_i=None, t_i=None, s_i=None, flip=False):
"""
Return subclusters with (randomly) rotated translated and scaled
positions. If R_i, s_i or t_i is given then that part of transformation
is not random.
"""
for sub in positions:
t = t_i or rand(2)*10
s = s_i or rand()*2
if R_i is None:
th = 2*pi*rand()
# ccw
R = array([[cos(th), -sin(th)], [sin(th), cos(th)]])
else:
R = R_i
if flip:
#TODO: make R with flip
pass
for node, pos in sub.items():
sub[node] = concatenate((dot(dot(s, R), pos[:2])+t, [nan]))
def helixRandom():
#Parameterise a helix (no noise)
fig4 = plt.figure()
t = arange(-1,1,0.001)
x = map(lambda x: x + gauss(0,0.01), (1 - t*t)*sin(4*pi*t))
y = map(lambda x: x + gauss(0,0.01), (1 - t*t)*cos(4*pi*t))
z = map(lambda x: x + gauss(0,0.01), t)
line = array(zip(x,y,z))
lpc = LPCImpl(h = 0.15, t0 = 0.2, mult = 2, it = 100, scaled = False)
lpc_curve = lpc.lpc(X=line)
ax = Axes3D(fig4)
ax.set_title('helixRandom')
curve = lpc_curve[0]['save_xd']
ax.scatter(x,y,z, c = 'red')
ax.plot(curve[:,0],curve[:,1],curve[:,2])
def sample_circle_data(n, noise_level=0.025, offset=0.05, seed_init=None):
if seed_init is not None:
seed(seed_init)
# decision surface for sampled data
thetas = linspace(0, pi / 2, n)
X = sin(thetas) * (1 + randn(n) * noise_level)
Y = cos(thetas) * (1 + randn(n) * noise_level)
# randomly select labels and distinguish data
labels = randint(0, 2, n) * 2 - 1
idx_a = labels > 0
idx_b = labels < 0
X[idx_a] *= (1. + offset)
Y[idx_a] *= (1. + offset)
X[idx_b] *= (1. - offset)
Y[idx_b] *= (1. - offset)
return asarray(zip(X, Y)), labels
def sample_circle_data(n, noise_level=0.025, offset=0.05, seed_init=None):
if seed_init is not None:
seed(seed_init)
# decision surface for sampled data
thetas = linspace(0, pi / 2, n)
X = sin(thetas) * (1 + randn(n) * noise_level)
Y = cos(thetas) * (1 + randn(n) * noise_level)
# randomly select labels and distinguish data
labels = randint(0, 2, n) * 2 - 1
idx_a = labels > 0
idx_b = labels < 0
X[idx_a] *= (1. + offset)
Y[idx_a] *= (1. + offset)
X[idx_b] *= (1. - offset)
Y[idx_b] *= (1. - offset)
return asarray(zip(X, Y)), labels
def drawKeypoints(self, image, keypoints, color=(0, 0, 255)):
"""
Draws SURF extracted keypoints fully emphasizing discovered features (center,response,angle).
@param image: Image to draw on.
@param keypoints: List of keypoints to be drawn.
"""
for keypoint in keypoints:
# fetch basic info
radius = int(keypoint.size / 2)
center = (int(keypoint.pt[0]), int(keypoint.pt[1]))
# draw circle in center
cv2.circle(image, center, radius, color)
# draw orientation
if keypoint.angle != -1:
angleRad = keypoint.angle * cv2.cv.CV_PI / 180
destPoint = (int(cos(angleRad) * radius) + center[0], int(sin(angleRad) * radius) + center[1])
cv2.line(image, center, destPoint, color)
else:
cv2.circle(image, center, 1, color)
def rotation(t):
mat = Matrix([[cos(t[0])*sin(t[2])-cos(t[1])*sin(t[0])*sin(t[2]), -cos(t[2])*sin(t[0])-cos(t[0])*cos(t[1])*sin(t[2]), sin(t[1])*sin(t[2])],
[cos(t[1])*cos(t[2])*sin(t[0])+cos(t[0])*sin(t[2]), cos(t[0])*cos(t[1])*cos(t[2])-sin(t[0])*sin(t[2]), -cos(t[2])*sin(t[1])],
[sin(t[0])*sin(t[1]), cos(t[0])*sin(t[1]), cos(t[1])]])
return mat
import matplotlib.pyplot as plt
import numpy as np
from numpy.ma.core import sin, cos
# plt.plot([1,2,3],[5,7,4])
# plt.show()
t = np.arange(0, 0.98, 0.01)
y1 = sin(2 * np.pi * 4 * t)
#Use subplots to display two plots side by side
# plt.subplot(1,2,1)
plt.plot(t, y1, 'b', label='sin')
y2 = cos(2 * np.pi * 4 * t)
# plt.subplot(1,2,2)
plt.plot(t, y2, 'r', label='cos')
plt.xlabel('`time')
plt.ylabel('value')
plt.title('My Plot')
plt.tight_layout()
plt.show()
# toroid
from numpy.ma.core import cos, sin
toroid_r = 1
r = 1
rs = 0
re = 2 * math.pi
ri = math.pi / 30 # rate
offset = [-.25, -.5, -.75]
start_toroid_r = 5
nl = 2
wires = [
[
rs, re, ri, lambda t, o, at:
(cos(0 * 2 * math.pi / 8) *
(toroid_r + (-start_toroid_r * (at - 1) if at <= 1 else 0) +
(r + o) * cos(t)), sin(0 * 2 * math.pi / 8) *
(toroid_r + (-start_toroid_r * (at - 1) if at <= 1 else 0) +
(r + o) * cos(t)), (r + o) * sin(t)), offset, nl,
lambda at: at >= 0
],
[
rs, re, ri, lambda t, o, at:
(cos(1 * 2 * math.pi / 8) *
(toroid_r + (-start_toroid_r * (at - 1) if at <= 1 else 0) +
(r + o) * cos(t)), sin(1 * 2 * math.pi / 8) *
(toroid_r + (-start_toroid_r * (at - 1) if at <= 1 else 0) +
(r + o) * cos(t)), (r + o) * sin(t)), offset, nl,
lambda at: at >= 0
],
def build_cosines_table():
for index, v in numpy.ndenumerate(alphas_table):
(x, y) = index
cosines_table[x, y] = cos(pi_over_8 * (x + .5) * y)
return
def __init__(self, q=None):
'''
Constructor
'''
if q==None:
self.w = 1.0
self.x = 0.
self.y = 0.
self.z = 0.
elif q.__class__==Quaternion:
self.w = q.w
self.x = q.x
self.y = q.y
self.z = q.z
elif q.__class__==Vector:
self.w = 0
self.x = q[0]
self.y = q[1]
self.z = q[2]
#self.normalize()
elif type(q)==list:
if len(q)==4:
self.w = q[0]
self.x = q[1]
self.y = q[2]
self.z = q[3]
elif len(q)==3:
q[0] = float(q[0])
q[1] = float(q[1])
q[2] = float(q[2])
self.w = cos(q[0]/2)*cos(q[1]/2)*cos(q[2]/2)+sin(q[0]/2)*sin(q[1]/2)*sin(q[2]/2)
self.x = sin(q[0]/2)*cos(q[1]/2)*cos(q[2]/2)-cos(q[0]/2)*sin(q[1]/2)*sin(q[2]/2)
self.y = cos(q[0]/2)*sin(q[1]/2)*cos(q[2]/2)+sin(q[0]/2)*cos(q[1]/2)*sin(q[2]/2)
self.z = cos(q[0]/2)*cos(q[1]/2)*sin(q[2]/2)-sin(q[0]/2)*sin(q[1]/2)*cos(q[2]/2)
def computeThetaDot(self, dt):
mat1 = Matrix([[1, sin(self.Angles[0])*tan(self.Angles[1]), cos(self.Angles[0])*tan(self.Angles[1])],
[0, cos(self.Angles[0]), -sin(self.Angles[0])],
[0, sin(self.Angles[0])/cos(self.Angles[1]), cos(self.Angles[0])/cos(self.Angles[1])]])
self.ThetaDot = mat1*self.Omega
def derivatives(p_1, p_2, theta_1, theta_2):
x_1, y_1 = p_1.copy()
x_2, y_2 = p_2.copy()
alpha_1, sx_1, sy_1, skx_1, sky_1, x0_1, y0_1 = theta_1.copy()
alpha_2, sx_2, sy_2, skx_2, sky_2, x0_2, y0_2 = theta_2.copy()
ksi_1 = sx_1 * x_1 + sky_1 * y_1
eps_1 = skx_1 * x_1 + sy_1 * y_1
ksi_2 = sx_2 * x_2 + sky_2 * y_2
eps_2 = skx_2 * x_2 + sy_2 * y_2
XX = ksi_1 * cos(alpha_1) - eps_1 * sin(alpha_1) - x0_1 - ksi_2 * cos(alpha_2) + eps_2 * sin(alpha_2) + x0_2
YY = ksi_1 * sin(alpha_1) + eps_1 * cos(alpha_1) - y0_1 - ksi_2 * sin(alpha_2) - eps_2 * cos(alpha_2) + y0_2
#Resulting derivatives for 1 parameters
deriv_1 = np.zeros(theta_1.size).reshape(theta_1.shape)
#Resulting derivatives for 2 parameters
deriv_2 = np.zeros(theta_2.size).reshape(theta_2.shape)
#alpha
deriv_1[0] = XX * (-ksi_1 * sin(alpha_1) - eps_1 * cos(alpha_1)) + \
YY * (ksi_1 * cos(alpha_1) - eps_1 * sin(alpha_1))
#sx
deriv_1[1] = XX * (x_1 * cos(alpha_1)) + YY * (x_1 * sin(alpha_1))
#sy
deriv_1[2] = XX * (-y_1 * sin(alpha_1)) + YY * (y_1 * cos(alpha_1))
#skx
deriv_1[3] = XX * (-x_1 * sin(alpha_1)) + YY * (x_1 * cos(alpha_1))
#sky
deriv_1[4] = XX * (y_1 * cos(alpha_1)) + YY * (y_1 * sin(alpha_1))
#x0
deriv_1[5] = XX * (-1)
#y0
deriv_1[6] = YY * (-1)
#alpha
deriv_2[0] = XX * (ksi_2 * sin(alpha_2) + eps_2 * cos(alpha_2)) + YY * (-ksi_2 * cos(alpha_2) + eps_2 * sin(alpha_2))
#sx
deriv_2[1] = XX * (-x_2 * cos(alpha_2)) + YY * (-x_2 * sin(alpha_2))
#sy
deriv_2[2] = XX * (y_2 * sin(alpha_2)) + YY * (-y_2 * cos(alpha_2))
#skx
deriv_2[3] = XX * (x_2 * sin(alpha_2)) + YY * (-x_2 * cos(alpha_2))
#sky
deriv_2[4] = XX * (-y_2 * cos(alpha_2)) + YY * (-y_2 * sin(alpha_2))
#x0
deriv_2[5] = XX
#y0
deriv_2[6] = YY
#print XX
#print YY
return np.array([deriv_1, deriv_2])
