Add optional Lagrange interpolation of quaternions to the ISD path

CHANGELOG.md

@@ -36,6 +36,7 @@ release.
 ## [Unreleased]
 
 ### Added
+- Added an optional order-8 Lagrange interpolation of quaternions to the ISD generation path, matching ISIS and USGSCSM. It is off by default (SLERP is unchanged) and selected with the `rotation_interpolation` prop.
 - Added order-8 Lagrange interpolation of quaternions to `ale::Orientations`, matching ISIS SpiceRotation, selectable via the new `LAGRANGE_ROTATION` interpolation type. [#726](https://github.com/DOI-USGS/ale/pull/726)
 - Re-enabled and fixed the TGO CaSSIS driver, which now emits the CaSSIS rational distortion. Validated against ISIS to within ~0.013 pixel across 130 framelets of two real stereo pairs. [#720](https://github.com/DOI-USGS/ale/pull/720)
 - `MroHiRisePds3LabelNaifSpiceDriver`, a PDS3 EDR label driver for HiRISE that generates an ISD directly from a raw EDR label without requiring an ISIS cube, paralleling the existing CTX PDS3 driver. [#702](https://github.com/DOI-USGS/ale/pull/702)

ale/base/type_sensor.py

@@ -80,6 +80,22 @@ def exposure_rates(self):
       return start_ets, stop_ets, exposure_durations
 
 
+    @property
+    def rotation_interpolation(self):
+        """
+        The method used to reinterpolate the sensor orientation onto the ISD
+        quaternion grid. Either 'slerp' (default, the historical 2-point spherical
+        linear interpolation) or 'lagrange' (order-8 Lagrange interpolation of the
+        quaternion components, matching ISIS and USGSCSM). Set 'lagrange' via the
+        'rotation_interpolation' prop to obtain an ISIS-consistent reconstruction.
+
+        Returns
+        -------
+        : str
+          The interpolation method, 'slerp' or 'lagrange'.
+        """
+        return self._props.get('rotation_interpolation', 'slerp').lower()
+
     @property
     def ephemeris_time(self):
         """
@@ -97,7 +113,7 @@ def ephemeris_time(self):
           ephemeris times split based on image lines
         """
         if not hasattr(self, "_ephemeris_time"):
-            # Determine reduction mode. Default is "None" (No reduction). 
+            # Determine reduction mode. Default is "None" (No reduction).
             # Set reduction=Linear via props to apply a linear reduction.
             reduction = self._props.get('reduction', 'none').lower()
 

ale/formatters/usgscsm_formatter.py

@@ -150,7 +150,7 @@ def to_usgscsm(driver):
         isd_data['starting_ephemeris_time'] = driver.ephemeris_start_time
         isd_data['center_ephemeris_time'] = center_time
 
-        rotation_interp = sensor_to_target.reinterpolate(interp_times)
+        rotation_interp = sensor_to_target.reinterpolate(interp_times, method=driver.rotation_interpolation)
         isd_data['sensor_orientation'] = {
             'quaternions' : rotation_interp.quats
         }

ale/isd_generate.py

@@ -158,6 +158,17 @@ def main():
         help="Select every Nth ephemeris time when generating an ISD. This should only be set if a linear "
              "reduction is applied."
     )
+    parser.add_argument(
+        "--rotation_interpolation",
+        type=str.lower,
+        choices=['slerp', 'lagrange'],
+        default='slerp',
+        help="Method used to reinterpolate the sensor orientation onto the ISD quaternion grid. "
+             "'slerp' (default) is the historical 2-point spherical linear interpolation. 'lagrange' "
+             "uses order-8 Lagrange interpolation of the quaternion components, matching ISIS and "
+             "USGSCSM, which is more accurate when the rotation rate is not constant. Only applies to "
+             "line scan sensors."
+    )
     args = parser.parse_args()
 
     if (args.reduction != "linear" and args.ephem_sample_rate):
@@ -207,8 +218,9 @@ def main():
                         compress=args.compress, only_isis_spice=args.only_isis_spice, 
                         only_naif_spice=args.only_naif_spice, use_web=args.use_web_spice, 
                         local=args.local, nadir=args.nadir, search_kernels=args.search_kernels,
-                        attach_kernels=args.attach_kernels, reduction=args.reduction, 
-                        ephem_sample_rate=args.ephem_sample_rate)
+                        attach_kernels=args.attach_kernels, reduction=args.reduction,
+                        ephem_sample_rate=args.ephem_sample_rate,
+                        rotation_interpolation=args.rotation_interpolation)
         except Exception as err:
             # Seriously, this just throws a generic Exception?
             sys.exit(f"File {args.input[0]}: {err}")
@@ -228,7 +240,8 @@ def main():
                                        "use_web":args.use_web_spice,
                                        "attach_kernels": args.attach_kernels,
                                        "reduction": args.reduction,
-                                       "ephem_sample_rate": args.ephem_sample_rate}
+                                       "ephem_sample_rate": args.ephem_sample_rate,
+                                       "rotation_interpolation": args.rotation_interpolation}
                 ): f for f in args.input
             }
             for f in concurrent.futures.as_completed(futures):
@@ -257,7 +270,8 @@ def file_to_isd(
     search_kernels=False,
     attach_kernels=False,
     reduction=None,
-    ephem_sample_rate=None):
+    ephem_sample_rate=None,
+    rotation_interpolation='slerp'):
     """
     Returns nothing, but acts as a thin wrapper to take the *file* and generate
     an ISD at *out* (if given, defaults to replacing the extension on *file*
@@ -297,12 +311,15 @@ def file_to_isd(
     if search_kernels: 
         props["search_kernels"] = search_kernels
 
-    if reduction: 
+    if reduction:
         props["reduction"] = reduction
         if reduction == "linear":
             if ephem_sample_rate:
                 props["ephem_sample_rate"] = ephem_sample_rate
 
+    if rotation_interpolation and rotation_interpolation != 'slerp':
+        props["rotation_interpolation"] = rotation_interpolation
+
     if kernels is not None:
         kernels = [str(PurePath(p)) for p in kernels]
         props["kernels"] = kernels

ale/rotation.py

@@ -2,6 +2,63 @@
 
 import numpy as np
 
+def lagrange_interpolate(times, values, time, order=8):
+    """
+    Interpolate a single value with an order-N Lagrange polynomial.
+
+    This is a Python port of ale::lagrangeInterpolate (src/InterpUtils.cpp), so
+    the ALE Python ISD path and the ALE C++ runtime path reconstruct pointing the
+    same way. The window is centered on the query time and clamped symmetrically
+    when there are not enough surrounding nodes, so the effective order degrades
+    to 2/4/6 near the ends and for short caches.
+
+    Parameters
+    ----------
+    times : 1darray
+            The node times, sorted in ascending order.
+    values : 1darray
+             The node values, one per time.
+    time : float
+           The time to interpolate at.
+    order : int
+            The interpolation order. Defaults to 8.
+
+    Returns
+    -------
+     : float
+       The interpolated value.
+    """
+    times = np.asarray(times)
+    values = np.asarray(values)
+    if times.size != values.size:
+        raise ValueError("Times and values must have the same length.")
+
+    # Find the interpolation index, matching ale::interpolationIndex: the first node
+    # strictly after the query time, clamped to the last node, then stepped back one.
+    index = int(np.searchsorted(times, time, side='right'))
+    if index >= times.size:
+        index = times.size - 1
+    if index != 0:
+        index -= 1
+
+    # Symmetric window clamped to the available nodes and to order / 2
+    window = min(index + 1, times.size - index - 1)
+    window = min(window, order // 2)
+    start = index - window + 1
+    end = index + window + 1
+
+    result = 0.0
+    for i in range(start, end):
+        weight = 1.0
+        numerator = 1.0
+        for j in range(start, end):
+            if i == j:
+                continue
+            weight *= times[i] - times[j]
+            numerator *= time - times[j]
+        result += numerator * values[i] / weight
+    return result
+
 class ConstantRotation:
     """
     A constant rotation between two 3D reference frames.
@@ -307,22 +364,82 @@ def _slerp(self, times):
         return interp_rots, interp_av
 
 
-    def reinterpolate(self, times):
+    def _lagrange(self, times, order=8):
+        """
+        Reconstruct the rotation at specific times with order-N Lagrange
+        interpolation of the quaternion components.
+
+        This matches the interpolation ISIS SpiceRotation and USGSCSM use, and
+        is more accurate than SLERP when the rotation rate is not constant,
+        because SLERP is only a 2-point (linear on the sphere) scheme and does
+        not see curvature across the cache. The quaternions are made
+        sign-continuous before interpolation (a quaternion and its negative are
+        the same rotation, but a sign jump between nodes corrupts a component-wise
+        interpolation), and the result is renormalized to unit length.
+
+        Parameters
+        ----------
+        times : 1darray or float
+                The new times to interpolate at.
+        order : int
+                The order of the Lagrange interpolation. Defaults to 8. The window
+                is clamped symmetrically when fewer nodes are available.
+
+        Returns
+        -------
+         : 2darray
+           The interpolated quaternions in scalar last format (x, y, z, w)
+        """
+        vec_times = np.atleast_1d(times)
+        if vec_times.ndim > 1:
+            raise ValueError('Input times must be either a float or a 1d iterable of floats')
+
+        # self.quats already returns sign-continuous quaternions (it flips any
+        # quaternion whose dominant coefficient disagrees with the global sign).
+        node_times = np.asarray(self.times)
+        node_quats = self.quats
+
+        interp_quats = np.empty((len(vec_times), 4))
+        for k, t in enumerate(vec_times):
+            for j in range(4):
+                interp_quats[k, j] = lagrange_interpolate(node_times, node_quats[:, j], t, order)
+
+        # Renormalize each interpolated quaternion to unit length
+        norms = np.linalg.norm(interp_quats, axis=1)
+        norms[norms == 0] = 1.0
+        interp_quats /= norms[:, None]
+
+        return interp_quats
+
+
+    def reinterpolate(self, times, method='slerp'):
         """
         Reinterpolate the rotation at a given set of times.
 
         Parameters
         ----------
         times : 1darray or float
                 The new times to interpolate at.
+        method : str
+                 The interpolation method, either 'slerp' (default) or 'lagrange'.
+                 'slerp' is the historical 2-point spherical linear interpolation.
+                 'lagrange' uses order-8 Lagrange interpolation of the quaternion
+                 components, matching ISIS and USGSCSM, and is more accurate when
+                 the rotation rate is not constant.
 
         Returns
         -------
          : TimeDependentRotation
            The new rotation at the input times
         """
-        new_rots, av = self._slerp(times)
-        return TimeDependentRotation(new_rots.as_quat(), times, self.source, self.dest, av=av)
+        if method == 'lagrange':
+            new_quats = self._lagrange(times)
+            return TimeDependentRotation(new_quats, times, self.source, self.dest)
+        elif method == 'slerp':
+            new_rots, av = self._slerp(times)
+            return TimeDependentRotation(new_rots.as_quat(), times, self.source, self.dest, av=av)
+        else:
+            raise ValueError(f"Unknown interpolation method '{method}', must be 'slerp' or 'lagrange'.")
 
     def __mul__(self, other):
         """

tests/pytests/test_rotation.py

@@ -2,7 +2,7 @@
 
 import numpy as np
 from scipy.spatial.transform import Rotation
-from ale.rotation import ConstantRotation, TimeDependentRotation
+from ale.rotation import ConstantRotation, TimeDependentRotation, lagrange_interpolate
 
 from conftest import compare_quats
 
@@ -206,6 +206,69 @@ def test_reinterpolate():
     assert new_rot.dest == rot.dest
     np.testing.assert_equal(new_rot.times, np.arange(-3, 5))
 
+def test_lagrange_interpolate_reproduces_polynomial():
+    # An order-8 Lagrange interpolation reproduces a low-degree polynomial exactly
+    times = np.linspace(0, 10, 11)
+    values = 2.0 + 0.5 * times - 0.1 * times**2 + 0.01 * times**3
+    for t in [1.3, 4.7, 8.2]:
+        expected = 2.0 + 0.5 * t - 0.1 * t**2 + 0.01 * t**3
+        np.testing.assert_allclose(lagrange_interpolate(times, values, t), expected)
+
+def _wobble_quats(times):
+    # A rotation about the x-axis whose angle theta(t) = sin(t) is not a constant
+    # rate, so SLERP (a 2-point scheme) cannot reproduce it but Lagrange can.
+    rotvec = np.zeros((len(times), 3))
+    rotvec[:, 0] = np.sin(times)
+    return Rotation.from_rotvec(rotvec)
+
+def test_reinterpolate_lagrange_reproduces_nodes():
+    # At the node times, Lagrange reconstruction returns the input rotations exactly
+    times = np.linspace(0, 10, 21)
+    quats = _wobble_quats(times).as_quat()
+    rot = TimeDependentRotation(quats, times, 1, 2)
+    new_rot = rot.reinterpolate(times, method='lagrange')
+    assert compare_quats(new_rot.quats, rot.quats)
+
+def test_reinterpolate_lagrange_more_accurate_than_slerp():
+    # On a rotation whose rate is not constant, Lagrange is far more accurate than
+    # SLERP, because SLERP is only a 2-point scheme and misses the curvature.
+    times = np.linspace(0, 10, 41)
+    quats = _wobble_quats(times).as_quat()
+    rot = TimeDependentRotation(quats, times, 1, 2)
+
+    # Query interior points, where the full order-8 window is available. Near the
+    # ends the window necessarily shrinks (fewer surrounding nodes), as in ISIS.
+    query = np.linspace(1.0, 9.0, 200)
+    truth = _wobble_quats(query)
+
+    lagrange = rot.reinterpolate(query, method='lagrange')
+    slerp = rot.reinterpolate(query, method='slerp')
+
+    lagrange_err = (Rotation.from_quat(lagrange.quats) * truth.inv()).magnitude().max()
+    slerp_err = (Rotation.from_quat(slerp.quats) * truth.inv()).magnitude().max()
+
+    assert lagrange_err < 1e-4
+    assert lagrange_err < slerp_err
+
+def test_reinterpolate_lagrange_sign_flip_invariant():
+    # Negating alternate input quaternions (same rotations, opposite sign branch)
+    # must not change the Lagrange result, because the quaternions are made
+    # sign-continuous before interpolation.
+    times = np.linspace(0, 10, 21)
+    quats = _wobble_quats(times).as_quat()
+    flipped = quats.copy()
+    flipped[::2] *= -1.0
+
+    query = np.linspace(0.05, 9.95, 50)
+    a = TimeDependentRotation(quats, times, 1, 2).reinterpolate(query, method='lagrange')
+    b = TimeDependentRotation(flipped, times, 1, 2).reinterpolate(query, method='lagrange')
+    assert compare_quats(a.quats, b.quats)
+
+def test_reinterpolate_invalid_method():
+    rot = TimeDependentRotation([[0, 0, 0, 1], [0, 0, 0, 1]], [0, 1], 1, 2)
+    with pytest.raises(ValueError):
+        rot.reinterpolate([0.5], method='bogus')
+
 def test_apply_at_single_time():
     test_quats = Rotation.from_euler('x', np.asarray([[-90], [0], [45]]), degrees=True).as_quat()
     rot = TimeDependentRotation(test_quats, [0, 1, 1.5], 1, 2)

tests/pytests/test_usgscsm_formatter.py

@@ -219,6 +219,21 @@ def test_line_scan_name_model(test_line_scan_driver):
     isd = usgscsm_formatter.to_usgscsm(test_line_scan_driver)
     assert isd['name_model'] == 'USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL'
 
+def test_line_scan_rotation_interpolation_default(test_line_scan_driver):
+    # SLERP is the default and the formatter still produces quaternions
+    assert test_line_scan_driver.rotation_interpolation == 'slerp'
+    isd = usgscsm_formatter.to_usgscsm(test_line_scan_driver)
+    assert len(isd['sensor_orientation']['quaternions']) > 0
+
+def test_line_scan_rotation_interpolation_lagrange():
+    # The rotation_interpolation prop selects the Lagrange path and the formatter runs
+    driver = TestLineScanner("", props={'rotation_interpolation': 'lagrange'})
+    assert driver.rotation_interpolation == 'lagrange'
+    isd = usgscsm_formatter.to_usgscsm(driver)
+    quats = np.asarray(isd['sensor_orientation']['quaternions'])
+    assert quats.shape[1] == 4
+    np.testing.assert_allclose(np.linalg.norm(quats, axis=1), 1.0, atol=1e-8)
+
 def test_name_platform(test_frame_driver):
     isd = usgscsm_formatter.to_usgscsm(test_frame_driver)
     assert isd['name_platform'] == 'Test Platform'