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 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 simulate_trajectory(calibration,
duration=5.,
num_frames=12,
num_landmarks=50,
num_imu_readings=100,
degree=3,
num_controls=8,
accel_timestamp_noise=0.,
accel_reading_noise=0.,
accel_orientation_noise=0.,
frame_timestamp_noise=0.,
frame_orientation_noise=0.,
feature_noise=0.):
num_knots = num_controls - degree + 1
spline_template = spline.SplineTemplate(np.linspace(0, duration, num_knots), degree, 3)
print 'Num landmarks:', num_landmarks
print 'Num frames:', num_frames
print 'Num IMU readings:', num_imu_readings
print 'Spline curve degree:', degree
# Both splines should start at 0,0,0
true_frame_timestamps = np.linspace(0, duration, num_frames)
true_accel_timestamps = np.linspace(0, duration, num_imu_readings)
true_rot_curve = spline_template.build_random(.1)
true_pos_curve = spline_template.build_random(first_control=np.zeros(3))
landmark_generator = 'normal'
if landmark_generator == 'normal':
true_landmarks = np.random.randn(num_landmarks, 3)*10
elif landmark_generator == 'near':
true_landmarks = []
for i in range(num_landmarks):
p = true_pos_curve.evaluate(true_frame_timestamps[i % len(true_frame_timestamps)])
true_landmarks.append(p + np.random.randn()*.1)
elif landmark_generator == 'far':
true_landmarks = []
for _ in range(num_landmarks):
true_landmarks.append(utils.normalized(np.random.randn(3)) * 100000.)
true_landmarks = np.asarray(true_landmarks)
true_frame_orientations = np.array(map(cayley.cayley, true_rot_curve.evaluate(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
# Sample IMU readings
true_imu_orientations = np.array(map(cayley.cayley, true_rot_curve.evaluate(true_accel_timestamps)))
true_accel_readings = np.array([
sensor_models.predict_accel(true_pos_curve, true_rot_curve, true_accel_bias, true_gravity, t)
for t in true_accel_timestamps])
# Sample features
num_behind = 0
true_features = []
for frame_id, t in enumerate(true_frame_timestamps):
r = cayley.cayley(true_rot_curve.evaluate(t))
p = true_pos_curve.evaluate(t)
a = np.dot(calibration.camera_matrix, np.dot(calibration.imu_to_camera, r))
ys = np.dot(true_landmarks - p, a.T)
for track_id, y in enumerate(ys):
if y[2] > 0:
true_features.append(structures.FeatureObservation(frame_id, track_id, geometry.pr(y)))
else:
num_behind += 1
if num_behind > 0:
print '%d landmarks were behind the camera (and %d were in front)' % (num_behind, len(true_features))
true_features, true_landmarks = utils.renumber_tracks(true_features, true_landmarks, min_track_length=2)
true_trajectory = structures.PositionEstimate(true_pos_curve, true_gravity, true_accel_bias, true_landmarks)
#
# Add sensor noise
#
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 = []
for f in true_features:
observed_features.append(structures.FeatureObservation(f.frame_id,
f.track_id,
utils.add_white_noise(f.position, feature_noise)))
measurements = structures.Measurements(observed_accel_timestamps,
observed_accel_orientations,
observed_accel_readings,
observed_frame_timestamps,
observed_frame_orientations,
observed_features)
return true_trajectory, measurements, spline_template
def run_optimize():
bezier_order = 3
num_gyro_readings = 50
num_frames = 5
frame_timestamp_noise = 1e-3
frame_orientation_noise = .02
gyro_timestamp_noise = 1e-3
gyro_noise = .01
#path = os.path.expanduser('~/Data/Initialization/closed_flat/gyro.txt')
#gyro_data = np.loadtxt(path)
#gyro_timestamps = gyro_data[:,0]
#gyro_readings = gyro_data[:,1:]
true_gyro_timestamps = np.linspace(0, 1, num_gyro_readings)
true_params = np.random.rand(bezier_order, 3)
true_gyro_bias = np.random.rand(3)
true_gyro_readings = np.array([predict_gyro(true_params, true_gyro_bias, t)
for t in true_gyro_timestamps])
true_frame_timestamps = np.linspace(0, 1, num_frames)
true_frame_orientations = np.array([predict_orientation(true_params, t) for t in true_frame_timestamps])
observed_gyro_timestamps = add_white_noise(true_gyro_timestamps, gyro_timestamp_noise)
observed_gyro_readings = add_white_noise(true_gyro_readings, gyro_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)
estimated_gyro_bias, estimated_params = estimate_orientation(bezier_order,
observed_gyro_timestamps,
observed_gyro_readings,
observed_frame_timestamps,
observed_frame_orientations)
print '\nTrue params:'
print true_params
print '\nEstimated params:'
print estimated_params
print '\nTrue gyro bias:'
print true_gyro_bias
print '\nEstimated gyro bias:'
print estimated_gyro_bias
plot_timestamps = np.linspace(0, 1, 50)
estimated_gyro_readings = np.array([predict_gyro(estimated_params, true_gyro_bias, t)
for t in plot_timestamps])
true_orientations = np.array([SO3.log(predict_orientation(true_params, t))
for t in plot_timestamps])
observed_orientations = np.array(map(SO3.log, observed_frame_orientations))
estimated_orientations = np.array([SO3.log(predict_orientation(estimated_params, t))
for t in plot_timestamps])
plt.figure(1)
plt.plot(true_gyro_timestamps, true_gyro_readings, '-', label='true')
plt.plot(true_gyro_timestamps, observed_gyro_readings, 'x', label='observed')
plt.plot(plot_timestamps, estimated_gyro_readings, ':', label='estimated')
plt.xlim(-.1, 1.5)
plt.legend()
plt.figure(2)
plt.plot(plot_timestamps, true_orientations, '-', label='true')
plt.plot(true_frame_timestamps, observed_orientations, 'x', label='observed')
plt.plot(plot_timestamps, estimated_orientations, ':', label='estimated')
plt.xlim(-.1, 1.5)
plt.legend()
plt.show()