diff --git a/python/fusion_engine_client/analysis/analyzer.py b/python/fusion_engine_client/analysis/analyzer.py index e232968d..54d66548 100755 --- a/python/fusion_engine_client/analysis/analyzer.py +++ b/python/fusion_engine_client/analysis/analyzer.py @@ -463,10 +463,12 @@ def _calc_stats(input): mode='markers', marker={'color': 'red'}), 1, 1) if dp1_stats is not None: - figure.add_trace(go.Scattergl(x=time, y=dp1_stats['max'], name='P1 Time Interval (Max)', + figure.add_trace(go.Scattergl(x=time, y=dp1_stats['max'], customdata=customdata, + name='P1 Time Interval (Max)', mode='markers', marker={'symbol': 'triangle-up-open'}), 1, 1) - figure.add_trace(go.Scattergl(x=time, y=dp1_stats['min'], name='P1 Time Interval (Min)', + figure.add_trace(go.Scattergl(x=time, y=dp1_stats['min'], customdata=customdata, + name='P1 Time Interval (Min)', mode='markers', marker={'symbol': 'triangle-down-open'}), 1, 1) @@ -474,10 +476,12 @@ def _calc_stats(input): mode='markers', marker={'color': 'green'}), 1, 1) if dgps_stats is not None: - figure.add_trace(go.Scattergl(x=time, y=dgps_stats['max'], name='GPS Time Interval (Max)', + figure.add_trace(go.Scattergl(x=time, y=dgps_stats['max'], customdata=customdata, + name='GPS Time Interval (Max)', mode='markers', marker={'symbol': 'triangle-up-open'}), 1, 1) - figure.add_trace(go.Scattergl(x=time, y=dgps_stats['min'], name='GPS Time Interval (Min)', + figure.add_trace(go.Scattergl(x=time, y=dgps_stats['min'], customdata=customdata, + name='GPS Time Interval (Min)', mode='markers', marker={'symbol': 'triangle-down-open'}), 1, 1) @@ -1353,16 +1357,16 @@ def plot_map(self, mapbox_token, reference: Optional[ReferenceData] = None): # `%{x}` tied to a real date-typed axis) -- but a customdata entry with no format spec at all is substituted # verbatim, so we precompute the UTC string in Python (cheap, vectorized) and reference it that way. _POSITION_HOVERTEMPLATE = ( - "LLA: %{lat:.8f}, %{lon:.8f}, %{customdata[5]:.2f}
" + "LLA: %{lat:.8f}, %{lon:.8f}, %{customdata[6]:.2f}
" "Rel: %{customdata[1]:.3f} sec (P1: %{customdata[2]:.3f} sec)
" "UTC: %{customdata[0]}
" - "GPS: %{customdata[3]:.0f}:%{customdata[4]:.3f}
" - "Std Dev: %{customdata[6]:.2f} m (2D), %{customdata[7]:.2f} m (3D)" + "GPS: %{customdata[3]:.0f}:%{customdata[4]:.3f} (%{customdata[5]:.3f} sec)
" + "Std Dev: %{customdata[7]:.2f} m (2D), %{customdata[8]:.2f} m (3D)" ) # Used instead of _POSITION_HOVERTEMPLATE when reference data is avaiable to compute position error. _POSITION_HOVERTEMPLATE_WITH_ERROR = ( _POSITION_HOVERTEMPLATE + - "
Error: %{customdata[8]:.2f} m (2D), %{customdata[9]:.2f} m (3D)" + "
Error: %{customdata[9]:.2f} m (2D), %{customdata[10]:.2f} m (3D)" ) def _build_position_customdata(p1_time: np.ndarray, gps_time: np.ndarray, lla_deg: np.ndarray, @@ -1384,7 +1388,7 @@ def _build_position_customdata(p1_time: np.ndarray, gps_time: np.ndarray, lla_de # array (that would coerce every column to strings, breaking the numeric %{customdata[N]:.3f}-style # formatting for the rest); build it as a plain list of per-point rows instead. error_enu_m, when # present, is appended after the UTC string so its indices stay fixed regardless of whether it's used. - numeric = np.column_stack((rel_time, p1_time, gps_week, gps_tow_sec, lla_deg[2], + numeric = np.column_stack((rel_time, p1_time, gps_week, gps_tow_sec, gps_time, lla_deg[2], np.linalg.norm(std_enu_m[0:2, :], axis=0), np.linalg.norm(std_enu_m, axis=0))) if error_enu_m is None: @@ -1767,8 +1771,8 @@ def _plot_gnss_skyplot_for_source(self, source_id: int, decimate=True): color_by_sv.append(color_by_prn[sv_id.get_prn()]) color_by_cn0.append(max_cn0_dbhz) - text = ['P1: %.1f sec
(Az, El): (%.2f, %.2f) deg
C/N0: %.1f dB-Hz' % - (t, a, e, c) for t, a, e, c in zip(p1_time, az_deg, el_deg, max_cn0_dbhz)] + text = ['(Az, El): (%.2f, %.2f) deg
C/N0: %.1f dB-Hz
%s' % (a, e, c, time_text) + for a, e, c, time_text in zip(az_deg, el_deg, max_cn0_dbhz, self._time_hover_text(p1_time))] # The per-point P1 time is what the time slider below the plot filters on (see _time_slider_js()). It # is not referenced by the hover text, which uses `text` above. @@ -4402,6 +4406,39 @@ def _time_hover_customdata(self, p1_time: np.ndarray, gps_time: Optional[np.ndar else: return np.vstack((p1_time,)) + def _time_hover_text(self, p1_time: np.ndarray, gps_time: Optional[np.ndarray] = None) -> List[str]: + """! + @brief Build the same Rel/P1/UTC/GPS hover text as `BuildTimeHoverTextFromTimes()` in Python. + + For plots whose hover text is generated ahead of time instead of by the injected hover JS (see + `plotly_data_support.js`). + + @param p1_time The P1 time for each point. + @param gps_time The GPS time for each point, or `None` to compute it from `p1_time`. + + @return A list of hover text strings, one per point, with lines separated by `
`. + """ + # Look up the GPS and UTC times for each point. + if gps_time is None: + gps_time = self.time_provider.p1_to_gps(p1_time) + utc_time = self.time_provider.gps_sec_to_datetime64_array(gps_time) + p1_t0_sec = float(self.t0) if self.t0 is not None else np.nan + + # Format the lines for each point, skipping any time that is not available. + result = [] + for p1, gps, utc in zip(p1_time, gps_time, utc_time): + lines = [] + if not np.isnan(p1): + lines.append(f'Rel: {p1 - p1_t0_sec:.3f} sec (P1: {p1:.3f} sec)') + if not np.isnan(gps): + if not np.isnat(utc): + lines.append(f"UTC: {np.datetime_as_string(utc, unit='ms').replace('T', ' ')}") + week = int(gps // SECONDS_PER_WEEK) + tow_sec = gps - week * SECONDS_PER_WEEK + lines.append(f'GPS: {week}:{tow_sec:.3f} ({gps:.3f} sec)') + result.append('
'.join(lines)) + return result + def _custom_tooltip_js(self, time_source: str = 'p1', precision: Optional[int] = 3, value_label: Optional[str] = None, show_name: bool = True, show_value: bool = True) -> str: """! diff --git a/python/fusion_engine_client/analysis/reference.py b/python/fusion_engine_client/analysis/reference.py index 9059994f..b84d4ee6 100644 --- a/python/fusion_engine_client/analysis/reference.py +++ b/python/fusion_engine_client/analysis/reference.py @@ -16,24 +16,44 @@ _OWN_LOG_STATISTICS = ('first', 'first_fixed', 'median', 'median_fixed') -def _interp_columns(x: np.ndarray, y: Optional[np.ndarray], x_new: np.ndarray) -> Optional[np.ndarray]: +def _interp_columns(x: np.ndarray, y: Optional[np.ndarray], x_new: np.ndarray, + angle_rows: tuple = ()) -> Optional[np.ndarray]: """! - @brief Linearly interpolate each row of `y` (sampled at `x`) onto `x_new`, returning NaN for any query point - outside the range of `x` or where too few valid (non-NaN) samples exist to interpolate. + @brief Linearly interpolate each row of `y` (sampled at `x`) onto `x_new`. + + NaN samples in `y` are skipped, so a query point between two valid samples is interpolated across any NaN gap + between them. The result is NaN for any query point outside the range of `x`, and for every query point in a row + with fewer than 2 valid samples. + + @param x The sample times, sorted in ascending order. + @param y An MxN array of values sampled at `x`, or `None`. + @param x_new The times to interpolate onto. + @param angle_rows Indices of rows in `y` containing angles (in degrees) that wrap at +/-180 degrees. These rows are + unwrapped before interpolating, so the result follows the shorter arc between samples, and the result is + wrapped back to [-180, 180). + + @return An Mx`len(x_new)` array of interpolated values, or `None` if `y` is `None`. """ if y is None: return None + # Values outside the range of the sample times are left as NaN. out = np.full((y.shape[0], len(x_new)), np.nan) in_range = np.logical_and(x_new >= x[0], x_new <= x[-1]) if not np.any(in_range): return out + # Interpolate each row using only its valid samples. for row in range(y.shape[0]): valid = ~np.isnan(y[row, :]) if np.count_nonzero(valid) < 2: continue - out[row, in_range] = np.interp(x_new[in_range], x[valid], y[row, valid]) + + if row in angle_rows: + y_valid = np.degrees(np.unwrap(np.radians(y[row, valid]))) + out[row, in_range] = (np.interp(x_new[in_range], x[valid], y_valid) + 180.0) % 360.0 - 180.0 + else: + out[row, in_range] = np.interp(x_new[in_range], x[valid], y[row, valid]) return out @@ -134,7 +154,8 @@ def get_coverage_mask(self, gps_time_sec: np.ndarray, warn: bool = True) -> np.n "range will be omitted." % (self.description, coverage_percent)) return in_range - def _interpolate_cached(self, key: str, y: np.ndarray, gps_time_sec: np.ndarray) -> np.ndarray: + def _interpolate_cached(self, key: str, y: np.ndarray, gps_time_sec: np.ndarray, + angle_rows: tuple = ()) -> np.ndarray: """! @brief Interpolate `y` onto `gps_time_sec`, caching the result to avoid recomputing for repeated queries. @@ -145,6 +166,7 @@ def _interpolate_cached(self, key: str, y: np.ndarray, gps_time_sec: np.ndarray) @param key A name identifying which field is being interpolated (e.g., 'position'). @param y The data to be interpolated, sampled at `self.gps_time_sec`. @param gps_time_sec The GPS timestamps (sec) to interpolate onto. + @param angle_rows Indices of rows in `y` containing angles (in degrees) that wrap at +/-180 degrees. @return The interpolated data. """ @@ -156,7 +178,7 @@ def _interpolate_cached(self, key: str, y: np.ndarray, gps_time_sec: np.ndarray) else: signature = None - result = _interp_columns(self.gps_time_sec, y, gps_time_sec) + result = _interp_columns(self.gps_time_sec, y, gps_time_sec, angle_rows=angle_rows) if signature is not None: self._interp_cache[key] = (signature, result) return result @@ -190,13 +212,16 @@ def interpolate_ypr_deg(self, gps_time_sec: np.ndarray) -> Optional[np.ndarray]: @brief Interpolate this reference's YPR orientation (deg) onto the specified GPS timestamps (sec). Returns `None` if orientation is not available. Entries outside the covered time range are set to NaN. + + Yaw and roll are interpolated along the shorter arc between samples, so they wrap correctly at +/-180 degrees, + and are returned in the range [-180, 180). Pitch is bounded to +/-90 degrees and is interpolated directly. """ if self.ypr_deg is None: return None elif self.is_stationary: return np.tile(self.ypr_deg.reshape(3, 1), (1, len(gps_time_sec))) else: - return self._interpolate_cached('ypr', self.ypr_deg, gps_time_sec) + return self._interpolate_cached('ypr', self.ypr_deg, gps_time_sec, angle_rows=(0, 2)) # ------------------------------------------------------------------------------------------------------------- # Constructors diff --git a/python/tests/test_reference.py b/python/tests/test_reference.py index f9076c25..c907ec85 100644 --- a/python/tests/test_reference.py +++ b/python/tests/test_reference.py @@ -337,6 +337,44 @@ def test_interpolate_velocity_with_all_nan_row(self, moving_ref): def test_interpolate_ypr_none_when_unavailable(self, moving_ref): assert moving_ref.interpolate_ypr_deg(np.array([5.0])) is None + @staticmethod + def _ypr_ref(ypr_deg): + return ReferenceData(description='test', is_truth=True, + position_ecef_m=np.zeros((3, ypr_deg.shape[1])), + gps_time_sec=np.arange(ypr_deg.shape[1], dtype=float) * 10.0, + ypr_deg=ypr_deg) + + def test_interpolate_ypr_wraps_yaw_and_roll(self): + # Yaw and roll midway between 179 and -179 degrees are 180, not 0. + ref = self._ypr_ref(np.array([[179.0, -179.0], [10.0, 20.0], [-179.0, 179.0]])) + ypr = ref.interpolate_ypr_deg(np.array([2.5, 5.0, 7.5])) + assert ypr[0, :] == pytest.approx([179.5, -180.0, -179.5]) + assert ypr[2, :] == pytest.approx([-179.5, -180.0, 179.5]) + assert ypr[1, :] == pytest.approx([12.5, 15.0, 17.5]) + + def test_interpolate_ypr_without_wrap_is_linear(self): + ref = self._ypr_ref(np.array([[10.0, 50.0], [-80.0, 80.0], [-30.0, 30.0]])) + ypr = ref.interpolate_ypr_deg(np.array([0.0, 5.0, 10.0])) + assert ypr[0, :] == pytest.approx([10.0, 30.0, 50.0]) + assert ypr[1, :] == pytest.approx([-80.0, 0.0, 80.0]) + assert ypr[2, :] == pytest.approx([-30.0, 0.0, 30.0]) + + def test_interpolate_ypr_does_not_unwrap_pitch(self): + # Pitch values far apart are interpolated directly, not along the shorter arc through +/-180. + ref = self._ypr_ref(np.array([[0.0, 0.0], [89.0, -89.0], [0.0, 0.0]])) + ypr = ref.interpolate_ypr_deg(np.array([5.0])) + assert ypr[1, 0] == pytest.approx(0.0) + + def test_interpolate_ypr_nan_handling(self): + # A NaN sample is skipped (the wrap is still followed across the gap), an all-NaN row stays NaN, and query + # times outside the reference are NaN. + ref = self._ypr_ref(np.array([[170.0, np.nan, -170.0], [np.nan, np.nan, np.nan], [0.0, 1.0, 2.0]])) + ypr = ref.interpolate_ypr_deg(np.array([-1.0, 10.0, 21.0])) + assert ypr[0, 1] == pytest.approx(-180.0) + assert np.all(np.isnan(ypr[1, :])) + assert ypr[2, 1] == pytest.approx(1.0) + assert np.all(np.isnan(ypr[:, [0, 2]])) + def test_stationary_interpolation_tiles(self): ref = ReferenceData.from_stationary_ecef(np.array([1.0, 2.0, 3.0])) pos = ref.interpolate_position_ecef_m(np.array([0.0, 1.0, 2.0]))