def cayley_inv(r): w = SO3.log(r) theta = np.linalg.norm(w) if theta < 1e-8: return w else: return w * np.tan(theta / 2.) / theta
def main(): orientation_data = np.loadtxt('out/frame_orientations.txt') accel_data = np.loadtxt('out/accelerometer.txt') frame_timestamps = orientation_data[:,0] frame_orientations = orientation_data[:, 1:].reshape((-1, 3, 3)) accel_timestamps = accel_data[:,0] accel_readings = accel_data[:,1:] accel_orientations = [] for accel_time in accel_timestamps: frame_index = bisect.bisect_left(frame_timestamps, accel_time) assert 0 < frame_index < len(frame_timestamps), 't='+accel_time t0 = frame_timestamps[frame_index-1] r0 = frame_orientations[frame_index-1] t1 = frame_timestamps[frame_index] r1 = frame_orientations[frame_index] w01 = SO3.log(np.dot(r1, r0.T)) a = (accel_time - t0) / (t1 - t0) assert 0 <= a <= 1 accel_orientations.append(np.dot(SO3.exp(a*w01), r0)) frame_ws = [] accel_ws = [] rbase = frame_orientations[0] for r in frame_orientations: frame_ws.append(SO3.log(np.dot(r, rbase.T))) for r in accel_orientations: accel_ws.append(SO3.log(np.dot(r, rbase.T))) np.savetxt('out/accel_orientations.txt', np.hstack((accel_timestamps[:,None], np.reshape(accel_orientations, (-1, 9))))) plt.clf() plt.hold('on') plt.plot(frame_timestamps, np.asarray(frame_orientations).reshape((-1, 9))) plt.plot(accel_timestamps, np.asarray(accel_orientations).reshape((-1, 9)), '.') #plt.plot(frame_timestamps, map(np.linalg.norm, frame_ws)) #plt.plot(accel_timestamps, map(np.linalg.norm, accel_ws), '.') plt.show()
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 orientation_residuals(bezier_params, observed_timestamps, observed_orientations): return np.hstack([SO3.log(np.dot(predict_orientation(bezier_params, t).T, r)) for t, r in zip(observed_timestamps, observed_orientations)])
def angular_velocity_right(f, t, step=1e-8): return SO3.log(np.dot(f(t).T, f(t + step))) / step
def angular_velocity_left(f, t, step=1e-8): return SO3.log(np.dot(f(t + step), f(t).T)) / step
def run_optimize(): bezier_order = 3 num_gyro_readings = 50 num_frames = 5 frame_timestamp_noise = 1e-3 frame_orientation_noise = .02 gyro_timestamp_noise = 1e-3 gyro_noise = .01 #path = os.path.expanduser('~/Data/Initialization/closed_flat/gyro.txt') #gyro_data = np.loadtxt(path) #gyro_timestamps = gyro_data[:,0] #gyro_readings = gyro_data[:,1:] true_gyro_timestamps = np.linspace(0, 1, num_gyro_readings) true_params = np.random.rand(bezier_order, 3) true_gyro_bias = np.random.rand(3) true_gyro_readings = np.array([predict_gyro(true_params, true_gyro_bias, t) for t in true_gyro_timestamps]) true_frame_timestamps = np.linspace(0, 1, num_frames) true_frame_orientations = np.array([predict_orientation(true_params, t) for t in true_frame_timestamps]) observed_gyro_timestamps = add_white_noise(true_gyro_timestamps, gyro_timestamp_noise) observed_gyro_readings = add_white_noise(true_gyro_readings, gyro_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) estimated_gyro_bias, estimated_params = estimate_orientation(bezier_order, observed_gyro_timestamps, observed_gyro_readings, observed_frame_timestamps, observed_frame_orientations) print '\nTrue params:' print true_params print '\nEstimated params:' print estimated_params print '\nTrue gyro bias:' print true_gyro_bias print '\nEstimated gyro bias:' print estimated_gyro_bias plot_timestamps = np.linspace(0, 1, 50) estimated_gyro_readings = np.array([predict_gyro(estimated_params, true_gyro_bias, t) for t in plot_timestamps]) true_orientations = np.array([SO3.log(predict_orientation(true_params, t)) for t in plot_timestamps]) observed_orientations = np.array(map(SO3.log, observed_frame_orientations)) estimated_orientations = np.array([SO3.log(predict_orientation(estimated_params, t)) for t in plot_timestamps]) plt.figure(1) plt.plot(true_gyro_timestamps, true_gyro_readings, '-', label='true') plt.plot(true_gyro_timestamps, observed_gyro_readings, 'x', label='observed') plt.plot(plot_timestamps, estimated_gyro_readings, ':', label='estimated') plt.xlim(-.1, 1.5) plt.legend() plt.figure(2) plt.plot(plot_timestamps, true_orientations, '-', label='true') plt.plot(true_frame_timestamps, observed_orientations, 'x', label='observed') plt.plot(plot_timestamps, estimated_orientations, ':', label='estimated') plt.xlim(-.1, 1.5) plt.legend() plt.show()