Python Polynomial.coordinate方法代码示例

# 需要导入模块: from polynomial import Polynomial [as 别名]
# 或者: from polynomial.Polynomial import coordinate [as 别名]
def run_spline_epipolar():
    # Construct symbolic problem
    num_landmarks = 10
    num_frames = 3
    num_imu_readings = 8
    bezier_degree = 4
    out = 'out/epipolar_accel_bezier3'

    if not os.path.isdir(out):
        os.mkdir(out)

    # Both splines should start at 0,0,0
    frame_times = np.linspace(0, .9, num_frames)
    imu_times = np.linspace(0, 1, num_imu_readings)

    true_rot_controls = np.random.rand(bezier_degree-1, 3)
    true_pos_controls = np.random.rand(bezier_degree-1, 3)

    true_landmarks = np.random.randn(num_landmarks, 3)
    true_cayleys = np.array([zero_offset_bezier(true_rot_controls, t) for t in frame_times])
    true_positions = np.array([zero_offset_bezier(true_pos_controls, t) for t in frame_times])

    true_accels = np.array([zero_offset_bezier_second_deriv(true_pos_controls, t) for t in imu_times])

    true_qs = map(cayley_mat, true_cayleys)
    true_rotations = map(cayley, true_cayleys)

    true_uprojections = [[np.dot(R, x-p) for x in true_landmarks]
                         for R,p in zip(true_rotations, true_positions)]

    true_projections = [[normalized(zu) for zu in row] for row in true_uprojections]

    p0 = true_positions[0]
    q0 = true_qs[0]
    for i in range(1, num_frames):
        p = true_positions[i]
        q = true_qs[i]
        E = essential_matrix(q0, p0, q, p)
        for j in range(num_landmarks):
            z = true_projections[i][j]
            z0 = true_projections[0][j]
            #print np.dot(z, np.dot(E, z0))

    # construct symbolic versions of the above
    s_offs = 0
    p_offs = s_offs + (bezier_degree-1)*3
    num_vars = p_offs + (bezier_degree-1)*3

    sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
    sym_rot_controls = np.reshape(sym_vars[s_offs:s_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))
    sym_pos_controls = np.reshape(sym_vars[p_offs:p_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))

    true_vars = np.hstack((true_rot_controls.flatten(),
                           true_pos_controls.flatten()))

    residuals = []

    # Accel residuals
    for i in range(num_imu_readings):
        sym_a = zero_offset_bezier_second_deriv(sym_pos_controls, imu_times[i])
        residual = sym_a - true_accels[i]
        residuals.extend(residual)

    # Epipolar residuals
    p0 = np.zeros(3)
    R0 = np.eye(3)
    for i in range(1, num_frames):
        sym_s = zero_offset_bezier(sym_rot_controls, frame_times[i])
        sym_p = zero_offset_bezier(sym_pos_controls, frame_times[i])
        sym_q = cayley_mat(sym_s)
        #sym_q = np.eye(3) * (1. - np.dot(sym_s, sym_s)) + 2.*skew(sym_s) + 2.*np.outer(sym_s, sym_s)
        sym_E = essential_matrix(R0, p0, sym_q, sym_p)
        for j in range(num_landmarks):
            z = true_projections[i][j]
            z0 = true_projections[0][j]
            residual = np.dot(z, np.dot(sym_E, z0))
            residuals.append(residual)

    print 'Num vars:',num_vars
    print 'Num residuals:',len(residuals)

    print 'Residuals:', len(residuals)
    cost = Polynomial(num_vars)
    for r in residuals:
        cost += r*r
        print '  %f   (degree=%d, length=%d)' % (r(*true_vars), r.total_degree, len(r))

    print '\nCost:'
    print '  Num terms: %d' % len(cost)
    print '  Degree: %d' % cost.total_degree

    print '\nGradients:'
    gradients = cost.partial_derivatives()
    for gradient in gradients:
        print '  %d  (degree=%d, length=%d)' % (gradient(*true_vars), gradient.total_degree, len(gradient))

    jacobians = [r.partial_derivatives() for r in residuals]

    J = np.array([[J_ij(*true_vars) for J_ij in row] for row in jacobians])

#.........这里部分代码省略.........

# 需要导入模块: from polynomial import Polynomial [as 别名]
# 或者: from polynomial.Polynomial import coordinate [as 别名]
def run_position_only_spline_epipolar():
    #
    # Construct ground truth
    #
    num_landmarks = 50
    num_frames = 4
    num_imu_readings = 80
    bezier_degree = 4
    out = 'out/position_only_bezier3'

    print 'Num landmarks:', num_landmarks
    print 'Num frames:', num_frames
    print 'Num IMU readings:', num_imu_readings
    print 'Bezier curve degree:', bezier_degree

    if not os.path.isdir(out):
        os.mkdir(out)

    # Both splines should start at 0,0,0
    frame_times = np.linspace(0, .9, num_frames)
    imu_times = np.linspace(0, 1, num_imu_readings)

    true_rot_controls = np.random.rand(bezier_degree-1, 3)
    true_pos_controls = np.random.rand(bezier_degree-1, 3)

    true_landmarks = np.random.randn(num_landmarks, 3)

    true_positions = np.array([zero_offset_bezier(true_pos_controls, t) for t in frame_times])
    true_cayleys = np.array([zero_offset_bezier(true_rot_controls, t) for t in frame_times])
    true_rotations = map(cayley, true_cayleys)

    true_imu_cayleys = np.array([zero_offset_bezier(true_rot_controls, t) for t in imu_times])
    true_imu_rotations = map(cayley, true_imu_cayleys)

    true_gravity = normalized(np.random.rand(3)) * 9.8
    true_accel_bias = np.random.rand(3)
    true_global_accels = np.array([zero_offset_bezier_second_deriv(true_pos_controls, t) for t in imu_times])
    true_accels = [np.dot(R, a + true_gravity) + true_accel_bias
                   for R, a in zip(true_imu_rotations, true_global_accels)]

    true_uprojections = [[np.dot(R, x-p) for x in true_landmarks]
                         for R, p in zip(true_rotations, true_positions)]

    true_projections = [[normalized(zu) for zu in row] for row in true_uprojections]

    #
    # Construct symbolic versions of the above
    #
    position_offs = 0
    accel_bias_offset = position_offs + (bezier_degree-1)*3
    gravity_offset = accel_bias_offset + 3
    num_vars = gravity_offset + 3

    sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
    sym_pos_controls = np.reshape(sym_vars[position_offs:position_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))
    sym_accel_bias = np.asarray(sym_vars[accel_bias_offset:accel_bias_offset+3])
    sym_gravity = np.asarray(sym_vars[gravity_offset:gravity_offset+3])

    true_vars = np.hstack((true_pos_controls.flatten(), true_accel_bias, true_gravity))
    assert len(true_vars) == len(sym_vars)

    residuals = []

    #
    # Accel residuals
    #
    print '\nAccel residuals:'
    for i in range(num_imu_readings):
        true_R = true_imu_rotations[i]
        sym_global_accel = zero_offset_bezier_second_deriv(sym_pos_controls, imu_times[i])
        sym_accel = np.dot(true_R, sym_global_accel + sym_gravity) + sym_accel_bias
        residual = sym_accel - true_accels[i]
        for i in range(3):
            print '  Degree of global accel = %d, local accel = %d, residual = %d' % \
                  (sym_global_accel[i].total_degree, sym_accel[i].total_degree, residual[i].total_degree)
        residuals.extend(residual)

    #
    # Epipolar residuals
    #
    p0 = np.zeros(3)
    R0 = np.eye(3)
    for i in range(1, num_frames):
        true_s = true_cayleys[i]
        true_R = cayley_mat(true_s)
        sym_p = zero_offset_bezier(sym_pos_controls, frame_times[i])
        sym_E = essential_matrix(R0, p0, true_R, sym_p)
        for j in range(num_landmarks):
            z = true_projections[i][j]
            z0 = true_projections[0][j]
            residual = np.dot(z, np.dot(sym_E, z0))
            residuals.append(residual)

    print '\nNum vars:', num_vars
    print 'Num residuals:', len(residuals)

    print '\nResiduals:', len(residuals)
    cost = Polynomial(num_vars)
    for r in residuals:
        cost += r*r
#.........这里部分代码省略.........

# 需要导入模块: from polynomial import Polynomial [as 别名]
# 或者: from polynomial.Polynomial import coordinate [as 别名]
def run_sfm():
    # Construct symbolic problem
    num_landmarks = 4
    num_frames = 2

    print 'Num observations: ', num_landmarks * num_frames * 2
    print 'Num vars: ', num_frames*6 + num_landmarks*3 + num_frames*num_landmarks

    true_landmarks = np.random.randn(num_landmarks, 3)
    true_positions = np.random.rand(num_frames, 3)
    true_cayleys = np.random.rand(num_frames, 3)

    true_qs = map(cayley_mat, true_cayleys)
    true_betas = map(cayley_denom, true_cayleys)
    true_rotations = [(q/b) for (q,b) in zip(true_qs, true_betas)]

    true_uprojections = [[np.dot(R, x-p) for x in true_landmarks]
                         for R,p in zip(true_rotations, true_positions)]

    true_projections = [[normalized(zu) for zu in row] for row in true_uprojections]
    true_alphas = [[np.linalg.norm(zu) for zu in row] for row in true_uprojections]

    true_vars = np.hstack((true_cayleys.flatten(),
                           true_positions.flatten(),
                           true_landmarks.flatten(),
                           np.asarray(true_alphas).flatten()))

    #true_projection_mat = np.reshape(true_projections, (num_frames, num_landmarks, 2))

    for i in range(num_frames):
        p = true_positions[i]
        q = true_qs[i]
        beta = true_betas[i]
        for j in range(num_landmarks):
            x = true_landmarks[j]
            z = true_projections[i][j]
            alpha = true_alphas[i][j]
            print alpha * beta * z - np.dot(q, x-p)

    # construct symbolic versions of the above
    s_offs = 0
    p_offs = s_offs + num_frames*3
    x_offs = p_offs + num_frames*3
    a_offs = x_offs + num_landmarks*3
    num_vars = a_offs + num_landmarks*num_frames

    sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
    sym_cayleys = np.reshape(sym_vars[s_offs:s_offs+num_frames*3], (num_frames, 3))
    sym_positions = np.reshape(sym_vars[p_offs:p_offs+num_frames*3], (num_frames, 3))
    sym_landmarks = np.reshape(sym_vars[x_offs:x_offs+num_landmarks*3], (num_landmarks, 3))
    sym_alphas = np.reshape(sym_vars[a_offs:], (num_frames, num_landmarks))

    residuals = []
    for i in range(num_frames):
        sym_p = sym_positions[i]
        sym_s = sym_cayleys[i]
        for j in range(num_landmarks):
            sym_x = sym_landmarks[j]
            sym_a = sym_alphas[i,j]
            true_z = true_projections[i][j]
            residual = np.dot(cayley_mat(sym_s), sym_x-sym_p) - sym_a * cayley_denom(sym_s) * true_z
            residuals.extend(residual)

    print 'Residuals:'
    cost = Polynomial(num_vars)
    for residual in residuals:
        cost += np.dot(residual, residual)
        print '  ',residual(*true_vars)  #ri.num_vars, len(true_vars)

    print '\nGradients:'
    gradient = [cost.partial_derivative(i) for i in range(num_vars)]
    for gi in gradient:
        print gi(*true_vars)

    j = np.array([[r.partial_derivative(i)(*true_vars) for i in range(num_vars)]
                  for r in residuals])

    print '\nJacobian singular values:'
    print j.shape
    u, s, v = np.linalg.svd(j)
    print s

    print '\nHessian eigenvalues:'
    h = np.dot(j.T, j)
    print h.shape
    print np.linalg.eigvals(h)

# 需要导入模块: from polynomial import Polynomial [as 别名]
# 或者: from polynomial.Polynomial import coordinate [as 别名]
def run_epipolar():
    # Construct symbolic problem
    num_landmarks = 10
    num_frames = 3

    true_landmarks = np.random.randn(num_landmarks, 3)
    true_positions = np.vstack((np.zeros(3),
                                np.random.rand(num_frames-1, 3)))
    true_cayleys = np.vstack((np.zeros(3),
                              np.random.rand(num_frames-1, 3)))

    true_qs = map(cayley_mat, true_cayleys)
    true_rotations = map(cayley, true_cayleys)

    true_uprojections = [[np.dot(R, x-p) for x in true_landmarks]
                         for R,p in zip(true_rotations, true_positions)]

    true_projections = [[normalized(zu) for zu in row] for row in true_uprojections]

    p0 = true_positions[0]
    q0 = true_qs[0]
    for i in range(1, num_frames):
        p = true_positions[i]
        q = true_qs[i]
        E = essential_matrix(q0, p0, q, p)
        for j in range(num_landmarks):
            z = true_projections[i][j]
            z0 = true_projections[0][j]
            print np.dot(z, np.dot(E, z0))

    # construct symbolic versions of the above
    s_offs = 0
    p_offs = s_offs + (num_frames-1)*3
    num_vars = p_offs + (num_frames-1)*3

    sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
    sym_cayleys = np.reshape(sym_vars[s_offs:s_offs+(num_frames-1)*3], (num_frames-1, 3))
    sym_positions = np.reshape(sym_vars[p_offs:p_offs+(num_frames-1)*3], (num_frames-1, 3))

    true_vars = np.hstack((true_cayleys[1:].flatten(),
                           true_positions[1:].flatten()))

    residuals = []
    p0 = np.zeros(3)
    R0 = np.eye(3)
    for i in range(1, num_frames):
        sym_p = sym_positions[i-1]
        sym_s = sym_cayleys[i-1]
        sym_q = cayley_mat(sym_s)
        sym_E = essential_matrix(R0, p0, sym_q, sym_p)
        for j in range(num_landmarks):
            z = true_projections[i][j]
            z0 = true_projections[0][j]
            residual = np.dot(z, np.dot(sym_E, z0))
            print 'Residual poly: ',len(residual), residual.total_degree
            residuals.append(residual)

    print 'Num sym_vars:',num_vars
    print 'Num residuals:',len(residuals)

    print 'Residuals:', len(residuals)
    cost = Polynomial(num_vars)
    for residual in residuals:
        #cost += np.dot(residual, residual)
        print '  ',residual(*true_vars)  #ri.num_vars, len(true_vars)

    print '\nGradients:'
    gradient = [cost.partial_derivative(i) for i in range(num_vars)]
    for gi in gradient:
        print '  ',gi(*true_vars)

    J = np.array([[r.partial_derivative(i)(*true_vars) for i in range(num_vars)]
                  for r in residuals])

    print '\nJacobian singular values:'
    print J.shape
    U,S,V = np.linalg.svd(J)
    print S
    print V[-1]
    print V[-2]

    print '\nHessian eigenvalues:'
    H = np.dot(J.T, J)
    print H.shape
    print np.linalg.eigvals(H)

# 需要导入模块: from polynomial import Polynomial [as 别名]
# 或者: from polynomial.Polynomial import coordinate [as 别名]
def main():
    np.random.seed(1)

    #
    # Construct ground truth
    #
    num_frames = 5
    num_landmarks = 10
    num_imu_readings = 8
    bezier_degree = 3
    out = 'out/full_initialization'

    print 'Num landmarks:', num_landmarks
    print 'Num frames:', num_frames
    print 'Num IMU readings:', num_imu_readings
    print 'Bezier curve degree:', bezier_degree

    if not os.path.isdir(out):
        os.mkdir(out)

    # Both splines should start at 0,0,0
    frame_times = np.linspace(0, .9, num_frames)
    accel_times = np.linspace(0, 1, num_imu_readings)

    true_pos_controls = np.random.randn(bezier_degree-1, 3)
    true_orient_controls = np.random.randn(bezier_degree-1, 3)

    true_landmarks = np.random.randn(num_landmarks, 3)

    true_frame_positions = np.array([zero_offset_bezier(true_pos_controls, t) for t in frame_times])
    true_frame_cayleys = np.array([zero_offset_bezier(true_orient_controls, t) for t in frame_times])
    true_frame_orientations = np.array(map(cayley, true_frame_cayleys))

    true_imu_cayleys = np.array([zero_offset_bezier(true_orient_controls, t) for t in accel_times])
    true_imu_orientations = np.array(map(cayley, true_imu_cayleys))

    true_gravity_magnitude = 9.8
    true_gravity = normalized(np.random.rand(3)) * true_gravity_magnitude
    true_accel_bias = np.random.randn(3)
    true_global_accels = np.array([zero_offset_bezier_second_deriv(true_pos_controls, t) for t in accel_times])
    true_accels = np.array([np.dot(R, a + true_gravity) + true_accel_bias
                            for R, a in zip(true_imu_orientations, true_global_accels)])

    true_features = np.array([[normalized(np.dot(R, x-p)) for x in true_landmarks]
                              for R, p in zip(true_frame_orientations, true_frame_positions)])

    true_vars = np.hstack((true_pos_controls.flatten(),
                           true_orient_controls.flatten(),
                           true_accel_bias,
                           true_gravity))

    print np.min(true_features.reshape((-1, 3)), axis=0)
    print np.max(true_features.reshape((-1, 3)), axis=0)

    #
    # Add sensor noise
    #

    accel_noise = 0
    feature_noise = 0

    observed_features = true_features.copy()
    observed_accels = true_accels.copy()

    if accel_noise > 0:
        observed_accels += np.random.randn(*observed_accels.shape) * accel_noise

    if feature_noise > 0:
        observed_features += np.random.rand(*observed_features.shape) * feature_noise

    #
    # Construct symbolic versions of the above
    #
    num_position_vars = (bezier_degree-1)*3
    num_orientation_vars = (bezier_degree-1)*3
    num_accel_bias_vars = 3
    num_gravity_vars = 3

    block_sizes = [num_position_vars, num_orientation_vars, num_accel_bias_vars, num_gravity_vars]
    num_vars = sum(block_sizes)

    sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
    sym_pos_controls, sym_orient_controls, sym_accel_bias, sym_gravity = map(np.array, chop(sym_vars, block_sizes))

    sym_pos_controls = sym_pos_controls.reshape((-1, 3))
    sym_orient_controls = sym_orient_controls.reshape((-1, 3))

    assert len(true_vars) == len(sym_vars)

    #
    # Accel residuals
    #
    residuals = []

    print 'Accel residuals:'
    for i, t in enumerate(accel_times):
        sym_cayley = zero_offset_bezier(sym_orient_controls, t)
        sym_orient = cayley_mat(sym_cayley)
        sym_denom = cayley_denom(sym_cayley)
        sym_global_accel = zero_offset_bezier_second_deriv(sym_pos_controls, t)
#.........这里部分代码省略.........

# 需要导入模块: from polynomial import Polynomial [as 别名]
# 或者: from polynomial.Polynomial import coordinate [as 别名]
def run_from_data():
    #
    # Load data
    #

    path = Path('/Users/alexflint/Code/spline-initialization/out')

    frame_orientation_data = np.loadtxt(str(path / 'frame_orientations.txt'))
    frame_timestamps = frame_orientation_data[:, 0]
    frame_orientations = frame_orientation_data[:, 1:].reshape((-1, 3, 3))

    accel_data = np.loadtxt(str(path / 'accelerometer.txt'))
    accel_timestamps = accel_data[:, 0]
    accel_readings = accel_data[:, 1:]

    accel_orientation_data = np.loadtxt(str(path / 'accel_orientations.txt'))
    accel_orientations = accel_orientation_data[:, 1:].reshape((-1, 3, 3))

    feature_data = np.loadtxt(str(path / 'features.txt'))
    landmarks_ids = sorted(set(feature_data[:, 0].astype(int)))
    frame_ids = sorted(set(feature_data[:, 1].astype(int)))
    landmark_index_by_id = {idx: i for i, idx in enumerate(landmarks_ids)}
    frame_index_by_id = {idx: i for i, idx in enumerate(frame_ids)}

    assert len(accel_orientations) == len(accel_readings)
    assert len(frame_ids) == len(frame_orientations) == len(frame_timestamps)

    num_frames = len(frame_ids)
    num_landmarks = len(landmarks_ids)
    num_imu_readings = len(accel_readings)
    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

    #
    # Make feature table
    #
    features = np.ones((num_frames, num_landmarks, 2))
    features.fill(np.nan)
    for landmark_id, frame_id, feature in zip(landmarks_ids, frame_ids, feature_data[:, 2:]):
        i = frame_index_by_id[frame_id]
        j = landmark_index_by_id[landmark_id]
        features[i, j] = feature
        feature_mask[i, j] = True

    #
    # Normalize timestamps to [0,1]
    #
    begin_time = min(np.min(accel_timestamps), np.min(frame_timestamps))
    end_time = max(np.max(accel_timestamps), np.max(frame_timestamps))
    accel_timestamps = (accel_timestamps - begin_time) / (end_time - begin_time)
    frame_timestamps = (frame_timestamps - begin_time) / (end_time - begin_time)

    #
    # Construct symbolic versions of the above
    #
    position_offs = 0
    accel_bias_offset = position_offs + (bezier_degree-1)*3
    gravity_offset = accel_bias_offset + 3
    num_vars = gravity_offset + 3

    sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
    sym_pos_controls = np.reshape(sym_vars[position_offs:position_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))
    sym_accel_bias = np.asarray(sym_vars[accel_bias_offset:accel_bias_offset+3])
    sym_gravity = np.asarray(sym_vars[gravity_offset:gravity_offset+3])

    #
    # Compute residuals
    #
    epipolar_residuals = evaluate_epipolar_residuals(sym_pos_controls, frame_timestamps,
                                                     frame_orientations, features, feature_mask)
    accel_residuals = evaluate_accel_residuals(sym_pos_controls, sym_accel_bias, sym_gravity,
                                               accel_timestamps, accel_readings, accel_orientations)

    residuals = accel_residuals + epipolar_residuals

    print '\nNum vars:', num_vars
    print 'Num residuals:', len(residuals)

    print '\nResiduals:', len(residuals)
    cost = Polynomial(num_vars)
    for r in residuals:
        cost += r*r
        print '  degree=%d, length=%d' % (r.total_degree, len(r))

    print '\nCost:'
    print '  Num terms: %d' % len(cost)
    print '  Degree: %d' % cost.total_degree

    # Solve
    A, b, k = quadratic_form(cost)
    estimated_vars = np.squeeze(np.linalg.solve(A*2, -b))
    estimated_pos_controls = np.reshape(estimated_vars[position_offs:position_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))
    estimated_positions = np.array([bezier.zero_offset_bezier(estimated_pos_controls, t) for t in frame_timestamps])
    estimated_accel_bias = np.asarray(estimated_vars[accel_bias_offset:accel_bias_offset+3])
    estimated_gravity = np.asarray(estimated_vars[gravity_offset:gravity_offset+3])

#.........这里部分代码省略.........

# 需要导入模块: from polynomial import Polynomial [as 别名]
# 或者: from polynomial.Polynomial import coordinate [as 别名]
def run_simulation():
    np.random.seed(1)

    #
    # Construct ground truth
    #
    num_frames = 5
    num_landmarks = 50
    num_imu_readings = 80
    bezier_degree = 4
    out = 'out/position_only_bezier3'

    print 'Num landmarks:', num_landmarks
    print 'Num frames:', num_frames
    print 'Num IMU readings:', num_imu_readings
    print 'Bezier curve degree:', bezier_degree

    if not os.path.isdir(out):
        os.mkdir(out)

    # Both splines should start at 0,0,0
    frame_times = np.linspace(0, .9, num_frames)
    imu_times = 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)

    true_frame_cayleys = np.array([bezier.zero_offset_bezier(true_rot_controls, t) for t in frame_times])
    true_frame_orientations = np.array(map(cayley, true_frame_cayleys))
    true_frame_positions = np.array([bezier.zero_offset_bezier(true_pos_controls, t) for t in frame_times])

    true_imu_cayleys = np.array([bezier.zero_offset_bezier(true_rot_controls, t) for t in imu_times])
    true_imu_orientations = np.array(map(cayley, true_imu_cayleys))

    true_gravity_magnitude = 9.8
    true_gravity = normalized(np.random.rand(3)) * true_gravity_magnitude
    true_accel_bias = np.random.randn(3)
    true_global_accels = np.array([bezier.zero_offset_bezier_second_deriv(true_pos_controls, t) for t in imu_times])
    true_accels = np.array([np.dot(R, a + true_gravity) + true_accel_bias
                            for R, a in zip(true_imu_orientations, true_global_accels)])

    true_features = np.array([[normalized(np.dot(R, x-p)) for x in true_landmarks]
                              for R, p in zip(true_frame_orientations, true_frame_positions)])

    print np.min(true_features.reshape((-1, 3)), axis=0)
    print np.max(true_features.reshape((-1, 3)), axis=0)

    #
    # Add sensor noise
    #

    accel_noise = 0#0.001
    feature_noise = 0#0.01
    orientation_noise = 0.01

    observed_frame_orientations = true_frame_orientations.copy()
    observed_imu_orientations = true_imu_orientations.copy()
    observed_features = true_features.copy()
    observed_accels = true_accels.copy()

    if orientation_noise > 0:
        for i, R in enumerate(observed_frame_orientations):
            R_noise = SO3.exp(np.random.randn(3)*orientation_noise)
            observed_frame_orientations[i] = np.dot(R_noise, R)
        for i, R in enumerate(observed_imu_orientations):
            R_noise = SO3.exp(np.random.randn(3)*orientation_noise)
            observed_imu_orientations[i] = np.dot(R_noise, R)

    if accel_noise > 0:
        observed_accels += np.random.randn(*observed_accels.shape) * accel_noise

    if feature_noise > 0:
        observed_features += np.random.rand(*observed_features.shape) * feature_noise

    #
    # Construct symbolic versions of the above
    #
    position_offs = 0
    accel_bias_offset = position_offs + (bezier_degree-1)*3
    gravity_offset = accel_bias_offset + 3
    num_vars = gravity_offset + 3

    sym_vars = [Polynomial.coordinate(i, num_vars, Fraction) for i in range(num_vars)]
    sym_pos_controls = np.reshape(sym_vars[position_offs:position_offs+(bezier_degree-1)*3], (bezier_degree-1, 3))
    sym_accel_bias = np.asarray(sym_vars[accel_bias_offset:accel_bias_offset+3])
    sym_gravity = np.asarray(sym_vars[gravity_offset:gravity_offset+3])

    true_vars = np.hstack((true_pos_controls.flatten(), true_accel_bias, true_gravity))
    assert len(true_vars) == len(sym_vars)

    #
    # Compute residuals
    #

    epipolar_residuals = evaluate_epipolar_residuals(sym_pos_controls, frame_times,
                                                     observed_frame_orientations, observed_features)
    accel_residuals = evaluate_accel_residuals(sym_pos_controls, sym_accel_bias, sym_gravity,
                                               imu_times, observed_accels, observed_imu_orientations)
#.........这里部分代码省略.........