def test_line_scan_driver():
j2000 = FrameNode(1)
body_rotation = TimeDependentRotation(
np.array([[0, 0, 0, 1], [0, 0, 0, 1]]),
np.array([0, 1]),
100,
1
)
body_fixed = FrameNode(100, parent=j2000, rotation=body_rotation)
spacecraft_rotation = TimeDependentRotation(
np.array([[0, 0, 0, 1], [0, 0, 0, 1]]),
np.array([0, 1]),
1000,
1
)
spacecraft = FrameNode(1000, parent=j2000, rotation=spacecraft_rotation)
sensor_rotation = ConstantRotation(np.array([0, 0, 0, 1]), 1010, 1000)
sensor = FrameNode(1010, parent=spacecraft, rotation=sensor_rotation)
driver = TestLineScanner()
driver.target_body_radii = (1100, 1000)
driver.sensor_position = (
[[0, 1, 2], [3, 4, 5]],
[[0, -1, -2], [-3, -4, -5]],
[800, 900]
)
driver.sun_position = (
[[0, 1, 2], [3, 4, 5]],
[[0, -1, -2], [-3, -4, -5]],
[800, 900]
)
driver.sensor_frame_id = 1010
driver.target_frame_id = 100
driver.frame_chain = j2000
driver.sample_summing = 2
driver.line_summing = 4
driver.focal_length = 500
driver.detector_center_line = 0.5
driver.detector_center_sample = 512
driver.detector_start_line = 0
driver.detector_start_sample = 8
driver.focal2pixel_lines = [0.1, 0.2, 0.3]
driver.focal2pixel_samples = [0.3, 0.2, 0.1]
driver.usgscsm_distortion_model = {
'radial' : {
'coefficients' : [0.0, 1.0, 0.1]
}
}
driver.image_lines = 10000
driver.image_samples = 1024
driver.platform_name = 'Test Platform'
driver.sensor_name = 'Test Line Scan Sensor'
driver.ephemeris_stop_time = 900
driver.ephemeris_start_time = 800
return driver
def frame_chain(self):
"""
Return the root node of the rotation frame tree/chain.
The root node is the J2000 reference frame. The other nodes in the
tree can be accessed via the methods in the FrameNode class.
This property expects the ephemeris_time property/attribute to be defined.
It should be a list of the ephemeris seconds past the J2000 epoch for each
exposure in the image.
Returns
-------
FrameNode
The root node of the frame tree. This will always be the J2000 reference frame.
"""
if not hasattr(self, '_root_frame'):
j2000_id = 1 #J2000 is our root reference frame
self._root_frame = FrameNode(j2000_id)
sensor_quats = np.zeros((len(self.ephemeris_time), 4))
sensor_times = np.array(self.ephemeris_time)
body_quats = np.zeros((len(self.ephemeris_time), 4))
body_times = np.array(self.ephemeris_time)
for i, time in enumerate(self.ephemeris_time):
sensor2j2000 = spice.pxform(spice.frmnam(self.sensor_frame_id),
spice.frmnam(j2000_id), time)
q_sensor = spice.m2q(sensor2j2000)
sensor_quats[i, :3] = q_sensor[1:]
sensor_quats[i, 3] = q_sensor[0]
body2j2000 = spice.pxform(spice.frmnam(self.target_frame_id),
spice.frmnam(j2000_id), time)
q_body = spice.m2q(body2j2000)
body_quats[i, :3] = q_body[1:]
body_quats[i, 3] = q_body[0]
sensor2j2000_rot = TimeDependentRotation(sensor_quats,
sensor_times,
self.sensor_frame_id,
j2000_id)
sensor_node = FrameNode(self.sensor_frame_id,
parent=self._root_frame,
rotation=sensor2j2000_rot)
body2j2000_rot = TimeDependentRotation(body_quats, body_times,
self.target_frame_id,
j2000_id)
body_node = FrameNode(self.target_frame_id,
parent=self._root_frame,
rotation=body2j2000_rot)
return self._root_frame
def frame_chain(self):
j2000 = FrameNode(1)
body_rotation = TimeDependentRotation(
np.array([[0, 0, 0, 1], [0, 0, 0, 1]]), np.array([0, 1]), 100, 1)
body_fixed = FrameNode(100, parent=j2000, rotation=body_rotation)
spacecraft_rotation = TimeDependentRotation(
np.array([[0, 0, 0, 1], [0, 0, 0, 1]]), np.array([0, 1]), 1000, 1)
spacecraft = FrameNode(1000,
parent=j2000,
rotation=spacecraft_rotation)
sensor_rotation = ConstantRotation(np.array([0, 0, 0, 1]), 1010, 1000)
sensor = FrameNode(1010, parent=spacecraft, rotation=sensor_rotation)
return j2000
def frame_chain(self):
frame_chain = FrameChain()
body_rotation = TimeDependentRotation(
np.array([[0, 0, 0, 1], [0, 0, 0, 1]]), np.array([0, 1]), 100, 1)
frame_chain.add_edge(rotation=body_rotation)
spacecraft_rotation = TimeDependentRotation(
np.array([[0, 0, 0, 1], [0, 0, 0, 1]]), np.array([0, 1]), 1000, 1)
frame_chain.add_edge(rotation=spacecraft_rotation)
sensor_rotation = ConstantRotation(np.array([0, 0, 0, 1]), 1010, 1000)
frame_chain.add_edge(rotation=sensor_rotation)
return frame_chain
def test_last_time_dependent_frame_between():
"""
Test frame tree structure:
1
/ \
/ \
2 4
/ \
/ \
3 5
The rotations from 3 to 2 and 1 to 4 are time dependent.
All other rotations are constant.
"""
frame_chain = FrameChain()
rotations = [
ConstantRotation(np.array([1, 0, 0, 0]), 2, 1),
TimeDependentRotation(
np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
np.array([1]), 3, 2),
TimeDependentRotation(
np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
np.array([1]), 4, 1),
ConstantRotation(np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
5, 4)
]
frame_chain.add_edge(rotation=rotations[0])
frame_chain.add_edge(rotation=rotations[1])
frame_chain.add_edge(rotation=rotations[2])
frame_chain.add_edge(rotation=rotations[3])
# last frame from node 1 to node 3
s31, d31, _ = frame_chain.last_time_dependent_frame_between(1, 3)
assert s31 == 2
assert d31 == 3
# last frame from node 3 to node 1
s13, d13, _ = frame_chain.last_time_dependent_frame_between(3, 1)
assert s13 == 3
assert d13 == 2
# last frame from node 3 to node 5
s35, d35, _ = frame_chain.last_time_dependent_frame_between(3, 5)
assert s35 == 1
assert d35 == 4
# last frame from node 5 to node 3
s53, d53, _ = frame_chain.last_time_dependent_frame_between(5, 3)
assert s53 == 2
assert d53 == 3
def create_rotations(rotation_table):
"""
Convert an ISIS rotation table into rotation objects.
Parameters
----------
rotation_table : dict
The rotation ISIS table as a dictionary
Returns
-------
: list
A list of time dependent or constant rotation objects from the table. This
list will always have either 1 or 2 elements. The first rotation will be
time dependent and the second rotation will be constant. The rotations will
be ordered such that the reference frame the first rotation rotates to is
the reference frame the second rotation rotates from.
"""
rotations = []
root_frame = rotation_table['TimeDependentFrames'][-1]
last_time_dep_frame = rotation_table['TimeDependentFrames'][0]
# Case 1: It's a table of quaternions and times
if 'J2000Q0' in rotation_table:
# SPICE quaternions are (W, X, Y, Z) and ALE uses (X, Y, Z, W).
quats = np.array([
rotation_table['J2000Q1'], rotation_table['J2000Q2'],
rotation_table['J2000Q3'], rotation_table['J2000Q0']
]).T
time_dep_rot = TimeDependentRotation(quats, rotation_table['ET'],
root_frame, last_time_dep_frame)
rotations.append(time_dep_rot)
# Case 2: It's a table of Euler angle coefficients
elif 'J2000Ang1' in rotation_table:
ephemeris_times = np.linspace(rotation_table['CkTableStartTime'],
rotation_table['CkTableEndTime'],
rotation_table['CkTableOriginalSize'])
base_time = rotation_table['J2000Ang1'][-1]
time_scale = rotation_table['J2000Ang2'][-1]
scaled_times = (ephemeris_times - base_time) / time_scale
coeffs = np.array([
rotation_table['J2000Ang1'][:-1], rotation_table['J2000Ang2'][:-1],
rotation_table['J2000Ang3'][:-1]
]).T
angles = polyval(scaled_times, coeffs).T
# ISIS is hard coded to ZXZ (313) Euler angle axis order.
# SPICE also interprets Euler angle rotations as negative rotations,
# so negate them before passing to scipy.
time_dep_rot = TimeDependentRotation.from_euler(
'zxz', -angles, ephemeris_times, root_frame, last_time_dep_frame)
rotations.append(time_dep_rot)
if 'ConstantRotation' in rotation_table:
last_constant_frame = rotation_table['ConstantFrames'][0]
rot_mat = np.reshape(np.array(rotation_table['ConstantRotation']),
(3, 3))
constant_rot = ConstantRotation.from_matrix(rot_mat,
last_time_dep_frame,
last_constant_frame)
rotations.append(constant_rot)
return rotations
def test_apply_at_single_time():
test_quats = Rotation.from_euler('x', np.array([-90, 0, 45]),
degrees=True).as_quat()
rot = TimeDependentRotation(test_quats, [0, 1, 1.5], 1, 2)
input_vec = np.asarray([1, 2, 3])
rot_vec = rot.apply_at(input_vec, 0)
np.testing.assert_almost_equal(rot_vec, np.asarray([[1, 3, -2]]))
def from_spice(cls,
*args,
sensor_frame,
target_frame,
center_ephemeris_time,
ephemeris_times=[],
**kwargs):
frame_chain = cls()
times = np.array(ephemeris_times)
sensor_time_dependent_frames, sensor_constant_frames = cls.frame_trace(
sensor_frame, center_ephemeris_time)
target_time_dependent_frames, target_constant_frames = cls.frame_trace(
target_frame, center_ephemeris_time)
time_dependent_frames = list(
zip(sensor_time_dependent_frames[:-1],
sensor_time_dependent_frames[1:]))
constant_frames = list(
zip(sensor_constant_frames[:-1], sensor_constant_frames[1:]))
target_time_dependent_frames = list(
zip(target_time_dependent_frames[:-1],
target_time_dependent_frames[1:]))
target_constant_frames = list(
zip(target_constant_frames[:-1], target_constant_frames[1:]))
time_dependent_frames.extend(target_time_dependent_frames)
constant_frames.extend(target_constant_frames)
for s, d in time_dependent_frames:
quats = np.zeros((len(times), 4))
avs = np.zeros((len(times), 3))
for j, time in enumerate(times):
state_matrix = spice.sxform(spice.frmnam(s), spice.frmnam(d),
time)
rotation_matrix, avs[j] = spice.xf2rav(state_matrix)
quat_from_rotation = spice.m2q(rotation_matrix)
quats[j, :3] = quat_from_rotation[1:]
quats[j, 3] = quat_from_rotation[0]
rotation = TimeDependentRotation(quats, times, s, d, av=avs)
frame_chain.add_edge(rotation=rotation)
for s, d in constant_frames:
quats = np.zeros(4)
rotation_matrix = spice.pxform(spice.frmnam(s), spice.frmnam(d),
times[0])
quat_from_rotation = spice.m2q(rotation_matrix)
quats[:3] = quat_from_rotation[1:]
quats[3] = quat_from_rotation[0]
rotation = ConstantRotation(quats, s, d)
frame_chain.add_edge(rotation=rotation)
return frame_chain
def test_last_time_dependent_frame_between():
"""
Test frame tree structure:
1
/ \
/ \
2 4
/ \
/ \
3 5
The rotations from 3 to 2 and 1 to 4 are time dependent.
All other rotations are constant.
"""
rotations = [
ConstantRotation(np.array([1, 0, 0, 0]), 2, 1),
TimeDependentRotation(
np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
np.array([1]), 4, 1),
TimeDependentRotation(
np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
np.array([1]), 5, 4),
ConstantRotation(np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
3, 2)
]
root_node = FrameNode(1)
child_node_1 = FrameNode(2, parent=root_node, rotation=rotations[0])
child_node_2 = FrameNode(3, parent=child_node_1, rotation=rotations[1])
child_node_3 = FrameNode(4, parent=root_node, rotation=rotations[2])
child_node_4 = FrameNode(5, parent=child_node_3, rotation=rotations[3])
last_frame_from_root_to_child_2 = root_node.last_time_dependent_frame_between(
child_node_2)
last_frame_from_child_2_to_root = child_node_2.last_time_dependent_frame_between(
root_node)
last_frame_from_child_2_to_child_4 = child_node_2.last_time_dependent_frame_between(
child_node_4)
last_frame_from_child_4_to_child_2 = child_node_4.last_time_dependent_frame_between(
child_node_2)
assert last_frame_from_root_to_child_2 == child_node_2
assert last_frame_from_child_2_to_root == child_node_1
assert last_frame_from_child_2_to_child_4 == child_node_3
assert last_frame_from_child_4_to_child_2 == child_node_2
def test_time_dependent_time_dependent_composition():
# 90 degree rotation about the X-axis to a 180 degree rotation about the X-axis
quats1_2 = [[1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], [1, 0, 0, 0]]
times1_2 = [0, 1]
rot1_2 = TimeDependentRotation(quats1_2, times1_2, 1, 2)
# -90 degree rotation about the X-axis to a 90 degree rotation about the X-axis
quats2_3 = [[1.0 / np.sqrt(2), 0, 0, -1.0 / np.sqrt(2)],
[1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]]
times2_3 = [0, 2]
rot2_3 = TimeDependentRotation(quats2_3, times2_3, 2, 3)
# compose to get no rotation to a 180 degree rotation about the X-axis to no rotation
rot1_3 = rot2_3 * rot1_2
assert isinstance(rot1_3, TimeDependentRotation)
assert rot1_3.source == 1
assert rot1_3.dest == 3
expected_times = np.array([0, 1])
expected_quats = np.array([[0, 0, 0, -1], [-1, 0, 0, 0]])
np.testing.assert_equal(rot1_3.times, expected_times)
np.testing.assert_almost_equal(rot1_3.quats, expected_quats)
def test_slerp_single_time():
rot = TimeDependentRotation([[0, 0, 0, 1]], [0],
1,
2,
av=[[np.pi / 2, 0, 0]])
new_rot, new_avs = rot._slerp([-1, 3])
expected_quats = [[-1 / np.sqrt(2), 0, 0, 1 / np.sqrt(2)],
[1 / np.sqrt(2), 0, 0, -1 / np.sqrt(2)]]
expected_av = [[np.pi / 2, 0, 0], [np.pi / 2, 0, 0]]
np.testing.assert_almost_equal(new_rot.as_quat(), expected_quats)
np.testing.assert_equal(new_avs, expected_av)
def test_slerp():
test_quats = Rotation.from_euler('x',
np.array([-135, -90, 0, 45, 90]),
degrees=True).as_quat()
rot = TimeDependentRotation(test_quats, [-0.5, 0, 1, 1.5, 2], 1, 2)
new_rots, new_avs = rot._slerp(np.arange(-3, 5))
expected_rot = Rotation.from_euler(
'x', [-360, -270, -180, -90, 0, 90, 180, 270], degrees=True)
np.testing.assert_almost_equal(new_rots.as_quat(), expected_rot.as_quat())
np.testing.assert_almost_equal(np.degrees(new_avs),
np.repeat([[90, 0, 0]], 8, 0))
def test_time_dependent_inverse():
quats1_2 = [[1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], [1, 0, 0, 0]]
times1_2 = [0, 1]
rot1_2 = TimeDependentRotation(quats1_2, times1_2, 1, 2)
rot2_1 = rot1_2.inverse()
assert rot2_1.source == 2
assert rot2_1.dest == 1
expected_quats = np.array([[1.0 / np.sqrt(2), 0, 0, -1.0 / np.sqrt(2)],
[1, 0, 0, 0]])
np.testing.assert_equal(rot2_1.times, np.array(times1_2))
np.testing.assert_almost_equal(rot2_1.quats, expected_quats)
def test_rotate_velocity_at():
test_quats = Rotation.from_euler(
'xyz', [[0, 0, 0], [-90, 0, 0], [-90, 180, 0], [-90, 180, 90]],
degrees=True).as_quat()
rot = TimeDependentRotation(test_quats, [0, 1, 2, 3], 1, 2)
input_pos = [[1, 2, 3], [1, 2, 3], [1, 2, 3]]
input_vel = [[-1, -2, -3], [-1, -2, -3], [-1, -2, -3]]
input_times = [1, 2, 3]
rot_vel = rot.rotate_velocity_at(input_pos, input_vel, input_times)
np.testing.assert_almost_equal(
rot_vel,
[[-1, -3 + np.pi, 2 + 3 * np.pi / 2], [1 + 3 * np.pi, -3 + np.pi, -2],
[3, 1 - np.pi, -2 - np.pi / 2]])
def test_time_dependent_inverse():
quats1_2 = [[1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], [1, 0, 0, 0]]
times1_2 = [0, 1]
av1_2 = [[np.pi / 2, 0, 0], [np.pi / 2, 0, 0]]
rot1_2 = TimeDependentRotation(quats1_2, times1_2, 1, 2, av=av1_2)
rot2_1 = rot1_2.inverse()
assert rot2_1.source == 2
assert rot2_1.dest == 1
expected_quats = [[1.0 / np.sqrt(2), 0, 0, -1.0 / np.sqrt(2)],
[1, 0, 0, 0]]
expected_av = [[-np.pi / 2, 0, 0], [-np.pi / 2, 0, 0]]
np.testing.assert_equal(rot2_1.times, times1_2)
np.testing.assert_almost_equal(rot2_1.quats, expected_quats)
np.testing.assert_almost_equal(rot2_1.av, expected_av)
def test_slerp_variable_velocity():
test_quats = Rotation.from_euler(
'xyz', [[0, 0, 0], [-90, 0, 0], [-90, 180, 0], [-90, 180, 90]],
degrees=True).as_quat()
rot = TimeDependentRotation(test_quats, [0, 1, 2, 3], 1, 2)
new_rots, new_avs = rot._slerp([-0.5, 0.5, 1.5, 2.5, 3.5])
expected_rot = Rotation.from_euler('xyz',
[[45, 0, 0], [-45, 0, 0], [-90, 90, 0],
[-90, 180, 45], [-90, 180, 135]],
degrees=True)
np.testing.assert_almost_equal(new_rots.as_quat(), expected_rot.as_quat())
np.testing.assert_almost_equal(
np.degrees(new_avs),
[[-90, 0, 0], [-90, 0, 0], [0, 180, 0], [0, 0, 90], [0, 0, 90]])
def test_time_dependent_constant_composition():
rot1_2 = ConstantRotation([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], 1, 2)
quats = [[1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], [1, 0, 0, 0]]
times = [0, 1]
av = [[np.pi / 2, 0, 0], [np.pi / 2, 0, 0]]
rot2_3 = TimeDependentRotation(quats, times, 2, 3, av=av)
rot1_3 = rot2_3 * rot1_2
assert isinstance(rot1_3, TimeDependentRotation)
assert rot1_3.source == 1
assert rot1_3.dest == 3
expected_quats = [[1, 0, 0, 0],
[1.0 / np.sqrt(2), 0, 0, -1.0 / np.sqrt(2)]]
np.testing.assert_equal(rot1_3.times, times)
np.testing.assert_almost_equal(rot1_3.quats, expected_quats)
np.testing.assert_almost_equal(rot1_3.av, av)
def test_time_dependent_constant_composition():
# 90 degree rotation about the X-axis
rot1_2 = ConstantRotation([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], 1, 2)
# 90 degree rotation about the X-axis to a 180 degree rotation about the X-axis
quats = [[1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], [1, 0, 0, 0]]
times = [0, 1]
rot2_3 = TimeDependentRotation(quats, times, 2, 3)
# compose to get a 180 degree rotation about the X-axis to a 270 degree rotation about the X-axis
rot1_3 = rot2_3 * rot1_2
assert isinstance(rot1_3, TimeDependentRotation)
assert rot1_3.source == 1
assert rot1_3.dest == 3
expected_quats = np.array([[1, 0, 0, 0],
[1.0 / np.sqrt(2), 0, 0, -1.0 / np.sqrt(2)]])
np.testing.assert_equal(rot1_3.times, np.array(times))
np.testing.assert_almost_equal(rot1_3.quats, expected_quats)
def frame_chain(self):
if not hasattr(self, '_frame_chain'):
nadir = self._props.get('nadir', False)
self._frame_chain = FrameChain.from_spice(
sensor_frame=self.sensor_frame_id,
target_frame=self.target_frame_id,
center_ephemeris_time=self.center_ephemeris_time,
ephemeris_times=self.ephemeris_time,
nadir=nadir)
if nadir:
# Logic for nadir calculation was taken from ISIS3
# SpiceRotation::setEphemerisTimeNadir
rotation = self._frame_chain.compute_rotation(
self.target_frame_id, 1)
p_vec, v_vec, times = self.sensor_position
rotated_positions = rotation.apply_at(p_vec, times)
rotated_velocities = rotation.rotate_velocity_at(
p_vec, v_vec, times)
p_vec = rotated_positions
v_vec = rotated_velocities
velocity_axis = 2
# Get the default line translation with no potential flipping
# from the driver
trans_x = np.array(
list(spice.gdpool('INS{}_ITRANSL'.format(self.ikid), 0,
3)))
if (trans_x[0] < trans_x[1]):
velocity_axis = 1
quats = [
spice.m2q(
spice.twovec(-p_vec[i], 3, v_vec[i], velocity_axis))
for i, time in enumerate(times)
]
quats = np.array(quats)[:, [1, 2, 3, 0]]
rotation = TimeDependentRotation(quats, times, 1,
self.sensor_frame_id)
self._frame_chain.add_edge(rotation)
return self._frame_chain
def test_reinterpolate():
rot = TimeDependentRotation([[0, 0, 0, 1], [0, 0, 0, 1]], [0, 1], 1, 2)
new_rot = rot.reinterpolate(np.arange(-3, 5))
assert new_rot.source == rot.source
assert new_rot.dest == rot.dest
np.testing.assert_equal(new_rot.times, np.arange(-3, 5))
def test_slerp_constant_rotation():
rot = TimeDependentRotation([[0, 0, 0, 1]], [0], 1, 2)
new_rot, new_avs = rot._slerp([-1, 3])
np.testing.assert_equal(new_rot.as_quat(), [[0, 0, 0, 1], [0, 0, 0, 1]])
np.testing.assert_equal(new_avs, [[0, 0, 0], [0, 0, 0]])