def test_inverse(self):
# quaternion calc from:
# https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3
r = Rotation.from_quaternion([[+2],
[-1],
[-3],
[+0]], ctype=ctypes.c_double)
r_inv = r.inverse()
e_inv = array_normalize([[-1/7.],
[+1/14.],
[+3/14.],
[0]])
numpy.testing.assert_allclose(
r_inv.quaternion(),
e_inv,
1e-15
)
r = Rotation.from_quaternion([[+2],
[-1],
[-3],
[+0]], ctype=ctypes.c_float)
r_inv = r.inverse()
numpy.testing.assert_allclose(
r_inv.quaternion(),
e_inv,
1e-7
)
def test_inverse(self):
# quaternion calc from:
# https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3
r = Rotation.from_quaternion([[+2],
[-1],
[-3],
[+0]], ctype=ctypes.c_double)
r_inv = r.inverse()
numpy.testing.assert_equal(
r_inv.quaternion(),
[[-1/7.],
[+1/14.],
[+3/14.],
[0]]
)
r = Rotation.from_quaternion([[+2],
[-1],
[-3],
[+0]], ctype=ctypes.c_float)
r_inv = r.inverse()
numpy.testing.assert_equal(
r_inv.quaternion(),
[[numpy.float32(-1/7.)],
[numpy.float32(+1/14.)],
[numpy.float32(+3/14.)],
[0]]
)
def test_compose(self):
expected_quat = [[+2],
[-1],
[-3],
[+0]]
r_ident_d = Rotation(ctypes.c_double)
r_ident_f = Rotation(ctypes.c_float)
r_other_d = Rotation.from_quaternion(expected_quat, ctypes.c_double)
r_other_f = Rotation.from_quaternion(expected_quat, ctypes.c_float)
r_res_d = r_ident_d.compose(r_other_d)
nose.tools.assert_is_not(r_other_d, r_res_d)
numpy.testing.assert_equal(r_res_d, r_other_d)
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f.compose(r_other_f)
nose.tools.assert_is_not(r_other_f, r_res_f)
numpy.testing.assert_equal(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
# Should also work with multiply operator
r_res_d = r_ident_d * r_other_d
nose.tools.assert_is_not(r_other_d, r_res_d)
numpy.testing.assert_equal(r_res_d, r_other_d)
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f * r_other_f
nose.tools.assert_is_not(r_other_f, r_res_f)
numpy.testing.assert_equal(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
# Rotation of non-congruent types should be converted automatically
r_res_d = r_ident_d.compose(r_other_f)
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_equal(r_res_d.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f.compose(r_other_d)
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
r_res_d = r_ident_d * r_other_f
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_equal(r_res_d.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f * r_other_d
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
def test_inverse(self):
# quaternion calc from:
# https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3
r = Rotation.from_quaternion([+2, -1, -3, +0], ctype='d')
r_inv = r.inverse()
e_inv = array_normalize([-1 / 7., +1 / 14., +3 / 14., 0])
numpy.testing.assert_allclose(r_inv.quaternion(), e_inv, 1e-15)
r = Rotation.from_quaternion([+2, -1, -3, +0], ctype='f')
r_inv = r.inverse()
numpy.testing.assert_allclose(r_inv.quaternion(), e_inv, 1e-7)
def test_compose(self):
expected_quat = [[+2], [-1], [-3], [+0]]
r_ident_d = Rotation(ctypes.c_double)
r_ident_f = Rotation(ctypes.c_float)
r_other_d = Rotation.from_quaternion(expected_quat, ctypes.c_double)
r_other_f = Rotation.from_quaternion(expected_quat, ctypes.c_float)
r_res_d = r_ident_d.compose(r_other_d)
nose.tools.assert_is_not(r_other_d, r_res_d)
numpy.testing.assert_equal(r_res_d, r_other_d)
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f.compose(r_other_f)
nose.tools.assert_is_not(r_other_f, r_res_f)
numpy.testing.assert_equal(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
# Should also work with multiply operator
r_res_d = r_ident_d * r_other_d
nose.tools.assert_is_not(r_other_d, r_res_d)
numpy.testing.assert_equal(r_res_d, r_other_d)
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f * r_other_f
nose.tools.assert_is_not(r_other_f, r_res_f)
numpy.testing.assert_equal(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
# Rotation of non-congruent types should be converted automatically
r_res_d = r_ident_d.compose(r_other_f)
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_equal(r_res_d.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f.compose(r_other_d)
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
r_res_d = r_ident_d * r_other_f
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_equal(r_res_d.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f * r_other_d
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
def test_inverse(self):
# quaternion calc from:
# https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3
r = Rotation.from_quaternion([[+2], [-1], [-3], [+0]],
ctype=ctypes.c_double)
r_inv = r.inverse()
numpy.testing.assert_equal(r_inv.quaternion(),
[[-1 / 7.], [+1 / 14.], [+3 / 14.], [0]])
r = Rotation.from_quaternion([[+2], [-1], [-3], [+0]],
ctype=ctypes.c_float)
r_inv = r.inverse()
numpy.testing.assert_equal(
r_inv.quaternion(),
[[numpy.float32(-1 / 7.)], [numpy.float32(+1 / 14.)],
[numpy.float32(+3 / 14.)], [0]])
def test_read_write_krtd_file(self):
# Use a random string filename to avoid name collision.
fname = 'temp_camera_test_read_write_krtd_file.txt'
try:
for _ in range(100):
c = (rand(3)*2-1)*100
center = EigenArray.from_array([c])
rotation = Rotation.from_quaternion(numpy.random.rand(4)*2-1)
intrinsics = CameraIntrinsics(10, (5, 5), 1.2, 0.5, [4, 5, 6])
c1 = Camera(center, rotation,
intrinsics)
c1.write_krtd_file(fname)
c2 = Camera.from_krtd_file(fname)
err = numpy.linalg.norm(c1.center-c2.center)
assert err < 1e-9, ''.join(['Centers are different by ',
str(err)])
c1.rotation.angle_from(c2.rotation) < 1e-12
attr = ['focal_length','aspect_ratio','principle_point','skew',
'dist_coeffs']
for att in attr:
v1 = numpy.array(getattr(c1.intrinsics,att))
v2 = numpy.array(getattr(c2.intrinsics,att))
err = numpy.linalg.norm(v1-v2)
assert err < 1e-8, ''.join(['Difference ',str(err),
' for attribute: ',att])
finally:
if os.path.isfile(fname):
os.remove(fname)
def test_from_matrix(self):
# Create a non-identity matrix from a different constructor that we
# assume works
# Create new rotation with that matrix.
# New rotation to_matrix method should produce the same matrix
pre_r = Rotation.from_quaternion([[+2], [-1], [-3], [+0]])
mat = pre_r.matrix()
r = Rotation.from_matrix(mat)
numpy.testing.assert_equal(mat, r.matrix())
def test_from_quaternion(self):
q = array_normalize([[+2],
[-1],
[-3],
[+0]], float)
r = Rotation.from_quaternion(q)
numpy.testing.assert_equal(
r.quaternion(), q
)
def test_from_matrix(self):
# Create a non-identity matrix from a different constructor that we
# assume works
# Create new rotation with that matrix.
# New rotation to_matrix method should produce the same matrix
pre_r = Rotation.from_quaternion([+2, -1, -3, +0])
mat = pre_r.matrix()
r = Rotation.from_matrix(mat)
numpy.testing.assert_allclose(mat, r.matrix(), 1e-15)
def test_from_matrix(self):
# Create a non-identity matrix from a different constructor that we
# assume works
# Create new rotation with that matrix.
# New rotation to_matrix method should produce the same matrix
pre_r = Rotation.from_quaternion([+2, -1, -3, +0])
mat = pre_r.matrix()
r = Rotation.from_matrix(mat)
numpy.testing.assert_allclose(mat, r.matrix(), 1e-15)
def test_from_quaternion(self):
q_list = [[+2],
[-1],
[-3],
[+0]]
r = Rotation.from_quaternion(q_list)
numpy.testing.assert_equal(
r.quaternion(), q_list
)
def test_inverse(self):
# quaternion calc from:
# https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3
r = Rotation.from_quaternion([+2, -1, -3, +0], ctype='d')
r_inv = r.inverse()
e_inv = array_normalize([-1/7., +1/14., +3/14., 0])
numpy.testing.assert_allclose(
r_inv.quaternion(),
e_inv,
1e-15
)
r = Rotation.from_quaternion([+2, -1, -3, +0], ctype='f')
r_inv = r.inverse()
numpy.testing.assert_allclose(
r_inv.quaternion(),
e_inv,
1e-7
)
def test_from_matrix(self):
# Create a non-identity matrix from a different constructor that we
# assume works
# Create new rotation with that matrix.
# New rotation to_matrix method should produce the same matrix
pre_r = Rotation.from_quaternion([[+2],
[-1],
[-3],
[+0]])
mat = pre_r.matrix()
r = Rotation.from_matrix(mat)
numpy.testing.assert_equal(mat, r.matrix())
def test_eq(self):
# Identities should equal
r1 = Rotation('d')
r2 = Rotation('d')
nose.tools.assert_equal(r1, r2)
r1 = Rotation('f')
r2 = Rotation('f')
nose.tools.assert_equal(r1, r2)
r1 = Rotation('d')
r2 = Rotation('f')
# r2 should get converted into a double instance for checking
nose.tools.assert_equal(r1, r2)
r1 = Rotation.from_quaternion([1,2,3,4], ctype='d')
r2 = Rotation.from_quaternion([1,2,3,4], ctype='d')
nose.tools.assert_equal(r1, r2)
r1 = Rotation.from_quaternion([1,2,3,4], ctype='d')
r2 = Rotation.from_quaternion([-1,-2,-3,-4], ctype='d')
assert r1.angle_from(r2) < 1e-12
def test_eq(self):
# Identities should equal
r1 = Rotation('d')
r2 = Rotation('d')
nose.tools.assert_equal(r1, r2)
r1 = Rotation('f')
r2 = Rotation('f')
nose.tools.assert_equal(r1, r2)
r1 = Rotation('d')
r2 = Rotation('f')
# r2 should get converted into a double instance for checking
nose.tools.assert_equal(r1, r2)
r1 = Rotation.from_quaternion([1, 2, 3, 4], ctype='d')
r2 = Rotation.from_quaternion([1, 2, 3, 4], ctype='d')
nose.tools.assert_equal(r1, r2)
r1 = Rotation.from_quaternion([1, 2, 3, 4], ctype='d')
r2 = Rotation.from_quaternion([-1, -2, -3, -4], ctype='d')
assert r1.angle_from(r2) < 1e-12
def _new(self, s, r, t):
"""
:type s: float
:type r: Rotation | None
:type t: collections.Iterable[float] | None
"""
# noinspection PyProtectedMember
if r is None:
r = Rotation(self._ctype)
elif r._ctype != self._ctype:
# Create new Rotation sharing our type
r = Rotation.from_quaternion(r.quaternion(), self._ctype)
if t is None:
t = EigenArray.from_iterable((0, 0, 0), self._ctype)
else:
t = EigenArray.from_iterable(t, self._ctype, (3, 1))
return self._call_cfunc('vital_similarity_%s_new' % self._tchar, [
self._ctype,
Rotation.c_ptr_type(self._ctype),
EigenArray.c_ptr_type(3, 1, self._ctype)
], [s, r, t], self.C_TYPE_PTR)
def test_read_write_krtd_file(self):
# Use a random string filename to avoid name collision.
fname = 'temp_camera_test_read_write_krtd_file.txt'
try:
for _ in range(100):
c = (rand(3) * 2 - 1) * 100
center = EigenArray.from_array([c])
rotation = Rotation.from_quaternion(
numpy.random.rand(4) * 2 - 1)
intrinsics = CameraIntrinsics(10, (5, 5), 1.2, 0.5, [4, 5, 6])
c1 = Camera(center, rotation, intrinsics)
c1.write_krtd_file(fname)
c2 = Camera.from_krtd_file(fname)
err = numpy.linalg.norm(c1.center - c2.center)
assert err < 1e-9, ''.join(
['Centers are different by ',
str(err)])
c1.rotation.angle_from(c2.rotation) < 1e-12
attr = [
'focal_length', 'aspect_ratio', 'principle_point', 'skew',
'dist_coeffs'
]
for att in attr:
v1 = numpy.array(getattr(c1.intrinsics, att))
v2 = numpy.array(getattr(c2.intrinsics, att))
err = numpy.linalg.norm(v1 - v2)
assert err < 1e-8, ''.join(
['Difference ',
str(err), ' for attribute: ', att])
finally:
if os.path.isfile(fname):
os.remove(fname)
def test_from_quaternion(self):
q_list = [[+2], [-1], [-3], [+0]]
r = Rotation.from_quaternion(q_list)
numpy.testing.assert_equal(r.quaternion(), q_list)
def test_compose(self):
# Normalize quaternaion vector.
expected_quat = array_normalize([+2., -1., -3., +0.])
r_ident_d = Rotation('d')
r_ident_f = Rotation('f')
r_other_d = Rotation.from_quaternion(expected_quat, 'd')
r_other_f = Rotation.from_quaternion(expected_quat, 'f')
r_res_d = r_ident_d.compose(r_other_d)
nose.tools.assert_is_not(r_other_d, r_res_d)
numpy.testing.assert_equal(r_res_d, r_other_d)
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f.compose(r_other_f)
nose.tools.assert_is_not(r_other_f, r_res_f)
numpy.testing.assert_equal(r_res_f, r_other_f)
numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat,
1e-7)
# Should also work with multiply operator
r_res_d = r_ident_d * r_other_d
nose.tools.assert_is_not(r_other_d, r_res_d)
numpy.testing.assert_equal(r_res_d, r_other_d)
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f * r_other_f
nose.tools.assert_is_not(r_other_f, r_res_f)
numpy.testing.assert_equal(r_res_f, r_other_f)
numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat,
1e-7)
# Rotation of non-congruent types should be converted automatically
r_res_d = r_ident_d.compose(r_other_f)
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_allclose(r_res_d.quaternion(),
r_other_f.quaternion(), 1e-7)
numpy.testing.assert_allclose(r_res_d.quaternion(), expected_quat,
1e-7)
r_res_f = r_ident_f.compose(r_other_d)
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_allclose(r_res_f.quaternion(),
r_other_f.quaternion(), 1e-7)
numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat,
1e-7)
# Equality check between types should pass due to integrety resolution
# inside function.
r_res_d = r_ident_d * r_other_f
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_allclose(r_res_d.quaternion(),
r_other_f.quaternion(), 1e-7)
numpy.testing.assert_allclose(r_res_d.quaternion(), expected_quat,
1e-7)
r_res_f = r_ident_f * r_other_d
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat,
1e-7)
def test_from_quaternion(self):
q = array_normalize([+2, -1, -3, +0], float)
r = Rotation.from_quaternion(q)
numpy.testing.assert_equal(
r.quaternion(), q
)
def test_compose(self):
# Normalize quaternaion vector.
expected_quat = array_normalize([+2., -1., -3., +0.])
r_ident_d = Rotation('d')
r_ident_f = Rotation('f')
r_other_d = Rotation.from_quaternion(expected_quat, 'd')
r_other_f = Rotation.from_quaternion(expected_quat, 'f')
r_res_d = r_ident_d.compose(r_other_d)
nose.tools.assert_is_not(r_other_d, r_res_d)
numpy.testing.assert_equal(r_res_d, r_other_d)
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f.compose(r_other_f)
nose.tools.assert_is_not(r_other_f, r_res_f)
numpy.testing.assert_equal(r_res_f, r_other_f)
numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat,
1e-7)
# Should also work with multiply operator
r_res_d = r_ident_d * r_other_d
nose.tools.assert_is_not(r_other_d, r_res_d)
numpy.testing.assert_equal(r_res_d, r_other_d)
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f * r_other_f
nose.tools.assert_is_not(r_other_f, r_res_f)
numpy.testing.assert_equal(r_res_f, r_other_f)
numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat,
1e-7)
# Rotation of non-congruent types should be converted automatically
r_res_d = r_ident_d.compose(r_other_f)
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_allclose(r_res_d.quaternion(),
r_other_f.quaternion(),
1e-7)
numpy.testing.assert_allclose(r_res_d.quaternion(), expected_quat,
1e-7)
r_res_f = r_ident_f.compose(r_other_d)
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_allclose(r_res_f.quaternion(),
r_other_f.quaternion(),
1e-7)
numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat,
1e-7)
# Equality check between types should pass due to integrety resolution
# inside function.
r_res_d = r_ident_d * r_other_f
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_allclose(r_res_d.quaternion(),
r_other_f.quaternion(),
1e-7)
numpy.testing.assert_allclose(r_res_d.quaternion(), expected_quat,
1e-7)
r_res_f = r_ident_f * r_other_d
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat,
1e-7)
def test_from_quaternion(self):
q = array_normalize([+2, -1, -3, +0], float)
r = Rotation.from_quaternion(q)
numpy.testing.assert_equal(r.quaternion(), q)
def convert_pos2krtd_dir(frame_pattern, sussex_dir, pos_dir):
"""
The POS data format stores each frame's worth of metadata in its own text
file with the same name as the frame. The data is stored in the following
order:
Element Data Type Description
0 double Sensor yaw angle (degrees)
1 double Sensor pitch angle (degrees)
2 double Sensor roll angle (degrees)
3 double Sensor latitude in (decimal degrees)
4 double Sensor longitude in (decimal degrees)
5 double Sensor altitude (feet)
6 double GPS time - seconds in week
7 int GPS time - week of year
8 double Velocity in the North direction (m/s)
9 double Velocity in the East direction (m/s)
10 double Velocity in the up direction (m/s)
11 int IMU status
12 int Local adjustment
13 int Flags ?
Arguements:
frame_dir : str
Global pattern for image files.
sussex_dir : str
Directory for the Sussex MUTC metadata csv.
pos_dir : str
Directory for the output POS files. If it does not currently exist, it
will be created.
Example
frame_pattern = '/media/sf_Matt/data/24_Sept_2015_WAMI_Flight_1/frames/*.jpg'
sussex_dir = '/media/sf_Matt/data/24_Sept_2015_WAMI_Flight_1/novatel_ins_data_2015-09-24-10-01-18'
pos_dir = '/media/sf_Matt/data/24_Sept_2015_WAMI_Flight_1/pos'
"""
# Frind frames
frame_fnames = glob.glob(frame_pattern)
# Extract the frame number
frame_index = np.zeros(len(frame_fnames), dtype=np.int)
for i in range(len(frame_fnames)):
fnamei = os.path.split(frame_fnames[i])[1]
frame_index[i] = int(fnamei[fnamei.find('frame')+5:fnamei.find('_')])
# -------------------- Parse RawImageTopic.csv ---------------------------
fname = glob.glob(''.join([sussex_dir,'/*RawImageTopic.csv']))[0]
raw_image_topic_df = pd.read_csv(fname)
column_names = ['time','GPSWeek','GPSTime','FrameNumber','ImageHeight',
'ImageWidth','BitDepth','BufferNumber']
raw_image_topic_df.columns = column_names
image_height = np.unique(raw_image_topic_df.ImageHeight.as_matrix())
assert len(image_height) == 1, 'ImageHeight must be unique.'
image_height = image_height[0]
image_width = np.unique(raw_image_topic_df.ImageWidth.as_matrix())
assert len(image_width) == 1, 'ImageWidth must be unique.'
image_width = image_width[0]
bit_depth = np.unique(raw_image_topic_df.BitDepth.as_matrix())
assert len(bit_depth) == 1, 'BitDepth must be unique.'
bit_depth = bit_depth[0]
# Define mapping from frame number to UTC time
frame_times = dict(zip(raw_image_topic_df.FrameNumber.tolist(),
raw_image_topic_df.time.tolist()))
# Define mapping from frame number to gps_week
gps_week = dict(zip(raw_image_topic_df.FrameNumber.tolist(),
raw_image_topic_df.GPSWeek.tolist()))
# Define mapping from frame number to gps_sec
gps_sec = dict(zip(raw_image_topic_df.FrameNumber.tolist(),
raw_image_topic_df.GPSTime.tolist()))
# ------------------------------------------------------------------------
# --------------------------- Parse pva.csv ------------------------------
fname = glob.glob(''.join([sussex_dir,'/*mark_1_pva.csv']))[0]
pva_df = pd.read_csv(fname)
pva_df.columns = ['time','seq','stamp','frame_id','lat','lon','height',
'vel_n','vel_e','vel_u','roll','pitch','azimuth']
# Create interpolation objects against time.
time = pva_df.time.as_matrix()
# Remove large offset so that the interpolation is better behaved.
t0 = time[0]
time = time - t0
# Latitude (degrees)
get_lat = interp1d(time, pva_df.lat.tolist())
# Longitude (degrees)
get_lon = interp1d(time, pva_df.lon.tolist())
# Height above WGS84 ellispiod (meters)
get_height = interp1d(time, pva_df.height.tolist())
# yaw/pitch/roll in that order (degrees)
get_roll = interp1d(time, pva_df.roll.tolist())
get_pitch = interp1d(time, pva_df.pitch.tolist())
get_azimuth = interp1d(time, pva_df.azimuth.tolist())
if True:
# Show the raw yaw. pitch, and roll.
plt.subplot('311')
plt.plot(time, pva_df.azimuth.tolist())
plt.subplot('312')
plt.plot(time, pva_df.pitch.tolist())
plt.subplot('313')
plt.plot(time, pva_df.roll.tolist())
# ------------------------------------------------------------------------
# --------------------------- Parse pos_1.csv ------------------------------
fname = glob.glob(''.join([sussex_dir,'/*pos_1.csv']))[0]
pos_1_df = pd.read_csv(fname)
pos_1_df.columns = ['time','seq','stamp','frame_id','status','service',
'latitude','longitude','altitude',
'position_covariance0','position_covariance1',
'position_covariance2','position_covariance3',
'position_covariance4','position_covariance5',
'position_covariance6','position_covariance7',
'position_covariance8','position_covariance_type']
# Create interpolation objects against time.
time = pos_1_df.time.tolist()
if False:
# Check that the values in pos_1.csv are consistent with pva.csv.
# Remove large offset so that the interpolation is better behaved.
t0 = time[0]
time = time - t0
print('Maximum difference between pos_1.csv and pva.csv latitude',
np.max(np.abs(get_lat(time) - pos_1_df.latitude.tolist())))
print('Maximum difference between pos_1.csv and pva.csv longitude',
np.max(np.abs(get_lon(time) - pos_1_df.longitude.tolist())))
print('Maximum difference between pos_1.csv and pva.csv altitude',
np.max(np.abs(get_height(time) - pos_1_df.altitude.tolist())))
# ------------------------------------------------------------------------
try:
os.mkdir(pos_dir)
except:
pass
# Write out one POS txt file per image frame.
for i in range(len(frame_fnames)):
# The POS file name will match that of the image frame.
img_fname = os.path.splitext(os.path.split(frame_fnames[i])[1])[0]
pos_fname = ''.join([pos_dir,'/',img_fname,'.pos'])
pos_datai = np.zeros(14)
# Frame time in UTC nanoseconds.
timei = frame_times[frame_index[i]]
# ---------------- Determine Orientation of Camera -------------------
# yaw/pitch/roll of the INS.
ins_yaw = get_azimuth(timei-t0) # (degrees)
ins_pitch = get_pitch(timei-t0) # (degrees)
ins_roll = get_roll(timei-t0) # (degrees)
# Rotation operator that moves the INS coordinate system from aligned
# with the NED axes to its current state.
ins_rot = Rotation.from_quaternion(quat_from_ypr(ins_yaw, ins_pitch,
ins_roll,
in_degrees=True))
# Hard-coded camera orientation calibration. This rotates the INS into
# the Airforce's camera coordinate system, which is non-standard.
ins_to_cam_prime = Rotation.from_quaternion(quat_from_ypr(91.625, .867,
-.612,
in_degrees=True))
# The AF camera has the look direction as x, the y going out the right,
# and z going down. We need to rotate this into x-right, y-down,
# z-look direction.
cp_to_c = Rotation.from_quaternion(quat_from_ypr(90, 0, 90,
in_degrees=True))
rot_new = cp_to_c*ins_to_cam_prime*ins_rot
yaw, pitch, roll = ypr_from_quat(rot_new.quaternion(), in_degrees=True)
# Sensor yaw in (degrees)
pos_datai[0] = yaw
# Sensor pitch in (degrees)
pos_datai[1] = pitch
# Sensor roll (degrees)
pos_datai[2] = roll
# --------------------------------------------------------------------
# Sensor latitude in (decimal degrees)
pos_datai[3] = get_lat(timei-t0)
# Sensor longitude in (decimal degrees)
pos_datai[4] = get_lon(timei-t0)
# Sensor altitude (feet above WGS84)
pos_datai[5] = get_height(timei-t0)*3.28084
# GPS time - seconds in week
pos_datai[6] = gps_sec[frame_index[i]]
# Validate against the image filename
err = int(img_fname.split('_')[1]) - gps_sec[frame_index[i]]
assert np.abs(err) < 1, 'GPS seconds in image filename does not match.'
# GPS time - week of year
pos_datai[7] = gps_week[frame_index[i]]
# Velocity in the North direction (m/s)
pos_datai[8] = 0
# Velocity in the East direction (m/s)
pos_datai[9] = 0
# Velocity in the up direction (m/s)
pos_datai[10] = 0
# IMU status
pos_datai[11] = -1 # NOT SURE WHAT THIS DOES
# Local adjustment
pos_datai[12] = 0 # NOT SURE WHAT THIS DOES
# Flags ?
pos_datai[13] = 0 # NOT SURE WHAT THIS DOES
np.savetxt(pos_fname, np.atleast_2d(pos_datai), delimiter=',',
fmt='%.10f,'*7 + '%i,' + '%.10f,'*3 + '%i,'*2 + '%i')
