def test_plot_trajectory(self):
view_pos = np.vstack([v.position for v in self.sfm.views]).T
view_times = np.array([v.time for v in self.sfm.views])
assert view_pos.shape[0] == 3 and view_pos.ndim == 2
view_q = QuaternionArray([v.orientation for v in self.sfm.views])
view_q = view_q.unflipped()
t = view_times #np.linspace(self.ds.trajectory.startTime, self.ds.trajectory.endTime,
# num=2000)
traj_pos = self.ds.trajectory.position(t)
traj_q = self.ds.trajectory.rotation(t)
import matplotlib.pyplot as plt
plt.figure()
for i in range(3):
plt.subplot(3, 1, 1 + i)
plt.plot(view_times, view_pos[i], label='views')
plt.plot(t, traj_pos[i], label='trajectory')
plt.suptitle(self.__class__.__name__)
plt.legend()
plt.figure()
for i in range(4):
plt.subplot(5, 1, 1 + i)
plt.plot(view_times, view_q.array[:, i], label='views')
plt.plot(t, traj_q.array[:, i], label='trajectory')
dq = view_q.conjugate * traj_q
angles = [q.toAxisAngle()[1] for q in dq]
plt.subplot(5, 1, 5)
plt.plot(t, np.rad2deg(angles))
plt.ylabel('degrees of error')
plt.suptitle(self.__class__.__name__)
plt.legend()
plt.show()
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_resample_quaternion_array(self):
sfm = VisualSfmResult.from_file(NVM_EXAMPLE, camera_fps=CAMERA_FPS)
Q = QuaternionArray([view.orientation for view in sfm.views])
Q = Q.unflipped()
view_times = np.array([view.time for view in sfm.views])
new_size = 500
Q_resamp, Q_t = resample_quaternion_array(Q,
view_times,
resize=new_size)
self.assertEqual(len(Q_resamp), len(Q_t))
self.assertEqual(len(Q_resamp), new_size)
nt.assert_almost_equal(Q_t[0], view_times[0])
nt.assert_almost_equal(Q_t[-1], view_times[-1])
nt.assert_almost_equal(unpack_quat(Q_resamp[0]), unpack_quat(Q[0]))
nt.assert_almost_equal(unpack_quat(Q_resamp[-1]), unpack_quat(Q[-1]))
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 test_quaternion_array_interpolate():
for _ in range(100):
N = 100
qa = QuaternionArray([random_orientation() for _ in range(N)])
qtimes = np.random.uniform(-10, 10, size=N)
qtimes.sort()
t_intp = np.random.uniform(qtimes[0], qtimes[-1])
q_intp = quaternion_array_interpolate(qa, qtimes, t_intp)
i = np.flatnonzero(qtimes > t_intp)[0]
q0 = qa[i - 1]
q1 = qa[i]
t0 = qtimes[i - 1]
t1 = qtimes[i]
tau = np.clip((t_intp - t0) / (t1 - t0), 0, 1)
qslerp = quaternion_slerp(q0, q1, tau)
if qslerp.dot(q_intp) < 0:
qslerp = -qslerp
nt.assert_almost_equal(unpack_quat(q_intp), unpack_quat(qslerp))
from datasetgen.dataset import Dataset, DatasetBuilder, DatasetError
from datasetgen.dataset.utils import quaternion_slerp, quaternion_array_interpolate, resample_quaternion_array, \
create_bounds, orientation_from_gyro, orientation_from_sfm, position_from_sfm, landmarks_from_sfm
from datasetgen.sfm import SfmResult
from datasetgen.sfm.sfm import VisualSfmResult, OpenMvgResult
from tests.helpers import random_orientation, unpack_quat, gyro_data_to_quaternion_array, find_landmark
NVM_EXAMPLE = 'example.nvm'
OPENMVG_EXAMPLE = 'example_sfm_data.json'
GYRO_EXAMPLE_DATA = np.load('example_gyro_data.npy')
GYRO_EXAMPLE_TIMES = np.load('example_gyro_times.npy')
GYRO_DT = float(GYRO_EXAMPLE_TIMES[1] - GYRO_EXAMPLE_TIMES[0])
GYRO_EXAMPLE_DATA_INT = integrate_gyro_quaternion_uniform(
GYRO_EXAMPLE_DATA, GYRO_DT)
GYRO_EXAMPLE_DATA_Q = QuaternionArray(GYRO_EXAMPLE_DATA_INT)
assert len(GYRO_EXAMPLE_DATA_Q) == len(GYRO_EXAMPLE_TIMES)
CAMERA_FPS = 30.
class DatasetGyroTests(unittest.TestCase):
def test_orientation_from_gyro(self):
ds = Dataset()
ds.set_orientation_data(
*orientation_from_gyro(GYRO_EXAMPLE_DATA, GYRO_EXAMPLE_TIMES))
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]
i1 = np.flatnonzero(GYRO_EXAMPLE_TIMES <= t1)[-1]
t0 = GYRO_EXAMPLE_TIMES[i0]
t1 = GYRO_EXAMPLE_TIMES[i1]
def gyro_data_to_quaternion_array(gyro_data, gyro_times):
dt = float(gyro_times[1] - gyro_times[0])
nt.assert_almost_equal(np.diff(gyro_times), dt)
q = integrate_gyro_quaternion_uniform(gyro_data, dt)
return QuaternionArray(q)