def test_zero_offset_bezier_mat(self):
bezier_order = 4
t = .5
f = lambda x: bezier.zero_offset_bezier(x.reshape((bezier_order, 3)), t)
J_numeric = numdifftools.Jacobian(f)(np.zeros(bezier_order*3))
J_analytic = bezier.zero_offset_bezier_mat(t, bezier_order, 3)
np.testing.assert_array_almost_equal(J_numeric, J_analytic)
def main():
bezier_order = 4
num_samples = 10
true_controls = np.random.rand(bezier_order, 3)
true_ts = np.linspace(0, 1, num_samples)
true_ys = np.array([zero_offset_bezier(true_controls, t) for t in true_ts])
estimated_controls = fit_zero_offset_bezier(true_ts, true_ys, bezier_order)
plot_ts = np.linspace(0, 1, 50)
plot_true_ys = np.array([zero_offset_bezier(true_controls, t) for t in plot_ts])
estimated_ys = np.array([zero_offset_bezier(estimated_controls, t) for t in plot_ts])
plt.clf()
plt.plot()
plt.plot(plot_ts, estimated_ys, ':', alpha=1, label='estimated')
plt.plot(plot_ts, plot_true_ys, '-', alpha=.3, label='true')
plt.plot(true_ts, true_ys, 'x', alpha=1, label='observed')
plt.legend()
plt.show()
def run_simulation():
np.random.seed(1)
#
# Construct ground truth
#
num_frames = 5
num_landmarks = 50
num_imu_readings = 80
bezier_degree = 4
out = 'out/position_only_bezier3'
print 'Num landmarks:', num_landmarks
print 'Num frames:', num_frames
print 'Num IMU readings:', num_imu_readings
print 'Bezier curve degree:', bezier_degree
if not os.path.isdir(out):
os.mkdir(out)
# Both splines should start at 0,0,0
frame_times = np.linspace(0, .9, num_frames)
imu_times = np.linspace(0, 1, num_imu_readings)
true_rot_controls = np.random.randn(bezier_degree-1, 3)
true_pos_controls = np.random.randn(bezier_degree-1, 3)
true_landmarks = np.random.randn(num_landmarks, 3)
true_frame_cayleys = np.array([bezier.zero_offset_bezier(true_rot_controls, t) for t in frame_times])
true_frame_orientations = np.array(map(cayley, true_frame_cayleys))
true_frame_positions = np.array([bezier.zero_offset_bezier(true_pos_controls, t) for t in frame_times])
true_imu_cayleys = np.array([bezier.zero_offset_bezier(true_rot_controls, t) for t in imu_times])
true_imu_orientations = np.array(map(cayley, true_imu_cayleys))
true_gravity_magnitude = 9.8
true_gravity = normalized(np.random.rand(3)) * true_gravity_magnitude
true_accel_bias = np.random.randn(3)
true_global_accels = np.array([bezier.zero_offset_bezier_second_deriv(true_pos_controls, t) for t in imu_times])
true_accels = np.array([np.dot(R, a + true_gravity) + true_accel_bias
for R, a in zip(true_imu_orientations, true_global_accels)])
true_features = np.array([[normalized(np.dot(R, x-p)) for x in true_landmarks]
for R, p in zip(true_frame_orientations, true_frame_positions)])
print np.min(true_features.reshape((-1, 3)), axis=0)
print np.max(true_features.reshape((-1, 3)), axis=0)
#
# Add sensor noise
#
accel_noise = 0#0.001
feature_noise = 0#0.01
orientation_noise = 0.01
observed_frame_orientations = true_frame_orientations.copy()
observed_imu_orientations = true_imu_orientations.copy()
observed_features = true_features.copy()
observed_accels = true_accels.copy()
if orientation_noise > 0:
for i, R in enumerate(observed_frame_orientations):
R_noise = SO3.exp(np.random.randn(3)*orientation_noise)
observed_frame_orientations[i] = np.dot(R_noise, R)
for i, R in enumerate(observed_imu_orientations):
R_noise = SO3.exp(np.random.randn(3)*orientation_noise)
observed_imu_orientations[i] = np.dot(R_noise, R)
if accel_noise > 0:
observed_accels += np.random.randn(*observed_accels.shape) * accel_noise
if feature_noise > 0:
observed_features += np.random.rand(*observed_features.shape) * feature_noise
#
# Construct symbolic versions of the above
#
position_offs = 0
accel_bias_offset = position_offs + (bezier_degree-1)*3
gravity_offset = accel_bias_offset + 3
num_vars = gravity_offset + 3
sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
sym_pos_controls = np.reshape(sym_vars[position_offs:position_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))
sym_accel_bias = np.asarray(sym_vars[accel_bias_offset:accel_bias_offset+3])
sym_gravity = np.asarray(sym_vars[gravity_offset:gravity_offset+3])
true_vars = np.hstack((true_pos_controls.flatten(), true_accel_bias, true_gravity))
assert len(true_vars) == len(sym_vars)
#
# Compute residuals
#
epipolar_residuals = evaluate_epipolar_residuals(sym_pos_controls, frame_times,
observed_frame_orientations, observed_features)
accel_residuals = evaluate_accel_residuals(sym_pos_controls, sym_accel_bias, sym_gravity,
imu_times, observed_accels, observed_imu_orientations)
residuals = np.hstack((accel_residuals, epipolar_residuals))
print '\nNum vars:', num_vars
print 'Num residuals:', len(residuals)
print '\nResiduals:', len(residuals)
cost = Polynomial(num_vars)
for r in residuals:
cost += r*r
print ' %f (degree=%d, length=%d)' % (r(*true_vars), r.total_degree, len(r))
print '\nCost:'
print ' Num terms: %d' % len(cost)
print ' Degree: %d' % cost.total_degree
# Solve
A, b, k = quadratic_form(cost)
estimated_vars = np.squeeze(np.linalg.solve(A*2, -b))
estimated_pos_controls = np.reshape(estimated_vars[position_offs:position_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))
estimated_positions = np.array([bezier.zero_offset_bezier(estimated_pos_controls, t) for t in frame_times])
estimated_accel_bias = np.asarray(estimated_vars[accel_bias_offset:accel_bias_offset+3])
estimated_gravity = np.asarray(estimated_vars[gravity_offset:gravity_offset+3])
re_estimated_gravity = normalized(estimated_gravity) * true_gravity_magnitude
print '\nEstimated:'
print estimated_vars
print '\nGround truth:'
print true_vars
print '\nTotal Error:', np.linalg.norm(estimated_vars - true_vars)
print 'Accel bias error:', np.linalg.norm(estimated_accel_bias - true_accel_bias)
print 'Gravity error:', np.linalg.norm(estimated_gravity - true_gravity)
print ' True gravity:', true_gravity
print ' Estimated gravity:', estimated_gravity
print ' Estimated gravity magnitude:', np.linalg.norm(estimated_gravity)
print ' Re-normalized gravity error: ', np.linalg.norm(re_estimated_gravity - true_gravity)
for i in range(num_frames):
print 'Frame %d error: %f' % (i, np.linalg.norm(estimated_positions[i] - true_frame_positions[i]))
fig = plt.figure(figsize=(14,6))
ax = fig.add_subplot(1, 2, 1, projection='3d')
ts = np.linspace(0, 1, 100)
true_ps = np.array([bezier.zero_offset_bezier(true_pos_controls, t) for t in ts])
estimated_ps = np.array([bezier.zero_offset_bezier(estimated_pos_controls, t) for t in ts])
ax.plot(true_ps[:, 0], true_ps[:, 1], true_ps[:, 2], '-b')
ax.plot(estimated_ps[:, 0], estimated_ps[:, 1], estimated_ps[:, 2], '-r')
plt.show()
def epipolar_residual(pos_controls, ti, tj, zi, zj, Ri, Rj):
pi = bezier.zero_offset_bezier(pos_controls, ti)
pj = bezier.zero_offset_bezier(pos_controls, tj)
E = essential_matrix(Ri, pi, Rj, pj)
return dots(zj, E, zi)
def reprojection_residual(pos_controls, landmark, depth, timestamp, feature, orientation):
position = zero_offset_bezier(pos_controls, timestamp)
return np.dot(orientation, landmark - position) - depth * feature
def predict_gyro(bezier_params, gyro_bias, time):
s = zero_offset_bezier(bezier_params, time)
s_deriv = zero_offset_bezier_deriv(bezier_params, time)
orient = cayley(s)
angular_velocity = angular_velocity_from_cayley_deriv(s, s_deriv)
return np.dot(orient, angular_velocity) + gyro_bias
def run_position_only_spline_epipolar():
#
# Construct ground truth
#
num_landmarks = 50
num_frames = 4
num_imu_readings = 80
bezier_degree = 4
out = 'out/position_only_bezier3'
print 'Num landmarks:', num_landmarks
print 'Num frames:', num_frames
print 'Num IMU readings:', num_imu_readings
print 'Bezier curve degree:', bezier_degree
if not os.path.isdir(out):
os.mkdir(out)
# Both splines should start at 0,0,0
frame_times = np.linspace(0, .9, num_frames)
imu_times = np.linspace(0, 1, num_imu_readings)
true_rot_controls = np.random.rand(bezier_degree-1, 3)
true_pos_controls = np.random.rand(bezier_degree-1, 3)
true_landmarks = np.random.randn(num_landmarks, 3)
true_positions = np.array([zero_offset_bezier(true_pos_controls, t) for t in frame_times])
true_cayleys = np.array([zero_offset_bezier(true_rot_controls, t) for t in frame_times])
true_rotations = map(cayley, true_cayleys)
true_imu_cayleys = np.array([zero_offset_bezier(true_rot_controls, t) for t in imu_times])
true_imu_rotations = map(cayley, true_imu_cayleys)
true_gravity = normalized(np.random.rand(3)) * 9.8
true_accel_bias = np.random.rand(3)
true_global_accels = np.array([zero_offset_bezier_second_deriv(true_pos_controls, t) for t in imu_times])
true_accels = [np.dot(R, a + true_gravity) + true_accel_bias
for R, a in zip(true_imu_rotations, true_global_accels)]
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]
#
# Construct symbolic versions of the above
#
position_offs = 0
accel_bias_offset = position_offs + (bezier_degree-1)*3
gravity_offset = accel_bias_offset + 3
num_vars = gravity_offset + 3
sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
sym_pos_controls = np.reshape(sym_vars[position_offs:position_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))
sym_accel_bias = np.asarray(sym_vars[accel_bias_offset:accel_bias_offset+3])
sym_gravity = np.asarray(sym_vars[gravity_offset:gravity_offset+3])
true_vars = np.hstack((true_pos_controls.flatten(), true_accel_bias, true_gravity))
assert len(true_vars) == len(sym_vars)
residuals = []
#
# Accel residuals
#
print '\nAccel residuals:'
for i in range(num_imu_readings):
true_R = true_imu_rotations[i]
sym_global_accel = zero_offset_bezier_second_deriv(sym_pos_controls, imu_times[i])
sym_accel = np.dot(true_R, sym_global_accel + sym_gravity) + sym_accel_bias
residual = sym_accel - true_accels[i]
for i in range(3):
print ' Degree of global accel = %d, local accel = %d, residual = %d' % \
(sym_global_accel[i].total_degree, sym_accel[i].total_degree, residual[i].total_degree)
residuals.extend(residual)
#
# Epipolar residuals
#
p0 = np.zeros(3)
R0 = np.eye(3)
for i in range(1, num_frames):
true_s = true_cayleys[i]
true_R = cayley_mat(true_s)
sym_p = zero_offset_bezier(sym_pos_controls, frame_times[i])
sym_E = essential_matrix(R0, p0, true_R, 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))
residuals.append(residual)
print '\nNum vars:', num_vars
print 'Num residuals:', len(residuals)
print '\nResiduals:', len(residuals)
cost = Polynomial(num_vars)
for r in residuals:
cost += r*r
print ' %f (degree=%d, length=%d)' % (r(*true_vars), r.total_degree, len(r))
print '\nCost:'
print ' Num terms: %d' % len(cost)
print ' Degree: %d' % cost.total_degree
for term in cost:
print ' ',term
print '\nGradients:'
gradients = cost.partial_derivatives()
for gradient in gradients:
print ' %d (degree=%d, length=%d)' % (gradient(*true_vars), gradient.total_degree, len(gradient))
jacobians = np.array([r.partial_derivatives() for r in residuals])
J = evaluate_array(jacobians, *true_vars)
U, S, V = np.linalg.svd(J)
print '\nJacobian singular values:'
print J.shape
print S
print '\nHessian eigenvalues:'
H = np.dot(J.T, J)
print H.shape
print np.linalg.eigvals(H)
null_space_dims = sum(np.abs(S) < 1e-5)
print '\nNull space dimensions:', null_space_dims
if null_space_dims > 0:
for i in range(null_space_dims):
print ' ',V[-i]
null_monomial = (0,) * num_vars
coordinate_monomials = [list(var.monomials)[0] for var in sym_vars]
A, _ = matrix_form(gradients, coordinate_monomials)
b, _ = matrix_form(gradients, [null_monomial])
b = np.squeeze(b)
AA, bb, kk = quadratic_form(cost)
estimated_vars = np.squeeze(numpy.linalg.solve(AA*2, -b))
print '\nEstimated:'
print estimated_vars
print '\nGround truth:'
print true_vars
print '\nError:'
print np.linalg.norm(estimated_vars - true_vars)
# Output to file
write_polynomials(cost, out+'/cost.txt')
write_polynomials(residuals, out+'/residuals.txt')
write_polynomials(gradients, out+'/gradients.txt')
write_polynomials(jacobians.flat, out+'/jacobians.txt')
write_solution(true_vars, out+'/solution.txt')
def main():
np.random.seed(1)
#
# Construct ground truth
#
num_frames = 5
num_landmarks = 10
num_imu_readings = 8
bezier_degree = 3
out = 'out/full_initialization'
print 'Num landmarks:', num_landmarks
print 'Num frames:', num_frames
print 'Num IMU readings:', num_imu_readings
print 'Bezier curve degree:', bezier_degree
if not os.path.isdir(out):
os.mkdir(out)
# Both splines should start at 0,0,0
frame_times = np.linspace(0, .9, num_frames)
accel_times = np.linspace(0, 1, num_imu_readings)
true_pos_controls = np.random.randn(bezier_degree-1, 3)
true_orient_controls = np.random.randn(bezier_degree-1, 3)
true_landmarks = np.random.randn(num_landmarks, 3)
true_frame_positions = np.array([zero_offset_bezier(true_pos_controls, t) for t in frame_times])
true_frame_cayleys = np.array([zero_offset_bezier(true_orient_controls, t) for t in frame_times])
true_frame_orientations = np.array(map(cayley, true_frame_cayleys))
true_imu_cayleys = np.array([zero_offset_bezier(true_orient_controls, t) for t in accel_times])
true_imu_orientations = np.array(map(cayley, true_imu_cayleys))
true_gravity_magnitude = 9.8
true_gravity = normalized(np.random.rand(3)) * true_gravity_magnitude
true_accel_bias = np.random.randn(3)
true_global_accels = np.array([zero_offset_bezier_second_deriv(true_pos_controls, t) for t in accel_times])
true_accels = np.array([np.dot(R, a + true_gravity) + true_accel_bias
for R, a in zip(true_imu_orientations, true_global_accels)])
true_features = np.array([[normalized(np.dot(R, x-p)) for x in true_landmarks]
for R, p in zip(true_frame_orientations, true_frame_positions)])
true_vars = np.hstack((true_pos_controls.flatten(),
true_orient_controls.flatten(),
true_accel_bias,
true_gravity))
print np.min(true_features.reshape((-1, 3)), axis=0)
print np.max(true_features.reshape((-1, 3)), axis=0)
#
# Add sensor noise
#
accel_noise = 0
feature_noise = 0
observed_features = true_features.copy()
observed_accels = true_accels.copy()
if accel_noise > 0:
observed_accels += np.random.randn(*observed_accels.shape) * accel_noise
if feature_noise > 0:
observed_features += np.random.rand(*observed_features.shape) * feature_noise
#
# Construct symbolic versions of the above
#
num_position_vars = (bezier_degree-1)*3
num_orientation_vars = (bezier_degree-1)*3
num_accel_bias_vars = 3
num_gravity_vars = 3
block_sizes = [num_position_vars, num_orientation_vars, num_accel_bias_vars, num_gravity_vars]
num_vars = sum(block_sizes)
sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
sym_pos_controls, sym_orient_controls, sym_accel_bias, sym_gravity = map(np.array, chop(sym_vars, block_sizes))
sym_pos_controls = sym_pos_controls.reshape((-1, 3))
sym_orient_controls = sym_orient_controls.reshape((-1, 3))
assert len(true_vars) == len(sym_vars)
#
# Accel residuals
#
residuals = []
print 'Accel residuals:'
for i, t in enumerate(accel_times):
sym_cayley = zero_offset_bezier(sym_orient_controls, t)
sym_orient = cayley_mat(sym_cayley)
sym_denom = cayley_denom(sym_cayley)
sym_global_accel = zero_offset_bezier_second_deriv(sym_pos_controls, t)
sym_accel = np.dot(sym_orient, sym_global_accel + sym_gravity) + sym_denom * sym_accel_bias
residual = sym_accel - sym_denom * observed_accels[i]
residuals.extend(residual)
for r in residual:
print ' %f (degree=%d, length=%d)' % (r(*true_vars), r.total_degree, len(r))
#
# Epipolar residuals
#
print 'Epipolar residuals:'
for i, ti in enumerate(frame_times):
if i == 0: continue
sym_Ri = cayley_mat(zero_offset_bezier(sym_orient_controls, ti))
sym_pi = zero_offset_bezier(sym_pos_controls, ti)
sym_E = essential_matrix_from_relative_pose(sym_Ri, sym_pi)
for k in range(num_landmarks):
z1 = observed_features[0][k]
zi = observed_features[i][k]
residual = np.dot(zi, np.dot(sym_E, z1))
residuals.append(residual)
r = residual
print ' %f (degree=%d, length=%d)' % (r(*true_vars), r.total_degree, len(r))
#
# Construct cost
#
cost = Polynomial(num_vars)
for r in residuals:
cost += r*r
gradients = cost.partial_derivatives()
print '\nNum vars:', num_vars
print 'Num residuals:', len(residuals)
print '\nCost:'
print ' Num terms: %d' % len(cost)
print ' Degree: %d' % cost.total_degree
#
# Output to file
#
write_polynomials(cost, out+'/cost.txt')
write_polynomials(residuals, out+'/residuals.txt')
write_polynomials(gradients, out+'/gradients.txt')
write_solution(true_vars, out+'/solution.txt')
np.savetxt(out+'/feature_measurements.txt', observed_features.reshape((-1, 3)))
np.savetxt(out+'/accel_measurements.txt', observed_accels)
np.savetxt(out+'/problem_size.txt', [num_frames, num_landmarks, num_imu_readings])
np.savetxt(out+'/frame_times.txt', frame_times)
np.savetxt(out+'/accel_times.txt', accel_times)
np.savetxt(out+'/true_pos_controls.txt', true_pos_controls)
np.savetxt(out+'/true_orient_controls.txt', true_orient_controls)
np.savetxt(out+'/true_accel_bias.txt', true_accel_bias)
np.savetxt(out+'/true_gravity.txt', true_gravity)
return
#
# Plot
#
fig = plt.figure(figsize=(14,6))
ax = fig.add_subplot(1, 2, 1, projection='3d')
ts = np.linspace(0, 1, 100)
true_ps = np.array([zero_offset_bezier(true_pos_controls, t) for t in ts])
ax.plot(true_ps[:, 0], true_ps[:, 1], true_ps[:, 2], '-b')
plt.show()
def fit_zero_offset_bezier_1d(ts, ys, bezier_order):
jacobian = np.array([zero_offset_bezier_coefs(t, bezier_order) for t in ts])
residual = np.array([zero_offset_bezier(np.zeros(bezier_order), t) - y for t, y in zip(ts, ys)])
return np.linalg.lstsq(jacobian, -residual)[0]
def ba_reprojection_residual(pos_controls, landmark, timestamp, feature, orientation):
position = zero_offset_bezier(pos_controls, timestamp)
return pr(np.dot(orientation, landmark - position)) - pr(feature)
def predict_feature(pos_controls, orient_controls, landmark, t):
r = cayley.cayley(bezier.zero_offset_bezier(orient_controls, t))
p = bezier.zero_offset_bezier(pos_controls, t)
y = np.dot(r, landmark - p)
assert y[2] > 0
return geometry.pr(y)
def run_bezier_position_estimation():
np.random.seed(0)
#
# Construct ground truth
#
num_frames = 6
num_landmarks = 10
num_imu_readings = 100
bezier_degree = 4
print 'Num landmarks:', num_landmarks
print 'Num frames:', num_frames
print 'Num IMU readings:', num_imu_readings
print 'Bezier curve degree:', bezier_degree
# Both splines should start at 0,0,0
true_frame_timestamps = np.linspace(0, .9, num_frames)
true_accel_timestamps = np.linspace(0, 1, num_imu_readings)
true_rot_controls = np.random.randn(bezier_degree-1, 3) * .1
true_pos_controls = np.random.randn(bezier_degree-1, 3)
true_landmarks = np.random.randn(num_landmarks, 3)*5 + [0., 0., 20.]
true_frame_orientations = np.array([cayley.cayley(bezier.zero_offset_bezier(true_rot_controls, t))
for t in true_frame_timestamps])
true_frame_positions = np.array([bezier.zero_offset_bezier(true_pos_controls, t) for t in true_frame_timestamps])
true_gravity_magnitude = 9.8
true_gravity = utils.normalized(np.random.rand(3)) * true_gravity_magnitude
true_accel_bias = np.random.randn(3) * .01
print 'True gravity:', true_gravity
true_imu_orientations = np.array([cayley.cayley(bezier.zero_offset_bezier(true_rot_controls, t))
for t in true_accel_timestamps])
true_accel_readings = np.array([predict_accel(true_pos_controls, true_rot_controls, true_accel_bias, true_gravity, t)
for t in true_accel_timestamps])
true_features = np.array([[predict_feature(true_pos_controls, true_rot_controls, x, t) for x in true_landmarks]
for t in true_frame_timestamps])
true_vars = np.hstack((true_pos_controls.flatten(), true_accel_bias, true_gravity, true_landmarks.flatten()))
#
# Add sensor noise
#
accel_timestamp_noise = 1e-5
accel_reading_noise = 1e-5
accel_orientation_noise = 1e-5
frame_timestamp_noise = 1e-5
frame_orientation_noise = 1e-5
feature_noise = 1e-5
observed_accel_timestamps = utils.add_white_noise(true_accel_timestamps, accel_timestamp_noise)
observed_accel_readings = utils.add_white_noise(true_accel_readings, accel_reading_noise)
observed_accel_orientations = utils.add_orientation_noise(true_imu_orientations, accel_orientation_noise)
observed_frame_timestamps = utils.add_white_noise(true_frame_timestamps, frame_timestamp_noise)
observed_frame_orientations = utils.add_orientation_noise(true_frame_orientations, frame_orientation_noise)
observed_features = utils.add_white_noise(true_features, feature_noise)
#
# Solve
#
problem = construct_problem(
bezier_degree,
observed_accel_timestamps,
observed_accel_orientations,
observed_accel_readings,
observed_frame_timestamps,
observed_frame_orientations,
observed_features,
gravity_magnitude=true_gravity_magnitude+.1,
accel_tolerance=1e-3,
feature_tolerance=1e-3)
#problem.evaluate(true_vars)
result = socp.solve(problem, sparse=True)
if result['x'] is None:
print 'Did not find a feasible solution'
return
estimated_vars = np.squeeze(result['x'])
estimated_pos_controls = estimated_vars[:true_pos_controls.size].reshape((-1, 3))
estimated_accel_bias = estimated_vars[true_pos_controls.size:true_pos_controls.size+3]
estimated_gravity = estimated_vars[true_pos_controls.size+3:true_pos_controls.size+6]
estimated_landmarks = estimated_vars[true_pos_controls.size+6:].reshape((-1, 3))
estimated_frame_positions = np.array([bezier.zero_offset_bezier(estimated_pos_controls, t)
for t in true_frame_timestamps])
print 'Position norms:', np.linalg.norm(true_frame_positions, axis=1)
print 'Position errors:', np.linalg.norm(estimated_frame_positions - true_frame_positions, axis=1)
print 'Gravity error:', np.linalg.norm(estimated_gravity - true_gravity)
print 'Accel bias error:', np.linalg.norm(estimated_accel_bias - true_accel_bias)
print 'Max error:', np.max(estimated_vars - true_vars)
plt.clf()
plt.barh(np.arange(len(true_vars)), true_vars, height=.3, alpha=.3, color='g')
plt.barh(np.arange(len(true_vars))+.4, estimated_vars, height=.3, alpha=.3, color='r')
plt.savefig('out/vars.pdf')
def run_position_estimation():
#
# Parameters
#
bezier_degree = 4
num_frames = 8
num_landmarks = 120
num_accel_readings = 50
num_gyro_readings = 60
gyro_timestamp_noise = 0
gyro_reading_noise = 1e-3
accel_timestamp_noise = 0
accel_reading_noise = 1e-3
frame_timestamp_noise = 0
frame_orientation_noise = 1e-3
feature_noise = 1e-4
print 'Num landmarks:', num_landmarks
print 'Num frames:', num_frames
print 'Num accel readings:', num_accel_readings
print 'Num gyro readings:', num_gyro_readings
print 'Bezier curve degree:', bezier_degree
#
# Construct ground truth
#
true_frame_timestamps = np.linspace(0, 1, num_frames)
true_accel_timestamps = np.linspace(0, 1, num_accel_readings)
true_gyro_bias = np.random.rand(3)
true_accel_bias = np.random.randn(3)
true_gravity_magnitude = 9.8
true_gravity = normalized(np.random.rand(3)) * true_gravity_magnitude
true_rot_controls = np.random.randn(bezier_degree-1, 3)
true_pos_controls = np.random.randn(bezier_degree-1, 3)
true_landmarks = np.random.randn(num_landmarks, 3) * 5
true_landmarks[:, 2] += 20
true_frame_orientations = np.array([cayley(zero_offset_bezier(true_rot_controls, t)) for t in true_frame_timestamps])
true_frame_positions = np.array([zero_offset_bezier(true_pos_controls, t) for t in true_frame_timestamps])
true_accel_readings = np.array([predict_accel(true_pos_controls, true_rot_controls, true_accel_bias, true_gravity, t)
for t in true_accel_timestamps])
true_features = np.array([[predict_feature(true_pos_controls, true_rot_controls, x, t) for x in true_landmarks]
for t in true_frame_timestamps])
true_gyro_timestamps = np.linspace(0, 1, num_gyro_readings)
true_gyro_readings = np.array([predict_gyro(true_rot_controls, true_gyro_bias, t)
for t in true_gyro_timestamps])
#
# Add sensor noise
#
observed_gyro_timestamps = add_white_noise(true_gyro_timestamps, gyro_timestamp_noise)
observed_gyro_readings = add_white_noise(true_gyro_readings, gyro_reading_noise)
observed_accel_timestamps = add_white_noise(true_accel_timestamps, accel_timestamp_noise)
observed_accel_readings = add_white_noise(true_accel_readings, accel_reading_noise)
observed_frame_timestamps = add_white_noise(true_frame_timestamps, frame_timestamp_noise)
observed_frame_orientations = add_orientation_noise(true_frame_orientations, frame_orientation_noise)
observed_frame_orientations[0] = true_frame_orientations[0] # do not add noise to first frame
observed_features = add_white_noise(true_features, feature_noise)
#
# Plot features
#
plt.clf()
plot_tracks(true_features, 'x-g', limit=10, alpha=.4)
plot_tracks(observed_features, 'o-r', limit=10, alpha=.4)
plt.show()
return
#
# Solve for orientation and gyro bias
#
print 'Estimating orientation...'
estimated_gyro_bias, estimated_rot_controls = estimate_orientation(
bezier_degree,
observed_gyro_timestamps,
observed_gyro_readings,
observed_frame_timestamps,
observed_frame_orientations)
estimated_accel_orientations = np.array([predict_orientation(estimated_rot_controls, t)
for t in observed_accel_timestamps])
#
# Solve for position, accel bias, and gravity
#
print 'Estimating position...'
estimated_pos_controls, estimated_accel_bias, estimated_gravity = estimate_position(
bezier_degree,
observed_accel_timestamps,
estimated_accel_orientations,
observed_accel_readings,
observed_frame_timestamps,
observed_frame_orientations,
observed_features)
estimated_positions = np.array([zero_offset_bezier(estimated_pos_controls, t) for t in true_frame_timestamps])
estimated_pfeatures = np.array([[pr(predict_feature(estimated_pos_controls, true_rot_controls, x, t))
for x in true_landmarks]
for t in true_frame_timestamps])
true_pfeatures = pr(true_features)
observed_pfeatures = pr(observed_features)
#
# Report
#
print 'Gyro bias error:', np.linalg.norm(estimated_gyro_bias - true_gyro_bias)
print ' True gyro bias:', true_gyro_bias
print ' Estimated gyro bias:', estimated_gyro_bias
print 'Accel bias error:', np.linalg.norm(estimated_accel_bias - true_accel_bias)
print ' True accel bias:', true_accel_bias
print ' Estimated accel bias:', estimated_accel_bias
print 'Gravity error:', np.linalg.norm(estimated_gravity - true_gravity)
print ' True gravity:', true_gravity
print ' Estimated gravity:', estimated_gravity
print ' Estimated gravity magnitude:', np.linalg.norm(estimated_gravity)
for i in range(num_frames):
print 'Frame %d position error: %f' % (i, np.linalg.norm(estimated_positions[i] - true_frame_positions[i]))
#
# Plot orientation results
#
plot_timestamps = np.linspace(0, 1, 50)
estimated_gyro_readings = np.array([predict_gyro(estimated_rot_controls, true_gyro_bias, t)
for t in plot_timestamps])
true_orientations = np.array([SO3.log(predict_orientation(true_rot_controls, t))
for t in plot_timestamps])
estimated_orientations = np.array([SO3.log(predict_orientation(estimated_rot_controls, t))
for t in plot_timestamps])
observed_orientations = np.array(map(SO3.log, observed_frame_orientations))
plt.figure(figsize=(14, 6))
plt.subplot(1, 2, 1)
plt.title('Gyro readings')
plt.plot(plot_timestamps, estimated_gyro_readings, ':', label='estimated', alpha=1)
plt.plot(true_gyro_timestamps, true_gyro_readings, '-', label='true', alpha=.3)
plt.plot(true_gyro_timestamps, observed_gyro_readings, 'x', label='observed')
plt.xlim(-.1, 1.5)
plt.legend()
plt.subplot(1, 2, 2)
plt.title('Orientation')
plt.plot(plot_timestamps, estimated_orientations, ':', label='estimated', alpha=1)
plt.plot(plot_timestamps, true_orientations, '-', label='true', alpha=.3)
plt.plot(true_frame_timestamps, observed_orientations, 'x', label='observed')
plt.xlim(-.1, 1.5)
plt.legend()
#
# Plot position results
#
plot_timestamps = np.linspace(0, 1, 100)
true_positions = np.array([zero_offset_bezier(true_pos_controls, t) for t in plot_timestamps])
estimated_positions = np.array([zero_offset_bezier(estimated_pos_controls, t) for t in plot_timestamps])
true_accels = np.array([predict_accel(true_pos_controls, true_rot_controls, true_accel_bias, true_gravity, t)
for t in plot_timestamps])
estimated_accels = np.array([predict_accel(estimated_pos_controls, true_rot_controls, estimated_accel_bias, estimated_gravity, t)
for t in plot_timestamps])
fig = plt.figure(figsize=(14, 10))
ax = fig.add_subplot(2, 2, 1, projection='3d')
ax.plot(true_positions[:, 0], true_positions[:, 1], true_positions[:, 2], '-b')
ax.plot(estimated_positions[:, 0], estimated_positions[:, 1], estimated_positions[:, 2], '-r')
#ax.plot(true_landmarks[:,0], true_landmarks[:,1], true_landmarks[:,2], '.k', alpha=.2)
ax = fig.add_subplot(2, 2, 2)
ax.plot(plot_timestamps, estimated_accels, ':', label='estimated', alpha=1)
ax.plot(plot_timestamps, true_accels, '-', label='true', alpha=.3)
ax.plot(observed_accel_timestamps, observed_accel_readings, 'x', label='observed')
ax.legend()
ax.set_xlim(-.1, 1.5)
ax = fig.add_subplot(2, 2, 3)
ax.plot(true_pfeatures[1, :, 0], true_pfeatures[1, :, 1], 'x', label='true', alpha=.8)
ax.plot(estimated_pfeatures[1, :, 0], estimated_pfeatures[1, :, 1], 'o', label='estimated', alpha=.4)
ax = fig.add_subplot(2, 2, 4)
ax.plot(true_pfeatures[-1, :, 0], true_pfeatures[-1, :, 1], '.', label='true', alpha=.8)
ax.plot(observed_pfeatures[-1, :, 0], observed_pfeatures[-1, :, 1], 'x', label='observed', alpha=.8)
ax.plot(estimated_pfeatures[-1, :, 0], estimated_pfeatures[-1, :, 1], 'o', label='estimated', alpha=.4)
plt.show()
def test_zero_offset_second_derivative(self):
w = np.array([1.7, 2.8, 1.4, -3.6])
f = lambda t: bezier.zero_offset_bezier(w, t)
g2 = numdifftools.Hessdiag(f)
np.testing.assert_array_almost_equal(g2(.9),
bezier.zero_offset_bezier_second_deriv(w, .9))
def run_simulation_nonsymbolic():
np.random.seed(1)
#
# Construct ground truth
#
num_frames = 5
num_landmarks = 50
num_imu_readings = 80
bezier_degree = 4
out = 'out/position_only_bezier3'
print 'Num landmarks:', num_landmarks
print 'Num frames:', num_frames
print 'Num IMU readings:', num_imu_readings
print 'Bezier curve degree:', bezier_degree
if not os.path.isdir(out):
os.mkdir(out)
# Both splines should start at 0,0,0
frame_times = np.linspace(0, .9, num_frames)
accel_timestamps = np.linspace(0, 1, num_imu_readings)
true_rot_controls = np.random.randn(bezier_degree-1, 3)
true_pos_controls = np.random.randn(bezier_degree-1, 3)
true_landmarks = np.random.randn(num_landmarks, 3)
true_frame_cayleys = np.array([bezier.zero_offset_bezier(true_rot_controls, t) for t in frame_times])
true_frame_orientations = np.array(map(cayley, true_frame_cayleys))
true_frame_positions = np.array([bezier.zero_offset_bezier(true_pos_controls, t) for t in frame_times])
true_imu_cayleys = np.array([bezier.zero_offset_bezier(true_rot_controls, t) for t in accel_timestamps])
true_imu_orientations = np.array(map(cayley, true_imu_cayleys))
true_gravity_magnitude = 9.8
true_gravity = normalized(np.random.rand(3)) * true_gravity_magnitude
true_accel_bias = np.random.randn(3)
true_global_accels = np.array([bezier.zero_offset_bezier_second_deriv(true_pos_controls, t) for t in accel_timestamps])
true_accels = np.array([np.dot(R, a + true_gravity) + true_accel_bias
for R, a in zip(true_imu_orientations, true_global_accels)])
true_features = np.array([[normalized(np.dot(R, x-p)) for x in true_landmarks]
for R, p in zip(true_frame_orientations, true_frame_positions)])
print np.min(true_features.reshape((-1, 3)), axis=0)
print np.max(true_features.reshape((-1, 3)), axis=0)
#
# Add sensor noise
#
accel_noise = 0#0.001
feature_noise = 0#0.01
orientation_noise = 0.01
observed_frame_orientations = true_frame_orientations.copy()
observed_imu_orientations = true_imu_orientations.copy()
observed_features = true_features.copy()
observed_accels = true_accels.copy()
if orientation_noise > 0:
for i, R in enumerate(observed_frame_orientations):
R_noise = SO3.exp(np.random.randn(3)*orientation_noise)
observed_frame_orientations[i] = np.dot(R_noise, R)
for i, R in enumerate(observed_imu_orientations):
R_noise = SO3.exp(np.random.randn(3)*orientation_noise)
observed_imu_orientations[i] = np.dot(R_noise, R)
if accel_noise > 0:
observed_accels += np.random.randn(*observed_accels.shape) * accel_noise
if feature_noise > 0:
observed_features += np.random.rand(*observed_features.shape) * feature_noise
position_offs = 0
accel_bias_offset = position_offs + (bezier_degree-1)*3
gravity_offset = accel_bias_offset + 3
true_vars = np.hstack((true_pos_controls.flatten(), true_accel_bias, true_gravity))
#
# Compute system non-symbolically
#
accel_r = evaluate_accel_residuals(np.zeros((bezier_degree-1, 3)), np.zeros(3), np.zeros(3),
accel_timestamps, observed_accels, observed_imu_orientations)
accel_j = evaluate_accel_jacobians(bezier_degree-1, accel_timestamps, observed_imu_orientations)
epipolar_r = evaluate_epipolar_residuals(np.zeros((bezier_degree-1, 3)), frame_times,
observed_frame_orientations, observed_features)
epipolar_j = evaluate_epipolar_jacobians(bezier_degree-1, frame_times,
observed_frame_orientations, observed_features)
epipolar_j = np.hstack((epipolar_j, np.zeros((epipolar_j.shape[0], 6))))
residual = np.hstack((accel_r, epipolar_r))
jacobian = np.vstack((accel_j, epipolar_j))
#
# Solve
#
JtJ = np.dot(jacobian.T, jacobian)
Jtr = np.dot(jacobian.T, residual)
estimated_vars = np.squeeze(np.linalg.solve(JtJ, -Jtr))
#
# Unpack result and compute error
#
estimated_pos_controls = np.reshape(estimated_vars[position_offs:position_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))
estimated_positions = np.array([bezier.zero_offset_bezier(estimated_pos_controls, t) for t in frame_times])
estimated_accel_bias = np.asarray(estimated_vars[accel_bias_offset:accel_bias_offset+3])
estimated_gravity = np.asarray(estimated_vars[gravity_offset:gravity_offset+3])
re_estimated_gravity = normalized(estimated_gravity) * true_gravity_magnitude
print '\nEstimated:'
print estimated_vars
print '\nGround truth:'
print true_vars
print '\nTotal Error:', np.linalg.norm(estimated_vars - true_vars)
print 'Accel bias error:', np.linalg.norm(estimated_accel_bias - true_accel_bias)
print 'Gravity error:', np.linalg.norm(estimated_gravity - true_gravity)
print ' True gravity:', true_gravity
print ' Estimated gravity:', estimated_gravity
print ' Estimated gravity magnitude:', np.linalg.norm(estimated_gravity)
print ' Re-normalized gravity error: ', np.linalg.norm(re_estimated_gravity - true_gravity)
for i in range(num_frames):
print 'Frame %d error: %f' % (i, np.linalg.norm(estimated_positions[i] - true_frame_positions[i]))
fig = plt.figure(figsize=(14,6))
ax = fig.add_subplot(1, 2, 1, projection='3d')
ts = np.linspace(0, 1, 100)
true_ps = np.array([bezier.zero_offset_bezier(true_pos_controls, t) for t in ts])
estimated_ps = np.array([bezier.zero_offset_bezier(estimated_pos_controls, t) for t in ts])
ax.plot(true_ps[:, 0], true_ps[:, 1], true_ps[:, 2], '-b')
ax.plot(estimated_ps[:, 0], estimated_ps[:, 1], estimated_ps[:, 2], '-r')
plt.show()
def run_from_data():
#
# Load data
#
path = Path('/Users/alexflint/Code/spline-initialization/out')
frame_orientation_data = np.loadtxt(str(path / 'frame_orientations.txt'))
frame_timestamps = frame_orientation_data[:, 0]
frame_orientations = frame_orientation_data[:, 1:].reshape((-1, 3, 3))
accel_data = np.loadtxt(str(path / 'accelerometer.txt'))
accel_timestamps = accel_data[:, 0]
accel_readings = accel_data[:, 1:]
accel_orientation_data = np.loadtxt(str(path / 'accel_orientations.txt'))
accel_orientations = accel_orientation_data[:, 1:].reshape((-1, 3, 3))
feature_data = np.loadtxt(str(path / 'features.txt'))
landmarks_ids = sorted(set(feature_data[:, 0].astype(int)))
frame_ids = sorted(set(feature_data[:, 1].astype(int)))
landmark_index_by_id = {idx: i for i, idx in enumerate(landmarks_ids)}
frame_index_by_id = {idx: i for i, idx in enumerate(frame_ids)}
assert len(accel_orientations) == len(accel_readings)
assert len(frame_ids) == len(frame_orientations) == len(frame_timestamps)
num_frames = len(frame_ids)
num_landmarks = len(landmarks_ids)
num_imu_readings = len(accel_readings)
bezier_degree = 4
print 'Num landmarks:', num_landmarks
print 'Num frames:', num_frames
print 'Num IMU readings:', num_imu_readings
print 'Bezier curve degree:', bezier_degree
#
# Make feature table
#
features = np.ones((num_frames, num_landmarks, 2))
features.fill(np.nan)
for landmark_id, frame_id, feature in zip(landmarks_ids, frame_ids, feature_data[:, 2:]):
i = frame_index_by_id[frame_id]
j = landmark_index_by_id[landmark_id]
features[i, j] = feature
feature_mask[i, j] = True
#
# Normalize timestamps to [0,1]
#
begin_time = min(np.min(accel_timestamps), np.min(frame_timestamps))
end_time = max(np.max(accel_timestamps), np.max(frame_timestamps))
accel_timestamps = (accel_timestamps - begin_time) / (end_time - begin_time)
frame_timestamps = (frame_timestamps - begin_time) / (end_time - begin_time)
#
# Construct symbolic versions of the above
#
position_offs = 0
accel_bias_offset = position_offs + (bezier_degree-1)*3
gravity_offset = accel_bias_offset + 3
num_vars = gravity_offset + 3
sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
sym_pos_controls = np.reshape(sym_vars[position_offs:position_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))
sym_accel_bias = np.asarray(sym_vars[accel_bias_offset:accel_bias_offset+3])
sym_gravity = np.asarray(sym_vars[gravity_offset:gravity_offset+3])
#
# Compute residuals
#
epipolar_residuals = evaluate_epipolar_residuals(sym_pos_controls, frame_timestamps,
frame_orientations, features, feature_mask)
accel_residuals = evaluate_accel_residuals(sym_pos_controls, sym_accel_bias, sym_gravity,
accel_timestamps, accel_readings, accel_orientations)
residuals = accel_residuals + epipolar_residuals
print '\nNum vars:', num_vars
print 'Num residuals:', len(residuals)
print '\nResiduals:', len(residuals)
cost = Polynomial(num_vars)
for r in residuals:
cost += r*r
print ' degree=%d, length=%d' % (r.total_degree, len(r))
print '\nCost:'
print ' Num terms: %d' % len(cost)
print ' Degree: %d' % cost.total_degree
# Solve
A, b, k = quadratic_form(cost)
estimated_vars = np.squeeze(np.linalg.solve(A*2, -b))
estimated_pos_controls = np.reshape(estimated_vars[position_offs:position_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))
estimated_positions = np.array([bezier.zero_offset_bezier(estimated_pos_controls, t) for t in frame_timestamps])
estimated_accel_bias = np.asarray(estimated_vars[accel_bias_offset:accel_bias_offset+3])
estimated_gravity = np.asarray(estimated_vars[gravity_offset:gravity_offset+3])
print '\nEstimated:'
print estimated_vars
print 'Estimated accel bias:', estimated_accel_bias
print 'Estimated gravity:', estimated_gravity
print 'Estimated gravity magnitude:', np.linalg.norm(estimated_gravity)
fig = plt.figure(figsize=(14,6))
ax = fig.add_subplot(1, 2, 1, projection='3d')
ts = np.linspace(0, 1, 100)
estimated_ps = np.array([bezier.zero_offset_bezier(estimated_pos_controls, t) for t in ts])
ax.plot(estimated_ps[:, 0], estimated_ps[:, 1], estimated_ps[:, 2], '-r')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
def predict_accel(pos_controls, orient_controls, accel_bias, gravity, t):
orientation = cayley(zero_offset_bezier(orient_controls, t))
global_accel = zero_offset_bezier_second_deriv(pos_controls, t)
return np.dot(orientation, global_accel + gravity) + accel_bias
def predict_feature(pos_controls, orient_controls, landmark, t):
r = cayley(zero_offset_bezier(orient_controls, t))
p = zero_offset_bezier(pos_controls, t)
return normalized(np.dot(r, landmark - p))
def predict_depth(pos_controls, orient_controls, landmark, t):
r = cayley(zero_offset_bezier(orient_controls, t))
p = zero_offset_bezier(pos_controls, t)
return np.linalg.norm(np.dot(r, landmark - p))
def run_spline_epipolar():
# Construct symbolic problem
num_landmarks = 10
num_frames = 3
num_imu_readings = 8
bezier_degree = 4
out = 'out/epipolar_accel_bezier3'
if not os.path.isdir(out):
os.mkdir(out)
# Both splines should start at 0,0,0
frame_times = np.linspace(0, .9, num_frames)
imu_times = np.linspace(0, 1, num_imu_readings)
true_rot_controls = np.random.rand(bezier_degree-1, 3)
true_pos_controls = np.random.rand(bezier_degree-1, 3)
true_landmarks = np.random.randn(num_landmarks, 3)
true_cayleys = np.array([zero_offset_bezier(true_rot_controls, t) for t in frame_times])
true_positions = np.array([zero_offset_bezier(true_pos_controls, t) for t in frame_times])
true_accels = np.array([zero_offset_bezier_second_deriv(true_pos_controls, t) for t in imu_times])
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 + (bezier_degree-1)*3
num_vars = p_offs + (bezier_degree-1)*3
sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
sym_rot_controls = np.reshape(sym_vars[s_offs:s_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))
sym_pos_controls = np.reshape(sym_vars[p_offs:p_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))
true_vars = np.hstack((true_rot_controls.flatten(),
true_pos_controls.flatten()))
residuals = []
# Accel residuals
for i in range(num_imu_readings):
sym_a = zero_offset_bezier_second_deriv(sym_pos_controls, imu_times[i])
residual = sym_a - true_accels[i]
residuals.extend(residual)
# Epipolar residuals
p0 = np.zeros(3)
R0 = np.eye(3)
for i in range(1, num_frames):
sym_s = zero_offset_bezier(sym_rot_controls, frame_times[i])
sym_p = zero_offset_bezier(sym_pos_controls, frame_times[i])
sym_q = cayley_mat(sym_s)
#sym_q = np.eye(3) * (1. - np.dot(sym_s, sym_s)) + 2.*skew(sym_s) + 2.*np.outer(sym_s, 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))
residuals.append(residual)
print 'Num vars:',num_vars
print 'Num residuals:',len(residuals)
print 'Residuals:', len(residuals)
cost = Polynomial(num_vars)
for r in residuals:
cost += r*r
print ' %f (degree=%d, length=%d)' % (r(*true_vars), r.total_degree, len(r))
print '\nCost:'
print ' Num terms: %d' % len(cost)
print ' Degree: %d' % cost.total_degree
print '\nGradients:'
gradients = cost.partial_derivatives()
for gradient in gradients:
print ' %d (degree=%d, length=%d)' % (gradient(*true_vars), gradient.total_degree, len(gradient))
jacobians = [r.partial_derivatives() for r in residuals]
J = np.array([[J_ij(*true_vars) for J_ij in row] for row in jacobians])
U, S, V = np.linalg.svd(J)
print '\nJacobian singular values:'
print J.shape
print S
null_space_dims = sum(np.abs(S) < 1e-5)
if null_space_dims > 0:
print '\nNull space:'
for i in null_space_dims:
print V[-i]
print V[-2]
print '\nHessian eigenvalues:'
H = np.dot(J.T, J)
print H.shape
print np.linalg.eigvals(H)
# Output to file
write_polynomials(cost, out+'/cost.txt')
write_polynomials(residuals, out+'/residuals.txt')
write_polynomials(gradients, out+'/gradients.txt')
write_polynomials(jacobians, out+'/jacobians.txt')
write_solution(true_vars, out+'/solution.txt')
def run_position_estimation():
#
# Construct ground truth
#
num_frames = 5
num_landmarks = 150
num_imu_readings = 80
bezier_degree = 4
use_epipolar_constraints = False
print 'Num landmarks:', num_landmarks
print 'Num frames:', num_frames
print 'Num IMU readings:', num_imu_readings
print 'Bezier curve degree:', bezier_degree
# Both splines should start at 0,0,0
true_frame_timestamps = np.linspace(0, .9, num_frames)
true_accel_timestamps = np.linspace(0, 1, num_imu_readings)
true_rot_controls = np.random.randn(bezier_degree-1, 3)
true_pos_controls = np.random.randn(bezier_degree-1, 3)
true_landmarks = np.random.randn(num_landmarks, 3) * 5
true_landmarks[:, 2] += 20
true_frame_orientations = np.array([cayley(zero_offset_bezier(true_rot_controls, t)) for t in true_frame_timestamps])
true_frame_positions = np.array([zero_offset_bezier(true_pos_controls, t) for t in true_frame_timestamps])
true_gravity_magnitude = 9.8
true_gravity = normalized(np.random.rand(3)) * true_gravity_magnitude
true_accel_bias = np.random.randn(3)
print 'True gravity:', true_gravity
true_imu_orientations = np.array([cayley(zero_offset_bezier(true_rot_controls, t)) for t in true_accel_timestamps])
true_accel_readings = np.array([predict_accel(true_pos_controls, true_rot_controls, true_accel_bias, true_gravity, t)
for t in true_accel_timestamps])
true_features = np.array([[predict_feature(true_pos_controls, true_rot_controls, x, t) for x in true_landmarks]
for t in true_frame_timestamps])
true_depths = np.array([[predict_depth(true_pos_controls, true_rot_controls, x, t) for x in true_landmarks]
for t in true_frame_timestamps])
#
# Add sensor noise
#
accel_timestamp_noise = 0
accel_reading_noise = 1e-3
accel_orientation_noise = 1e-3
frame_timestamp_noise = 0
frame_orientation_noise = 1e-3
feature_noise = 5e-3
observed_accel_timestamps = add_white_noise(true_accel_timestamps, accel_timestamp_noise)
observed_accel_readings = add_white_noise(true_accel_readings, accel_reading_noise)
observed_accel_orientations = add_orientation_noise(true_imu_orientations, accel_orientation_noise)
observed_frame_timestamps = add_white_noise(true_frame_timestamps, frame_timestamp_noise)
observed_frame_orientations = add_orientation_noise(true_frame_orientations, frame_orientation_noise)
observed_features = add_white_noise(true_features, feature_noise)
#
# Plot
#
#plt.clf()
#plot_tracks(true_features, 'g-', alpha=.2, limit=1)
#plot_tracks(true_features, 'go', alpha=.6, limit=1)
#plot_tracks(observed_features, 'r-', alpha=.2, limit=1)
#plot_tracks(observed_features, 'rx', alpha=.6, limit=1)
#plt.show()
#
# Solve
#
if use_epipolar_constraints:
estimated_pos_controls, estimated_accel_bias, estimated_gravity = estimate_position(
bezier_degree,
observed_accel_timestamps,
observed_accel_orientations,
observed_accel_readings,
observed_frame_timestamps,
observed_frame_orientations,
observed_features,
vision_weight=1.)
else:
estimated_pos_controls, estimated_accel_bias, estimated_gravity, estimated_landmarks, estimated_depths = \
estimate_structure_and_motion(
bezier_degree,
observed_accel_timestamps,
observed_accel_orientations,
observed_accel_readings,
observed_frame_timestamps,
observed_frame_orientations,
observed_features,
vision_weight=1.)
r, j = structure_and_motion_system(
bezier_degree,
observed_accel_timestamps,
observed_accel_orientations,
observed_accel_readings,
observed_frame_timestamps,
observed_frame_orientations,
observed_features,
vision_weight=1.)
t0 = observed_frame_timestamps[0]
r0 = observed_frame_orientations[0]
z0 = observed_features[0, 0]
p0 = zero_offset_bezier(estimated_pos_controls, t0)
pp0 = zero_offset_bezier(true_pos_controls, t0)
x0 = estimated_landmarks[0]
xx0 = true_landmarks[0]
k0 = estimated_depths[0, 0]
kk0 = np.linalg.norm(np.dot(r0, xx0 - pp0))
print 'residual:'
print reprojection_residual(estimated_pos_controls, x0, k0, t0, z0, r0)
print reprojection_residual(true_pos_controls, xx0, kk0, t0, z0, r0)
print np.dot(r0, x0 - p0) - k0 * z0
print np.dot(r0, xx0 - pp0) - kk0 * z0
#true_structure = np.hstack((true_landmarks, true_depths[:, None]))
#true_params = np.hstack((true_pos_controls.flatten(), true_accel_bias, true_gravity, true_structure.flatten()))
#jtj = np.dot(j.T, j)
#jtr = np.dot(j.T, r)
#print jtj.shape, true_params.shape, jtr.shape
#print np.dot(jtj, true_params) - jtr
#return
estimated_positions = np.array([zero_offset_bezier(estimated_pos_controls, t)
for t in true_frame_timestamps])
estimated_accel_readings = np.array([predict_accel(estimated_pos_controls,
true_rot_controls,
estimated_accel_bias,
estimated_gravity,
t)
for t in true_accel_timestamps])
estimated_pfeatures = np.array([[pr(predict_feature(estimated_pos_controls, true_rot_controls, x, t))
for x in true_landmarks]
for t in true_frame_timestamps])
true_pfeatures = pr(true_features)
observed_pfeatures = pr(observed_features)
#
# Report
#
print 'Accel bias error:', np.linalg.norm(estimated_accel_bias - true_accel_bias)
print ' True accel bias:', true_accel_bias
print ' Estimated accel bias:', estimated_accel_bias
print 'Gravity error:', np.linalg.norm(estimated_gravity - true_gravity)
print ' True gravity:', true_gravity
print ' Estimated gravity:', estimated_gravity
print ' Estimated gravity magnitude:', np.linalg.norm(estimated_gravity)
for i in range(num_frames):
print 'Frame %d position error: %f' % (i, np.linalg.norm(estimated_positions[i] - true_frame_positions[i]))
fig = plt.figure(1, figsize=(14, 10))
ax = fig.add_subplot(2, 2, 1, projection='3d')
ts = np.linspace(0, 1, 100)
true_ps = np.array([zero_offset_bezier(true_pos_controls, t) for t in ts])
estimated_ps = np.array([zero_offset_bezier(estimated_pos_controls, t) for t in ts])
ax.plot(true_ps[:, 0], true_ps[:, 1], true_ps[:, 2], '-b')
ax.plot(estimated_ps[:, 0], estimated_ps[:, 1], estimated_ps[:, 2], '-r')
#ax.plot(true_landmarks[:,0], true_landmarks[:,1], true_landmarks[:,2], '.k')
ax = fig.add_subplot(2, 2, 2)
ax.plot(true_accel_timestamps, true_accel_readings, '-', label='true')
ax.plot(observed_accel_timestamps, observed_accel_readings, 'x', label='observed')
ax.plot(true_accel_timestamps, estimated_accel_readings, ':', label='estimated')
ax.legend()
ax.set_xlim(-.1, 1.5)
ax = fig.add_subplot(2, 2, 3)
ax.plot(true_pfeatures[1, :, 0], true_pfeatures[1, :, 1], 'x', label='true', alpha=.8)
ax.plot(estimated_pfeatures[1, :, 0], estimated_pfeatures[1, :, 1], 'o', label='estimated', alpha=.4)
ax = fig.add_subplot(2, 2, 4)
ax.plot(true_pfeatures[-1, :, 0], true_pfeatures[-1, :, 1], '.', label='true', alpha=.8)
ax.plot(observed_pfeatures[-1, :, 0], observed_pfeatures[-1, :, 1], 'x', label='observed', alpha=.8)
ax.plot(estimated_pfeatures[-1, :, 0], estimated_pfeatures[-1, :, 1], 'o', label='estimated', alpha=.4)
plt.show()
def analyze_polynomial2():
print 'Loading polynomials...'
varnames, true_values = load_solution('out/epipolar_accel_bezier3/solution.txt')
cost = load_functions('out/epipolar_accel_bezier3/cost.txt', varnames)[0]
residuals = load_functions('out/epipolar_accel_bezier3/residuals.txt', varnames)
jacobians = load_functions('out/epipolar_accel_bezier3/jacobians.txt', varnames)
gradients = load_functions('out/epipolar_accel_bezier3/gradients.txt', varnames)
residuals = np.array(residuals)
jacobians = np.array(jacobians).reshape((-1, len(varnames)))
def J(x):
print ' ... jacobian'
return evaluate_array(jacobians, *x)
def r(x):
print ' ... residual'
return evaluate_array(residuals, *x)
seed_values = true_values + np.random.rand(len(true_values)) * 100
out = scipy.optimize.leastsq(func=r, x0=seed_values, Dfun=J, full_output=True)
opt_values = out[0]
print '\nGradients:'
print evaluate_array(gradients, *opt_values)
print evaluate_array(gradients, *true_values)
print '\nCosts:'
print cost(*opt_values)
print cost(*true_values)
print '\nJacobian singular values:'
jac = J(opt_values)
U, S, V = np.linalg.svd(jac)
print S
print '\nTrue:'
print true_values
print '\nSeed:'
print seed_values
print '\nOptimized:'
print opt_values
print '\nError:'
print np.linalg.norm(opt_values - true_values)
true_pcontrols = true_values[:6].reshape((-1, 3))
opt_pcontrols = opt_values[:6].reshape((-1, 3))
fig = plt.figure(figsize=(14,6))
ax = fig.add_subplot(1, 2, 1, projection='3d')
ts = np.linspace(0, 1, 100)
true_ps = np.array([zero_offset_bezier(true_pcontrols, t) for t in ts])
opt_ps = np.array([zero_offset_bezier(opt_pcontrols, t) for t in ts])
ax.plot(true_ps[:,0], true_ps[:,1], true_ps[:,2], '-b')
ax.plot(opt_ps[:,0], opt_ps[:,1], opt_ps[:,2], '-r')
plt.show()
def predict_orientation(bezier_params, time):
return cayley(zero_offset_bezier(bezier_params, time))
