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)