Cahvor Model Mixin (#490)

* Adds cahvor model mixin to ALE

* Removed commented out lines and added doc strings

* Cleaned up Cahvor sensor and added tests for the mixin

* Removed leftover print

* Added more properties to cahvor model and removed properties moved to the concrete driver

* Addressed PR feedback

* Added tests and doc strings for cahvor mixing

* Added cahvor mixin to msl_driver

* Finished tests for msl driver

* Fixed failing tests

* Cached camera calculations from rotation matrix math

* Converted intermidiate rotation matrix elements to function calls

* Added function doc strings

ale/base/type_sensor.py

+import math
+
 import numpy as np
+from scipy.spatial.transform import Rotation
+
+from ale.transformation import FrameChain
+from ale.transformation import ConstantRotation
 
 class LineScanner():
     """
@@ -362,3 +368,174 @@ class RollingShutter():
         : array
         """
         raise NotImplementedError
+
+
+class Cahvor():
+    """
+    Mixin for largely ground based sensors to add an
+    extra step in the frame chain to go from ground camera to
+    the Camera
+    """
+
+    @property
+    def cahvor_camera_dict(self):
+        """
+        This function extracts and returns the elements for the
+        CAHVOR camera model from a concrete driver as a dictionary.
+        See the MSL MASTCAM Cahvor, Framer, Pds3Label, NaifSpice, Driver
+        """
+        raise NotImplementedError
+
+    def compute_h_c(self):
+        """
+        Computes the h_c element of a cahvor model for the conversion
+        to a photogrametric model
+
+        Returns
+        -------
+        : float
+          Dot product of A and H vectors
+        """
+        return np.dot(self.cahvor_camera_dict['A'], self.cahvor_camera_dict['H'])
+
+    def compute_h_s(self):
+        """
+        Computes the h_s element of a cahvor model for the conversion
+        to a photogrametric model
+
+        Returns
+        -------
+        : float
+          Norm of the cross product of A and H vectors
+        """
+        return np.linalg.norm(np.cross(self.cahvor_camera_dict['A'], self.cahvor_camera_dict['H']))
+
+    def compute_v_c(self):
+        """
+        Computes the v_c element of a cahvor model for the conversion
+        to a photogrametric model
+
+        Returns
+        -------
+        : float
+          Dot product of A and V vectors
+        """
+        return np.dot(self.cahvor_camera_dict['A'], self.cahvor_camera_dict['V'])
+
+    def compute_v_s(self):
+        """
+        Computes the v_s element of a cahvor model for the conversion
+        to a photogrametric model
+
+        Returns
+        -------
+        : float
+          Norm of the cross product of A and V vectors
+        """
+        return np.linalg.norm(np.cross(self.cahvor_camera_dict['A'], self.cahvor_camera_dict['V']))
+
+    @property
+    def cahvor_rotation_matrix(self):
+        """
+        Computes the cahvor rotation matrix for the instrument to Rover frame
+
+        Returns
+        -------
+        : array
+          Rotation Matrix as a 2D numpy array
+        """
+        if not hasattr(self, "_cahvor_rotation_matrix"):
+            h_c = self.compute_h_c()
+            h_s = self.compute_h_s()
+            v_c = self.compute_v_c()
+            v_s = self.compute_v_s()
+            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
+            r_matrix = np.array([H_prime, -V_prime, -self.cahvor_camera_dict['A']])
+
+            phi = math.asin(r_matrix[2][0])
+            w = - math.asin(r_matrix[2][1] / math.cos(phi))
+            k = math.acos(r_matrix[0][0] / math.cos(phi))
+
+            w = math.degrees(w)
+            phi = math.degrees(phi)
+            k = math.degrees(k)
+
+            # Rotational Matrix M generation
+            cahvor_rotation_matrix = np.zeros((3, 3))
+            cahvor_rotation_matrix[0, 0] = math.cos(phi) * math.cos(k)
+            cahvor_rotation_matrix[0, 1] = math.sin(w) * math.sin(phi) * math.cos(k) + \
+                math.cos(w) * math.sin(k)
+            cahvor_rotation_matrix[0, 2] = - math.cos(w) * math.sin(phi) * math.cos(k) + \
+                math.sin(w) * math.sin(k)
+            cahvor_rotation_matrix[1, 0] = - math.cos(phi) * math.sin(k)
+            cahvor_rotation_matrix[1, 1] = - math.sin(w) * math.sin(phi) * math.sin(k) + \
+                math.cos(w) * math.cos(k)
+            cahvor_rotation_matrix[1, 2] = math.cos(w) * math.sin(phi) * math.sin(k) + \
+                math.sin(w) * math.cos(k)
+            cahvor_rotation_matrix[2, 0] = math.sin(phi)
+            cahvor_rotation_matrix[2, 1] = - math.sin(w) * math.cos(phi)
+            cahvor_rotation_matrix[2, 2] = math.cos(w) * math.cos(phi)
+            self._cahvor_rotation_matrix = cahvor_rotation_matrix
+        return self._cahvor_rotation_matrix
+
+    @property
+    def frame_chain(self):
+        """
+        Returns a modified frame chain with the cahvor models extra rotation
+        added into the model
+
+        Returns
+        -------
+        : object
+          A networkx frame chain object
+        """
+        if not hasattr(self, '_frame_chain'):
+            self._frame_chain = FrameChain.from_spice(sensor_frame=self.ikid,
+                                                      target_frame=self.target_frame_id,
+                                                      center_ephemeris_time=self.center_ephemeris_time,
+                                                      ephemeris_times=self.ephemeris_time,
+                                                      nadir=False, exact_ck_times=False)
+            cahvor_quats = Rotation.from_matrix(self.cahvor_rotation_matrix).as_quat()
+            cahvor_rotation = ConstantRotation(cahvor_quats, self.sensor_frame_id, self.ikid)
+            self._frame_chain.add_edge(rotation = cahvor_rotation)
+        return self._frame_chain
+
+    @property
+    def detector_center_line(self):
+        """
+        Computes the detector center line using the cahvor model.
+        Equation for computation comes from MSL instrument kernels
+
+        Returns
+        -------
+        : float
+          The detector center line/boresight center line
+        """
+        return self.compute_v_c()
+
+    @property
+    def detector_center_sample(self):
+        """
+        Computes the detector center sample using the cahvor model.
+        Equation for computation comes from MSL instrument kernels
+
+        Returns
+        -------
+        : float
+          The detector center sample/boresight center sample
+        """
+        return self.compute_h_c()
+
+    @property
+    def pixel_size(self):
+        """
+        Computes the pixel size given the focal length from spice kernels
+        or other sources
+
+        Returns
+        -------
+        : float
+          Focal length of a cahvor model instrument
+        """
+        return self.focal_length/self.compute_h_s()

ale/drivers/msl_drivers.py

@@ -5,9 +5,10 @@ from ale.base.data_naif import NaifSpice
 from ale.base.label_pds3 import Pds3Label
 from ale.base.type_sensor import Framer
 from ale.base.type_distortion import NoDistortion
+from ale.base.type_sensor import Cahvor
 from ale.base.base import Driver
 
-class MslMastcamPds3NaifSpiceDriver(Framer, Pds3Label, NaifSpice, NoDistortion, Driver):
+class MslMastcamPds3NaifSpiceDriver(Cahvor, Framer, Pds3Label, NaifSpice, NoDistortion, Driver):
     @property
     def spacecraft_name(self):
         """

tests/pytests/test_cahvor_mixin.py

+import unittest
+from unittest.mock import PropertyMock, patch
+import pytest
+import pvl
+import numpy as np
+
+import ale
+
+from ale.base.type_sensor import Cahvor
+
+def cahvor_camera_dict():
+    camera_dict = {}
+    camera_dict['C'] = np.array([6.831825e-01, 5.243722e-01, -1.955875e+00])
+    camera_dict['A'] = np.array([-3.655151e-01, 5.396012e-01, 7.584387e-01])
+    camera_dict['H'] = np.array([-1.156881e+04, -7.518712e+03, 6.618359e+02])
+    camera_dict['V'] = np.array([5.843885e+03, -8.213856e+03, 9.438374e+03])
+    return camera_dict
+
+
+class test_cahvor_sensor(unittest.TestCase):
+
+    def setUp(self):
+        self.driver = Cahvor()
+        self.driver.focal_length = 100
+        self.driver.ikid = -76220
+        self.driver.sensor_frame_id = -76562
+        self.driver.target_frame_id = 10014
+        self.driver.center_ephemeris_time = 0
+        self.driver.ephemeris_time = [0]
+
+    @patch("ale.base.type_sensor.Cahvor.cahvor_camera_dict", new_callable=PropertyMock, return_value=cahvor_camera_dict())
+    def test_compute_functions(self, cahvor_camera_dict):
+        np.testing.assert_almost_equal(self.driver.compute_h_s(), 13796.844341513603)
+        np.testing.assert_almost_equal(self.driver.compute_h_c(), 673.4306859859296)
+        np.testing.assert_almost_equal(self.driver.compute_v_s(), 13796.847423351614)
+        np.testing.assert_almost_equal(self.driver.compute_v_c(), 590.1933422831007)
+
+    @patch("ale.base.type_sensor.Cahvor.cahvor_camera_dict", new_callable=PropertyMock, return_value=cahvor_camera_dict())
+    def test_cahvor_model_elements(self, cahvor_camera_dict):
+        cahvor_matrix = self.driver.cahvor_rotation_matrix
+        np.testing.assert_allclose(cahvor_matrix, [[-0.42447558, -0.7572992,  -0.49630475],
+                                                   [ 0.73821222,  0.02793007, -0.67399009],
+                                                   [ 0.52427398, -0.65247056,  0.54719189]])
+
+    @patch('ale.transformation.FrameChain.from_spice', return_value=ale.transformation.FrameChain())
+    @patch("ale.base.type_sensor.Cahvor.cahvor_camera_dict", new_callable=PropertyMock, return_value=cahvor_camera_dict())
+    def test_cahvor_frame_chain(self, cahvor_camera_dict, from_spice):
+      frame_chain = self.driver.frame_chain
+      assert len(frame_chain.nodes()) == 2
+      assert -76220 in frame_chain.nodes()
+      assert -76562 in frame_chain.nodes()
+      from_spice.assert_called_with(center_ephemeris_time=0, ephemeris_times=[0], sensor_frame=-76220, target_frame=10014, nadir=False, exact_ck_times=False)
+      np.testing.assert_allclose(frame_chain[-76562][-76220]['rotation'].quat, [0.0100307131, -0.4757136116, 0.6970899144, 0.5363409323])
+
+    @patch("ale.base.type_sensor.Cahvor.cahvor_camera_dict", new_callable=PropertyMock, return_value=cahvor_camera_dict())
+    def test_cahvor_detector_center_line(self, cahvor_camera_dict):
+        np.testing.assert_almost_equal(self.driver.detector_center_line, 590.1933422831007)
+    
+    @patch("ale.base.type_sensor.Cahvor.cahvor_camera_dict", new_callable=PropertyMock, return_value=cahvor_camera_dict())
+    def test_cahvor_detector_center_sample(self, cahvor_camera_dict):
+        np.testing.assert_almost_equal(self.driver.detector_center_sample, 673.4306859859296)
+
+    @patch("ale.base.type_sensor.Cahvor.cahvor_camera_dict", new_callable=PropertyMock, return_value=cahvor_camera_dict())
+    def test_cahvor_pixel_size(self, cahvor_camera_dict):
+        assert self.driver.pixel_size == 0.007248034226138798
\ No newline at end of file

tests/pytests/test_msl_drivers.py

 import numpy as np
 import unittest
 
-import ale
 from ale.drivers.msl_drivers import MslMastcamPds3NaifSpiceDriver
 
 from conftest import get_image_label
@@ -34,9 +33,16 @@ class test_mastcam_pds_naif(unittest.TestCase):
             assert self.driver.sensor_frame_id == -76562
             bods2c.assert_called_with("MSL_SITE_62")
     
-    # uncomment once cahvor mixin is merged
-    # def test_focal2pixel_lines(self):
-    #     np.testing.assert_allclose(self.driver.focal2pixel_lines, [0, 137968.44341513602, 0])
-
-    # def test_focal2pixel_samples(self):
-    #     np.testing.assert_allclose(self.driver.focal2pixel_samples, [137968.44341513602, 0, 0])
+    def test_focal2pixel_lines(self):
+        with patch('ale.drivers.msl_drivers.spice.bods2c', new_callable=PropertyMock, return_value=-76220) as bods2c, \
+             patch('ale.drivers.msl_drivers.spice.gdpool', new_callable=PropertyMock, return_value=[100]) as gdpool:
+            np.testing.assert_allclose(self.driver.focal2pixel_lines, [0, 137.96844341513602, 0])
+            bods2c.assert_called_with('MSL_MASTCAM_RIGHT')
+            gdpool.assert_called_with('INS-76220_FOCAL_LENGTH', 0, 1)
+
+    def test_focal2pixel_samples(self):
+        with patch('ale.drivers.msl_drivers.spice.bods2c', new_callable=PropertyMock, return_value=-76220) as bods2c, \
+             patch('ale.drivers.msl_drivers.spice.gdpool', new_callable=PropertyMock, return_value=[100]) as gdpool:
+            np.testing.assert_allclose(self.driver.focal2pixel_samples, [137.96844341513602, 0, 0])
+            bods2c.assert_called_with('MSL_MASTCAM_RIGHT')
+            gdpool.assert_called_with('INS-76220_FOCAL_LENGTH', 0, 1)