diff --git a/CHANGELOG.md b/CHANGELOG.md index 6987218a5..3b62e4cf2 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/ale/base/type_sensor.py b/ale/base/type_sensor.py index 8ac6cb520..d5b45c613 100644 --- a/ale/base/type_sensor.py +++ b/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() diff --git a/ale/formatters/usgscsm_formatter.py b/ale/formatters/usgscsm_formatter.py index 2b73d7611..9fbc02b26 100644 --- a/ale/formatters/usgscsm_formatter.py +++ b/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 } diff --git a/ale/isd_generate.py b/ale/isd_generate.py index c535859dd..baabe8671 100755 --- a/ale/isd_generate.py +++ b/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 diff --git a/ale/rotation.py b/ale/rotation.py index eff84455f..19bcde0f8 100644 --- a/ale/rotation.py +++ b/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,7 +364,55 @@ 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. @@ -315,14 +420,26 @@ def reinterpolate(self, times): ---------- 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): """ diff --git a/tests/pytests/test_rotation.py b/tests/pytests/test_rotation.py index c06f7d36c..b96663235 100644 --- a/tests/pytests/test_rotation.py +++ b/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) diff --git a/tests/pytests/test_usgscsm_formatter.py b/tests/pytests/test_usgscsm_formatter.py index b9118fbd9..627d44ca7 100644 --- a/tests/pytests/test_usgscsm_formatter.py +++ b/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'