def cayley_inv(r):
w = SO3.log(r)
theta = np.linalg.norm(w)
if theta < 1e-8:
return w
else:
return w * np.tan(theta / 2.) / theta
def main():
orientation_data = np.loadtxt('out/frame_orientations.txt')
accel_data = np.loadtxt('out/accelerometer.txt')
frame_timestamps = orientation_data[:,0]
frame_orientations = orientation_data[:, 1:].reshape((-1, 3, 3))
accel_timestamps = accel_data[:,0]
accel_readings = accel_data[:,1:]
accel_orientations = []
for accel_time in accel_timestamps:
frame_index = bisect.bisect_left(frame_timestamps, accel_time)
assert 0 < frame_index < len(frame_timestamps), 't='+accel_time
t0 = frame_timestamps[frame_index-1]
r0 = frame_orientations[frame_index-1]
t1 = frame_timestamps[frame_index]
r1 = frame_orientations[frame_index]
w01 = SO3.log(np.dot(r1, r0.T))
a = (accel_time - t0) / (t1 - t0)
assert 0 <= a <= 1
accel_orientations.append(np.dot(SO3.exp(a*w01), r0))
frame_ws = []
accel_ws = []
rbase = frame_orientations[0]
for r in frame_orientations:
frame_ws.append(SO3.log(np.dot(r, rbase.T)))
for r in accel_orientations:
accel_ws.append(SO3.log(np.dot(r, rbase.T)))
np.savetxt('out/accel_orientations.txt',
np.hstack((accel_timestamps[:,None], np.reshape(accel_orientations, (-1, 9)))))
plt.clf()
plt.hold('on')
plt.plot(frame_timestamps, np.asarray(frame_orientations).reshape((-1, 9)))
plt.plot(accel_timestamps, np.asarray(accel_orientations).reshape((-1, 9)), '.')
#plt.plot(frame_timestamps, map(np.linalg.norm, frame_ws))
#plt.plot(accel_timestamps, map(np.linalg.norm, accel_ws), '.')
plt.show()
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 orientation_residuals(bezier_params, observed_timestamps, observed_orientations):
return np.hstack([SO3.log(np.dot(predict_orientation(bezier_params, t).T, r))
for t, r in zip(observed_timestamps, observed_orientations)])
def angular_velocity_right(f, t, step=1e-8):
return SO3.log(np.dot(f(t).T, f(t + step))) / step
def angular_velocity_left(f, t, step=1e-8):
return SO3.log(np.dot(f(t + step), f(t).T)) / step
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()
