diff --git a/ale/base/type_sensor.py b/ale/base/type_sensor.py
index 0e060b1b47fcd7470c395f08f42347465cf5133f..9fa789215cb76611f1483ba6cf80e094aac6233b 100644
--- a/ale/base/type_sensor.py
+++ b/ale/base/type_sensor.py
@@ -1,4 +1,10 @@
+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()
diff --git a/ale/drivers/msl_drivers.py b/ale/drivers/msl_drivers.py
index 37d53fefd61772a8bf581109f59e1358f20f0efb..25ef1d8ca0f4e0dccb3c765494a258af54b4c0f0 100644
--- a/ale/drivers/msl_drivers.py
+++ b/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):
"""
diff --git a/tests/pytests/test_cahvor_mixin.py b/tests/pytests/test_cahvor_mixin.py
new file mode 100644
index 0000000000000000000000000000000000000000..7bd38f645446f98c5cf426816e749a37b7203fbf
--- /dev/null
+++ b/tests/pytests/test_cahvor_mixin.py
@@ -0,0 +1,65 @@
+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
diff --git a/tests/pytests/test_msl_drivers.py b/tests/pytests/test_msl_drivers.py
index 4053cb269279dbb26bf99e5817a3ea0949118a92..7b58c1a3c10b13ba6c7c691cfc7e3aff8bdc84f2 100644
--- a/tests/pytests/test_msl_drivers.py
+++ b/tests/pytests/test_msl_drivers.py
@@ -1,7 +1,6 @@
import numpy as np
import unittest
-import ale
from ale.drivers.msl_drivers import MslMastcamPds3NaifSpiceDriver
from conftest import get_image_label
@@ -33,10 +32,17 @@ class test_mastcam_pds_naif(unittest.TestCase):
with patch('ale.drivers.msl_drivers.spice.bods2c', return_value=-76562) as bods2c:
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)