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