def test_clone(trajectory): if type(trajectory) == UniformSO3SplineTrajectory: pytest.xfail('Test mutates position, which this trajectory does not have') t1, t2 = safe_time_span(trajectory, 4) times = np.arange(t1, t2, 0.01) def sample(trajectory): return np.vstack([trajectory.position(t) for t in times]) orig_pos = sample(trajectory) # Clone cloned_trajectory = trajectory.clone() cloned_pos = sample(cloned_trajectory) np.testing.assert_equal(orig_pos, cloned_pos) # Change original meas = [PositionMeasurement(t, np.random.uniform(-4, 1, size=3)) for t in np.arange(t1, t2, 0.1)] estimator = TrajectoryEstimator(trajectory) for m in meas: estimator.add_measurement(m) estimator.solve(max_iterations=10) new_pos = sample(trajectory) assert np.linalg.norm(new_pos - orig_pos) > 1 # Check that clone did NOT change new_cloned = sample(cloned_trajectory) np.testing.assert_equal(new_cloned, orig_pos)
def test_camera_measurement_time_offset(mcls, camera, split_trajectory): t1, t2 = safe_time_span(split_trajectory, 1) t1 += camera.max_time_offset t2 -= camera.max_time_offset d = np.random.uniform(-camera.max_time_offset, camera.max_time_offset) lm = Landmark() lm.inverse_depth = np.random.uniform(0.01, 1) views = [View(i, t) for i, t in enumerate([t1, t1 + 0.23])] ref, obs = [ v.create_observation(lm, np.random.uniform(100, 900, size=2)) for v in views ] lm.reference = ref m1 = mcls(camera, obs) y1 = m1.measure(split_trajectory) new_lm = Landmark() new_lm.inverse_depth = lm.inverse_depth new_views = [View(v.frame_nr, v.t0 - d) for v in views] new_ref, new_obs = [ v.create_observation(new_lm, o.uv) for v, o in zip(new_views, [ref, obs]) ] new_lm.reference = new_ref camera.time_offset = d m2 = mcls(camera, new_obs) y2 = m2.measure(split_trajectory) np.testing.assert_almost_equal(y1, y2)
def test_accelerometer_measurements(trajectory, imu): times = np.linspace(*safe_time_span(trajectory, 3.0), num=10, endpoint=False) from kontiki.trajectories import UniformSE3SplineTrajectory if type(trajectory) == UniformSE3SplineTrajectory: pytest.xfail( "SE3 fails because second order derivative is not the same as body acceleration" ) def true_acc(t): q = trajectory.orientation(t) acc_world = trajectory.acceleration(t) R = quat_to_rotation_matrix(q) gravity = np.array([0, 0, 9.80665]) acc = R.T @ (acc_world - gravity) return acc # Currently fails for ConstantBiasImu since we don't take bias into account for t in times: acc = true_acc(t) m = AccelerometerMeasurement(imu, t, acc) acc_hat = m.measure(trajectory) try: acc_hat -= imu.accelerometer_bias except AttributeError: pass # No bias to remove np.testing.assert_almost_equal(acc_hat, acc)
def simple_measurements(request, trajectory): length = 5. n = int(length * 3) times = np.linspace(*safe_time_span(trajectory, length), num=n) cls = request.param if cls == PositionMeasurement: return [cls(t, np.random.uniform(-1, 1, size=3)) for t in times] else: raise NotImplementedError( f"simple_measurements fixture not implemented for {cls}")
def imu_measurements(request, imu, trajectory): cls = request.param length = 5. n = int(length * 3) t1, t2 = safe_time_span(trajectory, length) t1 = max(t1, trajectory.min_time + imu.max_time_offset) t2 = min(t2, trajectory.max_time - imu.max_time_offset) times = np.linspace(t1, t2, endpoint=False, num=n) print('Creating from imu', imu) return [cls(imu, t, np.random.uniform(-1, 1, size=3)) for t in times]
def generate_valid_structure(camera, trajectory): fps = 30 nviews = 8 t1, t2 = safe_time_span(trajectory, nviews/fps) # Safety margins t1 += 1e-2 t2 -= 1e-2 times = t1 + np.arange(nviews) / fps views = [View(i, t) for i, t in enumerate(times)] nlandmarks = 5 start_probs = np.exp(-0.5*np.arange(len(views))) start_probs /= np.sum(start_probs) landmarks = [generate_landmark(views, camera, trajectory, start_probs) for _ in range(nlandmarks)] return views, landmarks
def test_gyroscope_measurements(trajectory, imu): times = np.linspace(*safe_time_span(trajectory, 3.0), num=10, endpoint=False) def true_gyro(t): q = trajectory.orientation(t) R = quat_to_rotation_matrix(q) w_world = trajectory.angular_velocity(t) w_body = R.T @ w_world return w_body for t in times: w = true_gyro(t) m = GyroscopeMeasurement(imu, t, w) w_hat = m.measure(trajectory) try: w_hat -= imu.gyroscope_bias except AttributeError: pass # No bias to remove np.testing.assert_almost_equal(w_hat, w)