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()