Unverified Commit 063d6ff2 authored by acpaquette's avatar acpaquette Committed by GitHub
Browse files

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
parent 1b5a5969

ale/base/type_sensor.py

+177 −0
Original line number Diff line number Diff line 
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

+2 −1
Original line number Diff line number Diff line
@@ -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

0 → 100644
+65 −0
Original line number Diff line number Diff line 
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

+14 −8
Original line number Diff line number Diff line 
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)