MSL Nadir Pointing (#564)

* Enabled nadir pointing in MSL/CAHVOR driver

* Removed commented out position changes

* Fix MSL test

* More msl fiddling

* Update for MSL Nadir pointing rotation in the cahvor mixin

* Fixed msl tests and added nadir test

ale/base/type_sensor.py

 import math
 
 import numpy as np
+import spiceypy as spice
 from scipy.spatial.transform import Rotation
 
 from ale.transformation import FrameChain
-from ale.transformation import ConstantRotation
+from ale.transformation import ConstantRotation, TimeDependentRotation
 
 class LineScanner():
     """
@@ -468,7 +469,7 @@ class Cahvor():
             H_prime = (self.cahvor_camera_dict['H'] - h_c * self.cahvor_camera_dict['A'])/h_s
             V_prime = (self.cahvor_camera_dict['V'] - v_c * self.cahvor_camera_dict['A'])/v_s
             if self._props.get("landed", False):
-              self._cahvor_rotation_matrix = np.array([H_prime, -V_prime, -self.cahvor_camera_dict['A']])
+              self._cahvor_rotation_matrix = np.array([-H_prime, -V_prime, self.cahvor_camera_dict['A']])
             else:
               self._cahvor_rotation_matrix = np.array([H_prime, V_prime, self.cahvor_camera_dict['A']])
         return self._cahvor_rotation_matrix
@@ -485,12 +486,39 @@ class Cahvor():
           A networkx frame chain object
         """
         if not hasattr(self, '_frame_chain'):
+            nadir = self._props.get("nadir", False)
             self._frame_chain = FrameChain.from_spice(sensor_frame=self.final_inst_frame,
                                                       target_frame=self.target_frame_id,
                                                       center_ephemeris_time=self.center_ephemeris_time,
                                                       ephemeris_times=self.ephemeris_time,
-                                                      nadir=False, exact_ck_times=False)
+                                                      nadir=nadir, exact_ck_times=False)
             cahvor_quats = Rotation.from_matrix(self.cahvor_rotation_matrix).as_quat()
+            
+            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(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.final_inst_frame)
+                self._frame_chain.add_edge(rotation)
+
             # If we are landed we only care about the final cahvor frame relative to the target
             if self._props.get("landed", False):
               cahvor_rotation = ConstantRotation(cahvor_quats, self.target_frame_id, self.sensor_frame_id)

ale/drivers/msl_drivers.py

@@ -108,6 +108,9 @@ class MslMastcamPds3NaifSpiceDriver(Cahvor, Framer, Pds3Label, NaifSpice, Cahvor
         : list<double>
           focal plane to detector lines
         """
+        if self._props.get("landed", False):
+            return [0, 0, -1/self.pixel_size]
+        else:
             return [0, 0, 1/self.pixel_size]
     
     @property
@@ -120,6 +123,9 @@ class MslMastcamPds3NaifSpiceDriver(Cahvor, Framer, Pds3Label, NaifSpice, Cahvor
         : list<double>
           focal plane to detector samples
         """
+        if (self._props.get("nadir", False)):
+            return [0, 1/self.pixel_size, 0]
+        else:
             return [0, -1/self.pixel_size, 0]
 
     @property

ale/isd_generate.py

@@ -70,9 +70,15 @@ def main():
     parser.add_argument(
         "-l", "--local",
         action="store_true",
-        help="Generate local spice data, or image that is unaware of itself relative to "
+        help="Generate local spice data, an isd that is unaware of itself relative to "
              "target body. This is largely used for landed/rover data."
     )
+    parser.add_argument(
+        "-N", "--nadir",
+        action="store_true",
+        help="Generate nadir spice pointing, an isd that has pointing directly towards "
+             "the center of the target body."
+    )
     parser.add_argument(
         '--version',
         action='version',
@@ -117,7 +123,8 @@ def main():
                                        "log_level": log_level, 
                                        "only_isis_spice": args.only_isis_spice, 
                                        "only_naif_spice": args.only_naif_spice,
-                                       "local": args.local}
+                                       "local": args.local,
+                                       "nadir": args.nadir}
                 ): f for f in args.input
             }
             for f in concurrent.futures.as_completed(futures):
@@ -138,7 +145,8 @@ def file_to_isd(
     log_level=logging.WARNING,
     only_isis_spice=False,
     only_naif_spice=False,
-    local=False
+    local=False,
+    nadir=False
 ):
     """
     Returns nothing, but acts as a thin wrapper to take the *file* and generate
@@ -167,6 +175,9 @@ def file_to_isd(
     if local:
         props['landed'] = local
 
+    if nadir:
+        props['nadir'] = nadir
+
     if kernels is not None:
         kernels = [str(PurePath(p)) for p in kernels]
         props["kernels"] = kernels

tests/pytests/data/isds/msl_nadir_isd.json

+{
+  "isis_camera_version": 1,
+  "image_lines": 1193,
+  "image_samples": 1338,
+  "name_platform": "MARS SCIENCE LABORATORY",
+  "name_sensor": "MAST CAMERA LEFT",
+  "reference_height": {
+    "maxheight": 1000,
+    "minheight": -1000,
+    "unit": "m"
+  },
+  "name_model": "USGS_ASTRO_FRAME_SENSOR_MODEL",
+  "center_ephemeris_time": 598494669.4412209,
+  "radii": {
+    "semimajor": 3396.19,
+    "semiminor": 3376.2,
+    "unit": "km"
+  },
+  "body_rotation": {
+    "time_dependent_frames": [
+      10014,
+      1
+    ],
+    "ck_table_start_time": 598494669.4412209,
+    "ck_table_end_time": 598494669.4412209,
+    "ck_table_original_size": 1,
+    "ephemeris_times": [
+      598494669.4412209
+    ],
+    "quaternions": [
+      [
+        -0.31921039676060065,
+        0.2937636389682398,
+        -0.12264933637077348,
+        0.8926168199781396
+      ]
+    ],
+    "angular_velocities": [
+      [
+        3.1623010827381965e-05,
+        -2.881378599775597e-05,
+        5.651578887273642e-05
+      ]
+    ],
+    "reference_frame": 1
+  },
+  "instrument_pointing": {
+    "time_dependent_frames": [
+      -76205,
+      1
+    ],
+    "ck_table_start_time": 598494669.4412209,
+    "ck_table_end_time": 598494669.4412209,
+    "ck_table_original_size": 1,
+    "ephemeris_times": [
+      598494669.4412209
+    ],
+    "quaternions": [
+      [
+        0.7419897630883615,
+        0.4156401046800741,
+        0.25055247114123014,
+        0.4625126528632889
+      ]
+    ],
+    "angular_velocities": null,
+    "reference_frame": 1,
+    "constant_frames": [
+      -76573,
+      -76205
+    ],
+    "constant_rotation": [
+      0.4165667270357225,
+      0.908831179877854,
+      -0.02231699819809352,
+      -0.7809014963237929,
+      0.37028293957648445,
+      0.503073948538243,
+      0.4654728939111354,
+      -0.19213649091316684,
+      0.8639551804888768
+    ]
+  },
+  "naif_keywords": {
+    "BODY499_RADII": [
+      3396.19,
+      3396.19,
+      3376.2
+    ],
+    "BODY_FRAME_CODE": 10014,
+    "BODY_CODE": 499,
+    "FRAME_-76210_NAME": "MSL_MASTCAM_LEFT",
+    "INS-76210_CAHVOR_H": [
+      712.373106,
+      4664.465028,
+      33.182389
+    ],
+    "TKFRAME_-76210_UNITS": "DEGREES",
+    "INS-76210_IFOV_VERTICAL": 0.01230196,
+    "INS-76210_CAHVOR_O": [
+      0.999627,
+      0.026908,
+      0.004759
+    ],
+    "INS-76210_CAHVOR_R": [
+      -0.000151,
+      -0.139189,
+      -1.250336
+    ],
+    "INS-76210_CAHVOR_V": [
+      570.612488,
+      -14.279011,
+      4648.733195
+    ],
+    "INS-76210_CAHVOR_FILE": "MSL_CAL_003_SN_3003_FILTER_0_FOCUS_02315-MCAML-FLIGHT.cahvor",
+    "INS-76210_DISTORTION_PIXEL": [
+      837.77915717,
+      592.14046615
+    ],
+    "TKFRAME_-76210_AXES": [
+      2.0,
+      1.0,
+      3.0
+    ],
+    "TKFRAME_-76210_SPEC": "ANGLES",
+    "INS-76210_FOCAL_LENGTH": 34.0,
+    "INS-76210_CAHVOR_QUAT": [
+      1e-05,
+      -0.00325,
+      -0.00104,
+      0.99999
+    ],
+    "INS-76210_CAHVOR_POS": [
+      0.80436,
+      0.55942,
+      -1.90608
+    ],
+    "INS-76210_FOV_BOUNDARY": [
+      0.17483767,
+      0.12730492,
+      0.97633255,
+      -0.0,
+      0.12834274,
+      0.99172987,
+      -0.17476887,
+      0.12719345,
+      0.9763594,
+      -0.17553059
+    ],
+    "INS-76210_PIXEL_LINES": 1200.0,
+    "INS-76210_CAHVOR_MODEL": " CAHVOR",
+    "INS-76210_PIXEL_SIZE": 0.0074,
+    "INS-76210_CAHVOR_HC": 829.187822,
+    "INS-76210_IFOV_NOMINAL": 0.01247026,
+    "INS-76210_CAHVOR_HS": 4645.242086,
+    "INS-76210_BORESIGHT_PIXEL": [
+      829.18782212,
+      601.33514402
+    ],
+    "FRAME_-76210_CLASS_ID": -76210.0,
+    "INS-76210_FOV_CENTER_PIXEL": [
+      823.5,
+      599.5
+    ],
+    "INS-76210_FOV_CLASS_SPEC": "CORNERS",
+    "INS-76210_BORESIGHT": [
+      -0.0,
+      -0.0,
+      1.0
+    ],
+    "INS-76210_CAHVOR_THETA": -1.5710039999999998,
+    "INS-76210_IFOV": 0.01228988,
+    "TKFRAME_-76210_RELATIVE": "MSL_RSM_HEAD",
+    "FRAME_-76210_CLASS": 4.0,
+    "INS-76210_CAHVOR_DIMS": [
+      1648.0,
+      1200.0
+    ],
+    "INS-76210_FOV_SHAPE": "POLYGON",
+    "INS-76210_PIXEL_SAMPLES": 1648.0,
+    "INS-76210_IFOV_HORIZONTAL": 0.0122778,
+    "INS-76210_CAHVOR_VC": 601.335144,
+    "INS-76210_CAHVOR_VS": 4644.882626,
+    "TKFRAME_-76210_ANGLES": [
+      -90.01,
+      1.484,
+      89.655
+    ],
+    "FRAME_-76210_CENTER": -76.0,
+    "INS-76210_CAHVOR_A": [
+      0.999664,
+      0.025047,
+      0.006727
+    ],
+    "INS-76210_CAHVOR_C": [
+      0.767151,
+      0.433709,
+      -1.971648
+    ],
+    "INS-76210_FOV_FRAME": "MSL_MASTCAM_LEFT",
+    "BODY499_POLE_DEC": [
+      52.8865,
+      -0.0609,
+      0.0
+    ],
+    "BODY499_POLE_RA": [
+      317.68143,
+      -0.1061,
+      0.0
+    ],
+    "BODY499_PM": [
+      176.63,
+      350.89198226,
+      0.0
+    ]
+  },
+  "detector_sample_summing": 1,
+  "detector_line_summing": 1,
+  "focal_length_model": {
+    "focal_length": 34.0
+  },
+  "detector_center": {
+    "line": 576.4026068104001,
+    "sample": 680.1442422028802
+  },
+  "focal2pixel_lines": [
+    0,
+    0,
+    136.49886775101945
+  ],
+  "focal2pixel_samples": [
+    0,
+    136.49886775101945,
+    0
+  ],
+  "optical_distortion": {
+    "cahvor": {
+      "coefficients": [
+        0,
+        0,
+        0,
+        0,
+        0
+      ]
+    }
+  },
+  "starting_detector_line": 0,
+  "starting_detector_sample": 0,
+  "instrument_position": {
+    "spk_table_start_time": 598494669.4412209,
+    "spk_table_end_time": 598494669.4412209,
+    "spk_table_original_size": 1,
+    "ephemeris_times": [
+      598494669.4412209
+    ],
+    "positions": [
+      [
+        -42.94908602840011,
+        -2878.1178384691016,
+        -1794.0007301446622
+      ]
+    ],
+    "velocities": [
+      [
+        0.2143509476698853,
+        0.054297300199827,
+        -0.09225670467004271
+      ]
+    ],
+    "reference_frame": 1
+  },
+  "sun_position": {
+    "spk_table_start_time": 598494669.4412209,
+    "spk_table_end_time": 598494669.4412209,
+    "spk_table_original_size": 1,
+    "ephemeris_times": [
+      598494669.4412209
+    ],
+    "positions": [
+      [
+        -178112289.0644448,
+        -111686023.67244285,
+        -46420243.18796099
+      ]
+    ],
+    "velocities": [
+      [
+        12.688344510456547,
+        -19.98128446099907,
+        -9.507348850576207
+      ]
+    ],
+    "reference_frame": 1
+  }
+}
\ No newline at end of file

tests/pytests/test_msl_drivers.py

@@ -5,7 +5,7 @@ import pytest
 import unittest
 
 import ale
-from conftest import get_image, get_image_label, get_isd, get_image_kernels, convert_kernels, compare_dicts
+from conftest import get_image_label, get_isd, get_image_kernels, convert_kernels, compare_dicts
 from ale.drivers.msl_drivers import MslMastcamPds3NaifSpiceDriver
 
 from conftest import get_image_label
@@ -20,11 +20,19 @@ def test_mastcam_kernels():
     for kern in binary_kernels:
         os.remove(kern)
 
-def test_msl_mastcam_load(test_mastcam_kernels):
+def test_msl_mastcam_load_local(test_mastcam_kernels):
     label_file = get_image_label('2264ML0121141200805116C00_DRCL', "pds3")
     compare_dict = get_isd("msl")
 
-    isd_str = ale.loads(label_file, props={'kernels': test_mastcam_kernels})
+    isd_str = ale.loads(label_file, props={'kernels': test_mastcam_kernels, 'local': True})
+    isd_obj = json.loads(isd_str)
+    assert compare_dicts(isd_obj, compare_dict) == []
+
+def test_msl_mastcam_load_nadir(test_mastcam_kernels):
+    label_file = get_image_label('2264ML0121141200805116C00_DRCL', "pds3")
+    compare_dict = get_isd("msl_nadir")
+
+    isd_str = ale.loads(label_file, props={'kernels': test_mastcam_kernels, 'nadir': True})
     isd_obj = json.loads(isd_str)
     assert compare_dicts(isd_obj, compare_dict) == []