def run_sfm():
# Construct symbolic problem
num_landmarks = 4
num_frames = 2
print 'Num observations: ', num_landmarks * num_frames * 2
print 'Num vars: ', num_frames*6 + num_landmarks*3 + num_frames*num_landmarks
true_landmarks = np.random.randn(num_landmarks, 3)
true_positions = np.random.rand(num_frames, 3)
true_cayleys = np.random.rand(num_frames, 3)
true_qs = map(cayley_mat, true_cayleys)
true_betas = map(cayley_denom, true_cayleys)
true_rotations = [(q/b) for (q,b) in zip(true_qs, true_betas)]
true_uprojections = [[np.dot(R, x-p) for x in true_landmarks]
for R,p in zip(true_rotations, true_positions)]
true_projections = [[normalized(zu) for zu in row] for row in true_uprojections]
true_alphas = [[np.linalg.norm(zu) for zu in row] for row in true_uprojections]
true_vars = np.hstack((true_cayleys.flatten(),
true_positions.flatten(),
true_landmarks.flatten(),
np.asarray(true_alphas).flatten()))
#true_projection_mat = np.reshape(true_projections, (num_frames, num_landmarks, 2))
for i in range(num_frames):
p = true_positions[i]
q = true_qs[i]
beta = true_betas[i]
for j in range(num_landmarks):
x = true_landmarks[j]
z = true_projections[i][j]
alpha = true_alphas[i][j]
print alpha * beta * z - np.dot(q, x-p)
# construct symbolic versions of the above
s_offs = 0
p_offs = s_offs + num_frames*3
x_offs = p_offs + num_frames*3
a_offs = x_offs + num_landmarks*3
num_vars = a_offs + num_landmarks*num_frames
sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
sym_cayleys = np.reshape(sym_vars[s_offs:s_offs+num_frames*3], (num_frames, 3))
sym_positions = np.reshape(sym_vars[p_offs:p_offs+num_frames*3], (num_frames, 3))
sym_landmarks = np.reshape(sym_vars[x_offs:x_offs+num_landmarks*3], (num_landmarks, 3))
sym_alphas = np.reshape(sym_vars[a_offs:], (num_frames, num_landmarks))
residuals = []
for i in range(num_frames):
sym_p = sym_positions[i]
sym_s = sym_cayleys[i]
for j in range(num_landmarks):
sym_x = sym_landmarks[j]
sym_a = sym_alphas[i,j]
true_z = true_projections[i][j]
residual = np.dot(cayley_mat(sym_s), sym_x-sym_p) - sym_a * cayley_denom(sym_s) * true_z
residuals.extend(residual)
print 'Residuals:'
cost = Polynomial(num_vars)
for residual in residuals:
cost += np.dot(residual, residual)
print ' ',residual(*true_vars) #ri.num_vars, len(true_vars)
print '\nGradients:'
gradient = [cost.partial_derivative(i) for i in range(num_vars)]
for gi in gradient:
print gi(*true_vars)
j = np.array([[r.partial_derivative(i)(*true_vars) for i in range(num_vars)]
for r in residuals])
print '\nJacobian singular values:'
print j.shape
u, s, v = np.linalg.svd(j)
print s
print '\nHessian eigenvalues:'
h = np.dot(j.T, j)
print h.shape
print np.linalg.eigvals(h)
def run_epipolar():
# Construct symbolic problem
num_landmarks = 10
num_frames = 3
true_landmarks = np.random.randn(num_landmarks, 3)
true_positions = np.vstack((np.zeros(3),
np.random.rand(num_frames-1, 3)))
true_cayleys = np.vstack((np.zeros(3),
np.random.rand(num_frames-1, 3)))
true_qs = map(cayley_mat, true_cayleys)
true_rotations = map(cayley, true_cayleys)
true_uprojections = [[np.dot(R, x-p) for x in true_landmarks]
for R,p in zip(true_rotations, true_positions)]
true_projections = [[normalized(zu) for zu in row] for row in true_uprojections]
p0 = true_positions[0]
q0 = true_qs[0]
for i in range(1, num_frames):
p = true_positions[i]
q = true_qs[i]
E = essential_matrix(q0, p0, q, p)
for j in range(num_landmarks):
z = true_projections[i][j]
z0 = true_projections[0][j]
print np.dot(z, np.dot(E, z0))
# construct symbolic versions of the above
s_offs = 0
p_offs = s_offs + (num_frames-1)*3
num_vars = p_offs + (num_frames-1)*3
sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
sym_cayleys = np.reshape(sym_vars[s_offs:s_offs+(num_frames-1)*3], (num_frames-1, 3))
sym_positions = np.reshape(sym_vars[p_offs:p_offs+(num_frames-1)*3], (num_frames-1, 3))
true_vars = np.hstack((true_cayleys[1:].flatten(),
true_positions[1:].flatten()))
residuals = []
p0 = np.zeros(3)
R0 = np.eye(3)
for i in range(1, num_frames):
sym_p = sym_positions[i-1]
sym_s = sym_cayleys[i-1]
sym_q = cayley_mat(sym_s)
sym_E = essential_matrix(R0, p0, sym_q, sym_p)
for j in range(num_landmarks):
z = true_projections[i][j]
z0 = true_projections[0][j]
residual = np.dot(z, np.dot(sym_E, z0))
print 'Residual poly: ',len(residual), residual.total_degree
residuals.append(residual)
print 'Num sym_vars:',num_vars
print 'Num residuals:',len(residuals)
print 'Residuals:', len(residuals)
cost = Polynomial(num_vars)
for residual in residuals:
#cost += np.dot(residual, residual)
print ' ',residual(*true_vars) #ri.num_vars, len(true_vars)
print '\nGradients:'
gradient = [cost.partial_derivative(i) for i in range(num_vars)]
for gi in gradient:
print ' ',gi(*true_vars)
J = np.array([[r.partial_derivative(i)(*true_vars) for i in range(num_vars)]
for r in residuals])
print '\nJacobian singular values:'
print J.shape
U,S,V = np.linalg.svd(J)
print S
print V[-1]
print V[-2]
print '\nHessian eigenvalues:'
H = np.dot(J.T, J)
print H.shape
print np.linalg.eigvals(H)
