diff --git a/python/fusion_engine_client/analysis/analyzer.py b/python/fusion_engine_client/analysis/analyzer.py
index 72f28889..13c3e06f 100755
--- a/python/fusion_engine_client/analysis/analyzer.py
+++ b/python/fusion_engine_client/analysis/analyzer.py
@@ -1,6 +1,6 @@
#!/usr/bin/env python3
-from typing import Tuple, Union, List, Any
+from typing import Union, List, Any, Optional
from collections import namedtuple, defaultdict
import copy
@@ -26,6 +26,7 @@
from ..messages import *
from .attitude import get_enu_rotation_matrix
from .data_loader import DataLoader, TimeRange
+from .reference import ReferenceData, _OWN_LOG_STATISTICS
from ..parsers.file_index import HostTimeIndexMap
from ..utils import trace as logging
from ..utils.argument_parser import ArgumentParser, ExtendedBooleanAction, TriStateBooleanAction, CSVAction
@@ -938,23 +939,42 @@ def _plot_data(name, idx, marker_style=None):
self._add_figure(name=f"{name}_top_down", figure=topo_figure, title=f"{source}: Top-Down (Topocentric)")
self._add_figure(name=f"{name}_vs_time", figure=time_figure, title=f"{source}: vs. Time")
- def plot_stationary_position_error(self, truth_lla_deg):
+ def plot_pose_error(self, reference: ReferenceData):
"""!
- @brief Plot position error vs. a known stationary location.
+ @brief Plot position error vs. a truth reference.
- @param truth_lla_deg The truth LLA location (in degrees/meters).
+ Position error is plotted both top-down (topocentric) and over time.
+
+ @param reference The truth reference to compare against.
"""
- truth_ecef_m = np.array(geodetic2ecef(*truth_lla_deg, deg=True))
- return self._plot_pose_displacement(title='Position Error', reference=truth_ecef_m)
+ return self._plot_pose_displacement(reference=reference)
- def plot_pose_displacement(self):
+ def plot_position_displacement(self, reference_type: str):
"""!
- @brief Generate a topocentric (top-down) plot of position displacement vs the median position, as well as plot
- of displacement over time.
+ @brief Plot position displacement over time compared with a fixed reference from the log data (first position,
+ median, etc.).
+
+ @param reference_type The desired reference type name.
"""
- return self._plot_pose_displacement(reference='median')
+ # Default to the median position taken from this log's own data (we use median instead of centroid just in
+ # case there are one or two huge outliers).
+ reference = ReferenceData.resolve_cli_argument(reference=reference_type, loader=self.reader,
+ source_id=self.default_source_id)
+ if reference is None:
+ self.logger.info('No valid position solutions detected. Skipping displacement plots.')
+ return None
+ else:
+ return self._plot_pose_displacement(reference=reference)
+
+ def _plot_pose_displacement(self, reference: Optional[ReferenceData] = None):
+ """!
+ @brief Generate a topocentric (top-down) plot of position displacement (or error, if `reference` is an
+ independent truth source -- see @ref ReferenceData) vs a reference position, as well as a plot of
+ displacement/error over time.
- def _plot_pose_displacement(self, title='Pose Displacement', reference=None):
+ @param reference The reference to compare against. If `None`, defaults to the median position taken from
+ this log's own data.
+ """
if self.output_dir is None:
return None
@@ -973,46 +993,39 @@ def _plot_pose_displacement(self, title='Pose Displacement', reference=None):
return None
time = pose_data.p1_time[valid_idx] - float(self.t0)
+ gps_time = pose_data.gps_time[valid_idx]
solution_type = pose_data.solution_type[valid_idx]
lla_deg = pose_data.lla_deg[:, valid_idx]
std_enu_m = pose_data.position_std_enu_m[:, valid_idx]
- # Convert to ENU displacement with respect to the median position (we use median instead of centroid just in
- # case there are one or two huge outliers).
position_ecef_m = np.array(geodetic2ecef(lat=lla_deg[0, :], lon=lla_deg[1, :], alt=lla_deg[2, :], deg=True))
- if reference is None:
- reference = 'median'
-
- if isinstance(reference, str):
- if reference == 'first':
- reference_ecef_m = position_ecef_m[:, 0]
- title_suffix = ' From First Position'
- elif reference == 'median':
- reference_ecef_m = np.median(position_ecef_m, axis=1)
- title_suffix = ' From Median Position'
- elif reference == 'median_fixed':
- idx = solution_type == SolutionType.RTKFixed
- if np.any(idx):
- reference_ecef_m = np.median(position_ecef_m[:, idx], axis=1)
- title_suffix = ' From Median Fixed Position'
- else:
- self.logger.warning('No fixed positions available. Using median as displacement plot reference.')
- reference_ecef_m = np.median(position_ecef_m, axis=1)
- title_suffix = ' From Median Position'
- else:
- raise ValueError('Unrecognized reference specifier.')
- else:
- reference_ecef_m = reference
- title_suffix = ''
-
- displacement_ecef_m = position_ecef_m - reference_ecef_m.reshape(3, 1)
+ # Interpolate the reference position onto this log's GPS timestamps, warning if the reference does not fully
+ # cover this log's time range. Any timestamps that fall outside the reference's coverage (or that could not
+ # be interpolated, e.g. due to a gap in the reference data) are dropped below.
+ valid_ref_idx = reference.get_coverage_mask(gps_time)
+ reference_ecef_m = reference.interpolate_position_ecef_m(gps_time)
+ valid_ref_idx = np.logical_and(valid_ref_idx, ~np.any(np.isnan(reference_ecef_m), axis=0))
+ if not np.any(valid_ref_idx):
+ self.logger.warning(f"Reference data '{reference.description}' does not overlap with this log's time "
+ f"range. Skipping displacement plots.")
+ return None
+ elif not np.all(valid_ref_idx):
+ time = time[valid_ref_idx]
+ solution_type = solution_type[valid_ref_idx]
+ lla_deg = lla_deg[:, valid_ref_idx]
+ std_enu_m = std_enu_m[:, valid_ref_idx]
+ position_ecef_m = position_ecef_m[:, valid_ref_idx]
+ reference_ecef_m = reference_ecef_m[:, valid_ref_idx]
+
+ displacement_ecef_m = position_ecef_m - reference_ecef_m
c_enu_ecef = get_enu_rotation_matrix(*lla_deg[0:2, 0], deg=True)
displacement_enu_m = c_enu_ecef.dot(displacement_ecef_m)
- axis_title = 'Error' if title == 'Position Error' else 'Displacement'
+ axis_title = reference.displacement_label
+ source = f'Position {axis_title} vs. {"Reference" if reference.is_truth else reference.description}'
- self._plot_displacement(source=f'{title}{title_suffix}', title=axis_title,
+ self._plot_displacement(source=source, title=axis_title,
time=time, solution_type=solution_type,
displacement_enu_m=displacement_enu_m, std_enu_m=std_enu_m)
@@ -2876,10 +2889,11 @@ def extract_times_before_reset(self):
return times_before_resets
- def generate_index(self, auto_open=True):
+ def generate_index(self, reference: Optional[ReferenceData] = None, auto_open: bool = True):
"""!
@brief Generate an `index.html` page with links to all generated figures.
+ @param reference Reference data, if loaded.
@param auto_open If `True`, open the page automatically in a web browser.
"""
if len(self.plots) == 0:
@@ -2913,9 +2927,14 @@ def generate_index(self, auto_open=True):
link = '
%s' % (os.path.relpath(entry['path'], index_dir), title)
links += link
+ body = ''
+ if reference is not None:
+ body += f'Reference data source: {reference.description}
'
+ body += links + '\n' + self.summary.replace('\n', '
') + ''
+
index_html = _page_template % {
'title': 'FusionEngine Output',
- 'body': links + '\n' + self.summary.replace('\n', '
') + ''
+ 'body': body
}
os.makedirs(index_dir, exist_ok=True)
@@ -3299,6 +3318,13 @@ def main(args=None):
""")
plot_group = parser.add_argument_group('Plot Control')
+ plot_group.add_argument(
+ '--displacement-type', '--displacement', choices=_OWN_LOG_STATISTICS, default='median_fixed',
+ help="Specify the position statistic to use as a reference for plotting position displacement:"
+ "\n- first - Use the first-available pose solution"
+ "\n- first_fixed - Use the first RTK-fixed pose solution"
+ "\n- median - Use the median pose solution across the entire log"
+ "\n- median_fixed - Use the median pose solution only when RTK-fixed")
plot_group.add_argument('--mapbox-token', metavar='TOKEN',
help="A Mapbox token to use for satellite imagery when generating a map. If unspecified, the token will be "
"read from the MAPBOX_ACCESS_TOKEN or MapboxAccessToken environment variables if set. If no token is "
@@ -3322,9 +3348,20 @@ def main(args=None):
(Analyzer.LONG_LOG_DURATION_SEC / 3600.0, Analyzer.HIGH_MEASUREMENT_RATE_HZ))
plot_group.add_argument(
'--reference', '--truth',
- help="Specify a reference data to use as a truth source for position, velocity, and orientation. Supported "
- "formats:"
- "\n- Stationary LLA position: 37.1234, -122.526335, 102.34")
+ help="Specify reference data to use as a truth source, or as an alternate reference, for position "
+ "displacement/error plots. Supported formats:"
+ "\n- The path to a separate log file, or a log hash/pattern to be located under --log-base-dir, whose "
+ "pose data will be used as a time-varying truth reference"
+ "\n- A stationary LLA (degrees, degrees, meters) or ECEF (meters) position, as 3 comma-separated values. "
+ "All spaces will be ignored."
+ "\n - 37.1234, -122.526335, 102.34"
+ "\n - lla: 37.1234, -122.526335, 102.34"
+ "\n - -2707071.0, -4321671.7, 3817403.2"
+ "\n - ecef: -2707071.0, -4321671.7, 3817403.2")
+ plot_group.add_argument(
+ '--reference-log-type', metavar='TYPE', default='auto',
+ help="If --reference specifies a separate log file/hash, the type of log data to load from it. See "
+ "--log-type for supported values.")
plot_function_names = [n for n in dir(Analyzer) if n.startswith('plot_')]
plot_group.add_argument(
@@ -3422,22 +3459,28 @@ def main(args=None):
_logger.error('Source identifiers must be integers. Exiting.')
sys.exit(1)
- # Parse truth data if specified.
- truth_lla_deg = None
- if options.reference is not None:
- m = re.match(r'^(-?\d+(?:\.\d+)),\s*(-?\d+(?:\.\d+)),\s*(-?\d+(?:\.\d+))$', options.reference)
- if m:
- truth_lla_deg = np.array((float(m.group(1)), float(m.group(2)), float(m.group(3))))
- else:
- _logger.error('Unrecognized reference data format.')
- sys.exit(1)
-
# Read pose data from the file.
analyzer = Analyzer(file=input_path, output_dir=output_dir, ignore_index=options.ignore_index,
prefix=options.prefix + '.' if options.prefix is not None else '',
time_range=time_range, time_axis=options.time_axis,
truncate_long_logs=options.truncate and options.plot is None, source_id=source_id)
+ # Resolve reference data, if specified. This must happen after the analyzer (and its DataLoader) for the primary
+ # log is constructed since some reference types (e.g., 'median') are derived from the primary log's own data.
+ if options.reference is None:
+ ref_path = os.path.join(log_dir, 'reference.p1log')
+ if os.path.exists(ref_path):
+ options.reference = ref_path
+
+ reference_data = None
+ if options.reference is not None:
+ reference_data = ReferenceData.resolve_cli_argument(
+ options.reference, loader=analyzer.reader, log_base_dir=options.log_base_dir,
+ log_type=options.reference_log_type, source_id=analyzer.default_source_id)
+ if reference_data is None:
+ _logger.error('Unable to resolve reference data.')
+ sys.exit(1)
+
if options.plot is None:
analyzer.plot_events()
analyzer.plot_time_scale()
@@ -3447,10 +3490,14 @@ def main(args=None):
analyzer.plot_stationary_status()
analyzer.plot_reset_timing()
analyzer.plot_pose()
- analyzer.plot_pose_displacement()
+ analyzer.plot_position_displacement(reference_type=options.displacement_type)
analyzer.plot_relative_position()
analyzer.plot_map(mapbox_token=options.mapbox_token)
analyzer.plot_calibration()
+
+ if reference_data is not None:
+ analyzer.plot_pose_error(reference=reference_data)
+
analyzer.plot_gnss_cn0()
analyzer.plot_gnss_signal_status()
analyzer.plot_gnss_skyplot()
@@ -3464,9 +3511,6 @@ def main(args=None):
# LG69T-AH), separate from other sensor measurements controlled by --measurements.
analyzer.plot_gnss_attitude_measurements()
- if truth_lla_deg is not None:
- analyzer.plot_stationary_position_error(truth_lla_deg)
-
if options.measurements:
analyzer.plot_imu()
analyzer.plot_wheel_data()
@@ -3500,15 +3544,17 @@ def main(args=None):
analyzer.plot_map(mapbox_token=options.mapbox_token)
elif func == 'plot_skyplot':
analyzer.plot_gnss_skyplot(decimate=False)
- elif func == 'plot_stationary_position_error':
- if truth_lla_deg is not None:
- analyzer.plot_stationary_position_error(truth_lla_deg)
+ elif func == 'plot_pose_error':
+ if reference_data is not None:
+ analyzer.plot_pose_error(reference_data)
else:
- _logger.warning('No truth data available. Cannot plot position error.')
+ _logger.warning('No reference data available. Cannot plot position error.')
+ elif func == 'plot_position_displacement':
+ analyzer.plot_position_displacement(reference_type=options.displacement_type)
else:
getattr(analyzer, func)()
- analyzer.generate_index(auto_open=not options.no_index)
+ analyzer.generate_index(reference=reference_data, auto_open=not options.no_index)
_logger.info("Output stored in '%s'." % os.path.abspath(output_dir))
return analyzer
diff --git a/python/fusion_engine_client/analysis/reference.py b/python/fusion_engine_client/analysis/reference.py
new file mode 100644
index 00000000..df0536d7
--- /dev/null
+++ b/python/fusion_engine_client/analysis/reference.py
@@ -0,0 +1,421 @@
+from typing import Optional, Union
+
+import re
+
+import numpy as np
+from pymap3d import geodetic2ecef
+
+from .data_loader import DataLoader, TimeAlignmentMode
+from ..messages import PoseMessage, PoseAuxMessage, SolutionType
+from ..utils import trace as logging
+from ..utils.log import locate_log, DEFAULT_LOG_BASE_DIR
+
+_logger = logging.getLogger('point_one.fusion_engine.analysis.reference')
+
+_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]:
+ """!
+ @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.
+ """
+ if y is None:
+ return None
+
+ 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
+
+ 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])
+
+ return out
+
+
+class ReferenceData:
+ """!
+ @brief Moving or stationary reference pose data.
+ """
+
+ def __init__(self, description: str, is_truth: bool,
+ position_ecef_m: np.ndarray,
+ gps_time_sec: Optional[np.ndarray] = None,
+ solution_type: Optional[np.ndarray] = None,
+ velocity_enu_mps: Optional[np.ndarray] = None,
+ ypr_deg: Optional[np.ndarray] = None):
+ """!
+ @brief Construct a reference data instance.
+
+ @param description A human-readable description of where this data came from, suitable for display in plot
+ titles (e.g., "Stationary Reference (37.123, -122.456, 12.3)", "Median Position",
+ "Reference Log 'abc123'").
+ @param is_truth If `True`, this data comes from an independent truth source (a stationary location, or a
+ separate reference log). If `False`, this data is derived from the primary log's own data (e.g., its
+ median or first position).
+ @param position_ecef_m A 3-element (stationary) or 3xN (time-varying) ECEF position array, in meters.
+ @param gps_time_sec GPS time of week/epoch (sec) for each time-varying sample, sorted in ascending order.
+ `None` for a stationary reference.
+ @param solution_type Solution type for each time-varying sample. `None` for a stationary reference.
+ @param velocity_enu_mps Optional ENU velocity (3-element or 3xN), in m/s.
+ @param ypr_deg Optional yaw/pitch/roll orientation (3-element or 3xN), in degrees.
+ """
+ self.description = description
+ self.is_truth = is_truth
+ self.position_ecef_m = np.asarray(position_ecef_m, dtype=float)
+ self.gps_time_sec = None if gps_time_sec is None else np.asarray(gps_time_sec, dtype=float)
+ self.solution_type = solution_type
+ self.velocity_enu_mps = velocity_enu_mps
+ self.ypr_deg = ypr_deg
+
+ # Cache of the most recent interpolate_*() result for each field, keyed by the query time vector's start
+ # time, end time, and sample count (not the full time vector) to avoid recomputing for repeated queries.
+ self._interp_cache = {}
+
+ @property
+ def is_stationary(self) -> bool:
+ return self.gps_time_sec is None
+
+ @property
+ def has_velocity(self) -> bool:
+ return self.velocity_enu_mps is not None
+
+ @property
+ def has_orientation(self) -> bool:
+ return self.ypr_deg is not None
+
+ @property
+ def displacement_label(self) -> str:
+ """!
+ @brief 'Error' if this is an independent truth source, or 'Displacement' if derived from the log's own data.
+ """
+ return 'Error' if self.is_truth else 'Displacement'
+
+ def __repr__(self):
+ extent = 'stationary' if self.is_stationary else f'{len(self.gps_time_sec)} samples'
+ return f'ReferenceData({self.description!r}, is_truth={self.is_truth}, {extent})'
+
+ def get_coverage_mask(self, gps_time_sec: np.ndarray, warn: bool = True) -> np.ndarray:
+ """!
+ @brief Determine which of the specified GPS timestamps fall within the time range covered by this reference.
+
+ @param gps_time_sec The GPS timestamps (sec) to be checked.
+ @param warn If `True` and this is a time-varying reference that does not fully cover `gps_time_sec`, log a
+ warning indicating the percentage of the requested range that is covered.
+
+ @return A boolean mask, the same length as `gps_time_sec`, `True` for entries within range.
+ """
+ if self.is_stationary:
+ return np.full(len(gps_time_sec), True)
+
+ in_range = np.logical_and(gps_time_sec >= self.gps_time_sec[0], gps_time_sec <= self.gps_time_sec[-1])
+ if warn and not np.all(in_range):
+ coverage_percent = 100.0 * np.count_nonzero(in_range) / len(in_range)
+ _logger.warning(
+ "Reference data '%s' only covers %.1f%% of the log's time range. Data outside the reference time "
+ "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:
+ """!
+ @brief Interpolate `y` onto `gps_time_sec`, caching the result to avoid recomputing for repeated queries.
+
+ The cache is keyed on the query time vector's start time, end time, and sample count (not the full time
+ vector), so a repeated call with an equivalent query (e.g., the same reference reused across multiple
+ plots) is served from cache instead of re-interpolated.
+
+ @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.
+
+ @return The interpolated data.
+ """
+ if len(gps_time_sec) > 0:
+ signature = (gps_time_sec[0], gps_time_sec[-1], len(gps_time_sec))
+ cached_signature, cached_result = self._interp_cache.get(key, (None, None))
+ if signature == cached_signature:
+ return cached_result
+ else:
+ signature = None
+
+ result = _interp_columns(self.gps_time_sec, y, gps_time_sec)
+ if signature is not None:
+ self._interp_cache[key] = (signature, result)
+ return result
+
+ def interpolate_position_ecef_m(self, gps_time_sec: np.ndarray) -> np.ndarray:
+ """!
+ @brief Interpolate this reference's ECEF position (m) onto the specified GPS timestamps (sec).
+
+ Entries outside the time range covered by this reference are set to NaN.
+ """
+ if self.is_stationary:
+ return np.tile(self.position_ecef_m.reshape(3, 1), (1, len(gps_time_sec)))
+ else:
+ return self._interpolate_cached('position', self.position_ecef_m, gps_time_sec)
+
+ def interpolate_velocity_enu_mps(self, gps_time_sec: np.ndarray) -> Optional[np.ndarray]:
+ """!
+ @brief Interpolate this reference's ENU velocity (m/s) onto the specified GPS timestamps (sec).
+
+ Returns `None` if velocity is not available. Entries outside the covered time range are set to NaN.
+ """
+ if self.velocity_enu_mps is None:
+ return None
+ elif self.is_stationary:
+ return np.tile(self.velocity_enu_mps.reshape(3, 1), (1, len(gps_time_sec)))
+ else:
+ return self._interpolate_cached('velocity', self.velocity_enu_mps, gps_time_sec)
+
+ 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.
+ """
+ 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)
+
+ # -------------------------------------------------------------------------------------------------------------
+ # Constructors
+ # -------------------------------------------------------------------------------------------------------------
+
+ @classmethod
+ def from_stationary_lla(cls, lla_deg: np.ndarray) -> 'ReferenceData':
+ """!
+ @brief Create a stationary truth reference from an LLA position (deg, deg, m).
+ """
+ lla_deg = np.asarray(lla_deg, dtype=float)
+ position_ecef_m = np.array(geodetic2ecef(*lla_deg, deg=True))
+ description = 'Stationary Reference (%.8f, %.8f, %.2f)' % tuple(lla_deg)
+ return cls(description=description, is_truth=True, position_ecef_m=position_ecef_m)
+
+ @classmethod
+ def from_stationary_ecef(cls, position_ecef_m: np.ndarray) -> 'ReferenceData':
+ """!
+ @brief Create a stationary truth reference from an ECEF position (m).
+ """
+ position_ecef_m = np.asarray(position_ecef_m, dtype=float)
+ description = 'Stationary Reference (ECEF %.2f, %.2f, %.2f)' % tuple(position_ecef_m)
+ return cls(description=description, is_truth=True, position_ecef_m=position_ecef_m)
+
+ @classmethod
+ def from_own_log(cls, loader: DataLoader, statistic: str, source_id: Optional[int] = None,
+ params: Optional[dict] = None) -> Optional['ReferenceData']:
+ """!
+ @brief Create a reference derived from the primary log's own pose data.
+
+ This is *not* an independent truth source -- it is simply a statistic computed from the same data being
+ analyzed, so comparisons against it should be labeled as "displacement", not "error".
+
+ @param loader The @ref DataLoader for the log being analyzed.
+ @param statistic One of 'first', 'first_fixed', 'median', or 'median_fixed'.
+ @param source_id If specified, only consider data from this source ID.
+ @param params Additional keyword arguments to forward to `loader.read()` (e.g., `time_range`).
+
+ @return A @ref ReferenceData instance, or `None` if no suitable data was found.
+ """
+ if statistic not in _OWN_LOG_STATISTICS:
+ raise ValueError(f"Unrecognized own-log reference statistic '{statistic}'.")
+
+ params = dict(params or {})
+ params.setdefault('return_numpy', True)
+ params.setdefault('show_progress', True)
+ source_ids = None if source_id is None else [source_id]
+ result = loader.read(message_types=[PoseMessage, PoseAuxMessage], source_ids=source_ids,
+ time_align=TimeAlignmentMode.INSERT, **params)
+ pose_data = result[PoseMessage.MESSAGE_TYPE]
+ aux_data = result[PoseAuxMessage.MESSAGE_TYPE]
+
+ valid_idx = np.logical_and(~np.isnan(pose_data.p1_time), pose_data.solution_type != SolutionType.Invalid)
+ if not np.any(valid_idx):
+ _logger.warning('No valid position solutions available in log. Cannot generate own-log reference.')
+ return None
+
+ if statistic == 'first':
+ selected_idx = np.array([np.flatnonzero(valid_idx)[0]])
+ description = 'First Position'
+ elif statistic == 'first_fixed':
+ fixed_idx = np.logical_and(valid_idx, pose_data.solution_type == SolutionType.RTKFixed)
+ if np.any(fixed_idx):
+ selected_idx = np.array([np.flatnonzero(fixed_idx)[0]])
+ description = 'First Fixed Position'
+ else:
+ _logger.warning('No fixed positions available. Using first instead.')
+ selected_idx = np.array([np.flatnonzero(valid_idx)[0]])
+ description = 'First Position'
+ elif statistic == 'median_fixed':
+ fixed_idx = np.logical_and(valid_idx, pose_data.solution_type == SolutionType.RTKFixed)
+ if np.any(fixed_idx):
+ selected_idx = np.flatnonzero(fixed_idx)
+ description = 'Median Fixed Position'
+ else:
+ _logger.warning('No fixed positions available. Using median of all valid positions instead.')
+ selected_idx = np.flatnonzero(valid_idx)
+ description = 'Median Position'
+ elif statistic == 'median':
+ selected_idx = np.flatnonzero(valid_idx)
+ description = 'Median Position'
+ else:
+ raise ValueError(f"Unrecognized statistic type '{statistic}'.")
+
+ lla_deg = pose_data.lla_deg[:, selected_idx]
+ position_ecef_m = np.array(geodetic2ecef(lat=lla_deg[0, :], lon=lla_deg[1, :], alt=lla_deg[2, :], deg=True))
+ velocity_enu_mps = aux_data.velocity_enu_mps[:, selected_idx]
+ ypr_deg = pose_data.ypr_deg[:, selected_idx]
+
+ if statistic in ('first', 'first_fixed'):
+ position_ecef_m = position_ecef_m[:, 0]
+ velocity_enu_mps = None if np.any(np.isnan(velocity_enu_mps[:, 0])) else velocity_enu_mps[:, 0]
+ ypr_deg = None if np.any(np.isnan(ypr_deg[:, 0])) else ypr_deg[:, 0]
+ else:
+ position_ecef_m = np.median(position_ecef_m, axis=1)
+ with np.errstate(invalid='ignore'):
+ velocity_enu_mps = (None if np.all(np.isnan(velocity_enu_mps))
+ else np.nanmedian(velocity_enu_mps, axis=1))
+ ypr_deg = None if np.all(np.isnan(ypr_deg)) else np.nanmedian(ypr_deg, axis=1)
+
+ return cls(description=description, is_truth=False, position_ecef_m=position_ecef_m,
+ velocity_enu_mps=velocity_enu_mps, ypr_deg=ypr_deg)
+
+ @classmethod
+ def from_reference_log(cls, path_or_loader: Union[str, DataLoader], log_base_dir: str = None,
+ log_type: str = 'auto', source_id: Optional[int] = None,
+ params: Optional[dict] = None) -> Optional['ReferenceData']:
+ """!
+ @brief Load time-varying truth reference data (position, velocity, orientation) from a separate log.
+
+ @param path_or_loader A @ref DataLoader instance, or a path/log pattern/hash identifying the reference log.
+ @param log_base_dir The base directory to search when resolving a log hash/pattern. Defaults to
+ @ref DEFAULT_LOG_BASE_DIR.
+ @param log_type The type of log data to load (see the `--log-type` help text).
+ @param source_id If specified, only consider data from this source ID within the reference log.
+ @param params Additional keyword arguments to forward to `loader.read()` (e.g., `time_range`).
+
+ @return A @ref ReferenceData instance, or `None` if the log could not be loaded, or contained no valid pose
+ data.
+ """
+ if isinstance(path_or_loader, DataLoader):
+ loader = path_or_loader
+ description = "Reference Log '%s'" % loader.get_input_path()
+ else:
+ if log_base_dir is None:
+ log_base_dir = DEFAULT_LOG_BASE_DIR
+
+ input_path, log_id = locate_log(input_path=path_or_loader, log_base_dir=log_base_dir,
+ return_output_dir=False, return_log_id=True, log_type=log_type)
+ if input_path is None:
+ # locate_log() will log an error.
+ return None
+
+ loader = DataLoader(input_path)
+ description = "Reference Log %s" % (log_id if log_id is not None else input_path)
+
+ params = dict(params or {})
+ params.setdefault('return_numpy', True)
+ params.setdefault('show_progress', True)
+ source_ids = None if source_id is None else [source_id]
+ result = loader.read(message_types=[PoseMessage, PoseAuxMessage], source_ids=source_ids,
+ time_align=TimeAlignmentMode.INSERT, **params)
+ pose_data = result[PoseMessage.MESSAGE_TYPE]
+ aux_data = result[PoseAuxMessage.MESSAGE_TYPE]
+
+ valid_idx = np.logical_and(~np.isnan(pose_data.p1_time), pose_data.solution_type != SolutionType.Invalid)
+ if not np.any(valid_idx):
+ _logger.warning("No valid position solutions available in reference log '%s'." % description)
+ return None
+
+ # Sort by GPS time since it is the only time base comparable across two independently recorded logs (P1 time
+ # is monotonic but device- and boot-specific, and is not meaningful when comparing two different logs).
+ gps_time_sec = pose_data.gps_time[valid_idx]
+ order = np.argsort(gps_time_sec)
+
+ gps_time_sec = gps_time_sec[order]
+ solution_type = pose_data.solution_type[valid_idx][order]
+ lla_deg = pose_data.lla_deg[:, valid_idx][:, order]
+ position_ecef_m = np.array(geodetic2ecef(lat=lla_deg[0, :], lon=lla_deg[1, :], alt=lla_deg[2, :], deg=True))
+ velocity_enu_mps = aux_data.velocity_enu_mps[:, valid_idx][:, order]
+ ypr_deg = pose_data.ypr_deg[:, valid_idx][:, order]
+
+ return cls(description=description, is_truth=True, position_ecef_m=position_ecef_m,
+ gps_time_sec=gps_time_sec, solution_type=solution_type, velocity_enu_mps=velocity_enu_mps,
+ ypr_deg=ypr_deg)
+
+ # -------------------------------------------------------------------------------------------------------------
+ # CLI argument parsing
+ # -------------------------------------------------------------------------------------------------------------
+
+ # An optional "lla:"/"ecef:" prefix followed by 3 comma-separated values (decimal point optional on each value,
+ # e.g., "37, -122, 10", "lla: 37.1234, -122.5, 10.2", or "ecef: -2707071.0, -4321671.7, 3817403.2"). If the
+ # prefix is omitted, the coordinate type is inferred from the magnitude of the values (see
+ # `_ECEF_MAGNITUDE_THRESHOLD_M`).
+ _COORD_REGEX = re.compile(
+ r'^(?:(lla|ecef)\s*:\s*)?(-?\d+(?:\.\d+)?),\s*(-?\d+(?:\.\d+)?),\s*(-?\d+(?:\.\d+)?)$', re.IGNORECASE)
+
+ # ECEF coordinates are always within a few hundred km of Earth's radius (~6.37e6 m) in magnitude, regardless of
+ # location, while LLA values (even treated naively as a raw 3-vector of degrees/degrees/meters) never approach
+ # that magnitude. This threshold sits comfortably between the two.
+ _ECEF_MAGNITUDE_THRESHOLD_M = 1.0e5
+
+ @classmethod
+ def _is_ecef_magnitude(cls, values: np.ndarray) -> bool:
+ return np.linalg.norm(values) > cls._ECEF_MAGNITUDE_THRESHOLD_M
+
+ @classmethod
+ def resolve_cli_argument(cls, reference: Optional[str],
+ loader: Optional[DataLoader] = None,
+ log_base_dir: str = None, log_type: str = 'auto',
+ source_id: Optional[int] = None) -> Optional['ReferenceData']:
+ """!
+ @brief Parse and resolve the `--reference` command-line argument into a @ref ReferenceData instance.
+
+ Supported formats:
+ - The path to a separate log file, or a log hash/pattern to be located under `log_base_dir`, whose pose data
+ will be used as a time-varying truth reference
+ - A stationary position, as 3 comma-separated values. LLA (degrees, degrees, meters) vs. ECEF (meters) is
+ inferred automatically from the magnitude of the values, or may be specified explicitly with an `lla:` or
+ `ecef:` prefix:
+ - LLA: `37.1234, -122.526335, 102.34`
+ - LLA: `lla: 37.1234, -122.526335, 102.34`
+ - ECEF: `-2707071.0, -4321671.7, 3817403.2`
+ - ECEF: `ecef: -2707071.0, -4321671.7, 3817403.2`
+ - A statistic computed from the log's own data:
+ - `first` - Use the first-available pose solution
+ - `first_fixed` - Use the first RTK-fixed pose solution
+ - `median` - Use the median pose solution across the entire log
+ - `median_fixed` - Use the median pose solution only when RTK-fixed
+
+ @param reference The `--reference` argument string, or `None`.
+ @param loader The @ref DataLoader for the primary log being analyzed. Used to resolve the `first`, `median`,
+ etc. statistics.
+ @param log_base_dir The base directory to search if `reference` specifies a log hash/pattern.
+ @param log_type The type of log data to load if `reference` specifies a separate log file.
+ @param source_id If specified, restrict reference data to this source ID.
+
+ @return A @ref ReferenceData instance, or `None` if `reference` is `None`, or could not be resolved.
+ """
+ if reference is None:
+ return None
+
+ m = cls._COORD_REGEX.match(reference)
+ if m:
+ prefix = m.group(1)
+ values = np.array([float(m.group(i)) for i in (2, 3, 4)])
+ is_ecef = cls._is_ecef_magnitude(values) if prefix is None else prefix.lower() == 'ecef'
+ return cls.from_stationary_ecef(values) if is_ecef else cls.from_stationary_lla(values)
+
+ reference_stat = reference.lower().replace('-', '_')
+ if reference_stat in _OWN_LOG_STATISTICS:
+ if loader is None:
+ raise ValueError(f'Data log not available. Cannot compute {reference_stat} statistic.')
+ return cls.from_own_log(loader, statistic=reference_stat, source_id=source_id)
+
+ return cls.from_reference_log(reference, log_base_dir=log_base_dir, log_type=log_type, source_id=source_id)
diff --git a/python/tests/test_reference.py b/python/tests/test_reference.py
new file mode 100644
index 00000000..7fff1b77
--- /dev/null
+++ b/python/tests/test_reference.py
@@ -0,0 +1,347 @@
+import json
+
+import numpy as np
+import pytest
+from pymap3d import geodetic2ecef
+
+from fusion_engine_client.analysis.data_loader import DataLoader
+from fusion_engine_client.analysis import reference as reference_module
+from fusion_engine_client.analysis.reference import ReferenceData
+from fusion_engine_client.messages import PoseMessage, PoseAuxMessage, SolutionType, Timestamp
+from fusion_engine_client.parsers import FusionEngineEncoder
+
+# A small, deterministic set of pose epochs used across several tests:
+# index 0: Invalid solution -- must be excluded from every statistic.
+# index 1: AutonomousGPS, has PoseAux data -- "first" valid epoch.
+# index 2: RTKFloat, PoseAux missing (tests NaN gap handling via TimeAlignmentMode.INSERT).
+# index 3: RTKFixed.
+# index 4: RTKFixed.
+_EPOCHS = [
+ dict(p1_time=1.0, gps_time=1000.0, solution_type=SolutionType.Invalid,
+ lla_deg=(0.0, 0.0, 0.0), ypr_deg=None, velocity_enu_mps=None),
+ dict(p1_time=2.0, gps_time=1001.0, solution_type=SolutionType.AutonomousGPS,
+ lla_deg=(37.0, -122.0, 10.0), ypr_deg=(10.0, 1.0, 2.0), velocity_enu_mps=(1.0, 0.1, 0.0)),
+ dict(p1_time=3.0, gps_time=1002.0, solution_type=SolutionType.RTKFloat,
+ lla_deg=(37.1, -122.1, 11.0), ypr_deg=(20.0, 2.0, 3.0), velocity_enu_mps=None),
+ dict(p1_time=4.0, gps_time=1003.0, solution_type=SolutionType.RTKFixed,
+ lla_deg=(37.2, -122.2, 12.0), ypr_deg=(30.0, 3.0, 4.0), velocity_enu_mps=(3.0, 0.3, 0.0)),
+ dict(p1_time=5.0, gps_time=1004.0, solution_type=SolutionType.RTKFixed,
+ lla_deg=(37.3, -122.3, 13.0), ypr_deg=(40.0, 4.0, 5.0), velocity_enu_mps=(4.0, 0.4, 0.0)),
+]
+
+
+def _build_messages(epochs):
+ messages = []
+ for epoch in epochs:
+ pose = PoseMessage()
+ pose.p1_time = Timestamp(epoch['p1_time'])
+ pose.gps_time = Timestamp(epoch['gps_time'])
+ pose.solution_type = epoch['solution_type']
+ pose.lla_deg = np.array(epoch['lla_deg'], dtype=float)
+ if epoch['ypr_deg'] is not None:
+ pose.ypr_deg = np.array(epoch['ypr_deg'], dtype=float)
+ messages.append(pose)
+
+ if epoch['velocity_enu_mps'] is not None:
+ aux = PoseAuxMessage()
+ aux.p1_time = Timestamp(epoch['p1_time'])
+ aux.velocity_enu_mps = np.array(epoch['velocity_enu_mps'], dtype=float)
+ messages.append(aux)
+
+ return messages
+
+
+def _write_log(path, epochs):
+ encoder = FusionEngineEncoder()
+ with open(path, 'wb') as f:
+ for message in _build_messages(epochs):
+ f.write(encoder.encode_message(message))
+
+
+@pytest.fixture
+def loader(tmp_path):
+ path = tmp_path / 'test_file.p1log'
+ _write_log(path, _EPOCHS)
+ return DataLoader(path=str(path))
+
+
+def _expected_ecef(*indices):
+ lla = np.array([_EPOCHS[i]['lla_deg'] for i in indices]).T
+ return np.array(geodetic2ecef(lat=lla[0, :], lon=lla[1, :], alt=lla[2, :], deg=True))
+
+
+class TestProperties:
+ def test_stationary(self):
+ ref = ReferenceData.from_stationary_lla(np.array([37.0, -122.0, 10.0]))
+ assert ref.is_stationary
+ assert not ref.has_velocity
+ assert not ref.has_orientation
+ assert ref.is_truth
+ assert ref.displacement_label == 'Error'
+
+ def test_own_log_is_not_truth(self, loader):
+ ref = ReferenceData.from_own_log(loader, statistic='median')
+ assert not ref.is_truth
+ assert ref.displacement_label == 'Displacement'
+
+ def test_reference_log_is_truth(self, loader):
+ ref = ReferenceData.from_reference_log(loader)
+ assert ref.is_truth
+ assert ref.displacement_label == 'Error'
+ assert not ref.is_stationary
+
+
+class TestStationaryConstructors:
+ def test_from_stationary_lla(self):
+ lla_deg = np.array([37.1234, -122.526335, 102.34])
+ ref = ReferenceData.from_stationary_lla(lla_deg)
+ expected_ecef_m = np.array(geodetic2ecef(*lla_deg, deg=True))
+ assert ref.position_ecef_m == pytest.approx(expected_ecef_m)
+ assert ref.is_truth
+ assert ref.gps_time_sec is None
+ assert '37.12340000' in ref.description
+
+ def test_from_stationary_ecef(self):
+ position_ecef_m = np.array([-2707071.0, -4321671.7, 3817403.2])
+ ref = ReferenceData.from_stationary_ecef(position_ecef_m)
+ assert ref.position_ecef_m == pytest.approx(position_ecef_m)
+ assert ref.is_truth
+ assert 'ECEF' in ref.description
+
+
+class TestFromOwnLog:
+ def test_median(self, loader):
+ ref = ReferenceData.from_own_log(loader, statistic='median')
+ assert not ref.is_truth
+ assert ref.description == 'Median Position'
+
+ expected_ecef_m = np.median(_expected_ecef(1, 2, 3, 4), axis=1)
+ assert ref.position_ecef_m == pytest.approx(expected_ecef_m)
+
+ # Epoch 2 (index 2) has no PoseAux data; the other 3 valid epochs do, so the median velocity should still be
+ # well-defined via nanmedian.
+ assert ref.has_velocity
+ expected_velocity = np.nanmedian(
+ np.array([[1.0, 0.1, 0.0], [np.nan, np.nan, np.nan], [3.0, 0.3, 0.0], [4.0, 0.4, 0.0]]).T, axis=1)
+ assert ref.velocity_enu_mps == pytest.approx(expected_velocity)
+
+ assert ref.has_orientation
+ expected_ypr = np.median(np.array([e['ypr_deg'] for e in _EPOCHS[1:]]).T, axis=1)
+ assert ref.ypr_deg == pytest.approx(expected_ypr)
+
+ def test_first(self, loader):
+ ref = ReferenceData.from_own_log(loader, statistic='first')
+ assert ref.description == 'First Position'
+
+ expected_ecef_m = _expected_ecef(1)[:, 0]
+ assert ref.position_ecef_m == pytest.approx(expected_ecef_m)
+
+ # The first valid epoch (index 1) has both orientation and velocity data.
+ assert ref.has_orientation
+ assert ref.ypr_deg == pytest.approx(np.array(_EPOCHS[1]['ypr_deg']))
+ assert ref.has_velocity
+ assert ref.velocity_enu_mps == pytest.approx(np.array(_EPOCHS[1]['velocity_enu_mps']))
+
+ def test_median_fixed(self, loader):
+ ref = ReferenceData.from_own_log(loader, statistic='median_fixed')
+ assert ref.description == 'Median Fixed Position'
+
+ expected_ecef_m = np.median(_expected_ecef(3, 4), axis=1)
+ assert ref.position_ecef_m == pytest.approx(expected_ecef_m)
+ assert ref.has_velocity
+
+ def test_median_fixed_falls_back_when_no_fixed_solutions(self, tmp_path, monkeypatch):
+ epochs = [e for e in _EPOCHS if e['solution_type'] != SolutionType.RTKFixed]
+ path = tmp_path / 'no_fixed.p1log'
+ _write_log(path, epochs)
+ no_fixed_loader = DataLoader(path=str(path))
+
+ warnings = []
+ monkeypatch.setattr(reference_module._logger, 'warning', lambda msg: warnings.append(msg))
+
+ ref = ReferenceData.from_own_log(no_fixed_loader, statistic='median_fixed')
+ assert ref.description == 'Median Position'
+ assert any('No fixed positions' in w for w in warnings)
+
+ def test_no_valid_solutions_returns_none(self, tmp_path):
+ epochs = [dict(e, solution_type=SolutionType.Invalid) for e in _EPOCHS]
+ path = tmp_path / 'all_invalid.p1log'
+ _write_log(path, epochs)
+ invalid_loader = DataLoader(path=str(path))
+
+ assert ReferenceData.from_own_log(invalid_loader, statistic='median') is None
+
+ def test_invalid_statistic_raises(self, loader):
+ with pytest.raises(ValueError):
+ ReferenceData.from_own_log(loader, statistic='bogus')
+
+
+class TestFromReferenceLog:
+ def test_loads_time_varying_data(self, loader):
+ ref = ReferenceData.from_reference_log(loader)
+ assert ref.is_truth
+ assert not ref.is_stationary
+
+ # 4 valid epochs (index 0 is Invalid and excluded).
+ assert len(ref.gps_time_sec) == 4
+ assert list(ref.gps_time_sec) == sorted(ref.gps_time_sec)
+ assert ref.gps_time_sec[0] == pytest.approx(1001.0)
+ assert ref.gps_time_sec[-1] == pytest.approx(1004.0)
+
+ expected_ecef_m = _expected_ecef(1, 2, 3, 4)
+ assert ref.position_ecef_m == pytest.approx(expected_ecef_m)
+
+ assert ref.has_velocity
+ # Epoch at gps_time=1002 (index 2) has no PoseAux data -> NaN gap.
+ assert np.all(np.isnan(ref.velocity_enu_mps[:, 1]))
+ assert ref.velocity_enu_mps[:, 0] == pytest.approx(np.array(_EPOCHS[1]['velocity_enu_mps']))
+
+ def test_no_valid_solutions_returns_none(self, tmp_path):
+ epochs = [dict(e, solution_type=SolutionType.Invalid) for e in _EPOCHS]
+ path = tmp_path / 'all_invalid.p1log'
+ _write_log(path, epochs)
+ invalid_loader = DataLoader(path=str(path))
+
+ assert ReferenceData.from_reference_log(invalid_loader) is None
+
+ def test_unresolvable_path_returns_none(self, tmp_path):
+ empty_log_base = tmp_path / 'log_base'
+ empty_log_base.mkdir()
+ assert ReferenceData.from_reference_log('no_such_log', log_base_dir=str(empty_log_base)) is None
+
+ def test_path_or_hash_resolution(self, tmp_path):
+ # Build /2024-01-15/abc123hash/fusion_engine.p1log with a manifest, mirroring a real FE log
+ # directory layout, and confirm from_reference_log() can locate and load it by log ID.
+ log_base = tmp_path / 'log_base'
+ log_dir = log_base / '2024-01-15' / 'abc123hash'
+ log_dir.mkdir(parents=True)
+ _write_log(log_dir / 'fusion_engine.p1log', _EPOCHS)
+ with open(log_dir / 'manifest.json', 'w') as f:
+ json.dump({'channels': ['fusion_engine.p1log'], 'guid': 'abc123hash'}, f)
+
+ ref = ReferenceData.from_reference_log('abc123hash', log_base_dir=str(log_base))
+ assert ref is not None
+ assert ref.is_truth
+ assert 'abc123hash' in ref.description
+ assert len(ref.gps_time_sec) == 4
+
+
+class TestInterpolation:
+ @pytest.fixture
+ def moving_ref(self):
+ return ReferenceData(description='test', is_truth=True,
+ position_ecef_m=np.array([[0.0, 10.0], [0.0, 20.0], [0.0, 30.0]]),
+ gps_time_sec=np.array([0.0, 10.0]),
+ velocity_enu_mps=np.array([[1.0, 3.0], [2.0, 4.0], [np.nan, np.nan]]))
+
+ def test_interpolate_midpoint(self, moving_ref):
+ pos = moving_ref.interpolate_position_ecef_m(np.array([5.0]))
+ assert pos[:, 0] == pytest.approx([5.0, 10.0, 15.0])
+
+ def test_interpolation_is_cached(self, moving_ref, monkeypatch):
+ original = reference_module._interp_columns
+ calls = []
+ monkeypatch.setattr(reference_module, '_interp_columns',
+ lambda *args, **kwargs: calls.append(1) or original(*args, **kwargs))
+
+ query = np.array([5.0])
+
+ # A repeated query with the same start/end/count is served from cache -- no additional call.
+ first = moving_ref.interpolate_position_ecef_m(query)
+ second = moving_ref.interpolate_position_ecef_m(query)
+ assert len(calls) == 1
+ assert first is second
+
+ # A different field has its own cache entry -- computing velocity doesn't disturb position's cache.
+ moving_ref.interpolate_velocity_enu_mps(query)
+ assert len(calls) == 2
+ moving_ref.interpolate_position_ecef_m(query)
+ assert len(calls) == 2
+
+ # A query with a different start/end/count is not a cache hit.
+ moving_ref.interpolate_position_ecef_m(np.array([0.0, 10.0]))
+ assert len(calls) == 3
+
+ def test_interpolate_out_of_range_is_nan(self, moving_ref):
+ pos = moving_ref.interpolate_position_ecef_m(np.array([-5.0, 15.0]))
+ assert np.all(np.isnan(pos))
+
+ def test_interpolate_velocity_with_all_nan_row(self, moving_ref):
+ vel = moving_ref.interpolate_velocity_enu_mps(np.array([5.0]))
+ assert vel[0, 0] == pytest.approx(2.0)
+ assert vel[1, 0] == pytest.approx(3.0)
+ # The third row is entirely NaN in the source data -- cannot be interpolated.
+ assert np.isnan(vel[2, 0])
+
+ def test_interpolate_ypr_none_when_unavailable(self, moving_ref):
+ assert moving_ref.interpolate_ypr_deg(np.array([5.0])) is None
+
+ 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]))
+ assert pos.shape == (3, 3)
+ expected = np.tile(np.array([1.0, 2.0, 3.0]).reshape(3, 1), (1, 3))
+ assert pos == pytest.approx(expected)
+
+ def test_get_coverage_mask_full(self, moving_ref):
+ mask = moving_ref.get_coverage_mask(np.array([0.0, 5.0, 10.0]))
+ assert np.all(mask)
+
+ def test_get_coverage_mask_partial_warns(self, moving_ref, monkeypatch):
+ warnings = []
+ monkeypatch.setattr(reference_module._logger, 'warning', lambda msg: warnings.append(msg))
+
+ mask = moving_ref.get_coverage_mask(np.array([-5.0, 5.0, 15.0]))
+ assert list(mask) == [False, True, False]
+ assert len(warnings) == 1
+
+ def test_get_coverage_mask_stationary_always_covers(self):
+ ref = ReferenceData.from_stationary_ecef(np.array([1.0, 2.0, 3.0]))
+ mask = ref.get_coverage_mask(np.array([-1e9, 0.0, 1e9]))
+ assert np.all(mask)
+
+
+class TestResolveCliArgument:
+ def test_none_returns_none(self, loader):
+ assert ReferenceData.resolve_cli_argument(None, loader=loader) is None
+
+ def test_lla_inferred_from_magnitude(self, loader):
+ ref = ReferenceData.resolve_cli_argument('37.1234, -122.526335, 102.34', loader=loader)
+ assert 'ECEF' not in ref.description
+ expected_ecef_m = np.array(geodetic2ecef(37.1234, -122.526335, 102.34, deg=True))
+ assert ref.position_ecef_m == pytest.approx(expected_ecef_m)
+
+ def test_ecef_inferred_from_magnitude(self, loader):
+ position_ecef_m = np.array([-2707071.0, -4321671.7, 3817403.2])
+ ref = ReferenceData.resolve_cli_argument(
+ '-2707071.0, -4321671.7, 3817403.2', loader=loader)
+ assert 'ECEF' in ref.description
+ assert ref.position_ecef_m == pytest.approx(position_ecef_m)
+
+ def test_lla_prefix_forces_lla(self, loader):
+ ref = ReferenceData.resolve_cli_argument('lla: 37.1234, -122.526335, 102.34', loader=loader)
+ assert 'ECEF' not in ref.description
+
+ def test_ecef_prefix_forces_ecef(self, loader):
+ ref = ReferenceData.resolve_cli_argument(
+ 'ecef: -2707071.0, -4321671.7, 3817403.2', loader=loader)
+ assert 'ECEF' in ref.description
+
+ def test_prefix_is_case_insensitive(self, loader):
+ ref = ReferenceData.resolve_cli_argument('ECEF: -2707071.0, -4321671.7, 3817403.2', loader=loader)
+ assert 'ECEF' in ref.description
+
+ @pytest.mark.parametrize('statistic', ['first', 'median', 'median_fixed'])
+ def test_own_log_keywords_dispatch(self, loader, statistic):
+ via_resolve = ReferenceData.resolve_cli_argument(statistic, loader=loader)
+ via_direct = ReferenceData.from_own_log(loader, statistic=statistic)
+ assert via_resolve.description == via_direct.description
+ assert via_resolve.position_ecef_m == pytest.approx(via_direct.position_ecef_m)
+ assert not via_resolve.is_truth
+
+ def test_unrecognized_string_falls_back_to_log_path_and_fails_gracefully(self, loader, tmp_path):
+ empty_log_base = tmp_path / 'log_base'
+ empty_log_base.mkdir()
+ result = ReferenceData.resolve_cli_argument(
+ 'not_a_real_log_hash', loader=loader, log_base_dir=str(empty_log_base))
+ assert result is None