def test_zero_offset_bezier_mat(self): bezier_order = 4 t = .5 f = lambda x: bezier.zero_offset_bezier(x.reshape((bezier_order, 3)), t) J_numeric = numdifftools.Jacobian(f)(np.zeros(bezier_order*3)) J_analytic = bezier.zero_offset_bezier_mat(t, bezier_order, 3) np.testing.assert_array_almost_equal(J_numeric, J_analytic)
def main(): bezier_order = 4 num_samples = 10 true_controls = np.random.rand(bezier_order, 3) true_ts = np.linspace(0, 1, num_samples) true_ys = np.array([zero_offset_bezier(true_controls, t) for t in true_ts]) estimated_controls = fit_zero_offset_bezier(true_ts, true_ys, bezier_order) plot_ts = np.linspace(0, 1, 50) plot_true_ys = np.array([zero_offset_bezier(true_controls, t) for t in plot_ts]) estimated_ys = np.array([zero_offset_bezier(estimated_controls, t) for t in plot_ts]) plt.clf() plt.plot() plt.plot(plot_ts, estimated_ys, ':', alpha=1, label='estimated') plt.plot(plot_ts, plot_true_ys, '-', alpha=.3, label='true') plt.plot(true_ts, true_ys, 'x', alpha=1, label='observed') plt.legend() plt.show()
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) residuals = np.hstack((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 ' %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 # 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_times]) 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]) re_estimated_gravity = normalized(estimated_gravity) * true_gravity_magnitude print '\nEstimated:' print estimated_vars print '\nGround truth:' print true_vars print '\nTotal Error:', np.linalg.norm(estimated_vars - true_vars) print 'Accel bias error:', np.linalg.norm(estimated_accel_bias - true_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) print ' Re-normalized gravity error: ', np.linalg.norm(re_estimated_gravity - true_gravity) for i in range(num_frames): print 'Frame %d error: %f' % (i, np.linalg.norm(estimated_positions[i] - true_frame_positions[i])) fig = plt.figure(figsize=(14,6)) ax = fig.add_subplot(1, 2, 1, projection='3d') ts = np.linspace(0, 1, 100) true_ps = np.array([bezier.zero_offset_bezier(true_pos_controls, t) for t in ts]) estimated_ps = np.array([bezier.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') plt.show()
def epipolar_residual(pos_controls, ti, tj, zi, zj, Ri, Rj): pi = bezier.zero_offset_bezier(pos_controls, ti) pj = bezier.zero_offset_bezier(pos_controls, tj) E = essential_matrix(Ri, pi, Rj, pj) return dots(zj, E, zi)
def reprojection_residual(pos_controls, landmark, depth, timestamp, feature, orientation): position = zero_offset_bezier(pos_controls, timestamp) return np.dot(orientation, landmark - position) - depth * feature
def predict_gyro(bezier_params, gyro_bias, time): s = zero_offset_bezier(bezier_params, time) s_deriv = zero_offset_bezier_deriv(bezier_params, time) orient = cayley(s) angular_velocity = angular_velocity_from_cayley_deriv(s, s_deriv) return np.dot(orient, angular_velocity) + gyro_bias
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 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 for term in cost: print ' ',term 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 = np.array([r.partial_derivatives() for r in residuals]) J = evaluate_array(jacobians, *true_vars) U, S, V = np.linalg.svd(J) print '\nJacobian singular values:' print J.shape print S print '\nHessian eigenvalues:' H = np.dot(J.T, J) print H.shape print np.linalg.eigvals(H) null_space_dims = sum(np.abs(S) < 1e-5) print '\nNull space dimensions:', null_space_dims if null_space_dims > 0: for i in range(null_space_dims): print ' ',V[-i] null_monomial = (0,) * num_vars coordinate_monomials = [list(var.monomials)[0] for var in sym_vars] A, _ = matrix_form(gradients, coordinate_monomials) b, _ = matrix_form(gradients, [null_monomial]) b = np.squeeze(b) AA, bb, kk = quadratic_form(cost) estimated_vars = np.squeeze(numpy.linalg.solve(AA*2, -b)) print '\nEstimated:' print estimated_vars print '\nGround truth:' print true_vars print '\nError:' print np.linalg.norm(estimated_vars - true_vars) # Output to file write_polynomials(cost, out+'/cost.txt') write_polynomials(residuals, out+'/residuals.txt') write_polynomials(gradients, out+'/gradients.txt') write_polynomials(jacobians.flat, out+'/jacobians.txt') write_solution(true_vars, out+'/solution.txt')
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) sym_accel = np.dot(sym_orient, sym_global_accel + sym_gravity) + sym_denom * sym_accel_bias residual = sym_accel - sym_denom * observed_accels[i] residuals.extend(residual) for r in residual: print ' %f (degree=%d, length=%d)' % (r(*true_vars), r.total_degree, len(r)) # # Epipolar residuals # print 'Epipolar residuals:' for i, ti in enumerate(frame_times): if i == 0: continue sym_Ri = cayley_mat(zero_offset_bezier(sym_orient_controls, ti)) sym_pi = zero_offset_bezier(sym_pos_controls, ti) sym_E = essential_matrix_from_relative_pose(sym_Ri, sym_pi) for k in range(num_landmarks): z1 = observed_features[0][k] zi = observed_features[i][k] residual = np.dot(zi, np.dot(sym_E, z1)) residuals.append(residual) r = residual print ' %f (degree=%d, length=%d)' % (r(*true_vars), r.total_degree, len(r)) # # Construct cost # cost = Polynomial(num_vars) for r in residuals: cost += r*r gradients = cost.partial_derivatives() print '\nNum vars:', num_vars print 'Num residuals:', len(residuals) print '\nCost:' print ' Num terms: %d' % len(cost) print ' Degree: %d' % cost.total_degree # # Output to file # write_polynomials(cost, out+'/cost.txt') write_polynomials(residuals, out+'/residuals.txt') write_polynomials(gradients, out+'/gradients.txt') write_solution(true_vars, out+'/solution.txt') np.savetxt(out+'/feature_measurements.txt', observed_features.reshape((-1, 3))) np.savetxt(out+'/accel_measurements.txt', observed_accels) np.savetxt(out+'/problem_size.txt', [num_frames, num_landmarks, num_imu_readings]) np.savetxt(out+'/frame_times.txt', frame_times) np.savetxt(out+'/accel_times.txt', accel_times) np.savetxt(out+'/true_pos_controls.txt', true_pos_controls) np.savetxt(out+'/true_orient_controls.txt', true_orient_controls) np.savetxt(out+'/true_accel_bias.txt', true_accel_bias) np.savetxt(out+'/true_gravity.txt', true_gravity) return # # Plot # fig = plt.figure(figsize=(14,6)) ax = fig.add_subplot(1, 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]) ax.plot(true_ps[:, 0], true_ps[:, 1], true_ps[:, 2], '-b') plt.show()
def fit_zero_offset_bezier_1d(ts, ys, bezier_order): jacobian = np.array([zero_offset_bezier_coefs(t, bezier_order) for t in ts]) residual = np.array([zero_offset_bezier(np.zeros(bezier_order), t) - y for t, y in zip(ts, ys)]) return np.linalg.lstsq(jacobian, -residual)[0]
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 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 test_zero_offset_second_derivative(self): w = np.array([1.7, 2.8, 1.4, -3.6]) f = lambda t: bezier.zero_offset_bezier(w, t) g2 = numdifftools.Hessdiag(f) np.testing.assert_array_almost_equal(g2(.9), bezier.zero_offset_bezier_second_deriv(w, .9))
def run_simulation_nonsymbolic(): 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) 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) 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 accel_timestamps]) 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 accel_timestamps]) 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 position_offs = 0 accel_bias_offset = position_offs + (bezier_degree-1)*3 gravity_offset = accel_bias_offset + 3 true_vars = np.hstack((true_pos_controls.flatten(), true_accel_bias, true_gravity)) # # Compute system non-symbolically # accel_r = evaluate_accel_residuals(np.zeros((bezier_degree-1, 3)), np.zeros(3), np.zeros(3), accel_timestamps, observed_accels, observed_imu_orientations) accel_j = evaluate_accel_jacobians(bezier_degree-1, accel_timestamps, observed_imu_orientations) epipolar_r = evaluate_epipolar_residuals(np.zeros((bezier_degree-1, 3)), frame_times, observed_frame_orientations, observed_features) epipolar_j = evaluate_epipolar_jacobians(bezier_degree-1, frame_times, observed_frame_orientations, observed_features) epipolar_j = np.hstack((epipolar_j, np.zeros((epipolar_j.shape[0], 6)))) residual = np.hstack((accel_r, epipolar_r)) jacobian = np.vstack((accel_j, epipolar_j)) # # Solve # JtJ = np.dot(jacobian.T, jacobian) Jtr = np.dot(jacobian.T, residual) estimated_vars = np.squeeze(np.linalg.solve(JtJ, -Jtr)) # # Unpack result and compute error # 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_times]) 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]) re_estimated_gravity = normalized(estimated_gravity) * true_gravity_magnitude print '\nEstimated:' print estimated_vars print '\nGround truth:' print true_vars print '\nTotal Error:', np.linalg.norm(estimated_vars - true_vars) print 'Accel bias error:', np.linalg.norm(estimated_accel_bias - true_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) print ' Re-normalized gravity error: ', np.linalg.norm(re_estimated_gravity - true_gravity) for i in range(num_frames): print 'Frame %d error: %f' % (i, np.linalg.norm(estimated_positions[i] - true_frame_positions[i])) fig = plt.figure(figsize=(14,6)) ax = fig.add_subplot(1, 2, 1, projection='3d') ts = np.linspace(0, 1, 100) true_ps = np.array([bezier.zero_offset_bezier(true_pos_controls, t) for t in ts]) estimated_ps = np.array([bezier.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') plt.show()
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]) print '\nEstimated:' print estimated_vars print 'Estimated accel bias:', estimated_accel_bias print 'Estimated gravity:', estimated_gravity print 'Estimated gravity magnitude:', np.linalg.norm(estimated_gravity) fig = plt.figure(figsize=(14,6)) ax = fig.add_subplot(1, 2, 1, projection='3d') ts = np.linspace(0, 1, 100) estimated_ps = np.array([bezier.zero_offset_bezier(estimated_pos_controls, t) for t in ts]) ax.plot(estimated_ps[:, 0], estimated_ps[:, 1], estimated_ps[:, 2], '-r') ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') plt.show()
def predict_accel(pos_controls, orient_controls, accel_bias, gravity, t): orientation = cayley(zero_offset_bezier(orient_controls, t)) global_accel = zero_offset_bezier_second_deriv(pos_controls, t) return np.dot(orientation, global_accel + gravity) + accel_bias
def predict_feature(pos_controls, orient_controls, landmark, t): r = cayley(zero_offset_bezier(orient_controls, t)) p = zero_offset_bezier(pos_controls, t) return normalized(np.dot(r, landmark - p))
def predict_depth(pos_controls, orient_controls, landmark, t): r = cayley(zero_offset_bezier(orient_controls, t)) p = zero_offset_bezier(pos_controls, t) return np.linalg.norm(np.dot(r, landmark - p))
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]) U, S, V = np.linalg.svd(J) print '\nJacobian singular values:' print J.shape print S null_space_dims = sum(np.abs(S) < 1e-5) if null_space_dims > 0: print '\nNull space:' for i in null_space_dims: print V[-i] print V[-2] print '\nHessian eigenvalues:' H = np.dot(J.T, J) print H.shape print np.linalg.eigvals(H) # Output to file write_polynomials(cost, out+'/cost.txt') write_polynomials(residuals, out+'/residuals.txt') write_polynomials(gradients, out+'/gradients.txt') write_polynomials(jacobians, out+'/jacobians.txt') write_solution(true_vars, out+'/solution.txt')
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 analyze_polynomial2(): print 'Loading polynomials...' varnames, true_values = load_solution('out/epipolar_accel_bezier3/solution.txt') cost = load_functions('out/epipolar_accel_bezier3/cost.txt', varnames)[0] residuals = load_functions('out/epipolar_accel_bezier3/residuals.txt', varnames) jacobians = load_functions('out/epipolar_accel_bezier3/jacobians.txt', varnames) gradients = load_functions('out/epipolar_accel_bezier3/gradients.txt', varnames) residuals = np.array(residuals) jacobians = np.array(jacobians).reshape((-1, len(varnames))) def J(x): print ' ... jacobian' return evaluate_array(jacobians, *x) def r(x): print ' ... residual' return evaluate_array(residuals, *x) seed_values = true_values + np.random.rand(len(true_values)) * 100 out = scipy.optimize.leastsq(func=r, x0=seed_values, Dfun=J, full_output=True) opt_values = out[0] print '\nGradients:' print evaluate_array(gradients, *opt_values) print evaluate_array(gradients, *true_values) print '\nCosts:' print cost(*opt_values) print cost(*true_values) print '\nJacobian singular values:' jac = J(opt_values) U, S, V = np.linalg.svd(jac) print S print '\nTrue:' print true_values print '\nSeed:' print seed_values print '\nOptimized:' print opt_values print '\nError:' print np.linalg.norm(opt_values - true_values) true_pcontrols = true_values[:6].reshape((-1, 3)) opt_pcontrols = opt_values[:6].reshape((-1, 3)) fig = plt.figure(figsize=(14,6)) ax = fig.add_subplot(1, 2, 1, projection='3d') ts = np.linspace(0, 1, 100) true_ps = np.array([zero_offset_bezier(true_pcontrols, t) for t in ts]) opt_ps = np.array([zero_offset_bezier(opt_pcontrols, t) for t in ts]) ax.plot(true_ps[:,0], true_ps[:,1], true_ps[:,2], '-b') ax.plot(opt_ps[:,0], opt_ps[:,1], opt_ps[:,2], '-r') plt.show()
def predict_orientation(bezier_params, time): return cayley(zero_offset_bezier(bezier_params, time))