def test_add_gyro_generate_quat(self):
db = DatasetBuilder()
N = 100
gdata = np.zeros((N, 3))
gtimes = np.zeros(N)
db.add_source_gyro(gdata, gtimes)
self.assertEqual(len(db._gyro_quat), len(gtimes))
def test_gyro_uniform(self):
N = 1000
gyro_data = np.zeros((3, N))
gyro_times = np.random.uniform(0, 100, size=N)
gyro_times.sort()
db = DatasetBuilder()
with self.assertRaises(DatasetError):
db.add_source_gyro(gyro_data, gyro_times)
def test_selected_sources(self):
db = DatasetBuilder()
db.set_landmark_source('sfm')
db.set_position_source('imu')
db.set_orientation_source('imu')
expected = {'orientation': 'imu', 'position': 'imu', 'landmark': 'sfm'}
self.assertEqual(db.selected_sources, expected)
def test_add_sfm_twice_fail(self):
db = DatasetBuilder()
sfm1 = SfmResult()
sfm2 = SfmResult()
db.add_source_sfm(sfm1)
with self.assertRaises(DatasetError):
db.add_source_sfm(sfm2)
def test_add_gyro_twice_fail(self):
db = DatasetBuilder()
N = 100
gdata = np.zeros((N, 3))
gtimes = np.zeros(N)
db.add_source_gyro(gdata, gtimes)
with self.assertRaises(DatasetError):
db.add_source_gyro(gdata, gtimes)
def test_add_gyro_shape(self):
N = 100
gyro_times = np.arange(N) / 100.
valid_shapes = [(N, 3)]
invalid_shapes = [(3, N), (4, N), (N, 2), (N, 5)]
for sh in valid_shapes:
db = DatasetBuilder()
gyro_data = np.random.normal(size=sh)
db.add_source_gyro(gyro_data, gyro_times)
for sh in invalid_shapes:
db = DatasetBuilder()
gyro_data = np.random.normal(size=sh)
with self.assertRaises(DatasetError):
db.add_source_gyro(gyro_data, gyro_times)
def test_sfm_aligned_imu_orientations(self):
db = DatasetBuilder()
camera_fps = 30.0
db.add_source_sfm(self.sfm)
db.add_source_gyro(GYRO_EXAMPLE_DATA, GYRO_EXAMPLE_TIMES)
orientations_aligned, new_times = db._sfm_aligned_imu_orientations()
Q_aligned = QuaternionArray(orientations_aligned).unflipped()
for view in self.sfm.views:
qt = quaternion_array_interpolate(Q_aligned, new_times, view.time)
if qt.dot(view.orientation) < 0:
qt = -qt
nt.assert_almost_equal(qt.components,
view.orientation.components,
decimal=1)
def setUp(self):
db = DatasetBuilder()
db.add_source_gyro(GYRO_EXAMPLE_DATA, GYRO_EXAMPLE_TIMES)
sfm = VisualSfmResult.from_file(NVM_EXAMPLE, camera_fps=CAMERA_FPS)
db.add_source_sfm(sfm)
db.set_landmark_source('sfm')
db.set_orientation_source('imu')
db.set_position_source('sfm')
self.ds = db.build()
self.testdir = tempfile.mkdtemp()
def test_rescale_speed(self):
db = DatasetBuilder()
db.add_source_sfm(self.sfm)
db.set_landmark_source('sfm')
db.set_position_source('sfm')
db.set_orientation_source('sfm')
ds = db.build()
t = np.linspace(ds.trajectory.startTime, ds.trajectory.endTime)
pos_org = ds.trajectory.position(t)
rot_org = ds.trajectory.rotation(t)
for speed in [1.4, 5]:
ds_r = ds.rescaled_avg_speed(speed)
pos = ds_r.trajectory.position(t)
rot = ds_r.trajectory.rotation(t)
nt.assert_equal(rot.array, rot_org.array)
scale_arr = pos_org / pos
nt.assert_allclose(scale_arr,
np.mean(scale_arr),
atol=0.2,
rtol=1e-2)
def test_rescale(self):
db = DatasetBuilder()
db.add_source_sfm(self.sfm)
db.set_landmark_source('sfm')
db.set_position_source('sfm')
db.set_orientation_source('sfm')
ds = db.build()
t = np.linspace(ds.trajectory.startTime, ds.trajectory.endTime)
pos_org = ds.trajectory.position(t)
rot_org = ds.trajectory.rotation(t)
for scale_factor in [0.1, 10.0]:
ds_r = ds.rescaled(scale_factor)
self.assertEqual(len(ds_r.landmarks), len(ds.landmarks))
for lm_r, lm in zip(ds_r.landmarks, ds.landmarks):
self.assertEqual(lm_r.id, lm.id)
nt.assert_almost_equal(lm_r.position,
scale_factor * lm.position)
self.assertEqual(sorted(lm_r.visibility),
sorted(lm.visibility))
pos = ds_r.trajectory.position(t)
rot = ds_r.trajectory.rotation(t)
nt.assert_almost_equal(pos, scale_factor * pos_org, decimal=1)
nt.assert_equal(rot.array, rot_org.array)
def test_with_gyro_velocity(self):
# NOTE: Since the gyro orientations are transported into the
# SfM coordinate frame, comparing the gyro velocity is not
# so simple, so we deactivate this test for now
db = DatasetBuilder()
db.add_source_gyro(GYRO_EXAMPLE_DATA, GYRO_EXAMPLE_TIMES)
db.add_source_sfm(self.sfm)
db.set_landmark_source('sfm')
db.set_position_source('sfm')
db.set_orientation_source('imu')
ds = db.build()
# 1) Dataset rotational velocity should match gyro (almost)
t0 = max(GYRO_EXAMPLE_TIMES[0], ds.trajectory.startTime)
t1 = min(GYRO_EXAMPLE_TIMES[-1], ds.trajectory.endTime)
i0 = np.flatnonzero(GYRO_EXAMPLE_TIMES >= t0)[0]
t0 = GYRO_EXAMPLE_TIMES[i0]
i1 = np.flatnonzero(GYRO_EXAMPLE_TIMES <= t1)[-1]
t1 = GYRO_EXAMPLE_TIMES[i1]
gyro_part_data = GYRO_EXAMPLE_DATA[i0:i1 + 1]
gyro_part_times = GYRO_EXAMPLE_TIMES[i0:i1 + 1]
rotvel_ds_world = ds.trajectory.rotationalVelocity(gyro_part_times)
rotvel_ds = ds.trajectory.rotation(gyro_part_times).rotateFrame(
rotvel_ds_world)
rotvel_err = np.linalg.norm(rotvel_ds - gyro_part_data.T, axis=0)
import matplotlib.pyplot as plt
fig1 = plt.figure()
for i in range(3):
plt.subplot(4, 1, 1 + i)
plt.plot(gyro_part_times,
gyro_part_data[:, i],
label='gyro',
linewidth=3)
plt.plot(gyro_part_times, rotvel_ds[i, :], label='ds')
plt.legend(loc='upper right')
plt.subplot(4, 1, 4)
plt.plot(gyro_part_times, rotvel_err)
plt.suptitle('Rotational velocity: ' + self.__class__.__name__)
fig1.savefig('/tmp/{}_w.pdf'.format(self.__class__.__name__))
view_q = QuaternionArray([v.orientation for v in self.sfm.views])
view_q = view_q.unflipped()
view_times = [v.time for v in self.sfm.views]
traj_q = ds.trajectory.rotation(gyro_part_times)
fig2 = plt.figure()
for i in range(4):
plt.subplot(4, 1, 1 + i)
plt.plot(view_times, view_q.array[:, i], '-o')
plt.plot(gyro_part_times, traj_q.array[:, i])
plt.suptitle('Quaternion: ' + self.__class__.__name__)
fig2.savefig('/tmp/{}_q.pdf'.format(self.__class__.__name__))
plt.show()
self.assertLess(np.mean(rotvel_err), 0.01)
def test_with_gyro_orientation(self):
db = DatasetBuilder()
db.add_source_gyro(GYRO_EXAMPLE_DATA, GYRO_EXAMPLE_TIMES)
db.add_source_sfm(self.sfm)
db.set_landmark_source('sfm')
db.set_position_source('sfm')
db.set_orientation_source('imu')
ds = db.build()
# 2) Dataset orientations should match approximately with NVM cameras
for view in self.sfm.views:
t = view.time
if ds.trajectory.startTime <= t <= ds.trajectory.endTime:
orientation_ds = ds.trajectory.rotation(t)
if orientation_ds.dot(view.orientation) < 0:
orientation_ds = -orientation_ds
nt.assert_almost_equal(orientation_ds.components,
view.orientation.components,
decimal=1)
def test_sfm_only(self):
db = DatasetBuilder()
db.add_source_sfm(self.sfm)
db.set_orientation_source('sfm')
db.set_position_source('sfm')
db.set_landmark_source('sfm')
ds = db.build()
for view in self.sfm.views:
t = view.time
if ds.trajectory.startTime <= t <= ds.trajectory.endTime:
p_ds = ds.trajectory.position(t).flatten()
q_ds = ds.trajectory.rotation(t)
nt.assert_almost_equal(p_ds, view.position, decimal=2)
q_sfm = view.orientation
if q_ds.dot(q_sfm) < 0:
q_sfm *= -1
nt.assert_almost_equal(q_ds.components,
q_sfm.components,
decimal=1)
# Assume landmark order intact
self.assertEqual(len(ds.landmarks), len(self.sfm.landmarks))
for sfm_lm, ds_lm in zip(self.sfm.landmarks, ds.landmarks):
nt.assert_equal(sfm_lm.position, ds_lm.position)
def test_missing_source_fail(self):
db = DatasetBuilder()
sfm = VisualSfmResult.from_file(NVM_EXAMPLE, camera_fps=30.)
db.add_source_sfm(sfm)
with self.assertRaises(DatasetError):
db.build()
db.set_position_source('sfm')
with self.assertRaises(DatasetError):
db.build()
db.set_orientation_source('sfm')
with self.assertRaises(DatasetError):
db.build()
db.set_landmark_source('sfm')
db.build() # all sources selected: OK
def test_source_types(self):
valid = ('imu', 'sfm')
landmark_valid = ('sfm', )
invalid = ('gyro', 'acc', 'bacon', 'openmvg', 'nvm')
db = DatasetBuilder()
for s in valid:
db.set_orientation_source(s)
db.set_position_source(s)
for s in invalid:
with self.assertRaises(DatasetError):
db.set_orientation_source(s)
with self.assertRaises(DatasetError):
db.set_position_source(s)
with self.assertRaises(DatasetError):
db.set_landmark_source(s)
for s in landmark_valid:
db.set_landmark_source(s)