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