Added initial nadir pointing (#332)

* Added nadir orientation calculation to frame_chain

* Added comments, replaced conditional dict access, replaced hard-coded bool for nadir parameter.

* Updated orientation to be obtained from the frame chain and added tests

* Updated the frame trace returns if nadir is true and propogated those changes to the frame chain from_spice function

* Updated focal2pixel_lines test for the time being

Co-authored-by: Adam Paquette <[email protected]>

ale/base/data_naif.py

@@ -392,39 +392,55 @@ def sensor_position(self):
     @property
     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)
+                                                      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
+                trans_x = self.focal2pixel_lines
+
+                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
 
+
     @property
     def sensor_orientation(self):
         """
         Returns quaternions describing the sensor orientation. Expects ephemeris_time
         to be defined. This must be a floating point number containing the
         ephemeris time. Expects instrument_id to be defined. This must be a string
         containing the short name of the instrument. Expects reference frame to be defined.
-        This must be a sring containing the name of the target reference frame.
+        This must be a string containing the name of the target reference frame.
 
         Returns
         -------
         : list
           Quaternions describing the orientation of the sensor
         """
         if not hasattr(self, '_orientation'):
-            ephem = self.ephemeris_time
-
-            qua = np.empty((len(ephem), 4))
-            for i, time in enumerate(ephem):
-                # Find the rotation matrix
-                camera2bodyfixed = spice.pxform(self.instrument_id,
-                                                self.reference_frame,
-                                                time)
-                q = spice.m2q(camera2bodyfixed)
-                qua[i,:3] = q[1:]
-                qua[i,3] = q[0]
-            self._orientation = qua
+            self._orientation = self.frame_chain.compute_rotation(self.sensor_frame_id, self.target_frame_id).quats
         return self._orientation.tolist()
 
     @property

ale/drivers/lro_drivers.py

@@ -142,10 +142,7 @@ def focal2pixel_lines(self):
           focal plane to detector lines
         """
         focal2pixel_lines = np.array(list(spice.gdpool('INS{}_ITRANSL'.format(self.ikid), 0, 3)))
-        if self.spacecraft_direction < 0:
-            return focal2pixel_lines
-        else:
-            return -focal2pixel_lines
+        return focal2pixel_lines
 
     @property
     def ephemeris_start_time(self):
@@ -402,10 +399,7 @@ def focal2pixel_lines(self):
           focal plane to detector lines
         """
         focal2pixel_lines = np.array(list(spice.gdpool('INS{}_ITRANSL'.format(self.ikid), 0, 3)))
-        if self.spacecraft_direction < 0:
-            return focal2pixel_lines
-        else:
-            return -focal2pixel_lines
+        return focal2pixel_lines
 
     @property
     def multiplicative_line_error(self):

ale/transformation.py

@@ -96,12 +96,12 @@ class FrameChain(nx.DiGraph):
                      of frame rotations in the frame chain
     """
     @classmethod
-    def from_spice(cls, *args, sensor_frame, target_frame, center_ephemeris_time, ephemeris_times=[], **kwargs):
+    def from_spice(cls, sensor_frame, target_frame, center_ephemeris_time, ephemeris_times=[], nadir=False):
         frame_chain = cls()
 
         times = np.array(ephemeris_times)
 
-        sensor_time_dependent_frames, sensor_constant_frames = cls.frame_trace(sensor_frame, center_ephemeris_time)
+        sensor_time_dependent_frames, sensor_constant_frames = cls.frame_trace(sensor_frame, center_ephemeris_time, nadir)
         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:]))
@@ -139,11 +139,14 @@ def from_spice(cls, *args, sensor_frame, target_frame, center_ephemeris_time, ep
         return frame_chain
 
     @staticmethod
-    def frame_trace(reference_frame, ephemeris_time):
+    def frame_trace(reference_frame, ephemeris_time, nadir=False):
         frame_codes = [reference_frame]
         _, frame_type, _ = spice.frinfo(frame_codes[-1])
         frame_types = [frame_type]
 
+        if nadir:
+            return [], []
+
         while(frame_codes[-1] != 1):
             try:
                 center, frame_type, frame_type_id = spice.frinfo(frame_codes[-1])

tests/pytests/test_data_naif.py

@@ -101,6 +101,25 @@ def test_sun_position(self):
         assert len(times) == 1
         np.testing.assert_allclose(times[0], 297088762.61698407)
 
+    def test_sensor_orientation(self):
+        self.driver.ephemeris_time = [297088762.61698407]
+        self.driver._props = {}
+        orientation = self.driver.sensor_orientation
+        np.testing.assert_allclose(orientation[0], [0.08410784798989432, 0.017724689780841133, 0.9945884195952942, 0.058357355025882435])
+
+    def test_sensor_position(self):
+        self.driver.ephemeris_time = [297088762.61698407]
+        positions, velocities, times = self.driver.sensor_position
+        np.testing.assert_allclose(positions[0], [-616295.93509894, -97815.27289939, -3573807.40392374])
+        np.testing.assert_allclose(velocities[0], [-3386.49396159, 411.4392769, 564.95648816])
+        np.testing.assert_allclose(times[0], 297088762.61698407)
+
+    def test_nadir_sensor_orientation(self):
+        self.driver.ephemeris_time = [297088762.61698407]
+        self.driver._props = {'nadir': True}
+        orientation = self.driver.sensor_orientation
+        np.testing.assert_allclose(orientation[0], [-0.08443224924851939, -0.017974644466439982, -0.9949019866167608, -0.052135827116906064])
+
 def test_light_time_correction_keyword():
     with patch('ale.base.data_naif.spice.gcpool', return_value=['NONE']) as gcpool, \
          patch('ale.base.data_naif.NaifSpice.ikid', new_callable=PropertyMock) as ikid:

tests/pytests/test_lro_drivers.py

@@ -292,7 +292,7 @@ def test_focal2pixel_lines(self):
             spacecraft_direction.return_value = -1
             np.testing.assert_array_equal(self.driver.focal2pixel_lines, [0, 1, 0])
             spacecraft_direction.return_value = 1
-            np.testing.assert_array_equal(self.driver.focal2pixel_lines, [0, -1, 0])
+            np.testing.assert_array_equal(self.driver.focal2pixel_lines, [0, 1, 0])
 
 
 # ========= Test isislabel and naifspice driver =========
@@ -383,4 +383,4 @@ def test_focal2pixel_lines(self):
             spacecraft_direction.return_value = -1
             np.testing.assert_array_equal(self.driver.focal2pixel_lines, [0, 1, 0])
             spacecraft_direction.return_value = 1
-            np.testing.assert_array_equal(self.driver.focal2pixel_lines, [0, -1, 0])
+            np.testing.assert_array_equal(self.driver.focal2pixel_lines, [0, 1, 0])