calibration/tools/audit_rtk_imu_factor_consistency.py

#!/usr/bin/env python3
'''Audit RTK/IMU factor conventions without running an optimizer.'''
from __future__ import annotations
import argparse, json, math, sys
from dataclasses import asdict, is_dataclass
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path: sys.path.insert(0, str(ROOT))
from imu_lidar.imu_preintegration import preintegrate_imu
from rtk_imu.rtk_imu_engineering import (
    G_ENU, MIN_SEGMENT_DURATION_S, MIN_SEGMENT_NODE_COUNT,
    NUISANCE_DOF_PER_SEGMENT, _Segment, _all_hpr, _audit,
    _dense_colored_jacobian, _enu, _height_reference,
    _hpr_factor_observation, _initial_parameters, _marginal_lever_information,
    _motion_flags, _nodes, _position_valid, _residual,
    _segment_residual_size, _world_rtk)
from rtk_imu.rtk_imu_multisource import _f, _truth, load_unified_sessions
MECHANICAL_L_I_M = np.array([-0.45072, -0.25682, 0.73208])

def _jsonable(value):
    if isinstance(value, np.ndarray): return _jsonable(value.tolist())
    if isinstance(value, np.generic): return _jsonable(value.item())
    if isinstance(value, float): return value if math.isfinite(value) else None
    if is_dataclass(value): return _jsonable(asdict(value))
    if isinstance(value, dict): return {str(k): _jsonable(v) for k, v in value.items()}
    if isinstance(value, (list, tuple)): return [_jsonable(v) for v in value]
    return value

def _summary(error):
    a = np.asarray(error, dtype=float)
    return _jsonable(_audit(list(a.reshape(-1, 3)), 3)) if a.size else _jsonable(_audit([], 3))

def _corr(a, b):
    out = np.full(3, np.nan)
    for axis in range(3):
        if len(a) >= 3 and np.std(a[:, axis]) > 1e-10 and np.std(b[:, axis]) > 1e-10:
            out[axis] = np.corrcoef(a[:, axis], b[:, axis])[0, 1]
    return out

def _best_arrays(session, reference):
    rows = []
    for row in session.rtk_by_type.get('BESTNAVA', []):
        v = np.array([_f(row, 'velocity_east_m_s'), _f(row, 'velocity_north_m_s'),
                      _f(row, 'vertical_speed_m_s')])
        if _position_valid(row, 'BESTNAVA') and _truth(row, 'doppler_velocity_valid') and np.all(np.isfinite(v)):
            rows.append(row)
    rows.sort(key=lambda row: _f(row, 't_device_s'))
    rows = [row for i, row in enumerate(rows) if i == 0 or _f(row, 't_device_s') > _f(rows[i-1], 't_device_s')]
    t = np.asarray([_f(row, 't_device_s') for row in rows])
    p = np.asarray([_enu(row, 'BESTNAVA', reference)[0] for row in rows]).reshape(-1, 3)
    v = np.asarray([[_f(row, 'velocity_east_m_s'), _f(row, 'velocity_north_m_s'),
                     _f(row, 'vertical_speed_m_s')] for row in rows]).reshape(-1, 3)
    return rows, t, p, v

def _gnss_audit(sessions, reference, max_dt):
    all_dpdt, all_v, per_session = [], [], {}
    for session in sessions:
        _, t, p, v = _best_arrays(session, reference)
        dt = np.diff(t); keep = (dt > 0) & (dt <= max_dt)
        dpdt = np.diff(p, axis=0)[keep] / dt[keep, None]
        v_avg = .5 * (v[:-1] + v[1:])[keep]
        all_dpdt.append(dpdt); all_v.append(v_avg)
        per_session[session.session_id] = {
            'interval_count': len(dpdt), 'nominal_correlation_xyz': _corr(dpdt, v_avg),
            'nominal_error_m_s': _summary(dpdt-v_avg)}
    dpdt, velocity = np.vstack(all_dpdt), np.vstack(all_v)
    hypotheses = []
    for swap in (False, True):
        base = velocity[:, [1,0,2]] if swap else velocity
        for sx in (-1.,1.):
            for sy in (-1.,1.):
                for sz in (-1.,1.):
                    transformed = base * [sx,sy,sz]
                    hypotheses.append({
                        'mapping': ('[N,E,Z]' if swap else '[E,N,Z]')+f'*[{sx:+.0f},{sy:+.0f},{sz:+.0f}]',
                        'correlation_xyz': _corr(dpdt, transformed),
                        'error_m_s': _summary(dpdt-transformed)})
    hypotheses.sort(key=lambda item: item['error_m_s']['vector_rms'])
    nominal = next(x for x in hypotheses if x['mapping'] == '[E,N,Z]*[+1,+1,+1]')
    return {'interval_count': len(dpdt), 'nominal': nominal, 'best_mapping': hypotheses[0],
            'all_hypotheses': hypotheses, 'per_session': per_session}

def _nearest_imu(session, t, tolerance=.03):
    right = int(np.searchsorted(session.imu.t_s, t))
    candidates = [i for i in (right-1,right) if 0 <= i < len(session.imu.t_s)]
    if not candidates: return None
    index = min(candidates, key=lambda i: abs(session.imu.t_s[i]-t))
    return index if abs(session.imu.t_s[index]-t) <= tolerance else None

def _static_audit(sessions, rotation):
    current, opposite, per_session = [], [], {}
    for session in sessions:
        hpr, local = _all_hpr(session), []
        last_t = -np.inf
        for hpr_index in hpr.valid_indices:
            t = float(hpr.t_s[hpr_index])
            if t-last_t < 1.: continue
            last_t = t
            gravity, _ = _motion_flags(session,t,np.zeros(0),np.zeros((0,3)))
            baseline, _, valid, _, _ = _hpr_factor_observation(hpr,t)
            imu_index = _nearest_imu(session,t)
            if not (gravity and valid and imu_index is not None):
                continue
            R_WI = _world_rtk(baseline) @ rotation
            accel = session.imu.acc_m_s2[imu_index]
            value = R_WI @ accel + G_ENU
            local.append(value); current.append(value); opposite.append(R_WI @ accel-G_ENU)
        per_session[session.session_id] = {'count':len(local),'current_formula_m_s2':_summary(local)}
    return {'formula':'a_W_linear = R_WI @ specific_force_I + G_ENU',
            'current_formula_m_s2':_summary(current),
            'opposite_gravity_sign_m_s2':_summary(opposite),'per_session':per_session}

def _closure_audit(sessions, reference, rotation, lever, max_dt):
    all_p, all_v, per_session = [], [], {}
    for session in sessions:
        _, t, p_ant, v_ant = _best_arrays(session, reference)
        hpr, local_p, local_v = _all_hpr(session), [], []
        for index, dt in enumerate(np.diff(t)):
            if not .5 <= dt <= max_dt: continue
            baseline, _, valid, _, _ = _hpr_factor_observation(hpr, t[index])
            i0, i1 = _nearest_imu(session, t[index]), _nearest_imu(session, t[index+1])
            if not valid or i0 is None or i1 is None: continue
            pre = preintegrate_imu(session.imu.t_s, session.imu.gyro_rad_s,
                                   session.imu.acc_m_s2, t[index], t[index+1])
            if pre.duration_s <= 0 or abs(pre.duration_s-dt) > 1e-6: continue
            R0 = _world_rtk(baseline) @ rotation
            p_i0 = p_ant[index] - R0 @ lever
            v_i0 = v_ant[index] - R0 @ np.cross(session.imu.gyro_rad_s[i0], lever)
            p_i1 = p_i0 + v_i0*dt + .5*G_ENU*dt**2 + R0@pre.delta_p
            v_i1 = v_i0 + G_ENU*dt + R0@pre.delta_v
            R1 = R0 @ pre.delta_R
            local_p.append(p_i1 + R1@lever - p_ant[index+1])
            local_v.append(v_i1 + R1@np.cross(session.imu.gyro_rad_s[i1],lever) - v_ant[index+1])
        all_p.extend(local_p); all_v.extend(local_v)
        per_session[session.session_id] = {
            'interval_count':len(local_p),'position_m':_summary(local_p),
            'velocity_m_s':_summary(local_v)}
    return {'method':'single-step forward closure; fixed R2G/mechanical lever/BEST p-v; no least_squares',
            'position_m':_summary(all_p),'velocity_m_s':_summary(all_v),
            'per_session':per_session}

def _qualified_runs(sessions, reference, period):
    result = []
    for session in sessions:
        nodes, start, number = _nodes(session, reference, period), 0, 0
        for end in range(1,len(nodes)+1):
            if end != len(nodes) and nodes[end].continuity_id == nodes[end-1].continuity_id:
                continue
            run, start = tuple(nodes[start:end]), end
            if (len(run) >= MIN_SEGMENT_NODE_COUNT
                    and run[-1].t_s-run[0].t_s >= MIN_SEGMENT_DURATION_S
                    and any(node.hpr_factor_valid for node in run)):
                result.append((session,run,f'{session.session_id}:{number:02d}'))
                number += 1
    return result

def _first_node_audit(runs, rotation, lever):
    records, ranges = [], {}
    for session, run, segment_id in runs:
        node = run[0]
        hpr_node = next(item for item in run if item.hpr_factor_valid)
        R_WI = _world_rtk(hpr_node.baseline_enu) @ rotation
        velocity = node.velocity_enu_m_s
        v = np.full(3,np.nan) if velocity is None else velocity
        records.append({
            'segment_id':segment_id,'source':node.source,'t_s':node.t_s,
            'first_position_global_enu_m':node.p_enu_m,'first_velocity_enu_m_s':v,
            'free_x0_l0_position_residual_m':np.zeros(3),
            'free_x0_l0_velocity_residual_m_s':-v,
            'mechanical_l_unshifted_x0_position_residual_m':R_WI@lever,
            'mechanical_l_unshifted_x0_velocity_residual_m_s':R_WI@np.cross(node.gyro_rad_s,lever)-v,
            'observation_seeded_mechanical_position_residual_m':np.zeros(3),
            'observation_seeded_mechanical_velocity_residual_m_s':
                np.zeros(3) if velocity is not None else np.full(3,np.nan)})
        ranges.setdefault(session.session_id,[]).append(node.p_enu_m)
    ranges = {key:{'count':len(value),'min_global_enu_m':np.min(value,axis=0),
                   'max_global_enu_m':np.max(value,axis=0)} for key,value in ranges.items()}
    return {'common_global_enu_reference':True,'segment_state_is_global_imu_position':True,
            'per_session_first_node_ranges':ranges,'segments':records}

def _gyro_score(session, run, category):
    keep = (session.imu.t_s >= run[0].t_s) & (session.imu.t_s <= run[-1].t_s)
    t, gyro = session.imu.t_s[keep], session.imu.gyro_rad_s[keep]
    if len(t) < 2: return 0.
    value = np.trapezoid(np.abs(gyro),t,axis=0)
    return float(value[2] if category != 'slope' else np.hypot(value[0],value[1]))

def _build_segment(session, run, segment_id):
    pre = tuple(preintegrate_imu(session.imu.t_s,session.imu.gyro_rad_s,
                                 session.imu.acc_m_s2,a.t_s,b.t_s)
                for a,b in zip(run[:-1],run[1:]))
    hpr = next(node for node in run if node.hpr_factor_valid)
    return _Segment(segment_id,session.session_id,run,pre,_world_rtk(hpr.baseline_enu))

def _subset_marginal(jacobian,residual,segments,indices):
    rows, row0 = [], 0
    for index,segment in enumerate(segments):
        count = _segment_residual_size(segment)
        if index in indices: rows.extend(range(row0,row0+count))
        row0 += count
    columns = [0,1,2]
    for index in indices:
        start = 3 + NUISANCE_DOF_PER_SEGMENT*index
        columns.extend(range(start,start+NUISANCE_DOF_PER_SEGMENT))
    rows, columns = np.asarray(rows), np.asarray(columns)
    return _marginal_lever_information(
        jacobian[np.ix_(rows,columns)],residual[rows])[0]

def _jacobian_schur_audit(runs,categories,rotation,lever):
    segments, indices = [], {}
    for category in ('circle','left_right','slope'):
        choices = [item for item in runs if categories[item[0].session_id] == category]
        if not choices:
            indices[category] = []; continue
        best = max(choices,key=lambda item:_gyro_score(item[0],item[1],category))
        indices[category] = [len(segments)]
        segments.append(_build_segment(*best))
    x = _initial_parameters(segments); x[:3] = lever
    residual = _residual(x,segments,rotation)
    jacobian = _dense_colored_jacobian(x,segments,rotation)
    comparisons = []
    for axis in range(3):
        plus, minus = x.copy(), x.copy()
        plus[axis] += .001; minus[axis] -= .001
        numeric = (_residual(plus,segments,rotation)-_residual(minus,segments,rotation))/.002
        current = jacobian[:,axis]; delta = numeric-current
        comparisons.append({
            'axis':'XYZ'[axis],'perturbation_m':.001,
            'relative_difference':float(np.linalg.norm(delta)/max(np.linalg.norm(numeric),1e-12)),
            'difference_norm':float(np.linalg.norm(delta)),
            'max_absolute_difference':float(np.max(np.abs(delta))),
            'correlation':float(np.corrcoef(numeric,current)[0,1])})
    full = _marginal_lever_information(jacobian,residual)[0]
    parts = {key:_subset_marginal(jacobian,residual,segments,value)
             for key,value in indices.items() if value}
    summed = sum(parts.values(),np.zeros((3,3)))
    error = np.linalg.norm(summed-full,ord='fro')
    return {'linearization':'same observation-seeded nuisance state and mechanical lever; no optimizer',
            'selected_segments':[segment.segment_id for segment in segments],
            'finite_difference_vs_solver_jacobian':comparisons,
            'full_marginal_information':full,'category_marginal_information':parts,
            'category_sum':summed,'additivity_error_fro':float(error),
            'additivity_relative_error':float(error/max(np.linalg.norm(full,ord='fro'),1e-12))}

def _legacy_diagnostics(path):
    if path is None or not path.exists():
        return {'available':False,'reason':'no prior artifact supplied'}
    payload = json.loads(path.read_text(encoding='utf-8'))
    final_l = np.asarray(payload.get('free_solution',{}).get('l_I_m',[np.nan]*3))
    return {'available':False,'source':str(path),'initial_cost':None,'final_cost':None,
            'cost_reduction':None,'nfev':None,'optimality':None,'gradient_norm':None,
            'initial_l_I_m':[0.,0.,0.],'final_l_I_m':final_l,
            'l_step_norm_m':float(np.linalg.norm(final_l)) if np.all(np.isfinite(final_l)) else None,
            'reason':'legacy artifact did not persist optimizer state; prohibited solve was not rerun'}

def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument('--manifest',type=Path,required=True)
    parser.add_argument('--output',type=Path,required=True)
    parser.add_argument('--circle-session',required=True)
    parser.add_argument('--left-right-session',required=True)
    parser.add_argument('--slope-session',required=True)
    parser.add_argument('--sample-period-s',type=float,default=1.)
    parser.add_argument('--max-best-interval-s',type=float,default=1.75)
    parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
                        default=[.4543066225,-.0026392019,.0122384129])
    parser.add_argument('--mechanical-l-I-m',nargs=3,type=float,
                        default=MECHANICAL_L_I_M.tolist())
    parser.add_argument('--previous-free-result',type=Path)
    parser.add_argument('--level-static-session',action='append',
                        default=['0819_20260819_072130','0819_20260819_073045'])
    args = parser.parse_args()
    categories = {args.circle_session:'circle',args.left_right_session:'left_right',
                  args.slope_session:'slope'}
    selected_ids = set(categories) | set(args.level_static_session)
    all_sessions = load_unified_sessions(args.manifest,selected_session_ids=selected_ids)
    sessions = [session for session in all_sessions if session.session_id in categories]
    static_sessions = [session for session in all_sessions
                       if session.session_id in set(args.level_static_session)]
    reference = _height_reference(sessions)
    if reference is None: raise RuntimeError('no valid BESTNAVA height reference')
    rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
    lever = np.asarray(args.mechanical_l_I_m)
    runs = _qualified_runs(sessions,reference,args.sample_period_s)
    payload = {
        'scope':'factor consistency only; no free solve/LOO/prior/bootstrap/sensitivity',
        'least_squares_called':False,'rotation_source':'R2G_gravity_level_prior',
        'rotation_rpy_deg':args.rotation_rpy_deg,'mechanical_l_I_m':lever,
        'common_enu_reference':reference,
        'gnss_position_difference_vs_doppler':_gnss_audit(
            sessions,reference,args.max_best_interval_s),
        'static_specific_force_gravity_sign':_static_audit(static_sessions,rotation),
        'one_step_imu_preintegration_closure':_closure_audit(
            sessions,reference,rotation,lever,args.max_best_interval_s),
        'first_node_origin_and_initial_residual':_first_node_audit(runs,rotation,lever),
        'lever_jacobian_and_category_schur':_jacobian_schur_audit(
            runs,categories,rotation,lever),
        'previous_free_fit_solver_diagnostics':_legacy_diagnostics(args.previous_free_result)}
    args.output.parent.mkdir(parents=True,exist_ok=True)
    args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
                                      allow_nan=False)+'\n',encoding='utf-8')
    print(json.dumps(_jsonable({
        'gnss':payload['gnss_position_difference_vs_doppler'],
        'static':payload['static_specific_force_gravity_sign'],
        'closure':payload['one_step_imu_preintegration_closure'],
        'jacobian_schur':payload['lever_jacobian_and_category_schur']}),
        ensure_ascii=False,indent=2))
    return 0

if __name__ == '__main__':
    raise SystemExit(main())