def estimate_trajectory_inf(spline_template,
observed_accel_timestamps,
observed_accel_orientations,
observed_accel_readings,
observed_frame_timestamps,
observed_frame_orientations,
observed_features,
imu_to_camera=np.eye(3),
camera_matrix=np.eye(3),
feature_tolerance=1e-2,
accel_tolerance=1e-3,
gravity_magnitude=9.8,
max_bias_magnitude=.1,
ground_truth=None,
**kwargs):
problem = construct_problem_inf(
spline_template,
observed_accel_timestamps,
observed_accel_orientations,
observed_accel_readings,
observed_frame_timestamps,
observed_frame_orientations,
observed_features,
imu_to_camera=imu_to_camera,
camera_matrix=camera_matrix,
feature_tolerance=feature_tolerance,
accel_tolerance=accel_tolerance,
gravity_magnitude=gravity_magnitude,
max_bias_magnitude=max_bias_magnitude)
print 'Constructed a problem with %d variables and %d constraints' % \
(len(problem.objective), len(problem.constraints))
# Evaluate at ground truth if requested
if ground_truth is not None:
problem.evaluate(ground_truth.flatten())
# Eliminate global position
print 'Eliminating the first position...'
problem = problem.conditionalize_indices(range(3))
# Solve
result = socp.solve(problem, sparse=True, **kwargs)
if result['x'] is None:
raise FeasibilityError('Solver returned status "%s"' % result['status'])
estimated_vars = np.hstack((np.zeros(3), np.squeeze(result['x'])))
spline_vars = spline_template.control_size
pos_controls = estimated_vars[:spline_vars].reshape((-1, 3))
gravity = estimated_vars[spline_vars:spline_vars+3]
accel_bias = estimated_vars[spline_vars+3:spline_vars+6]
landmarks = estimated_vars[spline_vars+6:].reshape((-1, 3))
curve = spline.Spline(spline_template, pos_controls)
return structures.PositionEstimate(curve, gravity, accel_bias, landmarks)
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')
