def test_constant_inverse():
rot1_2 = ConstantRotation([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], 1, 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)]
np.testing.assert_almost_equal(rot2_1.quat, expected_quats)
def frame_tree(request):
"""
Test frame tree structure:
1
/ \
/ \
2 4
/
/
3
"""
frame_chain = FrameChain()
rotations = [
ConstantRotation(np.array([1, 0, 0, 0]), 2, 1),
ConstantRotation(np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
3, 2),
ConstantRotation(np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
4, 1)
]
frame_chain.add_edge(rotation=rotations[0])
frame_chain.add_edge(rotation=rotations[1])
frame_chain.add_edge(rotation=rotations[2])
return frame_chain, rotations
def frame_tree(request):
"""
Test frame tree structure:
1
/ \
/ \
2 4
/
/
3
"""
rotations = [
ConstantRotation(np.array([1, 0, 0, 0]), 2, 1),
ConstantRotation(np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
3, 2),
ConstantRotation(np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
4, 1)
]
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])
nodes = [root_node, child_node_1, child_node_2, child_node_3]
return (nodes, rotations)
def test_constant_constant_composition():
rot1_2 = ConstantRotation([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], 1, 2)
rot2_3 = ConstantRotation([0, 1.0 / np.sqrt(2), 0, 1.0 / np.sqrt(2)], 2, 3)
rot1_3 = rot2_3 * rot1_2
assert isinstance(rot1_3, ConstantRotation)
assert rot1_3.source == 1
assert rot1_3.dest == 3
np.testing.assert_equal(rot1_3.quat, [0.5, 0.5, -0.5, 0.5])
def test_constant_inverse():
rot1_2 = ConstantRotation(
np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]), 1, 2)
rot2_1 = rot1_2.inverse()
assert rot2_1.source == 2
assert rot2_1.dest == 1
np.testing.assert_almost_equal(
rot2_1.quat, np.array([1.0 / np.sqrt(2), 0, 0, -1.0 / np.sqrt(2)]))
def test_constant_constant_composition():
# Two 90 degree rotation about the X-axis
rot1_2 = ConstantRotation([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], 1, 2)
rot2_3 = ConstantRotation([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], 2, 3)
# compose to get a 180 degree rotation about the X-axis
rot1_3 = rot2_3 * rot1_2
assert isinstance(rot1_3, ConstantRotation)
assert rot1_3.source == 1
assert rot1_3.dest == 3
np.testing.assert_equal(rot1_3.quat, np.array([1, 0, 0, 0]))
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 frame_chain(self):
"""
Construct the initial frame chain using the original sensor_frame_id
obtained from the ikid. Then tack on the ISIS iak rotation.
Returns
-------
: Object
Custom Cassini ALE Frame Chain object for rotation computation and application
"""
if not hasattr(self, '_frame_chain'):
try:
# Call frinfo to check if the ISIS iak has been loaded with the
# additional reference frame. Otherwise, Fail and add it manually
spice.frinfo(self.sensor_frame_id)
self._frame_chain = super().frame_chain
except spice.utils.exceptions.NotFoundError as e:
self._frame_chain = FrameChain.from_spice(sensor_frame=self._original_naif_sensor_frame_id,
target_frame=self.target_frame_id,
center_ephemeris_time=self.center_ephemeris_time,
ephemeris_times=self.ephemeris_time,)
rotation = ConstantRotation([[0, 0, 1, 0]], self.sensor_frame_id, self._original_naif_sensor_frame_id)
self._frame_chain.add_edge(rotation=rotation)
return self._frame_chain
def compute_rotation(self, source, destination):
"""
Returns the rotation to another node. Returns the identity rotation
if the other node is this node.
Parameters
----------
source : int
Integer id for the source node to rotate from
destination : int
Integer id for the node to rotate into from the source node
Returns
-------
rotation : Object
Returns either a TimeDependentRotation object or ConstantRotation
object depending on the number of rotations being multiplied
together
"""
if source == destination:
return ConstantRotation(np.array([0, 0, 0, 1]), source,
destination)
path = shortest_path(self, source, destination)
rotations = [
self.edges[path[i], path[i + 1]]['rotation']
for i in range(len(path) - 1)
]
rotation = rotations[0]
for next_rotation in rotations[1:]:
rotation = next_rotation * rotation
return rotation
def test_from_matrix():
mat = [[0, 0, 1], [1, 0, 0], [0, 1, 0]]
rot = ConstantRotation.from_matrix(mat, 0, 1)
expected_quats = np.asarray([0.5, 0.5, 0.5, 0.5])
np.testing.assert_almost_equal(rot.quat, expected_quats)
assert rot.source == 0
assert rot.dest == 1
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_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):
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_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 rotation_to(self, other):
"""
Returns the rotation to another node. Returns the identity rotation
if the other node is this node.
Parameters
----------
other : FrameNode
The other node to find the rotation to.
"""
if other == self:
return ConstantRotation(np.array([0, 0, 0, 1]), self.id, other.id)
forward_path, reverse_path = self.path_to(other)
rotations = [node.rotation for node in forward_path[:-1]]
rotations.extend([node.rotation.inverse() for node in reverse_path])
rotation = rotations[0]
for next_rotation in rotations[1:]:
rotation = next_rotation * rotation
return rotation
def test_rotation_matrix():
rot = ConstantRotation([0, 0, 0, 1], 1, 2)
mat = rot.rotation_matrix()
assert isinstance(mat, np.ndarray)
assert mat.shape == (3, 3)
