Commit 47a5925d authored by Jesse Mapel's avatar Jesse Mapel Committed by GitHub
Browse files

Changed usgscsm to reinterpolate positions and rotations (#221) 

* Changed usgscsm to reinterpolate positions and rotations

* cleaned up transposes a little

* Fixed minor bug
parent cd51f014

ale/formatters/usgscsm_formatter.py

+48 −9
Original line number Diff line number Diff line 
import json

import numpy as np

from scipy.interpolate import interp1d, BPoly



from ale.transformation import FrameChain
@@ -45,7 +47,6 @@ def to_usgscsm(driver): 
    frame_chain = driver.frame_chain

    sensor_to_target = frame_chain.compute_rotation(driver.sensor_frame_id, driver.target_frame_id)

    quaternions = sensor_to_target.quats

    rotation_times = sensor_to_target.times

    isd_data['sensor_orientation'] = {

        'quaternions' : quaternions

    }
@@ -81,21 +82,59 @@ def to_usgscsm(driver): 
    if isinstance(driver, LineScanner):

        isd_data['name_model'] = 'USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL'

        isd_data['interpolation_method'] = 'lagrange'



        start_lines, start_times, scan_rates = driver.line_scan_rate

        center_time = driver.center_ephemeris_time

        isd_data['line_scan_rate'] = [[line, time, rate] for line, time, rate in zip(start_lines, start_times, scan_rates)]

        isd_data['starting_ephemeris_time'] = driver.ephemeris_start_time

        isd_data['center_ephemeris_time'] = center_time

        isd_data['t0_ephemeris'] = position_times[0] - center_time

        if len(position_times) > 1:

            isd_data['dt_ephemeris'] = (position_times[-1] - position_times[0]) / (len(position_times) - 1)



        interp_times = np.linspace(position_times[0],

                                   position_times[-1],

                                   int(driver.image_lines / 64))



        if velocities is not None:

            positions = np.asarray(positions)

            velocities = np.asarray(velocities)

            pos_x, pos_y, pos_z = np.asarray(positions).T

            vel_x, vel_y, vel_z = np.asarray(velocities).T

            x_interp = BPoly.from_derivatives(position_times,

                                              np.vstack((pos_x, vel_x)).T,

                                              extrapolate=True)

            y_interp = BPoly.from_derivatives(position_times,

                                              np.vstack((pos_y, vel_y)).T,

                                              extrapolate=True)

            z_interp = BPoly.from_derivatives(position_times,

                                              np.vstack((pos_z, vel_z)).T,

                                              extrapolate=True)

            interp_pos = np.vstack((x_interp(interp_times),

                                    y_interp(interp_times),

                                    z_interp(interp_times))).T

            interp_vel = np.vstack((x_interp(interp_times, nu=1),

                                    y_interp(interp_times, nu=1),

                                    z_interp(interp_times, nu=1))).T

        else:

            isd_data['dt_ephemeris'] = 0

        isd_data['t0_quaternion'] = rotation_times[0] - center_time

        if len(rotation_times) > 1:

            isd_data['dt_quaternion'] = (rotation_times[-1] - rotation_times[0]) / (len(rotation_times) - 1)

            position_interp = interp1d(position_times, positions)

            interp_pos = position_interp(interp_times)

            interp_vel = None

        isd_data['sensor_position'] = {

            'positions' : interp_pos,

            'velocities' : interp_vel,

            'unit' : 'm'

        }



        rotation_interp = sensor_to_target.reinterpolate(interp_times)

        isd_data['sensor_orientation'] = {

            'quaternions' : rotation_interp.quats

        }



        isd_data['t0_ephemeris'] = interp_times[0] - center_time

        if len(interp_times) > 1:

            isd_data['dt_ephemeris'] = (interp_times[-1] - interp_times[0]) / (len(interp_times) - 1)

        else:

            isd_data['dt_quaternion'] = 0

            isd_data['dt_ephemeris'] = 0

        isd_data['t0_quaternion'] = isd_data['t0_ephemeris']

        isd_data['dt_quaternion'] = isd_data['dt_ephemeris']





    # frame sensor model specifics

ale/rotation.py

+17 −0
Original line number Diff line number Diff line
@@ -212,6 +212,23 @@ class TimeDependentRotation: 
        """

        return TimeDependentRotation(self._rots.inv().as_quat(), self.times, self.dest, self.source)



    def reinterpolate(self, times):

        """

        Reinterpolate the rotation at a given set of times.



        Parameters

        ----------

        times : 1darray

                The new times to interpolate at.



        Returns

        -------

         : TimeDependentRotation

           The new rotation that the input times

        """

        new_quats = Slerp(self.times, self._rots)(times).as_quat()

        return TimeDependentRotation(new_quats, times, self.source, self.dest)



    def __mul__(self, other):

        """

        Compose this rotation with another rotation.

tests/pytests/test_usgscsm_formatter.py

+16 −8
Original line number Diff line number Diff line
@@ -23,7 +23,7 @@ class TestDriver(Driver, NaifSpice): 


        body_rotation = TimeDependentRotation(

            np.array([[0, 0, 0, 1], [0, 0, 0, 1]]),

            np.array([0, 1]),

            np.array([800, 900]),

            100,

            1

        )
@@ -31,7 +31,7 @@ class TestDriver(Driver, NaifSpice): 


        spacecraft_rotation = TimeDependentRotation(

            np.array([[0, 0, 0, 1], [0, 0, 0, 1]]),

            np.array([0, 1]),

            np.array([800, 900]),

            1000,

            1

        )
@@ -145,7 +145,7 @@ class TestLineScanner(LineScanner, TestDriver): 
    def sensor_position(self):

        return (

            [[0, 1, 2], [3, 4, 5]],

            [[0, -1, -2], [-3, -4, -5]],

            [[0.03, 0.03, 0.03], [0.03, 0.03, 0.03]],

            [800, 900]

        )
@@ -309,12 +309,12 @@ def test_line_scan_rate(test_line_scan_driver): 
def test_position_times(test_line_scan_driver):

    isd = json.loads(usgscsm_formatter.to_usgscsm(test_line_scan_driver))

    assert isd['t0_ephemeris'] == -50

    assert isd['dt_ephemeris'] == 100

    assert isd['dt_ephemeris'] == 100.0 / 155.0



def test_rotation_times(test_line_scan_driver):

    isd = json.loads(usgscsm_formatter.to_usgscsm(test_line_scan_driver))

    assert isd['t0_quaternion'] == -850

    assert isd['dt_quaternion'] == 1

    assert isd['t0_quaternion'] == -50

    assert isd['dt_quaternion'] == 100.0 / 155.0



def test_interpolation_method(test_line_scan_driver):

    isd = json.loads(usgscsm_formatter.to_usgscsm(test_line_scan_driver))
@@ -323,8 +323,16 @@ def test_interpolation_method(test_line_scan_driver): 
def test_line_scan_sensor_position(test_line_scan_driver):

    isd = json.loads(usgscsm_formatter.to_usgscsm(test_line_scan_driver))

    sensor_position_obj = isd['sensor_position']

    assert sensor_position_obj['positions'] == [[0, 1, 2], [3, 4, 5]]

    assert sensor_position_obj['velocities'] == [[0, -1, -2], [-3, -4, -5]]

    expected_positions = np.vstack((np.linspace(0, 3, 156),

                                    np.linspace(1, 4, 156),

                                    np.linspace(2, 5, 156))).T

    expected_velocities = np.vstack((np.linspace(0.03, 0.03, 156),

                                     np.linspace(0.03, 0.03, 156),

                                     np.linspace(0.03, 0.03, 156))).T

    np.testing.assert_almost_equal(sensor_position_obj['positions'],

                                   expected_positions)

    np.testing.assert_almost_equal(sensor_position_obj['velocities'],

                                   expected_velocities)

    assert sensor_position_obj['unit'] == 'm'



def test_line_scan_sun_position(test_line_scan_driver):