def predict_feature_with_pose(r, p, x, imu_to_camera, camera_matrix, allow_behind=True):
    assert np.shape(r) == (3, 3), "shape was " + str(np.shape(r))
    assert np.shape(p) == (3,), "shape was " + str(np.shape(p))
    assert np.shape(x) == (3,), "shape was " + str(np.shape(x))
    assert np.shape(imu_to_camera) == (3, 3), "shape was " + str(np.shape(imu_to_camera))
    assert np.shape(camera_matrix) == (3, 3), "shape was " + str(np.shape(camera_matrix))
    y = np.dot(camera_matrix, np.dot(imu_to_camera, np.dot(r, x - p)))
    if not allow_behind and y[2] <= 0:
        return None
    return geometry.pr(y)
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 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 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
Пример #6
0
def run_sfm():
    num_points = 4
    num_frames = 2
    num_vars = num_points * 3 + num_frames * 3

    r0 = np.eye(3)
    p0 = np.zeros(3)

    r1 = lie.SO3.exp([.1, .2, .3])
    p1 = np.array([2., 3., 0.])

    rs = [r0, r1]
    ps = [p0, p1]
    xs = np.random.randn(num_points, 3)

    vars = np.hstack(list(ps) + list(xs))

    gamma = 1e-6

    problem = SocpProblem(np.ones(num_vars), [])
    for i, x in enumerate(xs):
        for j, (r, p) in enumerate(zip(rs, ps)):
            z = geometry.pr(np.dot(r, x-p))

            position_offset = j*3
            point_offset = num_frames*3 + i*3

            a = np.zeros((2, num_vars))
            a[:, position_offset:position_offset+3] = np.outer(z, r[2]) - r[:2]
            a[:, point_offset:point_offset+3] = r[:2] - np.outer(z, r[2])

            sign = 1. if np.dot(r[2], x-p) >= 0 else -1.

            c = np.zeros(num_vars)
            c[position_offset:position_offset+3] = -sign * gamma * r[2]
            c[point_offset:point_offset+3] = sign * gamma * r[2]

            b = np.zeros(2)
            d = 0.

            ax = np.dot(a, vars)
            cx = np.dot(c, vars)
            print 'Point %d, camera %d:' % (i, j)
            print '  ax=', ax
            print '  cx=', cx

            problem.constraints.append(SocpConstraint(a, b, c, d))

    problem.evaluate(vars)
    #return

    structure_problem = problem.conditionalize(np.arange(num_vars) < 6, np.hstack(ps))

    sol = solve(structure_problem)
    print sol['x']
    if sol['x'] is None:
        print 'Solution not found'
    else:
        estimated_xs = np.array(sol['x']).reshape((-1, 3))
        print 'True:'
        print xs
        print 'Estimated:'
        print estimated_xs
        print 'Errors:'
        print xs - estimated_xs
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()