重构RTK-IMU标定链路并完成机械先验工程验证
This commit is contained in:
lichun.qu
@@ -0,0 +1,313 @@
|
||||
#!/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 imu_lidar.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 imu_lidar.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())
|
||||
@@ -0,0 +1,151 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Audit BESTNAVA/Doppler factor yield for every unified RTK--IMU session."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import sys
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from imu_lidar.rtk_imu_engineering import (
|
||||
_all_hpr,
|
||||
_height_reference,
|
||||
_nodes,
|
||||
_position_valid,
|
||||
)
|
||||
from imu_lidar.rtk_imu_multisource import _f, _nearest_index, _truth, load_unified_sessions
|
||||
|
||||
|
||||
def _finite_doppler(row: dict[str, str]) -> bool:
|
||||
value = np.asarray([
|
||||
_f(row, "velocity_east_m_s"),
|
||||
_f(row, "velocity_north_m_s"),
|
||||
_f(row, "vertical_speed_m_s"),
|
||||
])
|
||||
return bool(_truth(row, "doppler_velocity_valid") and np.all(np.isfinite(value)))
|
||||
|
||||
|
||||
def _source_only_nodes(session, source: str, period_s: float):
|
||||
rows = {source: session.rtk_by_type.get(source, [])}
|
||||
if "GNHPR" in session.rtk_by_type:
|
||||
rows["GNHPR"] = session.rtk_by_type["GNHPR"]
|
||||
return _nodes(replace(session, rtk_by_type=rows), _height_reference([session]), period_s)
|
||||
|
||||
|
||||
def _audit_session(session, period_s: float) -> dict[str, object]:
|
||||
best = session.rtk_by_type.get("BESTNAVA", [])
|
||||
hpr = _all_hpr(session)
|
||||
counts = {
|
||||
"raw_bestnava": len(best),
|
||||
"checksum_valid": 0,
|
||||
"fixed": 0,
|
||||
"finite_position": 0,
|
||||
"finite_doppler": 0,
|
||||
"hpr_near": 0,
|
||||
"hpr_q4": 0,
|
||||
"imu_near": 0,
|
||||
"raw_bestnava_candidate": 0,
|
||||
}
|
||||
for row in best:
|
||||
if not _truth(row, "checksum_valid"):
|
||||
continue
|
||||
counts["checksum_valid"] += 1
|
||||
if not _truth(row, "position_fixed"):
|
||||
continue
|
||||
counts["fixed"] += 1
|
||||
if not _position_valid(row, "BESTNAVA"):
|
||||
continue
|
||||
counts["finite_position"] += 1
|
||||
if not _finite_doppler(row):
|
||||
continue
|
||||
counts["finite_doppler"] += 1
|
||||
t_s = _f(row, "t_device_s")
|
||||
hpr_index = _nearest_index(hpr.t_s, t_s, 0.12)
|
||||
if hpr_index is None:
|
||||
continue
|
||||
counts["hpr_near"] += 1
|
||||
if not hpr.valid[hpr_index]:
|
||||
continue
|
||||
counts["hpr_q4"] += 1
|
||||
if _nearest_index(session.imu.t_s, t_s, 0.03) is None:
|
||||
continue
|
||||
counts["imu_near"] += 1
|
||||
counts["raw_bestnava_candidate"] += 1
|
||||
|
||||
reference = _height_reference([session])
|
||||
combined_nodes = _nodes(session, reference, period_s) if reference is not None else []
|
||||
best_nodes = _source_only_nodes(session, "BESTNAVA", period_s) if reference is not None else []
|
||||
combined_best = [node for node in combined_nodes if node.source == "BESTNAVA"]
|
||||
best_velocity_nodes = [node for node in combined_best if node.velocity_enu_m_s is not None]
|
||||
source_best_velocity = [node for node in best_nodes if node.velocity_enu_m_s is not None]
|
||||
run_nodes: dict[int, list] = {}
|
||||
for node in combined_nodes:
|
||||
run_nodes.setdefault(node.continuity_id, []).append(node)
|
||||
qualifying_runs = [
|
||||
run for run in run_nodes.values()
|
||||
if len(run) >= 6 and run[-1].t_s - run[0].t_s >= 5.0
|
||||
]
|
||||
qualifying_best = [
|
||||
node for run in qualifying_runs for node in run if node.source == "BESTNAVA"
|
||||
]
|
||||
qualifying_doppler = [node for node in qualifying_best if node.velocity_enu_m_s is not None]
|
||||
return {
|
||||
"session_id": session.session_id,
|
||||
"batch_id": session.batch_id,
|
||||
"counts": counts,
|
||||
"combined_selected_nodes": len(combined_nodes),
|
||||
"combined_selected_bestnava": len(combined_best),
|
||||
"combined_selected_doppler": len(best_velocity_nodes),
|
||||
"best_only_selected_nodes": len(best_nodes),
|
||||
"best_only_selected_doppler": len(source_best_velocity),
|
||||
"best_suppressed_by_mixed_selection": max(0, len(best_nodes) - len(combined_best)),
|
||||
"doppler_suppressed_by_mixed_selection": max(0, len(source_best_velocity) - len(best_velocity_nodes)),
|
||||
"continuous_run_count": len(run_nodes),
|
||||
"qualified_run_count": len(qualifying_runs),
|
||||
"qualified_bestnava_factors": len(qualifying_best),
|
||||
"qualified_doppler_factors": len(qualifying_doppler),
|
||||
}
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--manifest", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument("--sample-period-s", type=float, default=1.0)
|
||||
args = parser.parse_args(argv)
|
||||
sessions = load_unified_sessions(args.manifest)
|
||||
audit = [_audit_session(session, args.sample_period_s) for session in sessions]
|
||||
totals: dict[str, int] = {}
|
||||
for item in audit:
|
||||
for key, value in item["counts"].items():
|
||||
totals[key] = totals.get(key, 0) + int(value)
|
||||
for key in (
|
||||
"combined_selected_nodes", "combined_selected_bestnava", "combined_selected_doppler",
|
||||
"best_only_selected_nodes", "best_only_selected_doppler",
|
||||
"best_suppressed_by_mixed_selection", "doppler_suppressed_by_mixed_selection",
|
||||
"continuous_run_count", "qualified_run_count", "qualified_bestnava_factors",
|
||||
"qualified_doppler_factors",
|
||||
):
|
||||
totals[key] = totals.get(key, 0) + int(item[key])
|
||||
payload = {
|
||||
"sample_period_s": args.sample_period_s,
|
||||
"session_count": len(audit),
|
||||
"totals": totals,
|
||||
"sessions": audit,
|
||||
}
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
args.output.write_text(json.dumps(payload, ensure_ascii=False, indent=2, allow_nan=False) + "\n", encoding="utf-8")
|
||||
print(json.dumps({"session_count": len(audit), "totals": totals}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,158 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Independent prediction innovations on the frozen 267 held-out windows.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
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.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem
|
||||
from tools.audit_rtk_imu_factor_consistency import _jsonable
|
||||
from tools.audit_rtk_imu_innovation_noise import (
|
||||
_factor_report,_hpr_innovations,_interval_innovations)
|
||||
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
|
||||
|
||||
LIMITS={
|
||||
'best_position':{'bias_norm':.10,'vector_p95':.50},
|
||||
'doppler':{'bias_norm':.10,'vector_p95':.50},
|
||||
'hpr':{'bias_norm':.01,'vector_p95':.05},
|
||||
}
|
||||
|
||||
def _finite_max_abs(values):
|
||||
a=np.asarray(values,dtype=float)
|
||||
return float(np.max(np.abs(a[np.isfinite(a)]))) if np.any(np.isfinite(a)) else np.inf
|
||||
|
||||
def _summary_gate(summary,limits,min_samples=20):
|
||||
if summary.get('sample_count',0)<min_samples:
|
||||
return {'evaluated':False,'passed':True,'reason':'insufficient_samples'}
|
||||
checks={'bias_norm':float(np.linalg.norm(summary['innovation_bias']))<=limits['bias_norm'],
|
||||
'vector_p95':summary['vector_p95']<=limits['vector_p95'],
|
||||
'lag1_autocorrelation_abs':_finite_max_abs(
|
||||
summary['temporal_autocorrelation_lag1'])<=.95}
|
||||
return {'evaluated':True,'thresholds':{**limits,'lag1_abs_max':.95},
|
||||
'checks':checks,'passed':bool(all(checks.values()))}
|
||||
|
||||
def _factor_gate(report,key):
|
||||
limits=LIMITS[key]; overall=_summary_gate(report['overall'],limits,1)
|
||||
grouped={}
|
||||
for group_name in ('by_session','by_motion','by_speed','by_gyro_norm'):
|
||||
grouped[group_name]={name:_summary_gate(value,limits)
|
||||
for name,value in report[group_name].items()}
|
||||
evaluated=[gate for group in grouped.values() for gate in group.values()
|
||||
if gate['evaluated']]
|
||||
passed=overall['passed'] and all(gate['passed'] for gate in evaluated)
|
||||
return {'overall':overall,'grouped':grouped,'passed':bool(passed)}
|
||||
|
||||
def _calibration_biases(engineering,problems):
|
||||
source=engineering['prior_constrained_solution'].get(
|
||||
'calibration_only_frozen_bias_by_session')
|
||||
if not source:
|
||||
raise RuntimeError('engineering result lacks calibration-only frozen bias')
|
||||
by_date={}
|
||||
for session_id,value in source.items():
|
||||
by_date.setdefault(session_id.split('_')[1],[]).append(value)
|
||||
result={}
|
||||
for problem in problems:
|
||||
session_id=problem.segment.session_id
|
||||
if session_id in result: continue
|
||||
if session_id in source:
|
||||
value=source[session_id]; origin='same_calibration_session'
|
||||
else:
|
||||
values=by_date.get(session_id.split('_')[1],list(source.values()))
|
||||
weights=np.asarray([x['node_count'] for x in values],dtype=float)
|
||||
value={'gyro_bias_rad_s':np.average(
|
||||
[x['gyro_bias_rad_s'] for x in values],axis=0,weights=weights),
|
||||
'accel_bias_m_s2':np.average(
|
||||
[x['accel_bias_m_s2'] for x in values],axis=0,weights=weights)}
|
||||
origin=('calibration_date_weighted_mean' if
|
||||
session_id.split('_')[1] in by_date else 'calibration_global_weighted_mean')
|
||||
result[session_id]={'gyro_bias_rad_s':value['gyro_bias_rad_s'],
|
||||
'accel_bias_m_s2':value['accel_bias_m_s2'],'source':origin}
|
||||
return result
|
||||
|
||||
def main():
|
||||
p=argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument('--manifest',type=Path,required=True)
|
||||
p.add_argument('--calibration-selection',type=Path,required=True)
|
||||
p.add_argument('--all-selection',type=Path,required=True)
|
||||
p.add_argument('--engineering-result',type=Path,required=True)
|
||||
p.add_argument('--output',type=Path,required=True)
|
||||
p.add_argument('--sample-period-s',type=float,default=1.)
|
||||
p.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
p.add_argument('--circle-session',default='0808_20260808_092827')
|
||||
p.add_argument('--left-right-session',default='0808_20260808_082148')
|
||||
p.add_argument('--slope-session',default='0815_20260812_123424')
|
||||
args=p.parse_args()
|
||||
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
|
||||
calibration=json.loads(args.calibration_selection.read_text(encoding='utf-8'))
|
||||
selected=json.loads(args.all_selection.read_text(encoding='utf-8'))
|
||||
calibration_ids={x['candidate_id'] for x in calibration['selected_windows']}
|
||||
heldout=[x for x in selected['selected_windows']
|
||||
if x['candidate_id'] not in calibration_ids]
|
||||
if len(calibration_ids)!=47 or len(heldout)!=267:
|
||||
raise RuntimeError(f'expected 47+267 windows, got {len(calibration_ids)}+{len(heldout)}')
|
||||
shared=sum(x.get('shared_sample_count_with_previous',{}).get(k,0)
|
||||
for x in selected['selected_windows'] for k in ('imu','gnss','hpr'))
|
||||
if shared: raise RuntimeError(f'selection contains {shared} shared samples')
|
||||
lever=np.asarray(engineering.get('engineering_l_I_m',
|
||||
engineering['prior_constrained_solution']['result']['final_l_I_m']),dtype=float)
|
||||
sessions=load_unified_sessions(
|
||||
args.manifest,selected_session_ids={x['session_id'] for x in heldout})
|
||||
reference=_height_reference(sessions)
|
||||
segments=_restore_segments(sessions,reference,heldout,args.sample_period_s)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
problems=[build_problem(segment,rotation,lever,args.hpr_direct_sigma_rad)
|
||||
for segment in segments]
|
||||
session_by_id={session.session_id:session for session in sessions}
|
||||
motion_by_session={args.circle_session:'circle',
|
||||
args.left_right_session:'left_right',args.slope_session:'slope'}
|
||||
records={key:[] for key in ('best_position','doppler','hpr','imu_preintegration')}
|
||||
biases=_calibration_biases(engineering,problems)
|
||||
for problem in problems:
|
||||
motion=motion_by_session.get(problem.segment.session_id,'other_recovered_dynamic')
|
||||
bias=biases[problem.segment.session_id]
|
||||
interval=_interval_innovations(problem,motion,
|
||||
bias['gyro_bias_rad_s'],bias['accel_bias_m_s2'])
|
||||
for key,value in interval.items(): records[key].extend(value)
|
||||
records['hpr'].extend(_hpr_innovations(
|
||||
session_by_id[problem.segment.session_id],problem,motion))
|
||||
factors={key:_factor_report(value) for key,value in records.items()}
|
||||
gates={key:_factor_gate(factors[key],key)
|
||||
for key in ('best_position','doppler','hpr')}
|
||||
passed=bool(all(value['passed'] for value in gates.values()))
|
||||
payload={'scope':'267-window independent held-out prediction innovations',
|
||||
'least_squares_called':False,'target_observation_used_by_predictor':False,
|
||||
'lever_reoptimized':False,'rotation_reoptimized':False,
|
||||
'covariance_parameters_modified':False,
|
||||
'fixed_l_I_m':lever,'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
|
||||
'calibration_window_count':47,'heldout_window_count':267,
|
||||
'calibration_heldout_overlap_count':0,
|
||||
'motion_class_mapping':motion_by_session,
|
||||
'prediction_definition':{
|
||||
'BEST_position':'previous GNSS p/v + IMU preintegration + fixed lever',
|
||||
'Doppler':'previous GNSS velocity + IMU preintegration + omega-cross-lever',
|
||||
'HPR':'withheld direct HPR predicted by LOO interpolation and gyro propagation',
|
||||
'IMU_preintegration':'two independently GNSS/HPR-anchored endpoints'},
|
||||
'bias_source':('frozen prior-node bias from the disjoint 47-window calibration set; '
|
||||
'same-session when available, otherwise calibration date-weighted mean; '
|
||||
'no held-out target observation and no covariance writeback'),
|
||||
'per_session_frozen_bias':biases,
|
||||
'factors':factors,'validation_limits_are_physical_not_covariance_retuning':LIMITS,
|
||||
'factor_gates':gates,'independent_heldout_innovation_passed':passed}
|
||||
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')
|
||||
compact={key:{'samples':value['overall'].get('sample_count',0),
|
||||
'bias':value['overall'].get('innovation_bias'),
|
||||
'vector_p95':value['overall'].get('vector_p95'),
|
||||
'nis_per_dof':value['overall'].get('nis_per_dof'),
|
||||
'gate':gates.get(key)} for key,value in factors.items()}
|
||||
print(json.dumps(_jsonable({'factors':compact,
|
||||
'independent_heldout_innovation_passed':passed}),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
if __name__=='__main__': raise SystemExit(main())
|
||||
@@ -0,0 +1,286 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Independent prediction/innovation noise audit for node-state RTK/IMU factors.'''
|
||||
from __future__ import annotations
|
||||
import argparse, json, math, sys
|
||||
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.geometry import so3_log
|
||||
from imu_lidar.imu_preintegration import apply_bias_correction_imu,preintegrate_gyro
|
||||
from imu_lidar.rtk_imu_engineering import (
|
||||
G_ENU,HPR_DIRECT_ANGULAR_SIGMA_RAD,_all_hpr,_height_reference,
|
||||
_world_rtk)
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem
|
||||
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
|
||||
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
|
||||
|
||||
BEST_SIGMA = np.array([.06,.06,.12])
|
||||
DOPPLER_SIGMA = np.array([.15,.15,.30])
|
||||
|
||||
def _whiten(value,covariance):
|
||||
covariance = .5*(covariance+covariance.T)+np.eye(len(value))*1e-12
|
||||
return np.linalg.solve(np.linalg.cholesky(covariance),value)
|
||||
|
||||
def _bin_speed(value):
|
||||
if value < .2: return 'speed_lt_0p2'
|
||||
if value < 1.: return 'speed_0p2_to_1'
|
||||
return 'speed_ge_1'
|
||||
|
||||
def _bin_gyro(value):
|
||||
if value < .02: return 'gyro_lt_0p02'
|
||||
if value < .10: return 'gyro_0p02_to_0p10'
|
||||
return 'gyro_ge_0p10'
|
||||
|
||||
def _bin_gap(value):
|
||||
if not np.isfinite(value): return 'gap_unavailable'
|
||||
if value <= .12: return 'gap_direct'
|
||||
if value <= .3: return 'gap_0p12_to_0p3'
|
||||
if value <= .5: return 'gap_0p3_to_0p5'
|
||||
return 'gap_gt_0p5'
|
||||
|
||||
def _distribution(values):
|
||||
a = np.asarray(values,dtype=float).reshape(-1)
|
||||
if not a.size:
|
||||
return {'count':0,'rms':np.nan,'p50_abs':np.nan,'p95_abs':np.nan,'p99_abs':np.nan}
|
||||
return {'count':len(a),'rms':float(np.sqrt(np.mean(a*a))),
|
||||
'p50_abs':float(np.percentile(np.abs(a),50.)),
|
||||
'p95_abs':float(np.percentile(np.abs(a),95.)),
|
||||
'p99_abs':float(np.percentile(np.abs(a),99.))}
|
||||
|
||||
def _autocorrelation(vectors):
|
||||
a = np.asarray(vectors,dtype=float)
|
||||
result = np.full(a.shape[1] if a.ndim == 2 else 0,np.nan)
|
||||
if a.ndim != 2 or len(a) < 3: return result
|
||||
for axis in range(a.shape[1]):
|
||||
left,right = a[:-1,axis],a[1:,axis]
|
||||
if np.std(left)>1e-12 and np.std(right)>1e-12:
|
||||
result[axis] = np.corrcoef(left,right)[0,1]
|
||||
return result
|
||||
|
||||
def _summarize(records):
|
||||
if not records: return {'sample_count':0}
|
||||
residual = np.asarray([item['residual'] for item in records])
|
||||
normalized = np.asarray([item['normalized'] for item in records])
|
||||
factor_only = np.asarray([item['normalized_factor_only'] for item in records])
|
||||
nis = np.sum(normalized*normalized,axis=1)
|
||||
dimension = residual.shape[1]
|
||||
effective_dimension = int(records[0].get('effective_dimension',dimension))
|
||||
total_dof = len(records)*effective_dimension
|
||||
norm = np.linalg.norm(residual,axis=1)
|
||||
centered_t = np.asarray([item['t_s'] for item in records],dtype=float)
|
||||
centered_t -= np.mean(centered_t)
|
||||
temporal_drift = np.zeros(dimension)
|
||||
if len(records)>=3 and np.ptp(centered_t)>1e-9:
|
||||
temporal_drift = np.asarray([
|
||||
np.polyfit(centered_t,residual[:,axis],1)[0]
|
||||
for axis in range(dimension)])
|
||||
return {'sample_count':len(records),'dimension':dimension,
|
||||
'effective_dof_per_sample':effective_dimension,
|
||||
'innovation_bias':np.mean(residual,axis=0),
|
||||
'innovation_distribution':_distribution(residual),
|
||||
'axis_rms':np.sqrt(np.mean(residual*residual,axis=0)),
|
||||
'axis_p50_abs':np.percentile(np.abs(residual),50.,axis=0),
|
||||
'axis_p95_abs':np.percentile(np.abs(residual),95.,axis=0),
|
||||
'axis_p99_abs':np.percentile(np.abs(residual),99.,axis=0),
|
||||
'vector_rms':float(np.sqrt(np.mean(norm*norm))),
|
||||
'vector_p50':float(np.percentile(norm,50.)),
|
||||
'vector_p95':float(np.percentile(norm,95.)),
|
||||
'vector_p99':float(np.percentile(norm,99.)),
|
||||
'normalized_distribution':_distribution(normalized),
|
||||
'empirical_covariance':np.cov(residual,rowvar=False),
|
||||
'nis_distribution':_distribution(nis),
|
||||
'nis_total':float(np.sum(nis)),'nis_per_dof':float(np.sum(nis)/total_dof),
|
||||
'alpha_factor':float(np.sum(nis)/total_dof),
|
||||
'alpha_composite_prediction':float(np.sum(nis)/total_dof),
|
||||
'alpha_factor_only_upper_bound':float(np.sum(factor_only*factor_only)/total_dof),
|
||||
'temporal_autocorrelation_lag1':_autocorrelation(residual),
|
||||
'temporal_linear_drift_per_s':temporal_drift}
|
||||
|
||||
def _group(records,key):
|
||||
values = {}
|
||||
for item in records: values.setdefault(str(item[key]),[]).append(item)
|
||||
return {name:_summarize(items) for name,items in values.items()}
|
||||
|
||||
def _base_record(session_id,motion,t,speed,gyro_norm,gap):
|
||||
return {'session':session_id,'motion':motion,'t_s':float(t),
|
||||
'speed_bin':_bin_speed(speed),'gyro_bin':_bin_gyro(gyro_norm),
|
||||
'hpr_gap_bin':_bin_gap(gap)}
|
||||
|
||||
def _interval_innovations(problem,motion,bg=None,ba=None):
|
||||
records = {'best_position':[],'doppler':[],'imu_preintegration':[]}
|
||||
lever = problem.fixed_l_I_m
|
||||
bg=np.zeros(3) if bg is None else np.asarray(bg,dtype=float)
|
||||
ba=np.zeros(3) if ba is None else np.asarray(ba,dtype=float)
|
||||
nodes = problem.segment.nodes
|
||||
for index,(left,right,pre) in enumerate(zip(nodes[:-1],nodes[1:],problem.segment.preintegrations)):
|
||||
if not (left.hpr_factor_valid and right.hpr_factor_valid): continue
|
||||
if left.velocity_enu_m_s is None or right.velocity_enu_m_s is None: continue
|
||||
R0 = _world_rtk(left.baseline_enu)@problem.R_RTK_IMU
|
||||
R1 = _world_rtk(right.baseline_enu)@problem.R_RTK_IMU
|
||||
p_i0 = left.p_enu_m-R0@lever
|
||||
p_i1 = right.p_enu_m-R1@lever
|
||||
v_i0 = left.velocity_enu_m_s-R0@np.cross(left.gyro_rad_s-bg,lever)
|
||||
v_i1 = right.velocity_enu_m_s-R1@np.cross(right.gyro_rad_s-bg,lever)
|
||||
dt = pre.duration_s
|
||||
delta_R,delta_v,delta_p=apply_bias_correction_imu(pre,bg,ba)
|
||||
pred_p_i1 = p_i0+v_i0*dt+.5*G_ENU*dt*dt+R0@delta_p
|
||||
pred_v_i1 = v_i0+G_ENU*dt+R0@delta_v
|
||||
pred_R1 = R0@delta_R
|
||||
p_innovation = pred_p_i1+pred_R1@lever-right.p_enu_m
|
||||
v_innovation = pred_v_i1+pred_R1@np.cross(
|
||||
right.gyro_rad_s-bg,lever)-right.velocity_enu_m_s
|
||||
speed = float(np.linalg.norm(left.velocity_enu_m_s))
|
||||
gyro_norm = float(pre.mean_gyro_norm)
|
||||
gap = max(left.hpr_support_gap_s,right.hpr_support_gap_s)
|
||||
base = _base_record(problem.segment.session_id,motion,right.t_s,speed,gyro_norm,gap)
|
||||
base.update({'interval_id':f'{problem.segment.segment_id}:{index}',
|
||||
'dt_s':float(dt),'R0_WI':R0})
|
||||
p_cov = (np.diag(BEST_SIGMA**2)+dt*dt*np.diag(DOPPLER_SIGMA**2)
|
||||
+R0@pre.cov[6:9,6:9]@R0.T+np.diag(BEST_SIGMA**2))
|
||||
v_cov = (np.diag(DOPPLER_SIGMA**2)+R0@pre.cov[3:6,3:6]@R0.T
|
||||
+np.diag(DOPPLER_SIGMA**2))
|
||||
records['best_position'].append({**base,'residual':p_innovation,
|
||||
'normalized':_whiten(p_innovation,p_cov),
|
||||
'normalized_factor_only':p_innovation/BEST_SIGMA})
|
||||
records['doppler'].append({**base,'residual':v_innovation,
|
||||
'normalized':_whiten(v_innovation,v_cov),
|
||||
'normalized_factor_only':v_innovation/DOPPLER_SIGMA})
|
||||
imu_error = np.concatenate([
|
||||
so3_log(delta_R.T@R0.T@R1),
|
||||
R0.T@(v_i1-v_i0-G_ENU*dt)-delta_v,
|
||||
R0.T@(p_i1-p_i0-v_i0*dt-.5*G_ENU*dt*dt)-delta_p])
|
||||
anchored_cov = pre.cov.copy()
|
||||
hpr_var = left.hpr_angular_sigma_rad**2+right.hpr_angular_sigma_rad**2
|
||||
anchored_cov[:3,:3] += np.eye(3)*hpr_var
|
||||
anchored_cov[3:6,3:6] += R0.T@np.diag(2.*DOPPLER_SIGMA**2)@R0
|
||||
anchored_cov[6:9,6:9] += R0.T@np.diag(
|
||||
2.*BEST_SIGMA**2+dt*dt*DOPPLER_SIGMA**2)@R0
|
||||
records['imu_preintegration'].append({**base,'residual':imu_error,
|
||||
'normalized':_whiten(imu_error,anchored_cov),
|
||||
'normalized_factor_only':_whiten(imu_error,pre.cov)})
|
||||
return records
|
||||
|
||||
def _hpr_innovations(session,problem,motion):
|
||||
hpr = _all_hpr(session)
|
||||
indices = [int(i) for i in hpr.valid_indices
|
||||
if problem.segment.nodes[0].t_s <= hpr.t_s[i] <= problem.segment.nodes[-1].t_s]
|
||||
records = []
|
||||
baseline_I = problem.R_RTK_IMU.T[:,0]
|
||||
node_times = np.asarray([node.t_s for node in problem.segment.nodes])
|
||||
for position in range(1,len(indices)-1):
|
||||
left,index,right = indices[position-1],indices[position],indices[position+1]
|
||||
t0,t,t1 = hpr.t_s[left],hpr.t_s[index],hpr.t_s[right]
|
||||
total_gap = float(t1-t0)
|
||||
if not 0. < total_gap <= .5: continue
|
||||
fraction = float((t-t0)/total_gap)
|
||||
predicted = (1.-fraction)*hpr.baseline_enu[left]+fraction*hpr.baseline_enu[right]
|
||||
predicted /= np.linalg.norm(predicted)
|
||||
innovation = np.cross(predicted,hpr.baseline_enu[index])
|
||||
sigma = HPR_DIRECT_ANGULAR_SIGMA_RAD*np.sqrt(
|
||||
1.+(1.-fraction)**2+fraction**2)
|
||||
nearest = int(np.argmin(np.abs(node_times-t)))
|
||||
node = problem.segment.nodes[nearest]
|
||||
speed = float(np.linalg.norm(node.velocity_enu_m_s)) if node.velocity_enu_m_s is not None else 0.
|
||||
gyro_norm = float(np.linalg.norm(node.gyro_rad_s))
|
||||
base = _base_record(session.session_id,motion,t,speed,gyro_norm,total_gap)
|
||||
records.append({**base,'method':'leave_one_out_interpolation',
|
||||
'effective_dimension':2,'residual':innovation,
|
||||
'normalized':innovation/sigma,
|
||||
'normalized_factor_only':innovation/HPR_DIRECT_ANGULAR_SIGMA_RAD})
|
||||
gyro_pre = preintegrate_gyro(session.imu.t_s,session.imu.gyro_rad_s,float(t0),float(t))
|
||||
R0 = _world_rtk(hpr.baseline_enu[left])@problem.R_RTK_IMU
|
||||
propagated = R0@gyro_pre.delta_R@baseline_I
|
||||
gyro_innovation = np.cross(propagated,hpr.baseline_enu[index])
|
||||
gyro_sigma = np.sqrt(2.*HPR_DIRECT_ANGULAR_SIGMA_RAD**2
|
||||
+float(np.trace(gyro_pre.cov))/3.)
|
||||
records.append({**base,'method':'gyro_propagation',
|
||||
'effective_dimension':2,'residual':gyro_innovation,
|
||||
'normalized':gyro_innovation/gyro_sigma,
|
||||
'normalized_factor_only':gyro_innovation/HPR_DIRECT_ANGULAR_SIGMA_RAD})
|
||||
return records
|
||||
|
||||
def _factor_report(records):
|
||||
return {'overall':_summarize(records),
|
||||
'by_session':_group(records,'session'),'by_motion':_group(records,'motion'),
|
||||
'by_speed':_group(records,'speed_bin'),'by_gyro_norm':_group(records,'gyro_bin'),
|
||||
'by_hpr_bridge_gap':_group(records,'hpr_gap_bin'),
|
||||
'by_method':_group(records,'method') if records and 'method' in records[0] else {}}
|
||||
|
||||
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('--target-duration-s',type=float,default=15.)
|
||||
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())
|
||||
args = parser.parse_args()
|
||||
categories = {'circle':args.circle_session,'left_right':args.left_right_session,
|
||||
'slope':args.slope_session}
|
||||
sessions = load_unified_sessions(args.manifest,selected_session_ids=set(categories.values()))
|
||||
reference = _height_reference(sessions)
|
||||
if reference is None: raise RuntimeError('no BEST reference')
|
||||
rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
lever = np.asarray(args.mechanical_l_I_m)
|
||||
selections, all_records = {}, {
|
||||
'best_position':[],'doppler':[],'hpr':[],'imu_preintegration':[]}
|
||||
for motion,session_id in categories.items():
|
||||
session = next(item for item in sessions if item.session_id == session_id)
|
||||
segment,selection = _select_window(
|
||||
[session],reference,args.sample_period_s,args.target_duration_s)
|
||||
selections[motion] = selection
|
||||
problem = build_problem(segment,rotation,lever)
|
||||
interval = _interval_innovations(problem,motion)
|
||||
for key,records in interval.items(): all_records[key].extend(records)
|
||||
all_records['hpr'].extend(_hpr_innovations(session,problem,motion))
|
||||
factors = {key:_factor_report(records) for key,records in all_records.items()}
|
||||
payload = {
|
||||
'scope':'independent prediction innovations; no node optimization/covariance writeback',
|
||||
'least_squares_called':False,'covariance_parameters_modified':False,
|
||||
'fixed_l_I_m':lever,'selections':selections,
|
||||
'current_physical_sigma':{
|
||||
'best_position_xyz_m':BEST_SIGMA,
|
||||
'doppler_xyz_m_s':DOPPLER_SIGMA,
|
||||
'hpr_direct_angular_rad':HPR_DIRECT_ANGULAR_SIGMA_RAD,
|
||||
'bias_random_walk_source':'unchanged device/static/Allan noise model'},
|
||||
'alpha_semantics':{
|
||||
'alpha_factor':'innovation NIS/dof using composite prediction covariance',
|
||||
'alpha_factor_only_upper_bound':(
|
||||
'innovation divided only by current factor covariance; includes predictor and '
|
||||
'endpoint-anchor noise and must not be written back directly')},
|
||||
'factors':factors,
|
||||
'recommended_covariance_writeback':False,
|
||||
'covariance_freeze_assessment':{
|
||||
'best_position':'predictor-confounded; unchanged',
|
||||
'doppler':'predictor-confounded; unchanged',
|
||||
'imu_preintegration':'endpoint-anchor dominated; unchanged',
|
||||
'bias_random_walk':'static/Allan/device-model based; unchanged',
|
||||
'hpr':{
|
||||
'identifiable':True,
|
||||
'evidence':'264 withheld samples; LOO and gyro predictions agree',
|
||||
'frozen_direct_sigma_rad':.006,
|
||||
'writeback_policy':'explicit audit decision, not automatic alpha'}}}
|
||||
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')
|
||||
compact = {key:{'count':value['overall'].get('sample_count',0),
|
||||
'bias':value['overall'].get('innovation_bias'),
|
||||
'rms':value['overall'].get('innovation_distribution',{}).get('rms'),
|
||||
'p95':value['overall'].get('innovation_distribution',{}).get('p95_abs'),
|
||||
'alpha_composite':value['overall'].get('alpha_composite_prediction'),
|
||||
'alpha_factor_only_upper_bound':value['overall'].get('alpha_factor_only_upper_bound'),
|
||||
'autocorrelation':value['overall'].get('temporal_autocorrelation_lag1')}
|
||||
for key,value in factors.items()}
|
||||
print(json.dumps(_jsonable(compact),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
if __name__ == '__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,145 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Audit strict RTK--IMU segments for lever-arm excitation and marginal information."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
from dataclasses import asdict
|
||||
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.rtk_imu_engineering import _fit_segments, _fit_summary, _segments
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
|
||||
|
||||
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 hasattr(value, "__dataclass_fields__"):
|
||||
return {key: _jsonable(item) for key, item in asdict(value).items()}
|
||||
if isinstance(value, dict):
|
||||
return {str(key): _jsonable(item) for key, item in value.items()}
|
||||
if isinstance(value, (tuple, list)):
|
||||
return [_jsonable(item) for item in value]
|
||||
return value
|
||||
|
||||
|
||||
def _orientation_spans_deg(segment, rotation_rtk_imu: np.ndarray) -> np.ndarray:
|
||||
matrices = [segment.R_WRTK_initial @ rotation_rtk_imu]
|
||||
for pre in segment.preintegrations:
|
||||
matrices.append(matrices[-1] @ pre.delta_R)
|
||||
euler = Rotation.from_matrix(np.asarray(matrices)).as_euler("xyz", degrees=False)
|
||||
return np.degrees(np.ptp(np.unwrap(euler, axis=0), axis=0))
|
||||
|
||||
|
||||
def _audit_segment(segment, rotation_rtk_imu: np.ndarray, max_nfev: int) -> dict[str, object]:
|
||||
gyro = np.asarray([node.gyro_rad_s for node in segment.nodes])
|
||||
gyro_norm = np.linalg.norm(gyro, axis=1)
|
||||
best_count = sum(node.source == "BESTNAVA" for node in segment.nodes)
|
||||
doppler_count = sum(
|
||||
node.source == "BESTNAVA" and node.velocity_enu_m_s is not None
|
||||
for node in segment.nodes
|
||||
)
|
||||
fit, residual, detail = _fit_segments([segment], rotation_rtk_imu, max_nfev=max_nfev)
|
||||
summary = _fit_summary(fit, residual, detail)
|
||||
item: dict[str, object] = {
|
||||
"segment_id": segment.segment_id,
|
||||
"session_id": segment.session_id,
|
||||
"node_count": len(segment.nodes),
|
||||
"duration_s": float(segment.nodes[-1].t_s - segment.nodes[0].t_s),
|
||||
"yaw_pitch_roll_span_deg": _orientation_spans_deg(segment, rotation_rtk_imu)[[2, 1, 0]],
|
||||
"gyro_rms_deg_s": np.degrees(np.sqrt(np.mean(gyro ** 2, axis=0))),
|
||||
"gyro_peak_deg_s": np.degrees(np.max(np.abs(gyro), axis=0)),
|
||||
"gyro_norm_rms_deg_s": float(np.degrees(np.sqrt(np.mean(gyro_norm ** 2)))),
|
||||
"gyro_norm_peak_deg_s": float(np.degrees(np.max(gyro_norm))),
|
||||
"bestnava_count": best_count,
|
||||
"doppler_count": doppler_count,
|
||||
"fit": None,
|
||||
}
|
||||
if summary is not None:
|
||||
item["fit"] = {
|
||||
"optimizer_converged": summary.optimizer_converged,
|
||||
"lever_information_singular_values": summary.lever_information_singular_values,
|
||||
"lever_information_condition_number": summary.lever_information_condition_number,
|
||||
"lever_precision_rank": summary.lever_precision_rank,
|
||||
"weakest_lever_direction_I": summary.weakest_lever_direction_I,
|
||||
"lever_std_m": summary.l_I_std_m,
|
||||
}
|
||||
singular = summary.lever_information_singular_values
|
||||
item["information_score"] = float(singular[-1]) if summary.lever_precision_rank == 3 else 0.0
|
||||
else:
|
||||
item["information_score"] = 0.0
|
||||
spans = np.asarray(item["yaw_pitch_roll_span_deg"])
|
||||
# Short strict runs rarely accumulate a full vehicle turn; retain clearly non-straight motion.
|
||||
item["turn_or_slope"] = bool(spans[0] >= 3.0 or abs(spans[1]) >= 0.5 or abs(spans[2]) >= 0.5)
|
||||
return item
|
||||
|
||||
|
||||
def _recommended(items: list[dict[str, object]]) -> list[str]:
|
||||
candidates = [
|
||||
item for item in items
|
||||
if item["turn_or_slope"] and int(item["bestnava_count"]) >= 6
|
||||
and int(item["doppler_count"]) >= 6
|
||||
and item["fit"] is not None
|
||||
and int(item["fit"]["lever_precision_rank"]) == 3
|
||||
]
|
||||
candidates.sort(key=lambda item: float(item["information_score"]), reverse=True)
|
||||
selected: list[str] = []
|
||||
per_session: dict[str, int] = {}
|
||||
for item in candidates:
|
||||
session_id = str(item["session_id"])
|
||||
if per_session.get(session_id, 0) >= 2:
|
||||
continue
|
||||
selected.append(str(item["segment_id"]))
|
||||
per_session[session_id] = per_session.get(session_id, 0) + 1
|
||||
return selected
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--manifest", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument("--sample-period-s", type=float, default=1.0)
|
||||
parser.add_argument("--rotation-rpy-deg", nargs=3, type=float,
|
||||
default=[0.4543066225, -0.0026392019, 0.0122384129])
|
||||
parser.add_argument("--max-nfev", type=int, default=80)
|
||||
args = parser.parse_args(argv)
|
||||
|
||||
sessions = load_unified_sessions(args.manifest)
|
||||
rotation = Rotation.from_euler("xyz", args.rotation_rpy_deg, degrees=True).as_matrix()
|
||||
items = [
|
||||
_audit_segment(segment, rotation, args.max_nfev)
|
||||
for segment in _segments(sessions, args.sample_period_s)
|
||||
]
|
||||
payload = {
|
||||
"session_count": len(sessions),
|
||||
"strict_segment_count": len(items),
|
||||
"sample_period_s": args.sample_period_s,
|
||||
"rotation_rpy_deg": args.rotation_rpy_deg,
|
||||
"recommended_segment_ids": _recommended(items),
|
||||
"segments": items,
|
||||
}
|
||||
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({
|
||||
"strict_segment_count": len(items),
|
||||
"recommended_segment_ids": payload["recommended_segment_ids"],
|
||||
}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,668 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Penetration audit for RTK--IMU motion excitation.
|
||||
|
||||
This is read-only diagnostics. It never applies a lever prior, solves a lever arm,
|
||||
or changes continuity/acceptance thresholds. Gyro trajectory integrals are the
|
||||
primary excitation metrics; start/end Euler differences are deliberately absent.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
from dataclasses import asdict
|
||||
from pathlib import Path
|
||||
from typing import Iterable
|
||||
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from imu_lidar.imu_audit import audit_imu
|
||||
from imu_lidar.rtk_imu_engineering import (
|
||||
MIN_SEGMENT_DURATION_S,
|
||||
MIN_SEGMENT_NODE_COUNT,
|
||||
_all_hpr,
|
||||
_height_reference,
|
||||
_hpr_factor_observation,
|
||||
_node_interval_threshold_s,
|
||||
_trajectory_continuity_reasons,
|
||||
_nearest_index,
|
||||
_nodes,
|
||||
_position_valid,
|
||||
_segments,
|
||||
_source_nodes,
|
||||
)
|
||||
from imu_lidar.rtk_imu_multisource import _f, _truth, load_unified_sessions
|
||||
|
||||
|
||||
RAW_GAP_S = 1.5
|
||||
|
||||
|
||||
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 hasattr(value, "__dataclass_fields__"):
|
||||
return {key: _jsonable(item) for key, item in asdict(value).items()}
|
||||
if isinstance(value, dict):
|
||||
return {str(key): _jsonable(item) for key, item in value.items()}
|
||||
if isinstance(value, (tuple, list)):
|
||||
return [_jsonable(item) for item in value]
|
||||
return value
|
||||
|
||||
|
||||
def _trapz(values: np.ndarray, t_s: np.ndarray) -> np.ndarray:
|
||||
if t_s.size < 2:
|
||||
return np.zeros(values.shape[1], dtype=float)
|
||||
return np.trapezoid(values, t_s, axis=0)
|
||||
|
||||
|
||||
def _interval_imu(session, start_s: float, end_s: float) -> tuple[np.ndarray, np.ndarray]:
|
||||
mask = (session.imu.t_s >= start_s) & (session.imu.t_s <= end_s)
|
||||
return session.imu.t_s[mask], session.imu.gyro_rad_s[mask]
|
||||
|
||||
|
||||
def _gyro_metrics(session, start_s: float, end_s: float, gyro_offset_rad_s: np.ndarray | None = None) -> dict[str, object]:
|
||||
t_s, gyro = _interval_imu(session, start_s, end_s)
|
||||
if gyro_offset_rad_s is not None:
|
||||
gyro = gyro - np.asarray(gyro_offset_rad_s, dtype=float).reshape(1, 3)
|
||||
if t_s.size < 2:
|
||||
nan = np.full(3, np.nan)
|
||||
return {
|
||||
"sample_count": int(t_s.size), "net_rotation_xyz_deg": nan,
|
||||
"unwrap_rotation_range_xyz_deg": nan,
|
||||
"cumulative_absolute_rotation_xyz_deg": nan,
|
||||
"gyro_integral_squared_xyz_rad2_s": nan,
|
||||
"gyro_rms_xyz_deg_s": nan, "gyro_peak_xyz_deg_s": nan,
|
||||
}
|
||||
dt = np.diff(t_s)
|
||||
midpoint = 0.5 * (gyro[:-1] + gyro[1:])
|
||||
trajectory = np.vstack([np.zeros(3), np.cumsum(midpoint * dt[:, None], axis=0)])
|
||||
# The integrated trajectory is continuous. Explicit unwrap documents that
|
||||
# the yaw range is never inferred from a wrapped heading/Euler endpoint.
|
||||
trajectory[:, 2] = np.unwrap(trajectory[:, 2])
|
||||
duration = float(t_s[-1] - t_s[0])
|
||||
return {
|
||||
"sample_count": int(t_s.size),
|
||||
"net_rotation_xyz_deg": np.degrees(trajectory[-1]),
|
||||
"unwrap_rotation_range_xyz_deg": np.degrees(np.ptp(trajectory, axis=0)),
|
||||
"cumulative_absolute_rotation_xyz_deg": np.degrees(_trapz(np.abs(gyro), t_s)),
|
||||
"gyro_integral_squared_xyz_rad2_s": _trapz(gyro * gyro, t_s),
|
||||
"gyro_rms_xyz_deg_s": np.degrees(np.sqrt(_trapz(gyro * gyro, t_s) / duration)),
|
||||
"gyro_peak_xyz_deg_s": np.degrees(np.max(np.abs(gyro), axis=0)),
|
||||
}
|
||||
|
||||
|
||||
def _interval_summary(session, start_s: float, end_s: float, *, label: str,
|
||||
best_rows: Iterable[dict[str, str]], hpr) -> dict[str, object]:
|
||||
best = list(best_rows)
|
||||
in_range = [row for row in best if start_s <= _f(row, "t_device_s") <= end_s]
|
||||
doppler = [
|
||||
row for row in in_range
|
||||
if _truth(row, "doppler_velocity_valid") and np.all(np.isfinite([
|
||||
_f(row, "velocity_east_m_s"), _f(row, "velocity_north_m_s"),
|
||||
_f(row, "vertical_speed_m_s"),
|
||||
]))
|
||||
]
|
||||
q4 = hpr.valid & (hpr.t_s >= start_s) & (hpr.t_s <= end_s)
|
||||
return {
|
||||
"label": label,
|
||||
"start_s": float(start_s), "end_s": float(end_s),
|
||||
"duration_s": float(max(0.0, end_s - start_s)),
|
||||
"bestnava_count": len(in_range), "doppler_count": len(doppler),
|
||||
"q4_hpr_count": int(np.count_nonzero(q4)),
|
||||
"gyro": _gyro_metrics(session, start_s, end_s),
|
||||
}
|
||||
|
||||
|
||||
def _coalesce(records: list[dict[str, object]], *, include: bool, label: str,
|
||||
session, best_rows, hpr) -> list[dict[str, object]]:
|
||||
result: list[dict[str, object]] = []
|
||||
current: list[dict[str, object]] = []
|
||||
key: tuple[str, ...] | None = None
|
||||
for record in records:
|
||||
active = bool(record["accepted"]) == include
|
||||
reasons = tuple(record["reasons"])
|
||||
same = (
|
||||
current and active and key == reasons
|
||||
and float(record["t_s"]) - float(current[-1]["t_s"]) <= RAW_GAP_S
|
||||
)
|
||||
if active and (not current or same):
|
||||
current.append(record)
|
||||
key = reasons
|
||||
continue
|
||||
if current:
|
||||
summary = _interval_summary(
|
||||
session, float(current[0]["t_s"]), float(current[-1]["t_s"]),
|
||||
label=label, best_rows=best_rows, hpr=hpr,
|
||||
)
|
||||
if not include:
|
||||
summary["cut_reason"] = list(key or ())
|
||||
result.append(summary)
|
||||
current = [record] if active else []
|
||||
key = reasons if active else None
|
||||
if current:
|
||||
summary = _interval_summary(
|
||||
session, float(current[0]["t_s"]), float(current[-1]["t_s"]),
|
||||
label=label, best_rows=best_rows, hpr=hpr,
|
||||
)
|
||||
if not include:
|
||||
summary["cut_reason"] = list(key or ())
|
||||
result.append(summary)
|
||||
return result
|
||||
|
||||
|
||||
def _raw_records(session) -> list[dict[str, object]]:
|
||||
"""Raw-valid is position/IMU validity; HPR support remains a separate factor audit."""
|
||||
hpr = _all_hpr(session)
|
||||
rows = session.rtk_by_type.get("BESTNAVA", [])
|
||||
ordered = sorted(rows, key=lambda row: _f(row, "t_device_s"))
|
||||
records: list[dict[str, object]] = []
|
||||
last_t = -np.inf
|
||||
for row in ordered:
|
||||
t_s = _f(row, "t_device_s")
|
||||
reasons: list[str] = []
|
||||
if not np.isfinite(t_s):
|
||||
reasons.append("position_device_time_invalid")
|
||||
elif t_s <= last_t:
|
||||
reasons.append("position_device_time_nonmonotonic")
|
||||
if np.isfinite(t_s):
|
||||
last_t = max(last_t, t_s)
|
||||
if not _truth(row, "checksum_valid"):
|
||||
reasons.append("position_checksum_invalid")
|
||||
if not _truth(row, "position_fixed"):
|
||||
reasons.append("position_not_fixed")
|
||||
if not _position_valid(row, "BESTNAVA"):
|
||||
reasons.append("position_required_field_invalid")
|
||||
imu_index = _nearest_index(session.imu.t_s, t_s, 0.03) if np.isfinite(t_s) else None
|
||||
if imu_index is None:
|
||||
reasons.append("imu_missing_near")
|
||||
_, _, hpr_factor_valid, hpr_method, hpr_gap = _hpr_factor_observation(hpr, t_s)
|
||||
doppler_ok = bool(
|
||||
_truth(row, "doppler_velocity_valid") and np.all(np.isfinite([
|
||||
_f(row, "velocity_east_m_s"), _f(row, "velocity_north_m_s"),
|
||||
_f(row, "vertical_speed_m_s"),
|
||||
]))
|
||||
)
|
||||
records.append({
|
||||
"t_s": t_s, "row": row, "accepted": not reasons,
|
||||
"reasons": sorted(set(reasons)), "doppler_valid": doppler_ok,
|
||||
"hpr_factor_valid": hpr_factor_valid, "hpr_factor_method": hpr_method,
|
||||
"hpr_support_gap_s": hpr_gap,
|
||||
})
|
||||
return records
|
||||
def _r0_runs_and_cuts(session, nodes, hpr, best_rows, period_s: float) -> tuple[list[dict[str, object]], list[dict[str, object]], list[dict[str, object]]]:
|
||||
runs: list[list] = []
|
||||
cuts: list[dict[str, object]] = []
|
||||
intervals: list[dict[str, object]] = []
|
||||
if not nodes:
|
||||
return [], [], []
|
||||
current = [nodes[0]]
|
||||
threshold = _node_interval_threshold_s(period_s)
|
||||
for previous, node in zip(nodes[:-1], nodes[1:]):
|
||||
dt = float(node.t_s - previous.t_s)
|
||||
structural_reasons = list(_trajectory_continuity_reasons(session, previous.t_s, node.t_s, period_s))
|
||||
continuity_break = node.continuity_id != previous.continuity_id
|
||||
reasons = structural_reasons or (["position_source_quality_or_merge_break"] if continuity_break else [])
|
||||
intervals.append({
|
||||
"left_t_s": float(previous.t_s), "right_t_s": float(node.t_s),
|
||||
"dt_s": dt, "threshold_s": threshold,
|
||||
"trajectory_continuous": not structural_reasons,
|
||||
"continuity_id_changed": continuity_break,
|
||||
"cut_reason": reasons,
|
||||
"left_hpr_factor": {"valid": previous.hpr_factor_valid, "method": previous.hpr_factor_method,
|
||||
"support_gap_s": previous.hpr_support_gap_s},
|
||||
"right_hpr_factor": {"valid": node.hpr_factor_valid, "method": node.hpr_factor_method,
|
||||
"support_gap_s": node.hpr_support_gap_s},
|
||||
})
|
||||
if not continuity_break:
|
||||
current.append(node)
|
||||
continue
|
||||
runs.append(current)
|
||||
cuts.append({
|
||||
**_interval_summary(session, previous.t_s, node.t_s, label="r0_cut", best_rows=best_rows, hpr=hpr),
|
||||
"dt_s": dt, "threshold_s": threshold, "cut_reason": reasons,
|
||||
})
|
||||
current = [node]
|
||||
runs.append(current)
|
||||
summaries = [
|
||||
_interval_summary(session, run[0].t_s, run[-1].t_s, label="R0_after_cuts", best_rows=best_rows, hpr=hpr)
|
||||
| {
|
||||
"node_count": len(run), "continuity_id": int(run[0].continuity_id),
|
||||
"bestnava_count": sum(node.source == "BESTNAVA" for node in run),
|
||||
"doppler_count": sum(node.velocity_enu_m_s is not None for node in run),
|
||||
"hpr_factor_count": sum(node.hpr_factor_valid for node in run),
|
||||
"hpr_factor_rejected_count": sum(not node.hpr_factor_valid for node in run),
|
||||
}
|
||||
for run in runs
|
||||
]
|
||||
return summaries, cuts, intervals
|
||||
def _qualified_summary(session, segments, hpr, best_rows) -> list[dict[str, object]]:
|
||||
return [
|
||||
_interval_summary(session, segment.nodes[0].t_s, segment.nodes[-1].t_s,
|
||||
label="qualified_segment", best_rows=best_rows, hpr=hpr)
|
||||
| {
|
||||
"segment_id": segment.segment_id, "node_count": len(segment.nodes),
|
||||
"bestnava_count": sum(node.source == "BESTNAVA" for node in segment.nodes),
|
||||
"doppler_count": sum(node.velocity_enu_m_s is not None for node in segment.nodes),
|
||||
"hpr_factor_count": sum(node.hpr_factor_valid for node in segment.nodes),
|
||||
"hpr_factor_rejected_count": sum(not node.hpr_factor_valid for node in segment.nodes),
|
||||
}
|
||||
for segment in segments
|
||||
]
|
||||
|
||||
def _dropped_r0_runs(session, r0_nodes, qualified, hpr, best_rows) -> list[dict[str, object]]:
|
||||
qualified_ranges = [(s.nodes[0].t_s, s.nodes[-1].t_s) for s in qualified]
|
||||
result: list[dict[str, object]] = []
|
||||
by_id: dict[int, list] = {}
|
||||
for node in r0_nodes:
|
||||
by_id.setdefault(node.continuity_id, []).append(node)
|
||||
for run in by_id.values():
|
||||
start_s, end_s = run[0].t_s, run[-1].t_s
|
||||
retained = any(abs(start_s - left) < 1e-6 and abs(end_s - right) < 1e-6 for left, right in qualified_ranges)
|
||||
if retained:
|
||||
continue
|
||||
reasons = []
|
||||
if len(run) < MIN_SEGMENT_NODE_COUNT:
|
||||
reasons.append("qualified_min_node_count")
|
||||
if end_s - start_s < MIN_SEGMENT_DURATION_S:
|
||||
reasons.append("qualified_min_duration")
|
||||
if not reasons:
|
||||
reasons.append("preintegration_or_segment_validation")
|
||||
result.append({
|
||||
**_interval_summary(session, start_s, end_s, label="dropped_before_qualified",
|
||||
best_rows=best_rows, hpr=hpr),
|
||||
"node_count": len(run), "cut_reason": reasons,
|
||||
})
|
||||
return result
|
||||
|
||||
|
||||
def _interval_overlap(left: dict[str, object], right: dict[str, object]) -> float:
|
||||
return max(0.0, min(float(left["end_s"]), float(right["end_s"])) - max(float(left["start_s"]), float(right["start_s"])))
|
||||
|
||||
|
||||
def _interval_penetration(raw_intervals, r0_intervals, qualified_intervals, cut_intervals):
|
||||
"""Link every raw-valid dynamic interval to its downstream R0/qualified survivors."""
|
||||
result = []
|
||||
for raw in raw_intervals:
|
||||
r0 = [item for item in r0_intervals if _interval_overlap(raw, item) > 0.0 or (
|
||||
item["start_s"] == item["end_s"] and raw["start_s"] <= item["start_s"] <= raw["end_s"]
|
||||
)]
|
||||
qualified = [item for item in qualified_intervals if _interval_overlap(raw, item) > 0.0]
|
||||
cuts = [item for item in cut_intervals if _interval_overlap(raw, item) > 0.0]
|
||||
raw_best = max(int(raw["bestnava_count"]), 1)
|
||||
raw_doppler = max(int(raw["doppler_count"]), 1)
|
||||
r0_duration = sum(_interval_overlap(raw, item) for item in r0)
|
||||
qualified_duration = sum(_interval_overlap(raw, item) for item in qualified)
|
||||
cut_reasons = sorted({reason for item in cuts for reason in item.get("cut_reason", [])})
|
||||
result.append({
|
||||
"raw_start_s": raw["start_s"], "raw_end_s": raw["end_s"],
|
||||
"raw_duration_s": raw["duration_s"], "raw_bestnava_count": raw["bestnava_count"],
|
||||
"raw_doppler_count": raw["doppler_count"], "raw_gyro": raw["gyro"],
|
||||
"R0_overlap_duration_s": r0_duration,
|
||||
"qualified_overlap_duration_s": qualified_duration,
|
||||
"R0_bestnava_count": sum(int(item["bestnava_count"]) for item in r0),
|
||||
"R0_doppler_count": sum(int(item["doppler_count"]) for item in r0),
|
||||
"qualified_bestnava_count": sum(int(item["bestnava_count"]) for item in qualified),
|
||||
"qualified_doppler_count": sum(int(item["doppler_count"]) for item in qualified),
|
||||
"retention": {
|
||||
"raw_to_R0_bestnava": sum(int(item["bestnava_count"]) for item in r0) / raw_best,
|
||||
"raw_to_R0_doppler": sum(int(item["doppler_count"]) for item in r0) / raw_doppler,
|
||||
"raw_to_R0_duration": r0_duration / max(float(raw["duration_s"]), 1e-9),
|
||||
"raw_to_qualified_bestnava": sum(int(item["bestnava_count"]) for item in qualified) / raw_best,
|
||||
"raw_to_qualified_doppler": sum(int(item["doppler_count"]) for item in qualified) / raw_doppler,
|
||||
"raw_to_qualified_duration": qualified_duration / max(float(raw["duration_s"]), 1e-9),
|
||||
},
|
||||
"cut_reason": cut_reasons,
|
||||
})
|
||||
return result
|
||||
|
||||
|
||||
def _union_time_intervals(intervals: list[dict[str, object]]) -> list[tuple[float, float]]:
|
||||
ordered = sorted(
|
||||
(float(item["start_s"]), float(item["end_s"])) for item in intervals
|
||||
if np.isfinite(float(item["start_s"])) and np.isfinite(float(item["end_s"]))
|
||||
)
|
||||
merged: list[list[float]] = []
|
||||
for start_s, end_s in ordered:
|
||||
if end_s < start_s:
|
||||
continue
|
||||
if not merged or start_s > merged[-1][1]:
|
||||
merged.append([start_s, end_s])
|
||||
else:
|
||||
merged[-1][1] = max(merged[-1][1], end_s)
|
||||
return [(start_s, end_s) for start_s, end_s in merged]
|
||||
|
||||
|
||||
def _stage_statistics(session, intervals: list[dict[str, object]]) -> dict[str, object]:
|
||||
"""Audit each stage on unique IMU samples over the union of its time ranges."""
|
||||
total = _stage_total(intervals)
|
||||
union = _union_time_intervals(intervals)
|
||||
selected_count = 0
|
||||
unique_mask = np.zeros(session.imu.t_s.size, dtype=bool)
|
||||
for item in intervals:
|
||||
mask = (session.imu.t_s >= float(item["start_s"])) & (session.imu.t_s <= float(item["end_s"]))
|
||||
selected_count += int(np.count_nonzero(mask))
|
||||
unique_mask |= mask
|
||||
unique_count = int(np.count_nonzero(unique_mask))
|
||||
input_duration = float(sum(max(0.0, float(item["end_s"]) - float(item["start_s"])) for item in intervals))
|
||||
union_duration = float(sum(end_s - start_s for start_s, end_s in union))
|
||||
net = np.zeros(3)
|
||||
unwrap_range_sum = np.zeros(3)
|
||||
cumulative_abs = np.zeros(3)
|
||||
energy = np.zeros(3)
|
||||
peak = np.zeros(3)
|
||||
metric_duration = 0.0
|
||||
for start_s, end_s in union:
|
||||
gyro = _gyro_metrics(session, start_s, end_s)
|
||||
current_net = np.asarray(gyro["net_rotation_xyz_deg"], dtype=float)
|
||||
if not np.all(np.isfinite(current_net)):
|
||||
continue
|
||||
net += current_net
|
||||
unwrap_range_sum += np.asarray(gyro["unwrap_rotation_range_xyz_deg"], dtype=float)
|
||||
cumulative_abs += np.asarray(gyro["cumulative_absolute_rotation_xyz_deg"], dtype=float)
|
||||
energy += np.asarray(gyro["gyro_integral_squared_xyz_rad2_s"], dtype=float)
|
||||
peak = np.maximum(peak, np.asarray(gyro["gyro_peak_xyz_deg_s"], dtype=float))
|
||||
metric_duration += max(0.0, end_s - start_s)
|
||||
total["unique_imu_coverage"] = {
|
||||
"input_interval_count": len(intervals),
|
||||
"union_interval_count": len(union),
|
||||
"input_duration_s": input_duration,
|
||||
"union_duration_s": union_duration,
|
||||
"overlap_duration_s": max(0.0, input_duration - union_duration),
|
||||
"selected_imu_sample_count_before_dedup": selected_count,
|
||||
"unique_imu_sample_count": unique_count,
|
||||
"duplicate_imu_sample_count": selected_count - unique_count,
|
||||
}
|
||||
total["gyro"] = {
|
||||
"net_rotation_xyz_deg": net,
|
||||
"sum_interval_unwrap_rotation_range_xyz_deg": unwrap_range_sum,
|
||||
"cumulative_absolute_rotation_xyz_deg": cumulative_abs,
|
||||
"gyro_integral_squared_xyz_rad2_s": energy,
|
||||
"gyro_rms_xyz_deg_s": np.degrees(np.sqrt(energy / max(metric_duration, 1e-9))),
|
||||
"gyro_peak_xyz_deg_s": peak,
|
||||
}
|
||||
return total
|
||||
|
||||
def _stage_total(intervals: list[dict[str, object]]) -> dict[str, object]:
|
||||
total = {"interval_count": len(intervals), "duration_s": 0.0, "bestnava_count": 0,
|
||||
"doppler_count": 0, "q4_hpr_count": 0}
|
||||
for item in intervals:
|
||||
for key in ("duration_s", "bestnava_count", "doppler_count", "q4_hpr_count"):
|
||||
total[key] += item[key]
|
||||
return total
|
||||
|
||||
|
||||
def _hpr_chain_diagnostics(hpr) -> dict[str, object]:
|
||||
if hpr.t_s.size < 2:
|
||||
return {"sample_count": int(hpr.t_s.size), "pair_count": 0}
|
||||
dt = np.diff(hpr.t_s)
|
||||
finite_vector = np.all(np.isfinite(hpr.baseline_enu), axis=1)
|
||||
dot = np.sum(hpr.baseline_enu[:-1] * hpr.baseline_enu[1:], axis=1)
|
||||
jump_deg = np.degrees(np.arccos(np.clip(dot, -1.0, 1.0)))
|
||||
rate = jump_deg / np.maximum(dt, 1e-12)
|
||||
return {
|
||||
"sample_count": int(hpr.t_s.size),
|
||||
"q4_valid_sample_count": int(np.count_nonzero(hpr.valid)),
|
||||
"pair_count": int(dt.size),
|
||||
"pair_with_invalid_endpoint_count": int(np.count_nonzero(~(hpr.valid[:-1] & hpr.valid[1:]))),
|
||||
"dt_s_p50_p95_max": np.percentile(dt[np.isfinite(dt)], [50.0, 95.0, 100.0]),
|
||||
"dt_too_short_count": int(np.count_nonzero(dt < 0.03)),
|
||||
"dt_too_long_count": int(np.count_nonzero(dt > 0.25)),
|
||||
"baseline_jump_rate_over_45deg_s_count": int(np.count_nonzero(
|
||||
finite_vector[:-1] & finite_vector[1:] & (rate > 45.0)
|
||||
)),
|
||||
}
|
||||
|
||||
|
||||
def _hpr_axis_mapping(session, hpr) -> dict[str, object]:
|
||||
valid = hpr.valid & np.isfinite(hpr.t_s)
|
||||
if np.count_nonzero(valid) < 8:
|
||||
return {"available": False, "reason": "fewer_than_8_q4_hpr_samples"}
|
||||
t = hpr.t_s[valid]
|
||||
dt = np.diff(t)
|
||||
keep = np.r_[True, (dt > 0.03) & (dt <= 0.25)]
|
||||
t = t[keep]
|
||||
# hpr arrays preserve GNHPR order after time sorting; heading must unwrap.
|
||||
hpr_rows = sorted(session.rtk_by_type.get("GNHPR", []), key=lambda row: _f(row, "t_device_s"))
|
||||
heading = np.unwrap(np.deg2rad(np.asarray([_f(row, "heading_deg") for row in hpr_rows])))[valid][keep]
|
||||
pitch = np.asarray([_f(row, "pitch_deg") for row in hpr_rows])[valid][keep]
|
||||
if t.size < 8:
|
||||
return {"available": False, "reason": "insufficient_contiguous_q4_hpr"}
|
||||
heading_rate = np.gradient(heading, t)
|
||||
gyro = np.column_stack([np.interp(t, session.imu.t_s, session.imu.gyro_rad_s[:, axis]) for axis in range(3)])
|
||||
correlation = []
|
||||
for axis in range(3):
|
||||
value = np.corrcoef(heading_rate, gyro[:, axis])[0, 1]
|
||||
correlation.append(float(value) if np.isfinite(value) else np.nan)
|
||||
best_axis = int(np.nanargmax(np.abs(correlation))) if np.any(np.isfinite(correlation)) else None
|
||||
return {
|
||||
"available": best_axis is not None,
|
||||
"hpr_heading_unwrapped_range_deg": float(np.degrees(np.ptp(heading))),
|
||||
"hpr_heading_net_rotation_deg": float(np.degrees(heading[-1] - heading[0])),
|
||||
"hpr_heading_cumulative_absolute_rotation_deg": float(np.degrees(np.sum(np.abs(np.diff(heading))))),
|
||||
"hpr_pitch_range_deg": float(np.ptp(pitch)),
|
||||
"hpr_pitch_cumulative_absolute_change_deg": float(np.sum(np.abs(np.diff(pitch)))),
|
||||
"heading_rate_to_imu_gyro_correlation_xyz": np.asarray(correlation),
|
||||
"best_correlated_imu_axis": best_axis,
|
||||
"expected_z_axis_correlation": correlation[2],
|
||||
"note": "heading is unwrapped; sign depends on GNHPR clockwise-from-north convention",
|
||||
}
|
||||
|
||||
|
||||
def _bias_absorption_check(session, intervals: list[dict[str, object]]) -> dict[str, object]:
|
||||
report = audit_imu(session.imu)
|
||||
checks = []
|
||||
for item in intervals:
|
||||
if item["duration_s"] < 1.0:
|
||||
continue
|
||||
start_s, end_s = item["start_s"], item["end_s"]
|
||||
raw = _gyro_metrics(session, start_s, end_s)
|
||||
static_corrected = _gyro_metrics(session, start_s, end_s, report.gyro_bias_rad_s)
|
||||
t, gyro = _interval_imu(session, start_s, end_s)
|
||||
mean = np.mean(gyro, axis=0) if gyro.size else np.zeros(3)
|
||||
mean_removed = _gyro_metrics(session, start_s, end_s, mean)
|
||||
raw_abs = np.asarray(raw["cumulative_absolute_rotation_xyz_deg"])
|
||||
removed_abs = np.asarray(mean_removed["cumulative_absolute_rotation_xyz_deg"])
|
||||
ratio = removed_abs / np.maximum(raw_abs, 1e-9)
|
||||
checks.append({
|
||||
"start_s": start_s, "end_s": end_s,
|
||||
"static_bias_rad_s": report.gyro_bias_rad_s,
|
||||
"segment_mean_gyro_rad_s": mean,
|
||||
"segment_mean_removed_to_raw_abs_rotation_ratio_xyz": ratio,
|
||||
"static_bias_corrected": static_corrected,
|
||||
"mean_removal_would_absorb_motion": bool(np.any(ratio < 0.5)),
|
||||
})
|
||||
return {"imu_static_audit": report, "interval_checks": checks,
|
||||
"note": "Engineering audit integrates raw gyro; it does not subtract a segment mean."}
|
||||
|
||||
|
||||
def _unit_check(session) -> dict[str, object]:
|
||||
gyro = session.imu.gyro_rad_s
|
||||
norm = np.linalg.norm(gyro, axis=1)
|
||||
p99 = float(np.percentile(norm, 99.0)) if norm.size else np.nan
|
||||
return {
|
||||
"gyro_p99_norm_rad_s": p99,
|
||||
"gyro_p99_norm_deg_s": float(np.degrees(p99)),
|
||||
"gyro_peak_norm_rad_s": float(np.max(norm)) if norm.size else np.nan,
|
||||
"suspect_deg_per_second_stored_as_rad_per_second": bool(np.isfinite(p99) and p99 > 20.0),
|
||||
"suspect_near_zero_gyro_scale": bool(np.isfinite(p99) and p99 < 1e-4),
|
||||
"unit_contract": "unified imu.npz gyro_rad_s is radians per second",
|
||||
}
|
||||
|
||||
|
||||
def _candidate_scores(session_audit: dict[str, object]) -> dict[str, float]:
|
||||
raw = session_audit["raw_valid_intervals"]
|
||||
if not raw:
|
||||
return {"circle": 0.0, "left_right": 0.0, "slope": 0.0}
|
||||
cumulative = np.zeros(3)
|
||||
net = np.zeros(3)
|
||||
for interval in raw:
|
||||
gyro = interval["gyro"]
|
||||
value = np.asarray(gyro["cumulative_absolute_rotation_xyz_deg"], dtype=float)
|
||||
signed = np.asarray(gyro["net_rotation_xyz_deg"], dtype=float)
|
||||
if np.all(np.isfinite(value)):
|
||||
cumulative += value
|
||||
if np.all(np.isfinite(signed)):
|
||||
net += signed
|
||||
axis = session_audit["axis_mapping"]
|
||||
heading_range = abs(float(axis.get("hpr_heading_unwrapped_range_deg", 0.0) or 0.0))
|
||||
heading_abs = abs(float(axis.get("hpr_heading_cumulative_absolute_rotation_deg", 0.0) or 0.0))
|
||||
heading_net = abs(float(axis.get("hpr_heading_net_rotation_deg", 0.0) or 0.0))
|
||||
pitch_range = abs(float(axis.get("hpr_pitch_range_deg", 0.0) or 0.0))
|
||||
pitch_abs = abs(float(axis.get("hpr_pitch_cumulative_absolute_change_deg", 0.0) or 0.0))
|
||||
return {
|
||||
"circle": max(heading_range, cumulative[2]),
|
||||
"left_right": max(0.0, heading_abs - heading_net, cumulative[2] - abs(net[2])),
|
||||
"slope": max(cumulative[0], cumulative[1]) ** 2 / max(1.0, cumulative[2]),
|
||||
}
|
||||
|
||||
|
||||
def _select_candidates(audits: list[dict[str, object]]) -> dict[str, dict[str, object] | None]:
|
||||
remaining = list(audits)
|
||||
chosen: dict[str, dict[str, object] | None] = {}
|
||||
for kind in ("circle", "left_right", "slope"):
|
||||
ranked = sorted(remaining, key=lambda item: item["candidate_scores"][kind], reverse=True)
|
||||
choice = ranked[0] if ranked and ranked[0]["candidate_scores"][kind] > 0.0 else None
|
||||
chosen[kind] = None if choice is None else {
|
||||
"session_id": choice["session_id"], "score_deg": choice["candidate_scores"][kind],
|
||||
"selection_metric": {
|
||||
"circle": "max(unwrapped HPR heading range, raw gyro-z net/absolute rotation)",
|
||||
"left_right": "unwrapped HPR heading cumulative change minus net change, cross-checked with gyro-z",
|
||||
"slope": "tilt-dominance: max(raw gyro-x/y cumulative rotation)^2 / raw gyro-z cumulative rotation",
|
||||
}[kind],
|
||||
}
|
||||
if choice is not None:
|
||||
remaining.remove(choice)
|
||||
return chosen
|
||||
|
||||
|
||||
def _audit_session(session, period_s: float) -> dict[str, object]:
|
||||
reference = _height_reference([session])
|
||||
hpr = _all_hpr(session)
|
||||
best_rows = session.rtk_by_type.get("BESTNAVA", [])
|
||||
records = _raw_records(session)
|
||||
raw_valid = _coalesce(records, include=True, label="raw_valid", session=session,
|
||||
best_rows=best_rows, hpr=hpr)
|
||||
raw_rejected = _coalesce(records, include=False, label="dropped_before_raw_valid", session=session,
|
||||
best_rows=best_rows, hpr=hpr)
|
||||
r0_nodes = [] if reference is None else _nodes(session, reference, period_s)
|
||||
r0_intervals, r0_cuts, r0_node_intervals = _r0_runs_and_cuts(
|
||||
session, r0_nodes, hpr, best_rows, period_s
|
||||
)
|
||||
all_qualified = _segments([session], period_s)
|
||||
qualified = [segment for segment in all_qualified if segment.session_id == session.session_id]
|
||||
qualified_intervals = _qualified_summary(session, qualified, hpr, best_rows)
|
||||
dropped_r0 = _dropped_r0_runs(session, r0_nodes, qualified, hpr, best_rows)
|
||||
r0_selected_times = np.asarray([node.t_s for node in r0_nodes])
|
||||
decimated = []
|
||||
for record in records:
|
||||
if not record["accepted"]:
|
||||
continue
|
||||
t_s = float(record["t_s"])
|
||||
selected = r0_selected_times.size and np.min(np.abs(r0_selected_times - t_s)) < 1e-8
|
||||
if not selected:
|
||||
decimated.append({**record, "accepted": False, "reasons": ["sample_period_decimation"]})
|
||||
decimation_cuts = _coalesce(decimated, include=False, label="dropped_raw_to_R0", session=session,
|
||||
best_rows=best_rows, hpr=hpr)
|
||||
raw_total, r0_total, qualified_total = map(_stage_total, (raw_valid, r0_intervals, qualified_intervals))
|
||||
retention = {
|
||||
"raw_to_R0": {
|
||||
"bestnava_count_ratio": r0_total["bestnava_count"] / max(raw_total["bestnava_count"], 1),
|
||||
"doppler_count_ratio": r0_total["doppler_count"] / max(raw_total["doppler_count"], 1),
|
||||
"duration_ratio": r0_total["duration_s"] / max(raw_total["duration_s"], 1e-9),
|
||||
},
|
||||
"R0_to_qualified": {
|
||||
"bestnava_count_ratio": qualified_total["bestnava_count"] / max(r0_total["bestnava_count"], 1),
|
||||
"doppler_count_ratio": qualified_total["doppler_count"] / max(r0_total["doppler_count"], 1),
|
||||
"duration_ratio": qualified_total["duration_s"] / max(r0_total["duration_s"], 1e-9),
|
||||
},
|
||||
}
|
||||
audit = {
|
||||
"session_id": session.session_id, "batch_id": session.batch_id,
|
||||
"raw_valid_intervals": raw_valid, "R0_after_cuts_intervals": r0_intervals,
|
||||
"qualified_segments": qualified_intervals,
|
||||
"stage_statistics": {
|
||||
"raw_valid": _stage_statistics(session, raw_valid),
|
||||
"R0_after_cuts": _stage_statistics(session, r0_intervals),
|
||||
"qualified": _stage_statistics(session, qualified_intervals),
|
||||
},
|
||||
"retention": retention,
|
||||
"cut_intervals": [*raw_rejected, *decimation_cuts, *r0_cuts, *dropped_r0],
|
||||
"r0_consecutive_node_intervals": r0_node_intervals,
|
||||
"interval_penetration": _interval_penetration(
|
||||
raw_valid, r0_intervals, qualified_intervals,
|
||||
[*raw_rejected, *decimation_cuts, *r0_cuts, *dropped_r0],
|
||||
),
|
||||
"hpr_chain_diagnostics": _hpr_chain_diagnostics(hpr),
|
||||
"axis_mapping": _hpr_axis_mapping(session, hpr),
|
||||
"unit_check": _unit_check(session),
|
||||
"bias_absorption_check": _bias_absorption_check(session, raw_valid),
|
||||
}
|
||||
audit["candidate_scores"] = _candidate_scores(audit)
|
||||
return audit
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--manifest", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument("--sample-period-s", type=float, default=1.0)
|
||||
parser.add_argument("--session", action="append", help="Optional session id; may repeat.")
|
||||
parser.add_argument(
|
||||
"--inventory-only", action="store_true",
|
||||
help="Only scan raw-valid gyro/HPR excitation; skip R0 and qualified-segment work.",
|
||||
)
|
||||
args = parser.parse_args(argv)
|
||||
sessions = load_unified_sessions(
|
||||
args.manifest,
|
||||
selected_session_ids=None if args.session is None else set(args.session),
|
||||
)
|
||||
if args.inventory_only:
|
||||
audits = []
|
||||
for session in sessions:
|
||||
hpr = _all_hpr(session)
|
||||
best_rows = session.rtk_by_type.get("BESTNAVA", [])
|
||||
records = _raw_records(session)
|
||||
raw_valid = _coalesce(records, include=True, label="raw_valid", session=session,
|
||||
best_rows=best_rows, hpr=hpr)
|
||||
audit = {
|
||||
"session_id": session.session_id,
|
||||
"batch_id": session.batch_id,
|
||||
"raw_valid_intervals": raw_valid,
|
||||
"hpr_chain_diagnostics": _hpr_chain_diagnostics(hpr),
|
||||
"axis_mapping": _hpr_axis_mapping(session, hpr),
|
||||
"unit_check": _unit_check(session),
|
||||
}
|
||||
audit["candidate_scores"] = _candidate_scores(audit)
|
||||
audits.append(audit)
|
||||
scope = "raw-valid motion inventory only; no R0/qualified work or optimisation"
|
||||
else:
|
||||
audits = [_audit_session(session, args.sample_period_s) for session in sessions]
|
||||
scope = "motion-excitation penetration audit only; no lever fit/prior/bootstrap/sensitivity"
|
||||
payload = {
|
||||
"scope": scope,
|
||||
"sample_period_s": args.sample_period_s,
|
||||
"session_count": len(audits),
|
||||
"selected_dynamic_candidates": _select_candidates(audits),
|
||||
"sessions": audits,
|
||||
}
|
||||
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({
|
||||
"session_count": len(audits),
|
||||
"selected_dynamic_candidates": payload["selected_dynamic_candidates"],
|
||||
}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,149 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Multi-horizon open-loop RTK/IMU propagation audit without optimization.'''
|
||||
from __future__ import annotations
|
||||
import argparse, json, sys
|
||||
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 imu_lidar.rtk_imu_engineering import (
|
||||
G_ENU, _all_hpr, _height_reference, _hpr_factor_observation, _world_rtk)
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from tools.audit_rtk_imu_factor_consistency import (
|
||||
MECHANICAL_L_I_M, _best_arrays, _jsonable, _nearest_imu, _summary)
|
||||
|
||||
HORIZONS_S = (1.,2.,5.,10.,20.)
|
||||
|
||||
def _angle_deg(left,right):
|
||||
return float(np.degrees(np.arccos(np.clip(np.dot(left,right),-1.,1.))))
|
||||
|
||||
def _scalar_summary(values):
|
||||
a = np.asarray(values,dtype=float)
|
||||
if not a.size:
|
||||
return {'count':0,'rms':np.nan,'p95_abs':np.nan}
|
||||
return {'count':len(a),'rms':float(np.sqrt(np.mean(a*a))),
|
||||
'p95_abs':float(np.percentile(np.abs(a),95.))}
|
||||
|
||||
def _evaluate_session(session,reference,rotation,lever,bg,ba):
|
||||
_,times,p_ant,v_ant = _best_arrays(session,reference)
|
||||
hpr = _all_hpr(session)
|
||||
baseline_I = rotation.T[:,0]
|
||||
values = {h:{'position':[],'velocity':[],'attitude':[],'baseline':[]}
|
||||
for h in HORIZONS_S}
|
||||
pair_pre = []
|
||||
for left,right in zip(times[:-1],times[1:]):
|
||||
if not .5 <= right-left <= 1.75:
|
||||
pair_pre.append(None)
|
||||
else:
|
||||
pair_pre.append(preintegrate_imu(
|
||||
session.imu.t_s,session.imu.gyro_rad_s,session.imu.acc_m_s2,
|
||||
float(left),float(right),bg,ba))
|
||||
for start,t0 in enumerate(times):
|
||||
baseline0,_,valid0,_,_ = _hpr_factor_observation(hpr,float(t0))
|
||||
imu0 = _nearest_imu(session,float(t0))
|
||||
if not valid0 or imu0 is None: continue
|
||||
R0 = _world_rtk(baseline0) @ rotation
|
||||
p_i0 = p_ant[start]-R0@lever
|
||||
v_i0 = v_ant[start]-R0@np.cross(session.imu.gyro_rad_s[imu0]-bg,lever)
|
||||
delta_R, delta_v, delta_p = np.eye(3), np.zeros(3), np.zeros(3)
|
||||
elapsed, matched = 0., set()
|
||||
for end in range(start+1,len(times)):
|
||||
pre = pair_pre[end-1]
|
||||
if pre is None: break
|
||||
delta_p = delta_p + delta_v*pre.duration_s + delta_R@pre.delta_p
|
||||
delta_v = delta_v + delta_R@pre.delta_v
|
||||
delta_R = delta_R@pre.delta_R
|
||||
elapsed = float(times[end]-t0)
|
||||
if elapsed > max(HORIZONS_S)+.25: break
|
||||
candidates = [h for h in HORIZONS_S if h not in matched and abs(elapsed-h) <= .25]
|
||||
if not candidates: continue
|
||||
horizon = min(candidates,key=lambda h:abs(elapsed-h))
|
||||
matched.add(horizon)
|
||||
baseline1,_,valid1,_,_ = _hpr_factor_observation(hpr,float(times[end]))
|
||||
imu1 = _nearest_imu(session,float(times[end]))
|
||||
if not valid1 or imu1 is None: continue
|
||||
p_i1 = p_i0+v_i0*elapsed+.5*G_ENU*elapsed**2+R0@delta_p
|
||||
v_i1 = v_i0+G_ENU*elapsed+R0@delta_v
|
||||
R1 = R0@delta_R
|
||||
predicted_p = p_i1+R1@lever
|
||||
predicted_v = v_i1+R1@np.cross(session.imu.gyro_rad_s[imu1]-bg,lever)
|
||||
observed_R1 = _world_rtk(baseline1)@rotation
|
||||
values[horizon]['position'].append(predicted_p-p_ant[end])
|
||||
values[horizon]['velocity'].append(predicted_v-v_ant[end])
|
||||
values[horizon]['attitude'].append(
|
||||
np.degrees(Rotation.from_matrix(observed_R1.T@R1).magnitude()))
|
||||
values[horizon]['baseline'].append(_angle_deg(R1@baseline_I,baseline1))
|
||||
summary = {str(int(h)):{
|
||||
'position_error_m':_summary(values[h]['position']),
|
||||
'velocity_error_m_s':_summary(values[h]['velocity']),
|
||||
'attitude_level_completed_error_deg':_scalar_summary(values[h]['attitude']),
|
||||
'observable_baseline_angular_error_deg':_scalar_summary(values[h]['baseline'])}
|
||||
for h in HORIZONS_S}
|
||||
return summary, values
|
||||
|
||||
def _summarize_values(values):
|
||||
return {str(int(h)):{
|
||||
'position_error_m':_summary(values[h]['position']),
|
||||
'velocity_error_m_s':_summary(values[h]['velocity']),
|
||||
'attitude_level_completed_error_deg':_scalar_summary(values[h]['attitude']),
|
||||
'observable_baseline_angular_error_deg':_scalar_summary(values[h]['baseline'])}
|
||||
for h in HORIZONS_S}
|
||||
|
||||
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('--session',action='append',required=True)
|
||||
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('--gyro-bias-rad-s',nargs=3,type=float,default=[0.,0.,0.])
|
||||
parser.add_argument('--accel-bias-m-s2',nargs=3,type=float,default=[0.,0.,0.])
|
||||
args = parser.parse_args()
|
||||
sessions = load_unified_sessions(args.manifest,selected_session_ids=set(args.session))
|
||||
reference = _height_reference(sessions)
|
||||
if reference is None: raise RuntimeError('no BEST height reference')
|
||||
rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
lever = np.asarray(args.mechanical_l_I_m)
|
||||
bg, ba = np.asarray(args.gyro_bias_rad_s), np.asarray(args.accel_bias_m_s2)
|
||||
combined = {h:{'position':[],'velocity':[],'attitude':[],'baseline':[]}
|
||||
for h in HORIZONS_S}
|
||||
per_session = {}
|
||||
for session in sessions:
|
||||
per_session[session.session_id], raw = _evaluate_session(
|
||||
session,reference,rotation,lever,bg,ba)
|
||||
for h in HORIZONS_S:
|
||||
for key in combined[h]: combined[h][key].extend(raw[h][key])
|
||||
aggregate = _summarize_values(combined)
|
||||
one, twenty = aggregate['1'], aggregate['20']
|
||||
growth = {
|
||||
'position_p95_ratio_20s_over_1s':twenty['position_error_m']['vector_p95']/one['position_error_m']['vector_p95'],
|
||||
'velocity_p95_ratio_20s_over_1s':twenty['velocity_error_m_s']['vector_p95']/one['velocity_error_m_s']['vector_p95'],
|
||||
'attitude_p95_ratio_20s_over_1s':twenty['observable_baseline_angular_error_deg']['p95_abs']/one['observable_baseline_angular_error_deg']['p95_abs']}
|
||||
significant = bool(
|
||||
growth['position_p95_ratio_20s_over_1s'] >= 3.
|
||||
and twenty['position_error_m']['vector_p95']-one['position_error_m']['vector_p95'] >= .5)
|
||||
payload = {
|
||||
'scope':'open-loop only; no least_squares/free/prior/LOO/bootstrap/sensitivity',
|
||||
'least_squares_called':False,'rotation_source':'R2G_gravity_level_prior',
|
||||
'mechanical_l_I_m':lever,'gyro_bias_rad_s':bg,'accel_bias_m_s2':ba,
|
||||
'bias_source':'current nominal engineering bias; configurable CLI; no fit artifact bias was persisted',
|
||||
'aggregate_by_horizon_s':aggregate,'per_session_by_horizon_s':per_session,
|
||||
'one_second_reanchored_control':{
|
||||
'definition':'each interval restarts from observed BEST p/v and HPR-completed R',
|
||||
'result':one},
|
||||
'growth_20s_over_1s':growth,
|
||||
'significant_accumulated_drift':significant,
|
||||
'legacy_long_segment_deterministic_model_deprecated':significant}
|
||||
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({'aggregate':aggregate,'growth':growth,
|
||||
'legacy_deprecated':significant}),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
if __name__ == '__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,304 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Root-cause audit for paired BEST/Doppler propagation innovations.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
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.rtk_imu_engineering import G0,G_ENU,_height_reference,_nodes,_world_rtk
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem
|
||||
from tools.audit_rtk_imu_factor_consistency import _jsonable
|
||||
from tools.audit_rtk_imu_heldout_innovation import _calibration_biases
|
||||
from tools.audit_rtk_imu_innovation_noise import _factor_report,_interval_innovations
|
||||
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
|
||||
|
||||
def _vector_summary(values):
|
||||
a=np.asarray(values,dtype=float).reshape(-1,3)
|
||||
if not len(a): return {'count':0}
|
||||
norm=np.linalg.norm(a,axis=1)
|
||||
return {'count':len(a),'bias':np.mean(a,axis=0),
|
||||
'axis_rms':np.sqrt(np.mean(a*a,axis=0)),
|
||||
'axis_p95_abs':np.percentile(np.abs(a),95,axis=0),
|
||||
'vector_rms':float(np.sqrt(np.mean(norm*norm))),
|
||||
'vector_p95':float(np.percentile(norm,95)),
|
||||
'empirical_covariance':np.cov(a,rowvar=False)}
|
||||
|
||||
def _correlation(left,right):
|
||||
a,b=np.asarray(left),np.asarray(right); result=np.full(3,np.nan)
|
||||
for axis in range(3):
|
||||
if len(a)>2 and np.std(a[:,axis])>1e-12 and np.std(b[:,axis])>1e-12:
|
||||
result[axis]=np.corrcoef(a[:,axis],b[:,axis])[0,1]
|
||||
return result
|
||||
|
||||
def _root_report(position,velocity):
|
||||
p={x['interval_id']:x for x in position}; v={x['interval_id']:x for x in velocity}
|
||||
ids=sorted(set(p)&set(v)); records=[]
|
||||
for key in ids:
|
||||
left,right=p[key],v[key]; dt=float(left['dt_s'])
|
||||
a_p=2.*np.asarray(left['residual'])/(dt*dt)
|
||||
a_v=np.asarray(right['residual'])/dt
|
||||
R=np.asarray(left['R0_WI'])
|
||||
records.append({**{k:left[k] for k in (
|
||||
'interval_id','session','motion','t_s','speed_bin','gyro_bin')},
|
||||
'dt_s':dt,'a_position':a_p,'a_velocity':a_v,
|
||||
'difference':a_v-a_p,'a_common':.5*(a_p+a_v),
|
||||
'a_common_body':R.T@(.5*(a_p+a_v))})
|
||||
def summarize(items):
|
||||
if not items:
|
||||
return {'interval_count':0,
|
||||
'a_err_from_position_m_s2':{'count':0},
|
||||
'a_err_from_velocity_m_s2':{'count':0},
|
||||
'per_axis_correlation':[np.nan]*3,
|
||||
'mean_direction_cosine':np.nan,
|
||||
'mean_magnitude_ratio_velocity_over_position':np.nan,
|
||||
'difference_velocity_minus_position_m_s2':{'count':0},
|
||||
'common_acceleration_world_m_s2':{'count':0},
|
||||
'common_acceleration_body_m_s2':{'count':0},
|
||||
'equivalent_horizontal_tilt_rad':np.nan,
|
||||
'equivalent_horizontal_tilt_deg':np.nan}
|
||||
ap=[x['a_position'] for x in items]; av=[x['a_velocity'] for x in items]
|
||||
diff=[x['difference'] for x in items]; common=[x['a_common'] for x in items]
|
||||
body=[x['a_common_body'] for x in items]
|
||||
mean_p=np.mean(ap,axis=0); mean_v=np.mean(av,axis=0)
|
||||
denom=np.linalg.norm(mean_p)*np.linalg.norm(mean_v)
|
||||
cosine=float(mean_p@mean_v/denom) if denom>1e-12 else np.nan
|
||||
ratio=float(np.linalg.norm(mean_v)/max(np.linalg.norm(mean_p),1e-12))
|
||||
body_bias=np.mean(body,axis=0)
|
||||
body_std=np.std(body,axis=0)
|
||||
horizontal=float(np.linalg.norm(body_bias[:2]))
|
||||
return {'interval_count':len(items),
|
||||
'a_err_from_position_m_s2':_vector_summary(ap),
|
||||
'a_err_from_velocity_m_s2':_vector_summary(av),
|
||||
'per_axis_correlation':_correlation(ap,av),
|
||||
'mean_direction_cosine':cosine,'mean_magnitude_ratio_velocity_over_position':ratio,
|
||||
'difference_velocity_minus_position_m_s2':_vector_summary(diff),
|
||||
'common_acceleration_world_m_s2':_vector_summary(common),
|
||||
'common_acceleration_body_m_s2':_vector_summary(body),
|
||||
'body_bias_stability_std_over_bias_norm':float(
|
||||
np.linalg.norm(body_std)/max(np.linalg.norm(body_bias),1e-12)),
|
||||
'constant_body_accelerometer_bias_direction_stable':bool(
|
||||
np.linalg.norm(body_std)<=np.linalg.norm(body_bias)),
|
||||
'equivalent_horizontal_tilt_rad':horizontal/G0,
|
||||
'equivalent_horizontal_tilt_deg':float(np.degrees(horizontal/G0)),
|
||||
'gravity_tilt_leakage_magnitude_le_0p5deg':bool(
|
||||
np.degrees(horizontal/G0)<=.5)}
|
||||
def grouped(key):
|
||||
groups={}
|
||||
for record in records: groups.setdefault(record[key],[]).append(record)
|
||||
return {name:summarize(items) for name,items in groups.items()}
|
||||
overall=summarize(records)
|
||||
detection_checks={}
|
||||
if records:
|
||||
mp=np.asarray(overall['a_err_from_position_m_s2']['bias'])
|
||||
mv=np.asarray(overall['a_err_from_velocity_m_s2']['bias'])
|
||||
detection_checks={'mean_direction_cosine_ge_0p95':
|
||||
overall['mean_direction_cosine']>=.95,
|
||||
'mean_magnitude_ratio_in_0p75_1p25':
|
||||
.75<=overall['mean_magnitude_ratio_velocity_over_position']<=1.25,
|
||||
'mean_acceleration_difference_norm_le_0p05_m_s2':
|
||||
np.linalg.norm(mv-mp)<=.05}
|
||||
detected=bool(all(detection_checks.values()))
|
||||
else:
|
||||
detected=False
|
||||
return {'overall':overall,'per_session':grouped('session'),
|
||||
'by_motion_class':grouped('motion'),'by_speed':grouped('speed_bin'),
|
||||
'by_gyro_norm':grouped('gyro_bin'),
|
||||
'common_acceleration_detection_gate':{
|
||||
'thresholds':{'direction_cosine_min':.95,
|
||||
'magnitude_ratio_range':[.75,1.25],
|
||||
'mean_difference_norm_max_m_s2':.05},
|
||||
'checks':detection_checks,'passed':detected},
|
||||
'common_constant_acceleration_error_detected':detected},records
|
||||
|
||||
def _outside_targets(t,intervals):
|
||||
return all(not (start-1.<=t<=end+1.) for start,end in intervals)
|
||||
|
||||
def _physical_static_biases(sessions,reference,R,heldout):
|
||||
ranges={}
|
||||
for item in heldout:
|
||||
ranges.setdefault(item['session_id'],[]).append((item['start_s'],item['end_s']))
|
||||
result={}
|
||||
for session in sessions:
|
||||
estimates=[]; times=[]
|
||||
for node in _nodes(session,reference,1.):
|
||||
if (node.zupt_static and node.hpr_factor_valid and
|
||||
_outside_targets(node.t_s,ranges.get(session.session_id,[]))):
|
||||
R_WI=_world_rtk(node.baseline_enu)@R
|
||||
estimates.append(node.accel_m_s2-R_WI.T@(-G_ENU)); times.append(node.t_s)
|
||||
if len(estimates)>=3:
|
||||
a=np.asarray(estimates)
|
||||
result[session.session_id]={'available':True,'sample_count':len(a),
|
||||
'time_min_s':min(times),'time_max_s':max(times),
|
||||
'physical_accel_bias_m_s2':np.median(a,axis=0),
|
||||
'sample_axis_std_m_s2':np.std(a,axis=0),
|
||||
'method':('target-excluded zupt_static + fixed R2G/HPR gravity; '
|
||||
'horizontal components remain gravity-tilt confounded')}
|
||||
else:
|
||||
result[session.session_id]={'available':False,'sample_count':len(estimates),
|
||||
'reason':'fewer than 3 target-excluded independent static nodes'}
|
||||
return result
|
||||
|
||||
def _bias_distribution(values,key):
|
||||
a=np.asarray([x[key] for x in values.values()],dtype=float)
|
||||
return {'session_count':len(a),'mean':np.mean(a,axis=0),'std':np.std(a,axis=0),
|
||||
'min':np.min(a,axis=0),'max':np.max(a,axis=0),
|
||||
'peak_to_peak':np.ptp(a,axis=0)}
|
||||
|
||||
def main():
|
||||
p=argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument('--manifest',type=Path,required=True)
|
||||
p.add_argument('--calibration-selection',type=Path,required=True)
|
||||
p.add_argument('--all-selection',type=Path,required=True)
|
||||
p.add_argument('--engineering-result',type=Path,required=True)
|
||||
p.add_argument('--output',type=Path,required=True)
|
||||
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
args=p.parse_args()
|
||||
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
|
||||
calibration=json.loads(args.calibration_selection.read_text(encoding='utf-8'))
|
||||
selected=json.loads(args.all_selection.read_text(encoding='utf-8'))
|
||||
calibration_ids={x['candidate_id'] for x in calibration['selected_windows']}
|
||||
heldout=[x for x in selected['selected_windows']
|
||||
if x['candidate_id'] not in calibration_ids]
|
||||
if len(calibration_ids)!=47 or len(heldout)!=267:
|
||||
raise RuntimeError(f'expected 47+267 windows, got {len(calibration_ids)}+{len(heldout)}')
|
||||
lever=np.asarray(engineering['prior_constrained_solution']['result']['final_l_I_m'])
|
||||
frozen=np.array([-.4518015159,-.2644749820,.7314656115])
|
||||
if not np.allclose(lever,frozen,atol=1e-10):
|
||||
raise RuntimeError('candidate lever differs from frozen root-cause value')
|
||||
sessions=load_unified_sessions(
|
||||
args.manifest,selected_session_ids={x['session_id'] for x in heldout})
|
||||
reference=_height_reference(sessions)
|
||||
segments=_restore_segments(sessions,reference,heldout,1.)
|
||||
R=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
problems=[build_problem(segment,R,lever,.006) for segment in segments]
|
||||
calibration_bias=_calibration_biases(engineering,problems)
|
||||
physical_bias=_physical_static_biases(sessions,reference,R,heldout)
|
||||
motion_map={'0808_20260808_092827':'circle',
|
||||
'0808_20260808_082148':'left_right',
|
||||
'0815_20260812_123424':'slope'}
|
||||
|
||||
def evaluate(mode):
|
||||
output={'best_position':[],'doppler':[],'imu_preintegration':[]}
|
||||
for problem in problems:
|
||||
session_id=problem.segment.session_id
|
||||
if mode=='calibration_frozen':
|
||||
bg=calibration_bias[session_id]['gyro_bias_rad_s']
|
||||
ba=calibration_bias[session_id]['accel_bias_m_s2']
|
||||
elif mode=='zero':
|
||||
bg=np.zeros(3); ba=np.zeros(3)
|
||||
else:
|
||||
source=physical_bias[session_id]
|
||||
if not source['available']: continue
|
||||
bg=np.zeros(3); ba=source['physical_accel_bias_m_s2']
|
||||
motion=motion_map.get(session_id,'other_recovered_dynamic')
|
||||
records=_interval_innovations(problem,motion,bg,ba)
|
||||
for key,value in records.items(): output[key].extend(value)
|
||||
root,pairs=_root_report(output['best_position'],output['doppler'])
|
||||
return {'bias_source':mode,
|
||||
'BEST_position_innovation':_factor_report(output['best_position']),
|
||||
'Doppler_innovation':_factor_report(output['doppler']),
|
||||
'IMU_preintegration_innovation':_factor_report(output['imu_preintegration']),
|
||||
'propagation_acceleration_consistency':root},pairs,output
|
||||
|
||||
modes={}; pairs={}; raw={}
|
||||
for mode in ('calibration_frozen','zero','session_static_physical'):
|
||||
modes[mode],pairs[mode],raw[mode]=evaluate(mode)
|
||||
static_ids={x['interval_id'] for x in pairs['session_static_physical']}
|
||||
common_subset={}
|
||||
for mode in ('calibration_frozen','zero','session_static_physical'):
|
||||
pos=[x for x in raw[mode]['best_position'] if x['interval_id'] in static_ids]
|
||||
vel=[x for x in raw[mode]['doppler'] if x['interval_id'] in static_ids]
|
||||
root,_=_root_report(pos,vel)
|
||||
common_subset[mode]={'BEST_position_innovation':_factor_report(pos),
|
||||
'Doppler_innovation':_factor_report(vel),
|
||||
'propagation_acceleration_consistency':root}
|
||||
def bias_norm(report,factor):
|
||||
return float(np.linalg.norm(report[factor]['overall']['innovation_bias']))
|
||||
A,C=common_subset['calibration_frozen'],common_subset['session_static_physical']
|
||||
static_available=bool(static_ids)
|
||||
best_reduction=(bias_norm(C,'BEST_position_innovation')/
|
||||
max(bias_norm(A,'BEST_position_innovation'),1e-12)
|
||||
if static_available else np.inf)
|
||||
doppler_reduction=(bias_norm(C,'Doppler_innovation')/
|
||||
max(bias_norm(A,'Doppler_innovation'),1e-12)
|
||||
if static_available else np.inf)
|
||||
nuisance_transfer=bool(static_available and best_reduction<=.5 and doppler_reduction<=.5)
|
||||
high_gyro={}
|
||||
for factor in ('BEST_position_innovation','Doppler_innovation'):
|
||||
summary=modes['calibration_frozen'][factor]['by_gyro_norm'].get('gyro_ge_0p10',{})
|
||||
limit=.5
|
||||
high_gyro[factor]={'sample_count':summary.get('sample_count',0),
|
||||
'bias_norm':float(np.linalg.norm(summary.get('innovation_bias',[np.inf]*3))),
|
||||
'vector_p95':summary.get('vector_p95',np.inf),
|
||||
'passed':bool(summary.get('sample_count',0)>=20 and
|
||||
np.linalg.norm(summary['innovation_bias'])<=.10 and
|
||||
summary['vector_p95']<=limit)}
|
||||
common_detected=modes['calibration_frozen'][
|
||||
'propagation_acceleration_consistency'][
|
||||
'common_constant_acceleration_error_detected']
|
||||
extrinsic_sensitive=bool(common_detected and
|
||||
all(x['passed'] for x in high_gyro.values()))
|
||||
propagation_passed=bool(nuisance_transfer)
|
||||
graph_bias=engineering['prior_constrained_solution'][
|
||||
'calibration_only_frozen_bias_by_session']
|
||||
graph_distribution=_bias_distribution(graph_bias,'accel_bias_m_s2')
|
||||
physical_available={k:v for k,v in physical_bias.items() if v['available']}
|
||||
physical_values={k:{'accel_bias_m_s2':v['physical_accel_bias_m_s2']}
|
||||
for k,v in physical_available.items()}
|
||||
payload={'scope':'propagation bias root-cause only; frozen extrinsic and covariance',
|
||||
'lever_reoptimized':False,'R2G_refit':False,'covariance_retuned':False,
|
||||
'R0_parser_modified':False,'new_window_selection':False,
|
||||
'data_only_free_bootstrap_LOO_called':False,
|
||||
'fixed_l_I_m':lever,'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
|
||||
'calibration_window_count':47,'heldout_window_count':267,
|
||||
'target_GNSS_observation_used_for_bias_estimation':False,
|
||||
'bias_semantics':{
|
||||
'graph_nuisance_accel_bias':(
|
||||
'node-graph nuisance absorbing IMU/model/attitude effects; '
|
||||
'not assumed transferable physical sensor zero bias'),
|
||||
'physical_IMU_accel_bias':(
|
||||
'target-excluded session static estimate; horizontal components '
|
||||
'remain gravity-tilt confounded')},
|
||||
'graph_nuisance_accel_bias_distribution_m_s2':graph_distribution,
|
||||
'per_session_graph_nuisance_bias':graph_bias,
|
||||
'per_session_static_physical_bias':physical_bias,
|
||||
'static_physical_bias_distribution_m_s2':(
|
||||
_bias_distribution(physical_values,'accel_bias_m_s2')
|
||||
if physical_values else {'session_count':0}),
|
||||
'bias_source_ablation':modes,
|
||||
'common_static_interval_subset_ablation':common_subset,
|
||||
'static_common_subset_interval_count':len(static_ids),
|
||||
'physical_over_calibration_bias_norm_ratio':{
|
||||
'BEST_position':best_reduction,'Doppler':doppler_reduction},
|
||||
'common_constant_acceleration_error_detected':common_detected,
|
||||
'nuisance_bias_transfer_failure_detected':nuisance_transfer,
|
||||
'physical_ba_ablation_available':static_available,
|
||||
'nuisance_bias_transfer_assessment':(
|
||||
'confirmed' if nuisance_transfer else
|
||||
'not_testable_no_independent_static_segments' if not static_available
|
||||
else 'not_confirmed_by_static_ablation'),
|
||||
'lever_sensitive_high_gyro_validation':high_gyro,
|
||||
'independent_propagation_validation_passed':propagation_passed,
|
||||
'independent_extrinsic_sensitive_validation_passed':extrinsic_sensitive,
|
||||
'engineering_translation_accepted':False,
|
||||
'acceptance_modified_by_this_audit':False}
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
compact={'common_constant_acceleration_error_detected':common_detected,
|
||||
'static_session_count':len(physical_available),
|
||||
'static_common_subset_interval_count':len(static_ids),
|
||||
'bias_norm_ratio':payload['physical_over_calibration_bias_norm_ratio'],
|
||||
'nuisance_bias_transfer_failure_detected':nuisance_transfer,
|
||||
'high_gyro':high_gyro,
|
||||
'independent_propagation_validation_passed':propagation_passed,
|
||||
'independent_extrinsic_sensitive_validation_passed':extrinsic_sensitive,
|
||||
'engineering_translation_accepted':False,
|
||||
'calibration_frozen_acceleration':
|
||||
modes['calibration_frozen']['propagation_acceleration_consistency']['overall']}
|
||||
print(json.dumps(_jsonable(compact),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
if __name__=='__main__': raise SystemExit(main())
|
||||
@@ -46,6 +46,9 @@ CSV_FIELDS = [
|
||||
"pitch_deg",
|
||||
"roll_deg",
|
||||
"heading_quality",
|
||||
"heading_satellites",
|
||||
"heading_age_s",
|
||||
"heading_station_id",
|
||||
"heading_valid",
|
||||
"gga_utc",
|
||||
"hpr_utc",
|
||||
@@ -170,6 +173,9 @@ def merged_row(
|
||||
"pitch_deg": hpr_fields.get("pitch_deg"),
|
||||
"roll_deg": hpr_fields.get("roll_deg"),
|
||||
"heading_quality": hpr_fields.get("heading_quality"),
|
||||
"heading_satellites": hpr_fields.get("heading_satellites"),
|
||||
"heading_age_s": hpr_fields.get("heading_age_s"),
|
||||
"heading_station_id": hpr_fields.get("heading_station_id"),
|
||||
"heading_valid": hpr_fields.get("heading_valid"),
|
||||
"gga_utc": fields.get("position_time_utc"),
|
||||
"hpr_utc": hpr_fields.get("position_time_utc"),
|
||||
|
||||
@@ -0,0 +1,290 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Export paired G90/HI13 captures without replacing sensor time by host time."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
import re
|
||||
import sys
|
||||
from collections import Counter
|
||||
from datetime import datetime, timedelta, timezone
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from tools.h32_dlog.timeutil import utc_dotnet_ticks_to_unix_s
|
||||
from tools.rscap_v2.capture_format_v2 import file_summary, read_capture
|
||||
from tools.rscap_v2.g90_rtk import RtkSentence, iter_g90_sentences
|
||||
from tools.rscap_v2.hi13_imu import Hi13Sample, iter_hi13_imu_samples
|
||||
from tools.time_alignment import AffineClockModel, fit_affine_clock
|
||||
|
||||
GPS_EPOCH_UNIX_S = datetime(1980, 1, 6, tzinfo=timezone.utc).timestamp()
|
||||
DEFAULT_SOURCES = (
|
||||
("0808", Path("D:/data/raw_serial_capture_v2"), "20260808"),
|
||||
("0815", Path("D:/data/0815/raw_serial_capture_v2"), None),
|
||||
("0819", Path("D:/data/0819/raw_serial_capture_v2"), None),
|
||||
)
|
||||
RTK_FIELDS = [
|
||||
"message_type", "t_device_s", "measurement_utc_s", "gnss_time_s",
|
||||
"gnss_week", "gnss_tow_ms", "leap_seconds", "host_receive_utc_s",
|
||||
"host_minus_measurement_s", "receive_utc_ticks", "checksum_valid",
|
||||
"position_time_utc", "lat_deg", "lon_deg", "altitude_m",
|
||||
"position_status", "position_type", "position_fixed", "fix_quality",
|
||||
"satellites", "solution_satellites", "hdop", "undulation_m",
|
||||
"lat_std_m", "lon_std_m", "altitude_std_m", "differential_age_s",
|
||||
"solution_age_s", "station_id", "heading_deg", "pitch_deg", "roll_deg",
|
||||
"heading_quality", "heading_satellites", "heading_age_s",
|
||||
"heading_station_id", "heading_valid", "baseline_length_m",
|
||||
"heading_type", "heading_solution_satellites", "velocity_status",
|
||||
"velocity_type", "doppler_velocity_valid", "velocity_latency_s",
|
||||
"velocity_age_s", "horizontal_speed_m_s", "track_ground_deg",
|
||||
"velocity_east_m_s", "velocity_north_m_s", "vertical_speed_m_s",
|
||||
"horizontal_speed_std_m_s", "vertical_speed_std_m_s", "raw_line",
|
||||
]
|
||||
|
||||
|
||||
def _capture_time(path: Path) -> datetime:
|
||||
match = re.search(r"_(\d{8}-\d{6}\.\d+)_", path.name)
|
||||
if not match:
|
||||
raise ValueError(f"capture filename has no timestamp: {path}")
|
||||
return datetime.strptime(match.group(1), "%Y%m%d-%H%M%S.%f")
|
||||
|
||||
|
||||
def discover_pairs(
|
||||
sources: tuple[tuple[str, Path, str | None], ...],
|
||||
*,
|
||||
max_start_delta_s: float = 1.5,
|
||||
) -> tuple[list[dict], list[dict]]:
|
||||
pairs: list[dict] = []
|
||||
unmatched: list[dict] = []
|
||||
for batch, folder, date_prefix in sources:
|
||||
rtk = sorted(folder.glob("wheeltec-g90*.rscap"))
|
||||
imu = sorted(folder.glob("hi13*.rscap"))
|
||||
if date_prefix:
|
||||
rtk = [path for path in rtk if date_prefix in path.name]
|
||||
imu = [path for path in imu if date_prefix in path.name]
|
||||
available = set(imu)
|
||||
for rtk_path in rtk:
|
||||
candidates = sorted(
|
||||
(
|
||||
(abs((_capture_time(path) - _capture_time(rtk_path)).total_seconds()), path)
|
||||
for path in available
|
||||
),
|
||||
key=lambda item: item[0],
|
||||
)
|
||||
if not candidates or candidates[0][0] > max_start_delta_s:
|
||||
unmatched.append({"batch": batch, "rtk_rscap": str(rtk_path)})
|
||||
continue
|
||||
delta, imu_path = candidates[0]
|
||||
available.remove(imu_path)
|
||||
stamp = _capture_time(rtk_path).strftime("%Y%m%d_%H%M%S")
|
||||
pairs.append(
|
||||
{
|
||||
"session_id": f"{batch}_{stamp}",
|
||||
"batch_id": batch,
|
||||
"rtk_rscap": rtk_path,
|
||||
"imu_rscap": imu_path,
|
||||
"capture_start_delta_s": delta,
|
||||
}
|
||||
)
|
||||
return pairs, unmatched
|
||||
|
||||
|
||||
def _fit_clock(samples: list[Hi13Sample]) -> AffineClockModel:
|
||||
stride = max(1, len(samples) // 20000)
|
||||
selected = samples[::stride]
|
||||
device = np.asarray([sample.t_s for sample in selected], dtype=float)
|
||||
host = np.asarray(
|
||||
[utc_dotnet_ticks_to_unix_s(sample.host_receive_utc_ticks) for sample in selected],
|
||||
dtype=float,
|
||||
)
|
||||
return fit_affine_clock(device, host)
|
||||
|
||||
|
||||
def _nmea_utc_to_unix_s(value: str, host_s: float) -> float:
|
||||
packed = float(value)
|
||||
hour = int(packed // 10000)
|
||||
minute = int((packed - hour * 10000) // 100)
|
||||
second = packed - hour * 10000 - minute * 100
|
||||
receive = datetime.fromtimestamp(host_s, tz=timezone.utc)
|
||||
midnight = datetime(receive.year, receive.month, receive.day, tzinfo=timezone.utc).timestamp()
|
||||
same_day = midnight + hour * 3600 + minute * 60 + second
|
||||
return min((same_day - 86400.0, same_day, same_day + 86400.0),
|
||||
key=lambda candidate: abs(candidate - host_s))
|
||||
|
||||
|
||||
def _measurement_time(row: RtkSentence) -> tuple[float, float | None]:
|
||||
fields = row.fields
|
||||
week = fields.get("gnss_week")
|
||||
tow_ms = fields.get("gnss_tow_ms")
|
||||
leap = fields.get("leap_seconds")
|
||||
if week is not None and tow_ms is not None:
|
||||
gnss_s = float(week) * 604800.0 + float(tow_ms) * 1e-3
|
||||
utc_s = GPS_EPOCH_UNIX_S + gnss_s - float(leap or 0)
|
||||
return utc_s, gnss_s
|
||||
value = fields.get("position_time_utc")
|
||||
if value is None:
|
||||
raise ValueError(f"{row.sentence_type} has no sensor measurement time")
|
||||
host_s = utc_dotnet_ticks_to_unix_s(row.receive_utc_ticks)
|
||||
return _nmea_utc_to_unix_s(str(value), host_s), None
|
||||
|
||||
|
||||
def _format(value: object) -> object:
|
||||
if value is None:
|
||||
return ""
|
||||
if isinstance(value, bool):
|
||||
return int(value)
|
||||
return value
|
||||
|
||||
|
||||
def _write_imu_npz(path: Path, samples: list[Hi13Sample]) -> None:
|
||||
np.savez_compressed(
|
||||
path,
|
||||
system_time_s=np.asarray([row.t_s for row in samples], dtype=np.float64),
|
||||
system_time_ms=np.asarray([row.system_time_ms for row in samples], dtype=np.uint32),
|
||||
host_receive_utc_s=np.asarray(
|
||||
[utc_dotnet_ticks_to_unix_s(row.host_receive_utc_ticks) for row in samples],
|
||||
dtype=np.float64,
|
||||
),
|
||||
gyro_rad_s=np.asarray([row.gyro_rad_s for row in samples], dtype=np.float64),
|
||||
accel_m_s2=np.asarray([row.accel_m_s2 for row in samples], dtype=np.float64),
|
||||
rpy_deg=np.asarray([row.rpy_deg for row in samples], dtype=np.float64),
|
||||
quaternion_wxyz=np.asarray([row.quaternion_wxyz for row in samples], dtype=np.float64),
|
||||
mag_ut=np.asarray([row.mag_ut for row in samples], dtype=np.float64),
|
||||
pps_sync_stamp_ms=np.asarray([row.pps_sync_stamp_ms for row in samples], dtype=np.uint16),
|
||||
temperature_c=np.asarray([row.temperature_c for row in samples], dtype=np.int16),
|
||||
air_pressure_pa=np.asarray([row.air_pressure_pa for row in samples], dtype=np.float64),
|
||||
frame_tag=np.asarray([row.frame_tag for row in samples], dtype=np.uint8),
|
||||
)
|
||||
|
||||
|
||||
def _write_rtk_csv(
|
||||
path: Path,
|
||||
rows: list[RtkSentence],
|
||||
clock: AffineClockModel,
|
||||
) -> Counter:
|
||||
counts: Counter = Counter()
|
||||
with path.open("w", encoding="utf-8", newline="") as stream:
|
||||
writer = csv.DictWriter(stream, fieldnames=RTK_FIELDS)
|
||||
writer.writeheader()
|
||||
for row in rows:
|
||||
host_s = utc_dotnet_ticks_to_unix_s(row.receive_utc_ticks)
|
||||
try:
|
||||
measurement_s, gnss_s = _measurement_time(row)
|
||||
t_device_s = clock.inverse(measurement_s)
|
||||
host_minus_measurement_s = host_s - measurement_s
|
||||
except (TypeError, ValueError):
|
||||
measurement_s = None
|
||||
gnss_s = None
|
||||
t_device_s = None
|
||||
host_minus_measurement_s = None
|
||||
counts[f"{row.sentence_type}_invalid_measurement_time"] += 1
|
||||
fields = dict(row.fields)
|
||||
fields.update(
|
||||
{
|
||||
"message_type": row.sentence_type,
|
||||
"t_device_s": t_device_s,
|
||||
"measurement_utc_s": measurement_s,
|
||||
"gnss_time_s": gnss_s,
|
||||
"host_receive_utc_s": host_s,
|
||||
"host_minus_measurement_s": host_minus_measurement_s,
|
||||
"receive_utc_ticks": row.receive_utc_ticks,
|
||||
"checksum_valid": row.checksum_valid,
|
||||
"raw_line": row.raw_line,
|
||||
}
|
||||
)
|
||||
writer.writerow({name: _format(fields.get(name)) for name in RTK_FIELDS})
|
||||
counts[row.sentence_type] += 1
|
||||
return counts
|
||||
|
||||
|
||||
def export_pair(pair: dict, output_root: Path, *, overwrite: bool) -> dict:
|
||||
destination = output_root / str(pair["session_id"])
|
||||
if destination.exists() and not overwrite:
|
||||
raise FileExistsError(f"{destination} exists; pass --overwrite")
|
||||
destination.mkdir(parents=True, exist_ok=True)
|
||||
imu_capture = read_capture(pair["imu_rscap"])
|
||||
rtk_capture = read_capture(pair["rtk_rscap"])
|
||||
imu_rows = iter_hi13_imu_samples(imu_capture)
|
||||
if len(imu_rows) < 2:
|
||||
raise ValueError(f"not enough CRC-valid HI91 rows: {pair['imu_rscap']}")
|
||||
if np.any(np.diff(np.asarray([row.t_s for row in imu_rows])) <= 0):
|
||||
raise ValueError(f"HI13 system_time is not strictly increasing: {pair['imu_rscap']}")
|
||||
clock = _fit_clock(imu_rows)
|
||||
rtk_rows = iter_g90_sentences(rtk_capture)
|
||||
_write_imu_npz(destination / "imu.npz", imu_rows)
|
||||
counts = _write_rtk_csv(destination / "rtk.csv", rtk_rows, clock)
|
||||
quaternion = np.asarray([row.quaternion_wxyz for row in imu_rows], dtype=float)
|
||||
quaternion_norm = np.linalg.norm(quaternion, axis=1)
|
||||
summary = {
|
||||
**{key: str(value) if isinstance(value, Path) else value for key, value in pair.items()},
|
||||
"time_policy": {
|
||||
"master": "HI13 system_time; RTK GNSS measurement time mapped into that clock",
|
||||
"host_receive_time": "diagnostic and affine cross-clock bridge only",
|
||||
},
|
||||
"imu_capture": file_summary(imu_capture),
|
||||
"rtk_capture": file_summary(rtk_capture),
|
||||
"imu_valid_rows": len(imu_rows),
|
||||
"imu_time_span_s": float(imu_rows[-1].t_s - imu_rows[0].t_s),
|
||||
"quaternion_norm_p01_p50_p99": [
|
||||
float(np.percentile(quaternion_norm, percentile)) for percentile in (1, 50, 99)
|
||||
],
|
||||
"rtk_records": dict(counts),
|
||||
"clock_model_device_to_host": clock.to_dict(),
|
||||
}
|
||||
(destination / "export_summary.json").write_text(
|
||||
json.dumps(summary, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
|
||||
)
|
||||
return summary
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--output-root", type=Path, required=True)
|
||||
parser.add_argument("--overwrite", action="store_true")
|
||||
parser.add_argument("--resume", action="store_true")
|
||||
parser.add_argument("--session", action="append")
|
||||
args = parser.parse_args(argv)
|
||||
pairs, unmatched = discover_pairs(DEFAULT_SOURCES)
|
||||
if args.session:
|
||||
selected = set(args.session)
|
||||
pairs = [pair for pair in pairs if pair["session_id"] in selected]
|
||||
args.output_root.mkdir(parents=True, exist_ok=True)
|
||||
summaries = []
|
||||
for index, pair in enumerate(pairs, 1):
|
||||
print(f"[{index}/{len(pairs)}] {pair['session_id']}", flush=True)
|
||||
summary_path = args.output_root / pair["session_id"] / "export_summary.json"
|
||||
if args.resume and summary_path.is_file():
|
||||
summaries.append(json.loads(summary_path.read_text(encoding="utf-8")))
|
||||
continue
|
||||
summaries.append(export_pair(pair, args.output_root, overwrite=args.overwrite))
|
||||
manifest = {
|
||||
"schema_version": 3,
|
||||
"session_count": len(summaries),
|
||||
"unmatched_rtk": unmatched,
|
||||
"sessions": [
|
||||
{
|
||||
"session_id": item["session_id"],
|
||||
"batch_id": item["batch_id"],
|
||||
"directory": str((args.output_root / item["session_id"]).resolve()),
|
||||
"rtk_records": item["rtk_records"],
|
||||
"imu_valid_rows": item["imu_valid_rows"],
|
||||
}
|
||||
for item in summaries
|
||||
],
|
||||
}
|
||||
(args.output_root / "manifest.json").write_text(
|
||||
json.dumps(manifest, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
|
||||
)
|
||||
print(json.dumps({"output_root": str(args.output_root), **manifest}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,139 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Combine final mechanical-prior engineering release gates without refitting.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
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.geometry import make_transform
|
||||
from tools.audit_rtk_imu_factor_consistency import _jsonable
|
||||
|
||||
SUMMARY=('Translation is mechanically anchored and dynamically validated. '
|
||||
'The current dataset does not independently observe translation accurately '
|
||||
'enough for data-only calibration, and does not provide meaningful refinement '
|
||||
'beyond the mechanical prior.')
|
||||
|
||||
def main():
|
||||
p=argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument('--calibration',type=Path,required=True)
|
||||
p.add_argument('--heldout-postfit',type=Path,required=True)
|
||||
p.add_argument('--innovation',type=Path,required=True)
|
||||
p.add_argument('--sensitivity',type=Path,required=True)
|
||||
p.add_argument('--convergence-retry',type=Path,required=True)
|
||||
p.add_argument('--propagation-root-cause',type=Path)
|
||||
p.add_argument('--output',type=Path,required=True)
|
||||
args=p.parse_args()
|
||||
calibration=json.loads(args.calibration.read_text(encoding='utf-8'))
|
||||
heldout=json.loads(args.heldout_postfit.read_text(encoding='utf-8'))
|
||||
innovation=json.loads(args.innovation.read_text(encoding='utf-8'))
|
||||
sensitivity=json.loads(args.sensitivity.read_text(encoding='utf-8'))
|
||||
retry=json.loads(args.convergence_retry.read_text(encoding='utf-8'))
|
||||
root_cause=(None if args.propagation_root_cause is None else
|
||||
json.loads(args.propagation_root_cause.read_text(encoding='utf-8')))
|
||||
lever=np.asarray(calibration['prior_constrained_solution']['result']['final_l_I_m'])
|
||||
R=Rotation.from_euler('xyz',[.4543066225,-.0026392019,.0122384129],
|
||||
degrees=True).as_matrix()
|
||||
candidate_T=make_transform(-R@lever,R)
|
||||
candidate_inverse=np.linalg.inv(candidate_T)
|
||||
inverse_error=float(np.linalg.norm(candidate_T@candidate_inverse-np.eye(4)))
|
||||
if inverse_error>=1e-10: raise RuntimeError('candidate transforms are not inverse')
|
||||
comparison=calibration['comparisons']
|
||||
def nonconflicting(name):
|
||||
value=comparison[name]
|
||||
return (value['relative_cost_delta']<=.05 and
|
||||
all(x['p95_abs_delta']<=.25
|
||||
for x in value['factor_residual_delta'].values()))
|
||||
mechanical_consistent=bool(
|
||||
nonconflicting('fixed_vs_free') and nonconflicting('prior_data_vs_free'))
|
||||
overall=heldout['heldout_validation']['overall']
|
||||
physical_checks={
|
||||
'all_267_converged_after_retry':
|
||||
retry['heldout_convergence_after_retry']==1.,
|
||||
'BEST_position_vector_p95_le_0p20_m':
|
||||
overall['best_position_physical_m']['vector_p95']<=.20,
|
||||
'Doppler_vector_p95_le_0p50_m_s':
|
||||
overall['doppler_physical_m_s']['vector_p95']<=.50,
|
||||
'HPR_normalized_p95_le_4':
|
||||
overall['residual_by_factor']['hpr']['p95_abs']<=4.,
|
||||
'preintegration_normalized_p95_le_3':
|
||||
overall['residual_by_factor']['imu_preintegration']['p95_abs']<=3.}
|
||||
physical_passed=bool(all(physical_checks.values()))
|
||||
statistical_passed=bool(.25<=overall['global_chi_square_per_dof']<=4.)
|
||||
underdispersion=bool(physical_passed and not statistical_passed and
|
||||
overall['global_chi_square_per_dof']<.25)
|
||||
independent=bool(innovation['independent_heldout_innovation_passed'])
|
||||
rotation=bool(sensitivity['rotation_sensitivity_passed'])
|
||||
accepted=bool(mechanical_consistent and physical_passed and independent and rotation)
|
||||
payload={'scope':'final mechanical-prior RTK-IMU engineering release decision',
|
||||
'no_refit_performed':True,'data_only_full_free_called':False,
|
||||
'bootstrap_called':False,'loo_called':False,
|
||||
'covariance_retuned':False,'new_window_selection_called':False,
|
||||
'parser_R0_modified':False,
|
||||
'data_only_translation_accepted':False,
|
||||
'translation_refined_by_data':False,
|
||||
'mechanical_prior_consistent_with_calibration':mechanical_consistent,
|
||||
'heldout_physical_validation_passed':physical_passed,
|
||||
'heldout_physical_gate_checks':physical_checks,
|
||||
'heldout_statistical_scale_passed':statistical_passed,
|
||||
'heldout_postfit_chi_square_per_dof':
|
||||
overall['global_chi_square_per_dof'],
|
||||
'heldout_covariance_underdispersion_warning':underdispersion,
|
||||
'independent_heldout_innovation_passed':independent,
|
||||
'common_constant_acceleration_error_detected':(
|
||||
None if root_cause is None else
|
||||
root_cause['common_constant_acceleration_error_detected']),
|
||||
'independent_propagation_validation_passed':(
|
||||
None if root_cause is None else
|
||||
root_cause['independent_propagation_validation_passed']),
|
||||
'independent_extrinsic_sensitive_validation_passed':(
|
||||
None if root_cause is None else
|
||||
root_cause['independent_extrinsic_sensitive_validation_passed']),
|
||||
'rotation_sensitivity_passed':rotation,
|
||||
'engineering_translation_acceptance_formula':(
|
||||
'mechanical_prior_consistent_with_calibration AND '
|
||||
'heldout_physical_validation_passed AND '
|
||||
'independent_heldout_innovation_passed AND rotation_sensitivity_passed'),
|
||||
'engineering_translation_accepted':accepted,
|
||||
'result_nature':'mechanically anchored + dynamically validated',
|
||||
'summary':SUMMARY,'forbidden_descriptions':[
|
||||
'data-only calibrated translation','dynamically refined mechanical lever'],
|
||||
'candidate_l_I_engineering_m':lever,
|
||||
'candidate_T_RTK_IMU':candidate_T,
|
||||
'candidate_T_IMU_RTK':candidate_inverse,
|
||||
'candidate_transform_inverse_error_norm':inverse_error,
|
||||
'l_I_engineering_m':lever if accepted else None,
|
||||
'T_RTK_IMU':candidate_T if accepted else None,
|
||||
'T_IMU_RTK':candidate_inverse if accepted else None,
|
||||
'transform_convention':{
|
||||
'equation':'p_RTK = R_RTK_IMU * p_IMU + t_RTK_IMU',
|
||||
'translation':'t_RTK_IMU = -R_RTK_IMU * l_I',
|
||||
'RTK_origin':'ANT1 phase center'},
|
||||
'rotation_source':'R2G_gravity_level_prior',
|
||||
'translation_conditional_on_rotation':True,
|
||||
'evidence':{
|
||||
'calibration_path':str(args.calibration),
|
||||
'heldout_postfit_path':str(args.heldout_postfit),
|
||||
'innovation_path':str(args.innovation),
|
||||
'sensitivity_path':str(args.sensitivity),
|
||||
'convergence_retry_path':str(args.convergence_retry),
|
||||
'propagation_root_cause_path':(
|
||||
None if args.propagation_root_cause is None
|
||||
else str(args.propagation_root_cause)),
|
||||
'posterior_prior_variance_ratio':
|
||||
calibration['prior_constrained_solution']['posterior_prior_variance_ratio'],
|
||||
'heldout_convergence_after_retry':
|
||||
retry['heldout_convergence_after_retry'],
|
||||
'rotation_sensitivity_summary':sensitivity['summary']}}
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
print(json.dumps(_jsonable({key:payload[key] for key in (
|
||||
'data_only_translation_accepted','translation_refined_by_data',
|
||||
'mechanical_prior_consistent_with_calibration',
|
||||
'heldout_physical_validation_passed','heldout_statistical_scale_passed',
|
||||
'heldout_covariance_underdispersion_warning',
|
||||
'independent_heldout_innovation_passed','rotation_sensitivity_passed',
|
||||
'engineering_translation_accepted')}),indent=2))
|
||||
return 0
|
||||
if __name__=='__main__': raise SystemExit(main())
|
||||
@@ -0,0 +1,137 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Refine non-overlapping windows after comparing predicted and actual lever information.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
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.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem,linearized_lever_information
|
||||
from tools.audit_rtk_imu_factor_consistency import _jsonable
|
||||
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
|
||||
from tools.select_rtk_imu_windows_by_lever_information import (
|
||||
STD_GATE,_overlap,_sample_keys,_summary,_window_candidates)
|
||||
|
||||
|
||||
def _sqrt_psd(matrix,inverse=False):
|
||||
values,vectors=np.linalg.eigh(.5*(matrix+matrix.T))
|
||||
values=np.maximum(values,1e-12)
|
||||
scale=1./np.sqrt(values) if inverse else np.sqrt(values)
|
||||
return (vectors*scale)@vectors.T
|
||||
|
||||
|
||||
def main():
|
||||
parser=argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--manifest',type=Path,required=True)
|
||||
parser.add_argument('--base-selection',type=Path,required=True)
|
||||
parser.add_argument('--actual-result',type=Path,required=True)
|
||||
parser.add_argument('--output',type=Path,required=True)
|
||||
parser.add_argument('--sample-period-s',type=float,default=1.)
|
||||
parser.add_argument('--window-duration-s',type=float,default=15.)
|
||||
parser.add_argument('--max-additional-windows',type=int,default=400)
|
||||
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
args=parser.parse_args()
|
||||
base=json.loads(args.base_selection.read_text(encoding='utf-8'))
|
||||
actual=json.loads(args.actual_result.read_text(encoding='utf-8'))
|
||||
actual_solution=next(iter(actual['solutions'].values()))
|
||||
lever=np.asarray(actual_solution['final_l_I_m'],dtype=float)
|
||||
actual_cov=np.asarray(actual_solution['lever_covariance_m2'],dtype=float)
|
||||
actual_information=np.linalg.pinv(actual_cov,rcond=1e-9)
|
||||
predicted_information=sum((np.asarray(item['single_window_information'])
|
||||
for item in base['selected_windows']),np.zeros((3,3)))
|
||||
correction=_sqrt_psd(actual_information)@_sqrt_psd(predicted_information,True)
|
||||
sessions=load_unified_sessions(args.manifest)
|
||||
reference=_height_reference(sessions)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
restored=_restore_segments(sessions,reference,base['selected_windows'],
|
||||
args.sample_period_s)
|
||||
session_by_id={session.session_id:session for session in sessions}
|
||||
selected=[]
|
||||
for item,segment in zip(base['selected_windows'],restored):
|
||||
selected.append({**item,'segment':segment,
|
||||
'session':session_by_id[item['session_id']]})
|
||||
candidates=[entry for entry in _window_candidates(
|
||||
sessions,reference,args.sample_period_s,args.window_duration_s)
|
||||
if not any(_overlap(entry,item) for item in selected)]
|
||||
for entry in candidates:
|
||||
problem=build_problem(entry['segment'],rotation,lever,args.hpr_direct_sigma_rad)
|
||||
raw,_,singular,weak=linearized_lever_information(problem,lever)
|
||||
entry['raw_information']=raw
|
||||
entry['information']=correction@raw@correction.T
|
||||
entry['single_window_singular_values']=singular
|
||||
entry['single_window_weakest_direction_I']=weak
|
||||
total=actual_information.copy()
|
||||
curve=[{'window_count':len(selected),'added_candidate_id':'actual_base',
|
||||
'information_source':'actual_joint_free_schur',**_summary(total)}]
|
||||
remaining=list(candidates); saturation_count=0
|
||||
stop_reason='candidate_exhausted'
|
||||
while remaining and len(selected)<len(base['selected_windows'])+args.max_additional_windows:
|
||||
feasible=[entry for entry in remaining
|
||||
if not any(_overlap(entry,item) for item in selected)]
|
||||
if not feasible: break
|
||||
ranked=[]
|
||||
for entry in feasible:
|
||||
summary=_summary(total+entry['information'])
|
||||
ranked.append((summary['lambda_min'],summary['logdet'],entry,summary))
|
||||
_,_,choice,summary=max(ranked,key=lambda item:(item[0],item[1]))
|
||||
gain=summary['lambda_min']-curve[-1]['lambda_min']
|
||||
selected.append(choice); remaining.remove(choice); total+=choice['information']
|
||||
curve.append({'window_count':len(selected),'added_candidate_id':choice['candidate_id'],
|
||||
'delta_lambda_min':gain,'information_source':'actual-calibrated prediction',
|
||||
**summary})
|
||||
saturation_count=saturation_count+1 if gain<.01 else 0
|
||||
if np.all(np.asarray(summary['std_m'])<=STD_GATE):
|
||||
stop_reason='actual_calibrated_observability_gate_reached'; break
|
||||
if saturation_count>=3:
|
||||
stop_reason='incremental_lambda_min_gain_saturated'; break
|
||||
else:
|
||||
if len(selected)>=len(base['selected_windows'])+args.max_additional_windows:
|
||||
stop_reason='max_additional_windows_reached'
|
||||
seen={'imu':set(),'gnss':set(),'hpr':set()}; duplicate={key:0 for key in seen}
|
||||
output=[]
|
||||
for order,entry in enumerate(selected):
|
||||
keys=_sample_keys(entry); shared={key:len(value&seen[key]) for key,value in keys.items()}
|
||||
for key,value in keys.items(): duplicate[key]+=shared[key]; seen[key].update(value)
|
||||
information=np.asarray(entry['information'] if 'information' in entry
|
||||
else entry['single_window_information'])
|
||||
output.append({'selection_order':order,'candidate_id':entry['candidate_id'],
|
||||
'session_id':entry['session_id'],'start_s':entry['start_s'],
|
||||
'end_s':entry['end_s'],'duration_s':entry['duration_s'],
|
||||
'node_count':entry['node_count'],'seed_window':order<len(base['selected_windows']),
|
||||
'single_window_information':information,
|
||||
'raw_single_window_information':entry.get('raw_information'),
|
||||
'sample_count':{key:len(value) for key,value in keys.items()},
|
||||
'shared_sample_count_with_previous':shared})
|
||||
overlap={'duplicate_sample_count':duplicate,
|
||||
'all_selected_windows_time_nonoverlapping_within_session':not any(
|
||||
_overlap(a,b) for i,a in enumerate(selected) for b in selected[i+1:]),
|
||||
'unique_sample_count':{key:len(value) for key,value in seen.items()}}
|
||||
payload={'scope':'actual-H calibrated incremental lever-information selection',
|
||||
'manual_prior_used':False,'base_window_count':len(base['selected_windows']),
|
||||
'candidate_count':len(candidates),'selected_window_count':len(selected),
|
||||
'actual_base_l_I_m':lever,'actual_base_information':actual_information,
|
||||
'predicted_base_information':predicted_information,
|
||||
'information_congruence_correction':correction,
|
||||
'selection_objective':'maximize lambda_min(H_l); break ties by logdet(H_l)',
|
||||
'std_gate_m':STD_GATE,'stop_reason':stop_reason,
|
||||
'selected_windows':output,'lever_std_vs_information_curve':curve,
|
||||
'sample_overlap_audit':overlap,
|
||||
'final_calibrated_linearized_observable':bool(
|
||||
np.all(np.asarray(curve[-1]['std_m'])<=STD_GATE))}
|
||||
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({'selected_window_count':len(selected),
|
||||
'additional_window_count':len(selected)-len(base['selected_windows']),
|
||||
'stop_reason':stop_reason,'final_curve':curve[-1],
|
||||
'sample_overlap_audit':overlap}),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,104 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Retry only held-out fixed-lever windows that ended at max_nfev.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from concurrent.futures import ProcessPoolExecutor
|
||||
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.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import (
|
||||
build_problem,fit_states_at_fixed_lever,summarize_fixed_state_values)
|
||||
from tools.audit_rtk_imu_factor_consistency import _jsonable
|
||||
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
|
||||
|
||||
def _retry(task):
|
||||
index,problem,lever,state,max_nfev=task
|
||||
value,meta=fit_states_at_fixed_lever(
|
||||
problem,lever,max_nfev,initial_state_values=state)
|
||||
return index,value,meta
|
||||
|
||||
def main():
|
||||
p=argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument('--manifest',type=Path,required=True)
|
||||
p.add_argument('--calibration-selection',type=Path,required=True)
|
||||
p.add_argument('--all-selection',type=Path,required=True)
|
||||
p.add_argument('--engineering-result',type=Path,required=True)
|
||||
p.add_argument('--checkpoint-dir',type=Path,required=True)
|
||||
p.add_argument('--output',type=Path,required=True)
|
||||
p.add_argument('--retry-checkpoint-dir',type=Path,required=True)
|
||||
p.add_argument('--max-nfev',type=int,default=120)
|
||||
p.add_argument('--workers',type=int,default=4)
|
||||
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
args=p.parse_args()
|
||||
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
|
||||
calibration=json.loads(args.calibration_selection.read_text(encoding='utf-8'))
|
||||
selected=json.loads(args.all_selection.read_text(encoding='utf-8'))
|
||||
ids={x['candidate_id'] for x in calibration['selected_windows']}
|
||||
heldout=[x for x in selected['selected_windows'] if x['candidate_id'] not in ids]
|
||||
lever=np.asarray(engineering['engineering_l_I_m'],dtype=float)
|
||||
bad=[]
|
||||
for index,item in enumerate(heldout):
|
||||
data=np.load(args.checkpoint_dir/f'{index:04d}.npz',allow_pickle=False)
|
||||
meta=json.loads(str(data['optimizer']))
|
||||
if not meta['success']:
|
||||
bad.append((index,item,data['state'].copy(),meta))
|
||||
if len(bad)!=10: raise RuntimeError(f'expected 10 non-converged windows, got {len(bad)}')
|
||||
sessions=load_unified_sessions(
|
||||
args.manifest,selected_session_ids={item['session_id'] for _,item,_,_ in bad})
|
||||
reference=_height_reference(sessions)
|
||||
selections=[item for _,item,_,_ in bad]
|
||||
segments=_restore_segments(sessions,reference,selections,1.)
|
||||
R=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
problems=[build_problem(segment,R,lever,.006) for segment in segments]
|
||||
tasks=[(local,problem,lever,bad[local][2],args.max_nfev)
|
||||
for local,problem in enumerate(problems)]
|
||||
with ProcessPoolExecutor(max_workers=args.workers) as executor:
|
||||
retried=list(executor.map(_retry,tasks))
|
||||
retried.sort(key=lambda x:x[0])
|
||||
args.retry_checkpoint_dir.mkdir(parents=True,exist_ok=True)
|
||||
mapping={'0808_20260808_092827':'circle',
|
||||
'0808_20260808_082148':'left_right',
|
||||
'0815_20260812_123424':'slope'}
|
||||
results=[]
|
||||
for local,state,meta in retried:
|
||||
original_index,item,_,previous=bad[local]
|
||||
summary=summarize_fixed_state_values([problems[local]],[state],lever)
|
||||
np.savez_compressed(args.retry_checkpoint_dir/f'{original_index:04d}.npz',
|
||||
state=state,optimizer=json.dumps(meta))
|
||||
results.append({'heldout_index':original_index,
|
||||
'candidate_id':item['candidate_id'],'session_id':item['session_id'],
|
||||
'time_range_s':[item['start_s'],item['end_s']],
|
||||
'motion_class':mapping.get(item['session_id'],'other_recovered_dynamic'),
|
||||
'original_termination_reason':previous['message'],
|
||||
'original_nfev':previous['nfev'],'original_final_cost':previous['cost'],
|
||||
'retry_started_from_original_final_state':True,
|
||||
'retry_termination_reason':meta['message'],'retry_nfev':meta['nfev'],
|
||||
'retry_initial_cost':meta['initial_cost'],'retry_final_cost':meta['cost'],
|
||||
'retry_hit_max_nfev':bool(not meta['success'] and meta['nfev']>=args.max_nfev),
|
||||
'retry_success':meta['success'],'residual':summary})
|
||||
converged=sum(x['retry_success'] for x in results)
|
||||
payload={'scope':'10 held-out max_nfev windows; exact-model continuation retry',
|
||||
'lever_reoptimized':False,'covariance_parameters_modified':False,
|
||||
'parser_R0_modified':False,'windows_removed':False,
|
||||
'fixed_l_I_m':lever,'retry_max_nfev':args.max_nfev,
|
||||
'initial_nonconverged_count':len(results),
|
||||
'converged_after_retry_count':converged,
|
||||
'heldout_convergence_after_retry':(257+converged)/267.,
|
||||
'remaining_nonconverged_count':len(results)-converged,
|
||||
'windows':results}
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
print(json.dumps(_jsonable({'converged_after_retry':converged,
|
||||
'remaining':len(results)-converged,
|
||||
'heldout_convergence_after_retry':payload['heldout_convergence_after_retry'],
|
||||
'windows':[{'index':x['heldout_index'],'session':x['session_id'],
|
||||
'motion':x['motion_class'],'success':x['retry_success'],
|
||||
'nfev':x['retry_nfev'],'cost':x['retry_final_cost']}
|
||||
for x in results]}),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
if __name__=='__main__': raise SystemExit(main())
|
||||
+163
-10
@@ -1,4 +1,9 @@
|
||||
"""Decode Wheeltec G90 NMEA (GGA / GNHPR) from a V2 .rscap capture."""
|
||||
"""Decode calibration-relevant Wheeltec G90 logs from a V2 capture.
|
||||
|
||||
GNSS-owned measurement time is preserved for every record. Host receive time
|
||||
only identifies the chunk that completed the line and must not be substituted
|
||||
for the measurement timestamp.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
@@ -23,6 +28,32 @@ def nmea_checksum_valid(line: str) -> bool:
|
||||
return value == expected
|
||||
|
||||
|
||||
def unicore_checksum_valid(line: str) -> bool:
|
||||
"""Validate the CRC32 suffix used by Unicore hash-prefixed logs."""
|
||||
|
||||
star = line.rfind("*")
|
||||
if star < 0:
|
||||
return False
|
||||
try:
|
||||
expected = int(line[star + 1 : star + 9], 16)
|
||||
except ValueError:
|
||||
return False
|
||||
crc = 0
|
||||
for value in line[1:star].encode("ascii", "replace"):
|
||||
crc ^= value
|
||||
for _ in range(8):
|
||||
crc = (crc >> 1) ^ (0xEDB88320 if crc & 1 else 0)
|
||||
return (crc & 0xFFFFFFFF) == expected
|
||||
|
||||
|
||||
def g90_checksum_valid(line: str) -> bool:
|
||||
if line.startswith("$"):
|
||||
return nmea_checksum_valid(line)
|
||||
if line.startswith("#"):
|
||||
return unicore_checksum_valid(line)
|
||||
return False
|
||||
|
||||
|
||||
def _safe_float(value: str):
|
||||
try:
|
||||
return float(value)
|
||||
@@ -76,8 +107,124 @@ def parse_gnhpr(line: str) -> dict:
|
||||
"pitch_deg": _safe_float(fields[3]),
|
||||
"roll_deg": _safe_float(fields[4]),
|
||||
"heading_quality": quality,
|
||||
"satellites": _safe_int(fields[6]),
|
||||
"heading_valid": quality in {4, 5},
|
||||
"heading_satellites": _safe_int(fields[6]),
|
||||
"heading_age_s": _safe_float(fields[7]) if len(fields) > 7 else None,
|
||||
"heading_station_id": fields[8] if len(fields) > 8 else None,
|
||||
"heading_valid": quality == 4,
|
||||
}
|
||||
|
||||
|
||||
def _split_unicore(line: str) -> tuple[list[str], list[str]]:
|
||||
before_checksum = line[: line.rfind("*")]
|
||||
header, payload = before_checksum.split(";", 1)
|
||||
return header[1:].split(","), payload.split(",")
|
||||
|
||||
|
||||
def _parse_unicore_header(fields: list[str]) -> dict:
|
||||
if len(fields) < 9:
|
||||
raise ValueError("Unicore ASCII header is incomplete")
|
||||
return {
|
||||
"gnss_week": _safe_int(fields[4]),
|
||||
"gnss_tow_ms": _safe_int(fields[5]),
|
||||
"leap_seconds": _safe_int(fields[8]),
|
||||
}
|
||||
|
||||
|
||||
def parse_bestnava(line: str) -> dict:
|
||||
"""Parse BESTNAVA position and Doppler-velocity fields."""
|
||||
|
||||
header, fields = _split_unicore(line)
|
||||
if len(fields) < 30:
|
||||
raise ValueError("BESTNAVA has too few fields")
|
||||
result = {
|
||||
"type": "BESTNAVA",
|
||||
**_parse_unicore_header(header),
|
||||
"position_status": fields[0],
|
||||
"position_type": fields[1],
|
||||
"lat_deg": _safe_float(fields[2]),
|
||||
"lon_deg": _safe_float(fields[3]),
|
||||
"altitude_m": _safe_float(fields[4]),
|
||||
"undulation_m": _safe_float(fields[5]),
|
||||
"lat_std_m": _safe_float(fields[7]),
|
||||
"lon_std_m": _safe_float(fields[8]),
|
||||
"altitude_std_m": _safe_float(fields[9]),
|
||||
"station_id": fields[10].strip('"'),
|
||||
"differential_age_s": _safe_float(fields[11]),
|
||||
"solution_age_s": _safe_float(fields[12]),
|
||||
"satellites": _safe_int(fields[13]),
|
||||
"solution_satellites": _safe_int(fields[14]),
|
||||
"velocity_status": fields[21],
|
||||
"velocity_type": fields[22],
|
||||
"velocity_latency_s": _safe_float(fields[23]),
|
||||
"velocity_age_s": _safe_float(fields[24]),
|
||||
"horizontal_speed_m_s": _safe_float(fields[25]),
|
||||
"track_ground_deg": _safe_float(fields[26]),
|
||||
"vertical_speed_m_s": _safe_float(fields[27]),
|
||||
"vertical_speed_std_m_s": _safe_float(fields[28]),
|
||||
"horizontal_speed_std_m_s": _safe_float(fields[29]),
|
||||
}
|
||||
speed = result["horizontal_speed_m_s"]
|
||||
track = result["track_ground_deg"]
|
||||
if speed is not None and track is not None:
|
||||
angle = math.radians(track)
|
||||
result["velocity_east_m_s"] = speed * math.sin(angle)
|
||||
result["velocity_north_m_s"] = speed * math.cos(angle)
|
||||
else:
|
||||
result["velocity_east_m_s"] = None
|
||||
result["velocity_north_m_s"] = None
|
||||
result["position_fixed"] = (
|
||||
result["position_status"] == "SOL_COMPUTED"
|
||||
and result["position_type"] == "NARROW_INT"
|
||||
)
|
||||
result["doppler_velocity_valid"] = (
|
||||
result["velocity_status"] == "SOL_COMPUTED"
|
||||
and result["velocity_type"] == "DOPPLER_VELOCITY"
|
||||
)
|
||||
return result
|
||||
|
||||
|
||||
def parse_pvtslna(line: str) -> dict:
|
||||
"""Parse PVTSLNA as a quality-rich fallback/diagnostic record."""
|
||||
|
||||
header, fields = _split_unicore(line)
|
||||
if len(fields) < 34:
|
||||
raise ValueError("PVTSLNA has too few fields")
|
||||
speed_north = _safe_float(fields[17])
|
||||
speed_east = _safe_float(fields[18])
|
||||
return {
|
||||
"type": "PVTSLNA",
|
||||
**_parse_unicore_header(header),
|
||||
"position_type": fields[0],
|
||||
"altitude_m": _safe_float(fields[1]),
|
||||
"lat_deg": _safe_float(fields[2]),
|
||||
"lon_deg": _safe_float(fields[3]),
|
||||
"altitude_std_m": _safe_float(fields[4]),
|
||||
"lat_std_m": _safe_float(fields[5]),
|
||||
"lon_std_m": _safe_float(fields[6]),
|
||||
"differential_age_s": _safe_float(fields[7]),
|
||||
"psr_position_type": fields[8],
|
||||
"undulation_m": _safe_float(fields[12]),
|
||||
"satellites": _safe_int(fields[13]),
|
||||
"solution_satellites": _safe_int(fields[14]),
|
||||
"velocity_north_m_s": speed_north,
|
||||
"velocity_east_m_s": speed_east,
|
||||
"horizontal_speed_m_s": (
|
||||
None if speed_north is None or speed_east is None
|
||||
else math.hypot(speed_north, speed_east)
|
||||
),
|
||||
"vertical_speed_m_s": _safe_float(fields[19]),
|
||||
"heading_type": fields[20],
|
||||
"baseline_length_m": _safe_float(fields[21]),
|
||||
"heading_deg": _safe_float(fields[22]),
|
||||
"pitch_deg": _safe_float(fields[23]),
|
||||
"heading_satellites": _safe_int(fields[24]),
|
||||
"heading_solution_satellites": _safe_int(fields[25]),
|
||||
"gdop": _safe_float(fields[28]),
|
||||
"pdop": _safe_float(fields[29]),
|
||||
"hdop": _safe_float(fields[30]),
|
||||
"htdop": _safe_float(fields[31]),
|
||||
"tdop": _safe_float(fields[32]),
|
||||
"position_fixed": fields[0] == "NARROW_INT",
|
||||
}
|
||||
|
||||
|
||||
@@ -105,7 +252,7 @@ class RtkSentence:
|
||||
|
||||
|
||||
def iter_g90_sentences(capture: CaptureFile) -> list[RtkSentence]:
|
||||
"""Parse GGA/GNHPR lines; host time comes from the containing serial chunk."""
|
||||
"""Parse native asynchronous GGA/GNHPR/BESTNAVA/PVTSLNA records."""
|
||||
|
||||
rows: list[RtkSentence] = []
|
||||
for _segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
@@ -122,21 +269,27 @@ def iter_g90_sentences(capture: CaptureFile) -> list[RtkSentence]:
|
||||
if not raw_line:
|
||||
continue
|
||||
line = raw_line.decode("ascii", "replace")
|
||||
if not (line.startswith("$GNGGA") or line.startswith("$GPGGA") or line.startswith("$GNHPR")):
|
||||
parser = None
|
||||
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
||||
parser = parse_gga
|
||||
elif line.startswith("$GNHPR"):
|
||||
parser = parse_gnhpr
|
||||
elif line.startswith("#BESTNAVA"):
|
||||
parser = parse_bestnava
|
||||
elif line.startswith("#PVTSLNA"):
|
||||
parser = parse_pvtslna
|
||||
if parser is None:
|
||||
continue
|
||||
ticks = _host_ticks_for_span(chunks, starts, end)
|
||||
try:
|
||||
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
||||
fields = parse_gga(line)
|
||||
else:
|
||||
fields = parse_gnhpr(line)
|
||||
fields = parser(line)
|
||||
except ValueError:
|
||||
continue
|
||||
rows.append(
|
||||
RtkSentence(
|
||||
sentence_type=str(fields["type"]),
|
||||
receive_utc_ticks=int(ticks),
|
||||
checksum_valid=nmea_checksum_valid(line),
|
||||
checksum_valid=g90_checksum_valid(line),
|
||||
fields=fields,
|
||||
raw_line=line,
|
||||
)
|
||||
|
||||
+57
-18
@@ -12,6 +12,7 @@ HI91 (preferred for calibration):
|
||||
from __future__ import annotations
|
||||
|
||||
import struct
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
@@ -22,6 +23,29 @@ G0 = 9.80665
|
||||
DEG2RAD = np.pi / 180.0
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Hi13Sample:
|
||||
"""One CRC-valid native HI91 record.
|
||||
|
||||
t_s/system_time_ms are sensor-owned. Host receive ticks are retained only
|
||||
for clock diagnostics and cross-clock fitting.
|
||||
"""
|
||||
|
||||
t_s: float
|
||||
system_time_ms: int
|
||||
device_timestamp_us: int
|
||||
host_receive_utc_ticks: int
|
||||
gyro_rad_s: tuple[float, float, float]
|
||||
accel_m_s2: tuple[float, float, float]
|
||||
rpy_deg: tuple[float, float, float]
|
||||
quaternion_wxyz: tuple[float, float, float, float]
|
||||
mag_ut: tuple[float, float, float]
|
||||
pps_sync_stamp_ms: int
|
||||
temperature_c: int
|
||||
air_pressure_pa: float
|
||||
frame_tag: int = 0x91
|
||||
|
||||
|
||||
def crc16_hi13(frame: bytes, payload_length: int) -> int:
|
||||
crc = 0
|
||||
for value in frame[:4]:
|
||||
@@ -41,8 +65,8 @@ def _update_crc16(crc: int, value: int) -> int:
|
||||
return crc
|
||||
|
||||
|
||||
def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[float, float, float], int] | None:
|
||||
"""Return (gyro_rad_s, accel_m_s2, device_timestamp_ms) for a CRC-valid HI91 frame."""
|
||||
def parse_hi91_sample(raw: bytes, host_receive_utc_ticks: int = 0) -> Hi13Sample | None:
|
||||
"""Decode every calibration-relevant field from a CRC-valid HI91 frame."""
|
||||
|
||||
if len(raw) < 6 + 76:
|
||||
return None
|
||||
@@ -54,12 +78,34 @@ def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[floa
|
||||
expected = raw[4] | (raw[5] << 8)
|
||||
if crc16_hi13(raw, payload_length) != expected:
|
||||
return None
|
||||
device_ms = struct.unpack_from("<I", raw, 14)[0]
|
||||
device_ms = int(struct.unpack_from("<I", raw, 14)[0])
|
||||
ax, ay, az = struct.unpack_from("<fff", raw, 18)
|
||||
gx, gy, gz = struct.unpack_from("<fff", raw, 30)
|
||||
gyro = (gx * DEG2RAD, gy * DEG2RAD, gz * DEG2RAD)
|
||||
accel = (ax * G0, ay * G0, az * G0)
|
||||
return gyro, accel, int(device_ms)
|
||||
return Hi13Sample(
|
||||
t_s=float(device_ms) * 1e-3,
|
||||
system_time_ms=device_ms,
|
||||
device_timestamp_us=device_ms * 1000,
|
||||
host_receive_utc_ticks=int(host_receive_utc_ticks),
|
||||
gyro_rad_s=gyro,
|
||||
accel_m_s2=accel,
|
||||
rpy_deg=struct.unpack_from("<fff", raw, 54),
|
||||
quaternion_wxyz=struct.unpack_from("<ffff", raw, 66),
|
||||
mag_ut=struct.unpack_from("<fff", raw, 42),
|
||||
pps_sync_stamp_ms=int(struct.unpack_from("<H", raw, 7)[0]),
|
||||
temperature_c=int(struct.unpack_from("<b", raw, 9)[0]),
|
||||
air_pressure_pa=float(struct.unpack_from("<f", raw, 10)[0]),
|
||||
)
|
||||
|
||||
|
||||
def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[float, float, float], int] | None:
|
||||
"""Backward-compatible compact HI91 decoder."""
|
||||
|
||||
sample = parse_hi91_sample(raw)
|
||||
if sample is None:
|
||||
return None
|
||||
return sample.gyro_rad_s, sample.accel_m_s2, sample.system_time_ms
|
||||
|
||||
|
||||
def iter_hi13_imu_samples(
|
||||
@@ -67,14 +113,14 @@ def iter_hi13_imu_samples(
|
||||
*,
|
||||
host_utc_ticks_min: int | None = None,
|
||||
host_utc_ticks_max: int | None = None,
|
||||
) -> list[ImuSample]:
|
||||
) -> list[Hi13Sample]:
|
||||
"""Return CRC-valid HI91 samples sorted by device timestamp.
|
||||
|
||||
Streams chunk-by-chunk (no giant join) and can skip whole chunks outside the
|
||||
host UTC receive window before parsing.
|
||||
"""
|
||||
|
||||
samples: list[ImuSample] = []
|
||||
samples: list[Hi13Sample] = []
|
||||
carry = b""
|
||||
for chunk in capture.chunks:
|
||||
if host_utc_ticks_min is not None and chunk.receive_utc_ticks < host_utc_ticks_min:
|
||||
@@ -102,25 +148,16 @@ def iter_hi13_imu_samples(
|
||||
if end > len(stream):
|
||||
carry = stream[sync:]
|
||||
break
|
||||
parsed = parse_hi91_frame(stream[sync:end])
|
||||
host_ticks = chunk.receive_utc_ticks
|
||||
parsed = parse_hi91_sample(stream[sync:end], host_ticks)
|
||||
cursor = end
|
||||
if parsed is None:
|
||||
continue
|
||||
gyro, accel, device_ms = parsed
|
||||
host_ticks = chunk.receive_utc_ticks
|
||||
if host_utc_ticks_min is not None and host_ticks < host_utc_ticks_min:
|
||||
continue
|
||||
if host_utc_ticks_max is not None and host_ticks > host_utc_ticks_max:
|
||||
continue
|
||||
samples.append(
|
||||
ImuSample(
|
||||
t_s=float(device_ms) * 1e-3,
|
||||
gyro_rad_s=gyro,
|
||||
accel_m_s2=accel,
|
||||
host_receive_utc_ticks=host_ticks,
|
||||
device_timestamp_us=int(device_ms) * 1000,
|
||||
)
|
||||
)
|
||||
samples.append(parsed)
|
||||
else:
|
||||
carry = b""
|
||||
samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
|
||||
@@ -129,8 +166,10 @@ def iter_hi13_imu_samples(
|
||||
|
||||
__all__ = [
|
||||
"ImuSample",
|
||||
"Hi13Sample",
|
||||
"crc16_hi13",
|
||||
"iter_hi13_imu_samples",
|
||||
"parse_hi91_frame",
|
||||
"parse_hi91_sample",
|
||||
"samples_to_arrays",
|
||||
]
|
||||
|
||||
@@ -12,7 +12,13 @@ ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from imu_lidar.rtk_imu_replay import load_inventory, load_sessions, run_calibration
|
||||
from imu_lidar.rtk_imu_replay import (
|
||||
DEFAULT_RTK_FRAME_DEFINITION,
|
||||
DEFAULT_RTK_REFERENCE_POINT,
|
||||
load_inventory,
|
||||
load_sessions,
|
||||
run_calibration,
|
||||
)
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
@@ -33,8 +39,8 @@ def main(argv: list[str] | None = None) -> int:
|
||||
parser.add_argument("--no-loo", action="store_true")
|
||||
parser.add_argument("--knot-step-s", type=float, default=2.0)
|
||||
parser.add_argument("--per-batch", action="store_true")
|
||||
parser.add_argument("--rtk-frame-definition", default='')
|
||||
parser.add_argument("--rtk-reference-point", default='')
|
||||
parser.add_argument("--rtk-frame-definition", default=DEFAULT_RTK_FRAME_DEFINITION)
|
||||
parser.add_argument("--rtk-reference-point", default=DEFAULT_RTK_REFERENCE_POINT)
|
||||
args = parser.parse_args(argv)
|
||||
|
||||
entries = load_inventory(args.inventory)
|
||||
|
||||
@@ -0,0 +1,125 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run the conditional engineering RTK--IMU 6DoF lever-arm branch."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
from dataclasses import asdict
|
||||
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.rtk_imu_engineering import (
|
||||
DEFAULT_MANUAL_L_I_M,
|
||||
DEFAULT_MANUAL_L_I_COVARIANCE_M2,
|
||||
solve_engineering_6dof,
|
||||
)
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
|
||||
|
||||
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 hasattr(value, "__dataclass_fields__"):
|
||||
return {key: _jsonable(item) for key, item in asdict(value).items()}
|
||||
if isinstance(value, dict):
|
||||
return {str(key): _jsonable(item) for key, item in value.items()}
|
||||
if isinstance(value, (list, tuple)):
|
||||
return [_jsonable(item) for item in value]
|
||||
return value
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--manifest", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument(
|
||||
"--session", action="append", required=True,
|
||||
help="Dynamic/static sessions intentionally admitted to this engineering run.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--rotation-rpy-deg", nargs=3, type=float,
|
||||
default=[0.4543066225, -0.0026392019, 0.0122384129],
|
||||
)
|
||||
parser.add_argument(
|
||||
"--manual-l-i-m", nargs=3, type=float, default=DEFAULT_MANUAL_L_I_M.tolist(),
|
||||
help="Mechanical ANT1 phase-centre lever arm p_ANT1^I in metres.",
|
||||
)
|
||||
prior_group = parser.add_mutually_exclusive_group()
|
||||
prior_group.add_argument(
|
||||
"--manual-l-i-std-m", nargs=3, type=float,
|
||||
default=np.sqrt(np.diag(DEFAULT_MANUAL_L_I_COVARIANCE_M2)).tolist(),
|
||||
help="Mechanical 1-sigma prior standard deviation for lx, ly, lz in metres.",
|
||||
)
|
||||
prior_group.add_argument(
|
||||
"--manual-l-i-covariance-m2", nargs=9, type=float,
|
||||
help="Row-major 3x3 mechanical prior covariance in m^2.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--no-manual-prior", action="store_true",
|
||||
help="Run a free solve only; retain the mechanical reference in no optimisation factor.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--segment-id", action="append",
|
||||
help="Strict-continuity segment id from the excitation audit; may be repeated.",
|
||||
)
|
||||
parser.add_argument("--sample-period-s", type=float, default=0.5)
|
||||
parser.add_argument("--skip-loo", action="store_true")
|
||||
parser.add_argument("--run-bootstrap", action="store_true")
|
||||
parser.add_argument("--bootstrap-repetitions", type=int, default=40)
|
||||
parser.add_argument("--bootstrap-seed", type=int, default=0)
|
||||
parser.add_argument("--run-rotation-sensitivity", action="store_true")
|
||||
args = parser.parse_args(argv)
|
||||
|
||||
manual_l_i_m = None if args.no_manual_prior else args.manual_l_i_m
|
||||
prior_covariance = None
|
||||
if not args.no_manual_prior:
|
||||
if args.manual_l_i_std_m is not None:
|
||||
prior_covariance = np.diag(np.square(args.manual_l_i_std_m))
|
||||
elif args.manual_l_i_covariance_m2 is not None:
|
||||
prior_covariance = np.asarray(args.manual_l_i_covariance_m2, dtype=float).reshape(3, 3)
|
||||
else:
|
||||
parser.error("mechanical prior requires --manual-l-i-std-m or --manual-l-i-covariance-m2")
|
||||
|
||||
sessions = load_unified_sessions(args.manifest, selected_session_ids=set(args.session))
|
||||
rotation = Rotation.from_euler("xyz", args.rotation_rpy_deg, degrees=True).as_matrix()
|
||||
result = solve_engineering_6dof(
|
||||
sessions,
|
||||
R_RTK_IMU=rotation,
|
||||
manual_l_I_m=manual_l_i_m,
|
||||
manual_l_I_covariance_m2=prior_covariance,
|
||||
sample_period_s=args.sample_period_s,
|
||||
run_loo=not args.skip_loo,
|
||||
run_bootstrap=args.run_bootstrap,
|
||||
bootstrap_repetitions=args.bootstrap_repetitions,
|
||||
bootstrap_seed=args.bootstrap_seed,
|
||||
run_rotation_sensitivity=args.run_rotation_sensitivity,
|
||||
selected_segment_ids=None if args.segment_id is None else set(args.segment_id),
|
||||
)
|
||||
payload = {
|
||||
"data_only_6dof_accepted": False,
|
||||
"data_only_translation_accepted": result.data_only_translation_accepted,
|
||||
"engineering_6dof": _jsonable(result),
|
||||
}
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
args.output.write_text(
|
||||
json.dumps(payload, ensure_ascii=False, indent=2, allow_nan=False) + "\n", encoding="utf-8"
|
||||
)
|
||||
print(json.dumps(payload, ensure_ascii=False, indent=2, allow_nan=False))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,254 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run one prior-free engineering base fit on high-excitation qualified segments.
|
||||
|
||||
This diagnostic never adds a mechanical lever factor, never profiles the
|
||||
mechanical reference, and never runs LOO/bootstrap/rotation sensitivity.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
from dataclasses import asdict
|
||||
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.rtk_imu_engineering import (
|
||||
MIN_SEGMENT_DURATION_S,
|
||||
MIN_SEGMENT_NODE_COUNT,
|
||||
NUISANCE_DOF_PER_SEGMENT,
|
||||
_Segment,
|
||||
_fit_segments,
|
||||
_fit_summary,
|
||||
_height_reference,
|
||||
_initial_parameters,
|
||||
_marginal_lever_information,
|
||||
_nodes,
|
||||
_residual,
|
||||
_segment_residual_size,
|
||||
_world_rtk,
|
||||
)
|
||||
from imu_lidar.imu_preintegration import preintegrate_imu
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
|
||||
MANUAL_REFERENCE_L_I_M = np.array([-0.45072, -0.25682, 0.73208], dtype=float)
|
||||
|
||||
|
||||
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 hasattr(value, "__dataclass_fields__"):
|
||||
return {key: _jsonable(item) for key, item in asdict(value).items()}
|
||||
if isinstance(value, dict):
|
||||
return {str(key): _jsonable(item) for key, item in value.items()}
|
||||
if isinstance(value, (tuple, list)):
|
||||
return [_jsonable(item) for item in value]
|
||||
return value
|
||||
|
||||
|
||||
def _r0_segment_candidates(sessions, period_s: float) -> list[tuple[object, tuple, str]]:
|
||||
"""Split R0 first; defer expensive preintegration until a candidate is selected."""
|
||||
reference = _height_reference(sessions)
|
||||
if reference is None:
|
||||
return []
|
||||
candidates: list[tuple[object, tuple, str]] = []
|
||||
for session in sessions:
|
||||
nodes = _nodes(session, reference, period_s)
|
||||
start = 0
|
||||
qualifying_index = 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 = tuple(nodes[start:end])
|
||||
start = end
|
||||
if len(run) < MIN_SEGMENT_NODE_COUNT or run[-1].t_s - run[0].t_s < MIN_SEGMENT_DURATION_S:
|
||||
continue
|
||||
if not any(node.hpr_factor_valid for node in run):
|
||||
continue
|
||||
candidates.append((session, run, f"{session.session_id}:{qualifying_index:02d}"))
|
||||
qualifying_index += 1
|
||||
return candidates
|
||||
|
||||
|
||||
def _build_segment(session, run: tuple, segment_id: str) -> _Segment | None:
|
||||
pre = tuple(preintegrate_imu(
|
||||
session.imu.t_s, session.imu.gyro_rad_s, session.imu.acc_m_s2, left.t_s, right.t_s,
|
||||
) for left, right in zip(run[:-1], run[1:]))
|
||||
if any(item.duration_s <= 0.0 for item in pre):
|
||||
return None
|
||||
initial_hpr = next((node for node in run if node.hpr_factor_valid), None)
|
||||
if initial_hpr is None:
|
||||
return None
|
||||
return _Segment(segment_id, session.session_id, run, pre, _world_rtk(initial_hpr.baseline_enu))
|
||||
|
||||
def _gyro_abs_rotation_deg(session, segment) -> np.ndarray:
|
||||
start_s, end_s = segment.nodes[0].t_s, segment.nodes[-1].t_s
|
||||
mask = (session.imu.t_s >= start_s) & (session.imu.t_s <= end_s)
|
||||
t_s = session.imu.t_s[mask]
|
||||
gyro = session.imu.gyro_rad_s[mask]
|
||||
if t_s.size < 2:
|
||||
return np.zeros(3)
|
||||
return np.degrees(np.trapezoid(np.abs(gyro), t_s, axis=0))
|
||||
|
||||
|
||||
def _category_score(category: str, gyro_abs_deg: np.ndarray) -> float:
|
||||
if category in {"circle", "left_right"}:
|
||||
return float(gyro_abs_deg[2])
|
||||
return float(np.hypot(gyro_abs_deg[0], gyro_abs_deg[1]))
|
||||
|
||||
|
||||
def _marginal_for_segment_indices(jacobian: np.ndarray, residual: np.ndarray,
|
||||
segments, indices: list[int]) -> tuple[np.ndarray, np.ndarray, float, int, np.ndarray]:
|
||||
row = 0
|
||||
rows: list[np.ndarray] = []
|
||||
for index, segment in enumerate(segments):
|
||||
count = _segment_residual_size(segment)
|
||||
if index in indices:
|
||||
rows.append(np.arange(row, row + count))
|
||||
row += count
|
||||
selected_rows = np.concatenate(rows) if rows else np.empty(0, dtype=int)
|
||||
columns = [0, 1, 2]
|
||||
for index in indices:
|
||||
offset = 3 + NUISANCE_DOF_PER_SEGMENT * index
|
||||
columns.extend(range(offset, offset + NUISANCE_DOF_PER_SEGMENT))
|
||||
marginal, singular, condition, rank, weakest, _ = _marginal_lever_information(
|
||||
jacobian[np.ix_(selected_rows, columns)], residual[selected_rows]
|
||||
)
|
||||
return marginal, singular, condition, rank, weakest
|
||||
|
||||
|
||||
def main() -> int:
|
||||
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("--top-per-category", type=int, default=10)
|
||||
parser.add_argument("--sample-period-s", type=float, default=1.0)
|
||||
parser.add_argument("--rotation-rpy-deg", nargs=3, type=float,
|
||||
default=[0.4543066225, -0.0026392019, 0.0122384129])
|
||||
args = parser.parse_args()
|
||||
category_by_session = {
|
||||
args.circle_session: "circle",
|
||||
args.left_right_session: "left_right",
|
||||
args.slope_session: "slope",
|
||||
}
|
||||
sessions = load_unified_sessions(args.manifest, selected_session_ids=set(category_by_session))
|
||||
candidates: dict[str, list[tuple[float, object, tuple, str, np.ndarray]]] = {
|
||||
key: [] for key in category_by_session.values()
|
||||
}
|
||||
all_candidates = _r0_segment_candidates(sessions, args.sample_period_s)
|
||||
for session, run, segment_id in all_candidates:
|
||||
category = category_by_session[session.session_id]
|
||||
gyro_abs = _gyro_abs_rotation_deg(session, type("Run", (), {"nodes": run})())
|
||||
candidates[category].append((_category_score(category, gyro_abs), session, run, segment_id, gyro_abs))
|
||||
selected: list[object] = []
|
||||
selected_entries: list[dict[str, object]] = []
|
||||
category_indices: dict[str, list[int]] = {}
|
||||
for category, entries in candidates.items():
|
||||
selected_before = len(selected)
|
||||
for score, session, run, segment_id, gyro_abs in sorted(entries, key=lambda item: item[0], reverse=True):
|
||||
segment = _build_segment(session, run, segment_id)
|
||||
if segment is None:
|
||||
continue
|
||||
selected.append(segment)
|
||||
selected_entries.append({
|
||||
"category": category, "segment_id": segment.segment_id,
|
||||
"session_id": segment.session_id, "duration_s": segment.nodes[-1].t_s - segment.nodes[0].t_s,
|
||||
"node_count": len(segment.nodes), "excitation_score_deg": score,
|
||||
"cumulative_absolute_gyro_rotation_xyz_deg": gyro_abs,
|
||||
})
|
||||
if len(selected) - selected_before >= args.top_per_category:
|
||||
break
|
||||
category_indices[category] = list(range(selected_before, len(selected)))
|
||||
rotation = Rotation.from_euler("xyz", args.rotation_rpy_deg, degrees=True).as_matrix()
|
||||
initial_parameters = _initial_parameters(selected)
|
||||
initial_residual = _residual(initial_parameters, selected, rotation)
|
||||
fit, residual, detail = _fit_segments(selected, rotation)
|
||||
summary = _fit_summary(fit, residual, detail)
|
||||
if fit is None or summary is None:
|
||||
raise RuntimeError("no selected qualified segment could be fit")
|
||||
contributions: dict[str, dict[str, object]] = {}
|
||||
contribution_sum = np.zeros((3, 3))
|
||||
for category, indices in category_indices.items():
|
||||
marginal, singular, condition, rank, weakest = _marginal_for_segment_indices(
|
||||
fit.jac, residual, selected, indices
|
||||
)
|
||||
contribution_sum += marginal
|
||||
contributions[category] = {
|
||||
"selected_segment_count": len(indices),
|
||||
"lever_marginal_information": marginal,
|
||||
"lever_information_singular_values": singular,
|
||||
"condition_number": condition,
|
||||
"precision_rank": rank,
|
||||
"weakest_direction_I": weakest,
|
||||
"axis_information_diagonal_I": np.diag(marginal),
|
||||
}
|
||||
total_diag = np.diag(summary.lever_marginal_information)
|
||||
for category, item in contributions.items():
|
||||
item["axis_information_fraction_of_total_I"] = np.divide(
|
||||
item["axis_information_diagonal_I"], total_diag,
|
||||
out=np.full(3, np.nan), where=np.abs(total_diag) > 1e-12,
|
||||
)
|
||||
payload = {
|
||||
'solver_diagnostics': {
|
||||
'initial_cost': 0.5 * float(np.dot(initial_residual, initial_residual)),
|
||||
'final_cost': 0.5 * float(np.dot(residual, residual)),
|
||||
'cost_reduction': 0.5 * float(
|
||||
np.dot(initial_residual, initial_residual) - np.dot(residual, residual)
|
||||
),
|
||||
'cost_definition': '0.5 * unmodified residual squared norm, comparable initial/final',
|
||||
'scipy_final_huber_cost': float(fit.cost),
|
||||
'nfev': int(fit.nfev), 'optimality': float(fit.optimality),
|
||||
'gradient_norm': float(np.linalg.norm(fit.grad)),
|
||||
'initial_l_I_m': initial_parameters[:3], 'final_l_I_m': fit.x[:3],
|
||||
'l_step_norm_m': float(np.linalg.norm(fit.x[:3] - initial_parameters[:3])),
|
||||
},
|
||||
"scope": "prior-free free base fit only; no mechanical factor/LOO/bootstrap/rotation sensitivity",
|
||||
"rotation_source": "R2G_gravity_level_prior",
|
||||
"translation_conditional_on_rotation": True,
|
||||
"manual_reference_comparison_only": {
|
||||
"manual_l_I_m": MANUAL_REFERENCE_L_I_M,
|
||||
"free_minus_manual_l_I_m": summary.l_I_m - MANUAL_REFERENCE_L_I_M,
|
||||
"euclidean_delta_m": float(np.linalg.norm(summary.l_I_m - MANUAL_REFERENCE_L_I_M)),
|
||||
},
|
||||
"sample_period_s": args.sample_period_s,
|
||||
"all_qualified_segment_count": len(all_candidates),
|
||||
"selected_segment_count": len(selected),
|
||||
"selected_segments": selected_entries,
|
||||
"free_solution": _jsonable(summary),
|
||||
"motion_category_information_contributions": _jsonable(contributions),
|
||||
"axis_information_total_I": np.diag(summary.lever_marginal_information),
|
||||
"marginal_additivity_error_fro": float(np.linalg.norm(
|
||||
contribution_sum - summary.lever_marginal_information, ord="fro"
|
||||
)),
|
||||
}
|
||||
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({
|
||||
"selected_segment_count": len(selected),
|
||||
"free_l_I_m": _jsonable(summary.l_I_m),
|
||||
"free_l_I_std_m": _jsonable(summary.l_I_std_m),
|
||||
"lever_information_singular_values": _jsonable(summary.lever_information_singular_values),
|
||||
"condition_number": summary.lever_information_condition_number,
|
||||
"precision_rank": summary.lever_precision_rank,
|
||||
"bestnava_xyz_vector_rms_p95_m": [summary.bestnava_xyz_residual.vector_rms, summary.bestnava_xyz_residual.vector_p95],
|
||||
"doppler_vector_rms_p95_m_s": [summary.doppler_velocity_residual.vector_rms, summary.doppler_velocity_residual.vector_p95],
|
||||
}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,144 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Run fixed-mechanical and soft-prior solutions on immutable 47-window baseline.'''
|
||||
from __future__ import annotations
|
||||
import argparse,hashlib,json,sys
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
from dataclasses import asdict
|
||||
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.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import (
|
||||
NODE_DOF,build_problem,fit_states_at_fixed_lever,solve_free_lever_many,
|
||||
summarize_fixed_state_values)
|
||||
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
|
||||
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
|
||||
|
||||
MANUAL_STD_M=np.array([.02,.02,.03])
|
||||
MANUAL_COVARIANCE_M2=np.diag(MANUAL_STD_M**2)
|
||||
|
||||
|
||||
def _factor_delta(candidate,baseline):
|
||||
output={}
|
||||
for key in ('best_position','doppler','hpr','imu_preintegration'):
|
||||
left,right=candidate['residual_by_factor'][key],baseline['residual_by_factor'][key]
|
||||
output[key]={'rms_delta':left['rms']-right['rms'],
|
||||
'p95_abs_delta':left['p95_abs']-right['p95_abs'],
|
||||
'nis_per_dof_delta':left['chi_square_per_dof']-right['chi_square_per_dof']}
|
||||
return output
|
||||
|
||||
|
||||
def main():
|
||||
parser=argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--manifest',type=Path,required=True)
|
||||
parser.add_argument('--selection',type=Path,required=True)
|
||||
parser.add_argument('--free-baseline',type=Path,required=True)
|
||||
parser.add_argument('--output',type=Path,required=True)
|
||||
parser.add_argument('--state-output',type=Path)
|
||||
parser.add_argument('--sample-period-s',type=float,default=1.)
|
||||
parser.add_argument('--fixed-max-nfev',type=int,default=120)
|
||||
parser.add_argument('--prior-max-nfev',type=int,default=120)
|
||||
parser.add_argument('--workers',type=int,default=4)
|
||||
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
args=parser.parse_args()
|
||||
selection=json.loads(args.selection.read_text(encoding='utf-8'))
|
||||
baseline=json.loads(args.free_baseline.read_text(encoding='utf-8'))
|
||||
if len(selection['selected_windows'])!=47:
|
||||
raise RuntimeError('engineering branch requires immutable 47-window selection')
|
||||
ids={item['session_id'] for item in selection['selected_windows']}
|
||||
sessions=load_unified_sessions(args.manifest,selected_session_ids=ids)
|
||||
reference=_height_reference(sessions)
|
||||
segments=_restore_segments(sessions,reference,selection['selected_windows'],
|
||||
args.sample_period_s)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
mechanical=MECHANICAL_L_I_M.copy()
|
||||
problems=[build_problem(segment,rotation,mechanical,args.hpr_direct_sigma_rad)
|
||||
for segment in segments]
|
||||
with ThreadPoolExecutor(max_workers=args.workers) as executor:
|
||||
fitted=list(executor.map(lambda problem:fit_states_at_fixed_lever(
|
||||
problem,mechanical,args.fixed_max_nfev),problems))
|
||||
fixed_states=[item[0] for item in fitted]
|
||||
fixed_optimizer=[item[1] for item in fitted]
|
||||
fixed_summary=summarize_fixed_state_values(problems,fixed_states,mechanical)
|
||||
prior_result,prior_states=solve_free_lever_many(
|
||||
problems,mechanical,args.prior_max_nfev,fixed_states,
|
||||
lever_prior_mean_m=mechanical,
|
||||
lever_prior_covariance_m2=MANUAL_COVARIANCE_M2,
|
||||
return_state_values=True)
|
||||
prior_result=asdict(prior_result)
|
||||
prior_data=summarize_fixed_state_values(
|
||||
problems,prior_states,np.asarray(prior_result['final_l_I_m']))
|
||||
bias_values={}
|
||||
for segment,state in zip(segments,prior_states):
|
||||
states=np.asarray(state).reshape(-1,NODE_DOF)
|
||||
values=bias_values.setdefault(segment.session_id,{'bg':[],'ba':[]})
|
||||
values['bg'].extend(states[:,9:12])
|
||||
values['ba'].extend(states[:,12:15])
|
||||
calibration_bias={session_id:{
|
||||
'gyro_bias_rad_s':np.median(values['bg'],axis=0),
|
||||
'accel_bias_m_s2':np.median(values['ba'],axis=0),
|
||||
'node_count':len(values['bg'])}
|
||||
for session_id,values in bias_values.items()}
|
||||
free=baseline['solutions']['mechanical']
|
||||
posterior_cov=np.asarray(prior_result['lever_covariance_m2'])
|
||||
variance_ratio=np.diag(posterior_cov)/np.diag(MANUAL_COVARIANCE_M2)
|
||||
prior_pull=(np.asarray(prior_result['final_l_I_m'])-mechanical)/MANUAL_STD_M
|
||||
translation_refined=bool(np.all(variance_ratio<=.90))
|
||||
comparisons={'fixed_vs_free':{
|
||||
'cost_delta':fixed_summary['cost']-free['final_cost'],
|
||||
'relative_cost_delta':fixed_summary['cost']/free['final_cost']-1.,
|
||||
'factor_residual_delta':_factor_delta(fixed_summary,free)},
|
||||
'prior_data_vs_free':{
|
||||
'cost_delta':prior_data['cost']-free['final_cost'],
|
||||
'relative_cost_delta':prior_data['cost']/free['final_cost']-1.,
|
||||
'factor_residual_delta':_factor_delta(prior_data,free)}}
|
||||
baseline_hash=hashlib.sha256(args.free_baseline.read_bytes()).hexdigest()
|
||||
payload={'scope':'47-window mechanical-prior engineering branch',
|
||||
'data_only_translation_accepted':False,
|
||||
'immutable_free_baseline':{'path':str(args.free_baseline),
|
||||
'sha256':baseline_hash,'solution':free},
|
||||
'selected_window_count':len(segments),'selection_path':str(args.selection),
|
||||
'manual_l_I_m':mechanical,'manual_l_I_std_m':MANUAL_STD_M,
|
||||
'manual_l_I_covariance_m2':MANUAL_COVARIANCE_M2,
|
||||
'fixed_mechanical_solution':{'l_I_m':mechanical,
|
||||
'window_optimizer':fixed_optimizer,'data_summary':fixed_summary},
|
||||
'prior_constrained_solution':{'result':prior_result,
|
||||
'data_only_summary_excluding_prior_factor':prior_data,
|
||||
'posterior_covariance_m2':posterior_cov,
|
||||
'posterior_prior_variance_ratio':variance_ratio,
|
||||
'prior_pull_sigma':prior_pull,
|
||||
'calibration_only_frozen_bias_by_session':calibration_bias},
|
||||
'comparisons':comparisons,
|
||||
'translation_refinement_gate':{
|
||||
'required_max_axis_variance_ratio':.90,
|
||||
'passed':translation_refined},
|
||||
'translation_refined_by_data':translation_refined,
|
||||
'heldout_validation_called':False,
|
||||
'engineering_translation_accepted':False}
|
||||
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')
|
||||
if args.state_output is not None:
|
||||
sizes=np.asarray([len(value) for value in prior_states],dtype=int)
|
||||
args.state_output.parent.mkdir(parents=True,exist_ok=True)
|
||||
np.savez_compressed(args.state_output,
|
||||
states=np.concatenate(prior_states),offsets=np.cumsum(np.r_[0,sizes]),
|
||||
candidate_ids=np.asarray(
|
||||
[item['candidate_id'] for item in selection['selected_windows']]))
|
||||
print(json.dumps(_jsonable({'fixed':fixed_summary,
|
||||
'prior_l_I_m':prior_result['final_l_I_m'],
|
||||
'prior_data_cost':prior_data['cost'],'prior_map_cost':prior_result['final_cost'],
|
||||
'variance_ratio':variance_ratio,'prior_pull_sigma':prior_pull,
|
||||
'translation_refined_by_data':translation_refined,
|
||||
'fixed_optimizer_failed_count':sum(not item['success'] for item in fixed_optimizer)}),
|
||||
ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,212 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Validate an engineering lever on disjoint held-out windows.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from concurrent.futures import ProcessPoolExecutor,as_completed
|
||||
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.geometry import make_transform
|
||||
from imu_lidar.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem,fit_states_at_fixed_lever,residual
|
||||
from tools.audit_rtk_imu_factor_consistency import _jsonable
|
||||
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
|
||||
|
||||
FACTORS=('best_position','doppler','hpr','imu_preintegration')
|
||||
PHYSICAL=('best_position_physical','doppler_physical','hpr_physical')
|
||||
|
||||
def _fit_checkpoint(task):
|
||||
index,problem,lever,max_nfev,path_text=task
|
||||
path=Path(path_text)
|
||||
if path.exists():
|
||||
data=np.load(path,allow_pickle=False)
|
||||
return index,data['state'],json.loads(str(data['optimizer']))
|
||||
state,meta=fit_states_at_fixed_lever(problem,lever,max_nfev)
|
||||
np.savez_compressed(path,state=state,optimizer=json.dumps(meta))
|
||||
return index,state,meta
|
||||
|
||||
def _stats(values,dof=None):
|
||||
a=np.asarray(values,dtype=float).reshape(-1)
|
||||
if not a.size:
|
||||
return {'count':0,'dof':0,'rms':np.nan,'p50_abs':np.nan,
|
||||
'p95_abs':np.nan,'p99_abs':np.nan,'nis':np.nan,
|
||||
'chi_square_per_dof':np.nan}
|
||||
dof=len(a) if dof is None else max(int(dof),1); nis=float(a@a)
|
||||
return {'count':len(a),'dof':dof,'rms':float(np.sqrt(np.mean(a*a))),
|
||||
'p50_abs':float(np.percentile(np.abs(a),50)),
|
||||
'p95_abs':float(np.percentile(np.abs(a),95)),
|
||||
'p99_abs':float(np.percentile(np.abs(a),99)),
|
||||
'nis':nis,'chi_square_per_dof':nis/dof}
|
||||
|
||||
def _vectors(values):
|
||||
a=np.asarray(values,dtype=float).reshape(-1,3)
|
||||
if not a.size:
|
||||
return {'count':0,'axis_rms':[np.nan]*3,'axis_p95_abs':[np.nan]*3,
|
||||
'vector_rms':np.nan,'vector_p95':np.nan}
|
||||
norm=np.linalg.norm(a,axis=1)
|
||||
return {'count':len(a),'axis_rms':np.sqrt(np.mean(a*a,axis=0)),
|
||||
'axis_p95_abs':np.percentile(np.abs(a),95,axis=0),
|
||||
'vector_rms':float(np.sqrt(np.mean(norm*norm))),
|
||||
'vector_p95':float(np.percentile(norm,95))}
|
||||
|
||||
def _summarize(records):
|
||||
factor={key:[] for key in FACTORS}; physical={key:[] for key in PHYSICAL}
|
||||
joined=[]; state_dimension=0; other_effective=0
|
||||
for record in records:
|
||||
joined.extend(record['all']); state_dimension+=record['state_dimension']
|
||||
other_effective+=record['other_effective']
|
||||
for key in FACTORS: factor[key].extend(record['factor'].get(key,[]))
|
||||
for key in PHYSICAL: physical[key].extend(record['physical'].get(key,[]))
|
||||
effective=sum(2*len(v)//3 if k=='hpr' else len(v)
|
||||
for k,v in factor.items())+other_effective
|
||||
dof=max(effective-state_dimension,1); a=np.asarray(joined,dtype=float)
|
||||
converged=sum(bool(x['optimizer']['success']) for x in records)
|
||||
return {'window_count':len(records),'optimizer_converged_count':converged,
|
||||
'optimizer_converged_fraction':converged/max(len(records),1),
|
||||
'total_residual_dimension':len(a),'state_dimension':state_dimension,
|
||||
'statistical_dof':dof,'total_nis':float(a@a),
|
||||
'global_chi_square_per_dof':float((a@a)/dof),
|
||||
'residual_by_factor':{key:_stats(value,2*len(value)//3 if key=='hpr' else None)
|
||||
for key,value in factor.items()},
|
||||
'best_position_physical_m':_vectors(physical['best_position_physical']),
|
||||
'doppler_physical_m_s':_vectors(physical['doppler_physical']),
|
||||
'hpr_physical_rad':_vectors(physical['hpr_physical'])}
|
||||
|
||||
def _gate(summary):
|
||||
factor=summary['residual_by_factor']
|
||||
checks={'optimizer_converged_fraction_ge_0p90':
|
||||
summary['optimizer_converged_fraction']>=.90,
|
||||
'global_chi_square_per_dof_in_0p25_4':
|
||||
.25<=summary['global_chi_square_per_dof']<=4.,
|
||||
'best_vector_p95_le_0p20_m':
|
||||
summary['best_position_physical_m']['vector_p95']<=.20,
|
||||
'doppler_vector_p95_le_0p50_m_s':
|
||||
summary['doppler_physical_m_s']['vector_p95']<=.50,
|
||||
'hpr_normalized_p95_le_4':factor['hpr']['p95_abs']<=4.,
|
||||
'preintegration_normalized_p95_le_3':
|
||||
factor['imu_preintegration']['p95_abs']<=3.}
|
||||
return {'uses_existing_P0p5_physical_and_statistical_health_gates':True,
|
||||
'checks':checks,'passed':bool(all(checks.values()))}
|
||||
|
||||
def main():
|
||||
p=argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument('--manifest',type=Path,required=True)
|
||||
p.add_argument('--calibration-selection',type=Path,required=True)
|
||||
p.add_argument('--all-selection',type=Path,required=True)
|
||||
p.add_argument('--engineering-result',type=Path,required=True)
|
||||
p.add_argument('--output',type=Path,required=True)
|
||||
p.add_argument('--checkpoint-dir',type=Path,required=True)
|
||||
p.add_argument('--sample-period-s',type=float,default=1.)
|
||||
p.add_argument('--max-nfev',type=int,default=120)
|
||||
p.add_argument('--workers',type=int,default=4)
|
||||
p.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
p.add_argument('--circle-session',default='0808_20260808_092827')
|
||||
p.add_argument('--left-right-session',default='0808_20260808_082148')
|
||||
p.add_argument('--slope-session',default='0815_20260812_123424')
|
||||
args=p.parse_args()
|
||||
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
|
||||
calibration=json.loads(args.calibration_selection.read_text(encoding='utf-8'))
|
||||
all_windows=json.loads(args.all_selection.read_text(encoding='utf-8'))
|
||||
calibration_ids={x['candidate_id'] for x in calibration['selected_windows']}
|
||||
heldout=[x for x in all_windows['selected_windows']
|
||||
if x['candidate_id'] not in calibration_ids]
|
||||
if len(calibration_ids)!=47 or len(heldout)!=267:
|
||||
raise RuntimeError(f'expected 47+267 windows, got {len(calibration_ids)}+{len(heldout)}')
|
||||
shared=sum(x.get('shared_sample_count_with_previous',{}).get(k,0)
|
||||
for x in all_windows['selected_windows'] for k in ('imu','gnss','hpr'))
|
||||
if shared: raise RuntimeError(f'selection contains {shared} shared samples')
|
||||
lever=np.asarray(engineering['prior_constrained_solution']['result']['final_l_I_m'])
|
||||
sessions=load_unified_sessions(
|
||||
args.manifest,selected_session_ids={x['session_id'] for x in heldout})
|
||||
reference=_height_reference(sessions)
|
||||
segments=_restore_segments(sessions,reference,heldout,args.sample_period_s)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
problems=[build_problem(x,rotation,lever,args.hpr_direct_sigma_rad) for x in segments]
|
||||
args.checkpoint_dir.mkdir(parents=True,exist_ok=True)
|
||||
fitted=[None]*len(problems)
|
||||
tasks=[(i,problem,lever,args.max_nfev,
|
||||
str(args.checkpoint_dir/f'{i:04d}.npz'))
|
||||
for i,problem in enumerate(problems)]
|
||||
with ProcessPoolExecutor(max_workers=args.workers) as executor:
|
||||
futures=[executor.submit(_fit_checkpoint,task) for task in tasks]
|
||||
completed=0
|
||||
for future in as_completed(futures):
|
||||
index,state,meta=future.result(); fitted[index]=(state,meta); completed+=1
|
||||
if completed%10==0: print(f'held-out checkpoint {completed}/{len(problems)}',flush=True)
|
||||
mapping={args.circle_session:'circle',args.left_right_session:'left_right',
|
||||
args.slope_session:'slope'}
|
||||
records=[]
|
||||
for item,problem,fit in zip(heldout,problems,fitted):
|
||||
state,optimizer=fit; detail={}
|
||||
all_residual=residual(problem,state,details=detail,lever_override=lever)
|
||||
known=sum(len(detail.get(k,[])) for k in FACTORS)
|
||||
records.append({'candidate_id':item['candidate_id'],
|
||||
'session_id':item['session_id'],
|
||||
'motion_class':mapping.get(item['session_id'],'other_recovered_dynamic'),
|
||||
'start_s':item['start_s'],'end_s':item['end_s'],
|
||||
'state_dimension':len(state),'optimizer':optimizer,
|
||||
'all':all_residual.tolist(),'other_effective':len(all_residual)-known,
|
||||
'factor':{k:detail.get(k,[]) for k in FACTORS},
|
||||
'physical':{k:detail.get(k,[]) for k in PHYSICAL}})
|
||||
overall=_summarize(records)
|
||||
by_session={key:_summarize([x for x in records if x['session_id']==key])
|
||||
for key in sorted({x['session_id'] for x in records})}
|
||||
by_motion={key:_summarize([x for x in records if x['motion_class']==key])
|
||||
for key in sorted({x['motion_class'] for x in records})}
|
||||
overall_gate=_gate(overall)
|
||||
session_gates={k:_gate(v) for k,v in by_session.items()}
|
||||
motion_gates={k:_gate(v) for k,v in by_motion.items()}
|
||||
heldout_passed=bool(overall_gate['passed'] and
|
||||
all(x['passed'] for x in session_gates.values()) and
|
||||
all(x['passed'] for x in motion_gates.values()))
|
||||
comparisons=engineering['comparisons']
|
||||
def nonconflicting(name):
|
||||
c=comparisons[name]
|
||||
return (c['relative_cost_delta']<=.05 and
|
||||
all(x['p95_abs_delta']<=.25 for x in c['factor_residual_delta'].values()))
|
||||
fit_nonconflict=nonconflicting('fixed_vs_free') and nonconflicting('prior_data_vs_free')
|
||||
T_rtk_imu=make_transform(-rotation@lever,rotation)
|
||||
T_imu_rtk=np.linalg.inv(T_rtk_imu)
|
||||
accepted=bool(fit_nonconflict and heldout_passed)
|
||||
payload={**engineering,
|
||||
'scope':'47-window mechanical-prior engineering branch + disjoint held-out validation',
|
||||
'heldout_validation_called':True,
|
||||
'engineering_lever_source':'prior_constrained_solution',
|
||||
'engineering_l_I_m':lever,'calibration_heldout_overlap_count':0,
|
||||
'heldout_window_count':len(heldout),
|
||||
'heldout_validation':{'lever_reoptimized':False,
|
||||
'nuisance_states_optimized_per_window':True,
|
||||
'motion_class_mapping':mapping,
|
||||
'other_sessions_class':'other_recovered_dynamic',
|
||||
'overall':overall,'by_session':by_session,'by_motion_class':by_motion,
|
||||
'overall_gate':overall_gate,'session_gates':session_gates,
|
||||
'motion_class_gates':motion_gates,'passed':heldout_passed},
|
||||
'calibration_fit_nonconflict_gate':{'relative_cost_increase_max':.05,
|
||||
'per_factor_normalized_p95_increase_max':.25,
|
||||
'fixed_passed':nonconflicting('fixed_vs_free'),
|
||||
'prior_passed':nonconflicting('prior_data_vs_free'),
|
||||
'passed':fit_nonconflict},
|
||||
'engineering_translation_accepted':accepted,
|
||||
'data_only_translation_accepted':False,
|
||||
'result_nature':'mechanical lever + dynamic-data consistency validation; not data-only translation calibration',
|
||||
'rotation_source':'R2G_gravity_level_prior',
|
||||
'translation_conditional_on_rotation':True,
|
||||
'candidate_T_RTK_IMU':T_rtk_imu,'candidate_T_IMU_RTK':T_imu_rtk,
|
||||
'T_RTK_IMU':T_rtk_imu if accepted else None,
|
||||
'T_IMU_RTK':T_imu_rtk if accepted else None,
|
||||
'transform_convention':('T_RTK_IMU maps IMU coordinates into ANT1 RTK frame; '
|
||||
'l_I=p_ANT1^I; T_IMU_RTK translation equals l_I')}
|
||||
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({'engineering_l_I_m':lever,
|
||||
'fit_nonconflict':fit_nonconflict,'heldout_overall':overall,
|
||||
'heldout_passed':heldout_passed,
|
||||
'engineering_translation_accepted':accepted}),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
if __name__=='__main__': raise SystemExit(main())
|
||||
@@ -0,0 +1,153 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Run 18 fixed-R2G perturbation prior-constrained node-graph solves.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from concurrent.futures import ProcessPoolExecutor,as_completed
|
||||
from dataclasses import asdict
|
||||
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.geometry import make_transform
|
||||
from imu_lidar.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import (
|
||||
build_problem,solve_free_lever_many,summarize_fixed_state_values)
|
||||
from tools.audit_rtk_imu_factor_consistency import _jsonable
|
||||
from tools.run_rtk_imu_node_graph_free_selected import _restore_segments
|
||||
|
||||
_CONTEXT={}
|
||||
MANUAL=np.array([-.45072,-.25682,.73208])
|
||||
STD=np.array([.02,.02,.03])
|
||||
COV=np.diag(STD**2)
|
||||
|
||||
def _initialize_worker(segments,states,rpy,max_nfev,sigma,checkpoint_dir):
|
||||
_CONTEXT.update(segments=segments,states=states,rpy=np.asarray(rpy),
|
||||
max_nfev=max_nfev,sigma=sigma,checkpoint_dir=Path(checkpoint_dir))
|
||||
|
||||
def _solve(task):
|
||||
axis,sign,angle=task
|
||||
name=f'{axis}_{sign*angle:+.1f}deg'
|
||||
path=_CONTEXT['checkpoint_dir']/f'{name}.json'
|
||||
if path.exists(): return json.loads(path.read_text(encoding='utf-8'))
|
||||
vector=np.zeros(3); vector['xyz'.index(axis)]=np.deg2rad(sign*angle)
|
||||
nominal_R=Rotation.from_euler('xyz',_CONTEXT['rpy'],degrees=True).as_matrix()
|
||||
perturbed_R=Rotation.from_rotvec(vector).as_matrix()@nominal_R
|
||||
problems=[build_problem(segment,perturbed_R,MANUAL,_CONTEXT['sigma'])
|
||||
for segment in _CONTEXT['segments']]
|
||||
result,states=solve_free_lever_many(problems,MANUAL,_CONTEXT['max_nfev'],
|
||||
_CONTEXT['states'],lever_prior_mean_m=MANUAL,
|
||||
lever_prior_covariance_m2=COV,return_state_values=True)
|
||||
result=asdict(result); lever=np.asarray(result['final_l_I_m'])
|
||||
data=summarize_fixed_state_values(problems,states,lever)
|
||||
transform=make_transform(-perturbed_R@lever,perturbed_R)
|
||||
payload={'name':name,'axis':axis,'signed_angle_deg':sign*angle,
|
||||
'success':result['success'],'message':result['message'],'nfev':result['nfev'],
|
||||
'final_l_I_m':lever,'delta_l_from_nominal_m':None,
|
||||
'map_cost':result['final_cost'],'data_cost':data['cost'],
|
||||
'global_chi_square_per_dof':data['chi_square_per_dof'],
|
||||
'residual_by_factor':data['residual_by_factor'],
|
||||
'physical_residual':{
|
||||
'BEST_position_m':data['best_position_physical_m'],
|
||||
'Doppler_m_s':data['doppler_physical_m_s'],
|
||||
'HPR_rad':data['hpr_physical_rad']},
|
||||
'posterior_covariance_m2':result['lever_covariance_m2'],
|
||||
'posterior_std_m':np.sqrt(np.diag(result['lever_covariance_m2'])),
|
||||
'prior_pull_sigma':(lever-MANUAL)/STD,
|
||||
'T_RTK_IMU':transform}
|
||||
path.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
return _jsonable(payload)
|
||||
|
||||
def main():
|
||||
p=argparse.ArgumentParser(description=__doc__)
|
||||
p.add_argument('--manifest',type=Path,required=True)
|
||||
p.add_argument('--selection',type=Path,required=True)
|
||||
p.add_argument('--engineering-result',type=Path,required=True)
|
||||
p.add_argument('--nominal-states',type=Path,required=True)
|
||||
p.add_argument('--output',type=Path,required=True)
|
||||
p.add_argument('--checkpoint-dir',type=Path,required=True)
|
||||
p.add_argument('--workers',type=int,default=3)
|
||||
p.add_argument('--max-nfev',type=int,default=120)
|
||||
p.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
p.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
args=p.parse_args()
|
||||
engineering=json.loads(args.engineering_result.read_text(encoding='utf-8'))
|
||||
selection=json.loads(args.selection.read_text(encoding='utf-8'))
|
||||
if len(selection['selected_windows'])!=47:
|
||||
raise RuntimeError('sensitivity requires immutable 47-window selection')
|
||||
sessions=load_unified_sessions(args.manifest,
|
||||
selected_session_ids={x['session_id'] for x in selection['selected_windows']})
|
||||
segments=_restore_segments(sessions,_height_reference(sessions),
|
||||
selection['selected_windows'],1.)
|
||||
saved=np.load(args.nominal_states,allow_pickle=False)
|
||||
offsets=saved['offsets']; flat=saved['states']
|
||||
states=[flat[offsets[i]:offsets[i+1]] for i in range(len(offsets)-1)]
|
||||
if list(saved['candidate_ids'])!=[x['candidate_id'] for x in selection['selected_windows']]:
|
||||
raise RuntimeError('nominal state checkpoint does not match selection order')
|
||||
args.checkpoint_dir.mkdir(parents=True,exist_ok=True)
|
||||
tasks=[(axis,sign,angle) for axis in 'xyz'
|
||||
for angle in (.1,.3,.5) for sign in (-1.,1.)]
|
||||
results=[]
|
||||
with ProcessPoolExecutor(max_workers=args.workers,initializer=_initialize_worker,
|
||||
initargs=(segments,states,args.rotation_rpy_deg,args.max_nfev,
|
||||
args.hpr_direct_sigma_rad,str(args.checkpoint_dir))) as executor:
|
||||
futures=[executor.submit(_solve,task) for task in tasks]
|
||||
for future in as_completed(futures):
|
||||
result=future.result(); results.append(result)
|
||||
print('completed',result['name'],flush=True)
|
||||
results.sort(key=lambda x:('xyz'.index(x['axis']),x['signed_angle_deg']))
|
||||
nominal=engineering['prior_constrained_solution']
|
||||
nominal_l=np.asarray(nominal['result']['final_l_I_m'])
|
||||
nominal_data=nominal['data_only_summary_excluding_prior_factor']
|
||||
nominal_R=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
nominal_T=make_transform(-nominal_R@nominal_l,nominal_R)
|
||||
for result in results:
|
||||
result['delta_l_from_nominal_m']=np.asarray(result['final_l_I_m'])-nominal_l
|
||||
result['delta_l_norm_m']=float(np.linalg.norm(result['delta_l_from_nominal_m']))
|
||||
result['transform_translation_delta_m']=(
|
||||
np.asarray(result['T_RTK_IMU'])[:3,3]-nominal_T[:3,3])
|
||||
result['transform_translation_delta_norm_m']=float(np.linalg.norm(
|
||||
result['transform_translation_delta_m']))
|
||||
result['transform_rotation_delta_deg']=abs(result['signed_angle_deg'])
|
||||
result['relative_data_cost_delta']=result['data_cost']/nominal_data['cost']-1.
|
||||
result['factor_p95_delta_sigma']={key:
|
||||
result['residual_by_factor'][key]['p95_abs']-
|
||||
nominal_data['residual_by_factor'][key]['p95_abs']
|
||||
for key in ('best_position','doppler','hpr','imu_preintegration')}
|
||||
delta=np.asarray([x['delta_l_from_nominal_m'] for x in results])
|
||||
at_point3=[x for x in results if abs(x['signed_angle_deg'])==.3]
|
||||
checks={'all_18_complete_and_converged':
|
||||
len(results)==18 and all(x['success'] for x in results),
|
||||
'max_delta_norm_at_0p3deg_le_0p10m':
|
||||
max(x['delta_l_norm_m'] for x in at_point3)<=.10,
|
||||
'max_delta_norm_all_le_0p15m':
|
||||
max(x['delta_l_norm_m'] for x in results)<=.15,
|
||||
'relative_data_cost_increase_all_le_0p05':
|
||||
max(x['relative_data_cost_delta'] for x in results)<=.05,
|
||||
'factor_normalized_p95_increase_all_le_0p5sigma':
|
||||
max(v for x in results for v in x['factor_p95_delta_sigma'].values())<=.5}
|
||||
passed=bool(all(checks.values()))
|
||||
payload={'scope':'18 prior-constrained fixed-R2G perturbation solves',
|
||||
'data_only_free_called':False,'bootstrap_called':False,'loo_called':False,
|
||||
'parser_R0_covariance_modified':False,'calibration_window_count':47,
|
||||
'nominal_rotation_rpy_deg':args.rotation_rpy_deg,
|
||||
'nominal_l_I_m':nominal_l,'nominal_T_RTK_IMU':nominal_T,
|
||||
'perturbations':results,'summary':{
|
||||
'max_abs_delta_l_xyz_m':np.max(np.abs(delta),axis=0),
|
||||
'max_delta_l_norm_m':float(np.max(np.linalg.norm(delta,axis=1))),
|
||||
'max_transform_translation_delta_norm_m':max(
|
||||
x['transform_translation_delta_norm_m'] for x in results)},
|
||||
'diagnostic_gate_thresholds_frozen_before_run':{
|
||||
'max_delta_norm_at_0p3deg_m':.10,'max_delta_norm_all_m':.15,
|
||||
'relative_data_cost_increase':.05,
|
||||
'factor_normalized_p95_increase_sigma':.5},
|
||||
'gate_checks':checks,'rotation_sensitivity_passed':passed}
|
||||
args.output.write_text(json.dumps(_jsonable(payload),ensure_ascii=False,indent=2,
|
||||
allow_nan=False)+'\n',encoding='utf-8')
|
||||
print(json.dumps(_jsonable({'summary':payload['summary'],
|
||||
'checks':checks,'rotation_sensitivity_passed':passed}),indent=2))
|
||||
return 0
|
||||
if __name__=='__main__': raise SystemExit(main())
|
||||
@@ -0,0 +1,42 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run R1b/R2V/R2G/R3 on unified native-time G90/HI13 exports."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from imu_lidar.rtk_imu_multisource import (
|
||||
load_unified_sessions,
|
||||
result_to_jsonable,
|
||||
solve_r3,
|
||||
)
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--manifest", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument("--session", action="append")
|
||||
parser.add_argument("--level-static", action="append", required=True)
|
||||
args = parser.parse_args(argv)
|
||||
selected = None if not args.session else set(args.session) | set(args.level_static)
|
||||
sessions = load_unified_sessions(args.manifest, selected_session_ids=selected)
|
||||
result = solve_r3(sessions, level_static_session_ids=set(args.level_static))
|
||||
payload = result_to_jsonable(result)
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
args.output.write_text(
|
||||
json.dumps(payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
|
||||
)
|
||||
print(json.dumps(payload, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,75 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Run one 10-20 s per-node state graph with a fixed mechanical lever.'''
|
||||
from __future__ import annotations
|
||||
import argparse, json, sys
|
||||
from dataclasses import asdict
|
||||
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.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem, solve_fixed_lever
|
||||
from tools.audit_rtk_imu_factor_consistency import (
|
||||
MECHANICAL_L_I_M, _build_segment, _jsonable, _qualified_runs)
|
||||
|
||||
def _select_window(sessions,reference,period,target_duration):
|
||||
best = None
|
||||
for session,run,segment_id in _qualified_runs(sessions,reference,period):
|
||||
for start in range(len(run)):
|
||||
target_t = run[start].t_s+target_duration
|
||||
end = int(np.searchsorted([node.t_s for node in run],target_t))
|
||||
if end >= len(run): continue
|
||||
window = tuple(run[start:end+1])
|
||||
duration = window[-1].t_s-window[0].t_s
|
||||
if not 10. <= duration <= 20.: continue
|
||||
best_count = sum(node.source == 'BESTNAVA' for node in window)
|
||||
doppler_count = sum(node.velocity_enu_m_s is not None for node in window)
|
||||
hpr_count = sum(node.hpr_factor_valid for node in window)
|
||||
gyro_score = sum(np.linalg.norm(node.gyro_rad_s) for node in window)
|
||||
score = 10.*best_count+10.*doppler_count+2.*hpr_count+gyro_score
|
||||
candidate = (score,session,window,f'{segment_id}:window_{start:03d}',
|
||||
best_count,doppler_count,hpr_count)
|
||||
if best is None or candidate[0] > best[0]: best = candidate
|
||||
if best is None: raise RuntimeError('no 10-20 s qualified window')
|
||||
score,session,window,segment_id,best_count,doppler_count,hpr_count = best
|
||||
segment = _build_segment(session,window,segment_id)
|
||||
if segment is None: raise RuntimeError('selected window preintegration failed')
|
||||
return segment,{'score':score,'session_id':session.session_id,
|
||||
'segment_id':segment_id,'start_s':window[0].t_s,'end_s':window[-1].t_s,
|
||||
'duration_s':window[-1].t_s-window[0].t_s,'node_count':len(window),
|
||||
'best_count':best_count,'doppler_count':doppler_count,'hpr_factor_count':hpr_count}
|
||||
|
||||
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('--session',required=True)
|
||||
parser.add_argument('--sample-period-s',type=float,default=1.)
|
||||
parser.add_argument('--target-duration-s',type=float,default=15.)
|
||||
parser.add_argument('--max-nfev',type=int,default=30)
|
||||
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())
|
||||
args = parser.parse_args()
|
||||
sessions = load_unified_sessions(args.manifest,selected_session_ids={args.session})
|
||||
reference = _height_reference(sessions)
|
||||
if reference is None: raise RuntimeError('no BEST reference')
|
||||
segment,selection = _select_window(
|
||||
sessions,reference,args.sample_period_s,args.target_duration_s)
|
||||
rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
problem = build_problem(segment,rotation,np.asarray(args.mechanical_l_I_m))
|
||||
result = solve_fixed_lever(problem,args.max_nfev)
|
||||
payload = {'scope':'single 10-20 s node-state graph; fixed mechanical lever',
|
||||
'translation_variable_enabled':False,'free_prior_loo_bootstrap_sensitivity_called':False,
|
||||
'selection':selection,'result':asdict(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(payload),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
if __name__ == '__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,79 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Three-start, no-prior free-lever solve on the frozen P0.5 circle window.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from dataclasses import asdict
|
||||
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.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem,solve_free_lever
|
||||
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
|
||||
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
|
||||
|
||||
|
||||
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('--sample-period-s',type=float,default=1.)
|
||||
parser.add_argument('--target-duration-s',type=float,default=15.)
|
||||
parser.add_argument('--max-nfev',type=int,default=120)
|
||||
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
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('--large-perturbation-m',nargs=3,type=float,
|
||||
default=[.5,-.5,.5])
|
||||
args=parser.parse_args()
|
||||
sessions=load_unified_sessions(
|
||||
args.manifest,selected_session_ids={args.circle_session})
|
||||
reference=_height_reference(sessions)
|
||||
segment,selection=_select_window(
|
||||
sessions,reference,args.sample_period_s,args.target_duration_s)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
mechanical=np.asarray(args.mechanical_l_I_m,dtype=float)
|
||||
problem=build_problem(segment,rotation,mechanical,args.hpr_direct_sigma_rad)
|
||||
starts={'zero':np.zeros(3),'mechanical':mechanical,
|
||||
'mechanical_large_perturbation':mechanical+args.large_perturbation_m}
|
||||
results={name:asdict(solve_free_lever(problem,value,args.max_nfev))
|
||||
for name,value in starts.items()}
|
||||
for result in results.values():
|
||||
covariance=np.asarray(result['lever_covariance_m2'])
|
||||
result['lever_std_m']=np.sqrt(np.maximum(np.diag(covariance),0.))
|
||||
result['delta_to_mechanical_m']=np.asarray(result['final_l_I_m'])-mechanical
|
||||
result['xy_marginal_covariance_m2']=covariance[:2,:2]
|
||||
result['xy_information_singular_values']=np.linalg.svd(
|
||||
np.linalg.pinv(covariance[:2,:2],rcond=1e-9),compute_uv=False)
|
||||
solutions=np.asarray([value['final_l_I_m'] for value in results.values()])
|
||||
spread=float(max(np.linalg.norm(a-b) for a in solutions for b in solutions))
|
||||
xy_spread=float(max(np.linalg.norm(a[:2]-b[:2]) for a in solutions for b in solutions))
|
||||
circle_xy_observable=bool(all(value['success'] and
|
||||
np.all(np.asarray(value['lever_std_m'])[:2]<=.15) for value in results.values()))
|
||||
payload={'scope':'circle single-segment no-prior three-start free lever',
|
||||
'translation_variable_enabled':True,'manual_prior_used':False,
|
||||
'loo_bootstrap_sensitivity_called':False,'selection':selection,
|
||||
'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
|
||||
'hpr_direct_sigma_rad':args.hpr_direct_sigma_rad,
|
||||
'mechanical_reference_m':mechanical,'large_perturbation_m':args.large_perturbation_m,
|
||||
'solutions':results,'maximum_solution_spread_m':spread,
|
||||
'maximum_xy_solution_spread_m':xy_spread,
|
||||
'circle_only_observability_gate':{
|
||||
'xy_marginal_std_max_m':.15,'passed':circle_xy_observable},
|
||||
'circle_only_data_only_accepted':circle_xy_observable,
|
||||
'joint_free_blocked_by_circle_gate':False,
|
||||
'covariance_postfit_scaled':False}
|
||||
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(payload),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,85 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Three-start, no-prior joint free-lever solve on fixed P0.5 motion windows.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from dataclasses import asdict
|
||||
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.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem,solve_free_lever_many
|
||||
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
|
||||
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
|
||||
|
||||
|
||||
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('--target-duration-s',type=float,default=15.)
|
||||
parser.add_argument('--max-nfev',type=int,default=120)
|
||||
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
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('--large-perturbation-m',nargs=3,type=float,
|
||||
default=[.5,-.5,.5])
|
||||
args=parser.parse_args()
|
||||
categories={'circle':args.circle_session,'left_right':args.left_right_session,
|
||||
'slope':args.slope_session}
|
||||
sessions=load_unified_sessions(
|
||||
args.manifest,selected_session_ids=set(categories.values()))
|
||||
reference=_height_reference(sessions)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
mechanical=np.asarray(args.mechanical_l_I_m,dtype=float)
|
||||
problems,selections=[],{}
|
||||
for motion,session_id in categories.items():
|
||||
segment,selection=_select_window(
|
||||
[s for s in sessions if s.session_id==session_id],reference,
|
||||
args.sample_period_s,args.target_duration_s)
|
||||
problems.append(build_problem(
|
||||
segment,rotation,mechanical,args.hpr_direct_sigma_rad))
|
||||
selections[motion]=selection
|
||||
starts={'zero':np.zeros(3),'mechanical':mechanical,
|
||||
'mechanical_large_perturbation':mechanical+args.large_perturbation_m}
|
||||
results={name:asdict(solve_free_lever_many(problems,value,args.max_nfev))
|
||||
for name,value in starts.items()}
|
||||
for result in results.values():
|
||||
covariance=np.asarray(result['lever_covariance_m2'])
|
||||
result['lever_std_m']=np.sqrt(np.maximum(np.diag(covariance),0.))
|
||||
result['delta_to_mechanical_m']=np.asarray(result['final_l_I_m'])-mechanical
|
||||
solutions=np.asarray([value['final_l_I_m'] for value in results.values()])
|
||||
spread=float(max(np.linalg.norm(a-b) for a in solutions for b in solutions))
|
||||
gates={name:{'optimizer_converged':value['success'],
|
||||
'lever_marginal_std':bool(np.all(np.asarray(value['lever_std_m'])<=[.15,.15,.20])),
|
||||
'lever_information_rank':value['lever_precision_rank']==3,
|
||||
'lever_information_condition':value['lever_information_condition_number']<=1e6,
|
||||
'lever_min_information':min(value['lever_information_singular_values'])>=1e-3}
|
||||
for name,value in results.items()}
|
||||
observable=bool(all(all(gate.values()) for gate in gates.values()))
|
||||
payload={'scope':'three-motion joint no-prior three-start free lever',
|
||||
'translation_variable_enabled':True,'manual_prior_used':False,
|
||||
'loo_bootstrap_sensitivity_called':False,'selections':selections,
|
||||
'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
|
||||
'hpr_direct_sigma_rad':args.hpr_direct_sigma_rad,
|
||||
'mechanical_reference_m':mechanical,'solutions':results,
|
||||
'maximum_solution_spread_m':spread,'observability_gates':gates,
|
||||
'joint_data_only_translation_observable':observable,
|
||||
'manual_prior_started':False,'covariance_postfit_scaled':False}
|
||||
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(payload),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,120 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Three-start joint free solve on information-selected windows.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
from dataclasses import asdict
|
||||
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.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import (
|
||||
build_problem,fit_states_at_fixed_lever,solve_free_lever_many)
|
||||
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_build_segment,_jsonable,_qualified_runs
|
||||
|
||||
|
||||
def _restore_segments(sessions,reference,selections,period_s):
|
||||
runs=list(_qualified_runs(sessions,reference,period_s)); restored=[]
|
||||
for item in selections:
|
||||
match=None
|
||||
for session,run,_ in runs:
|
||||
if session.session_id!=item['session_id']: continue
|
||||
contained=(run[0].t_s<=item['start_s']+1e-6 and
|
||||
run[-1].t_s>=item['end_s']-1e-6)
|
||||
if not contained: continue
|
||||
nodes=tuple(node for node in run
|
||||
if item['start_s']-1e-6<=node.t_s<=item['end_s']+1e-6)
|
||||
if len(nodes)==item['node_count']:
|
||||
match=_build_segment(session,nodes,item['candidate_id']); break
|
||||
if match is None: raise RuntimeError('cannot restore '+item['candidate_id'])
|
||||
restored.append(match)
|
||||
return restored
|
||||
|
||||
|
||||
def main():
|
||||
parser=argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--manifest',type=Path,required=True)
|
||||
parser.add_argument('--selection',type=Path,required=True)
|
||||
parser.add_argument('--output',type=Path,required=True)
|
||||
parser.add_argument('--sample-period-s',type=float,default=1.)
|
||||
parser.add_argument('--max-nfev',type=int,default=120)
|
||||
parser.add_argument('--prefit-max-nfev',type=int,default=50)
|
||||
parser.add_argument('--prefit-workers',type=int,default=4)
|
||||
parser.add_argument('--start-name',choices=['all','zero','mechanical',
|
||||
'mechanical_large_perturbation'],default='all')
|
||||
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
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('--large-perturbation-m',nargs=3,type=float,
|
||||
default=[.5,-.5,.5])
|
||||
args=parser.parse_args()
|
||||
selection=json.loads(args.selection.read_text(encoding='utf-8'))
|
||||
ids={item['session_id'] for item in selection['selected_windows']}
|
||||
sessions=load_unified_sessions(args.manifest,selected_session_ids=ids)
|
||||
reference=_height_reference(sessions)
|
||||
segments=_restore_segments(sessions,reference,selection['selected_windows'],
|
||||
args.sample_period_s)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
mechanical=np.asarray(args.mechanical_l_I_m,dtype=float)
|
||||
problems=[build_problem(segment,rotation,mechanical,args.hpr_direct_sigma_rad)
|
||||
for segment in segments]
|
||||
starts={'zero':np.zeros(3),'mechanical':mechanical,
|
||||
'mechanical_large_perturbation':mechanical+args.large_perturbation_m}
|
||||
if args.start_name!='all': starts={args.start_name:starts[args.start_name]}
|
||||
results={}; prefit={}
|
||||
for name,value in starts.items():
|
||||
with ThreadPoolExecutor(max_workers=args.prefit_workers) as executor:
|
||||
fitted=list(executor.map(
|
||||
lambda problem:fit_states_at_fixed_lever(
|
||||
problem,value,args.prefit_max_nfev),problems))
|
||||
state_values=[item[0] for item in fitted]
|
||||
prefit[name]=[item[1] for item in fitted]
|
||||
results[name]=asdict(solve_free_lever_many(
|
||||
problems,value,args.max_nfev,state_values))
|
||||
for result in results.values():
|
||||
covariance=np.asarray(result['lever_covariance_m2'])
|
||||
result['lever_std_m']=np.sqrt(np.maximum(np.diag(covariance),0.))
|
||||
result['delta_to_mechanical_m']=np.asarray(result['final_l_I_m'])-mechanical
|
||||
solutions=np.asarray([value['final_l_I_m'] for value in results.values()])
|
||||
spread=float(max(np.linalg.norm(a-b) for a in solutions for b in solutions))
|
||||
gates={name:{'optimizer_converged':value['success'],
|
||||
'lever_marginal_std':bool(np.all(np.asarray(value['lever_std_m'])<=[.15,.15,.20])),
|
||||
'lever_information_rank':value['lever_precision_rank']==3,
|
||||
'lever_information_condition':value['lever_information_condition_number']<=1e6,
|
||||
'lever_min_information':min(value['lever_information_singular_values'])>=1e-3}
|
||||
for name,value in results.items()}
|
||||
observable=bool(all(all(gate.values()) for gate in gates.values()))
|
||||
payload={'scope':'information-selected multi-window no-prior three-start free lever',
|
||||
'translation_variable_enabled':True,'manual_prior_used':False,
|
||||
'loo_bootstrap_sensitivity_called':False,
|
||||
'selection_artifact':str(args.selection),'selected_window_count':len(segments),
|
||||
'start_name':args.start_name,'nuisance_prefit':prefit,
|
||||
'nuisance_prefit_is_not_lever_prior':True,
|
||||
'sample_overlap_audit':selection['sample_overlap_audit'],
|
||||
'information_curve':selection['lever_std_vs_information_curve'],
|
||||
'fixed_rotation_rpy_deg':args.rotation_rpy_deg,
|
||||
'hpr_direct_sigma_rad':args.hpr_direct_sigma_rad,
|
||||
'mechanical_reference_m':mechanical,'solutions':results,
|
||||
'maximum_solution_spread_m':spread,'observability_gates':gates,
|
||||
'data_only_translation_observable':observable,
|
||||
'manual_prior_started':False,'covariance_postfit_scaled':False}
|
||||
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({'selected_window_count':len(segments),
|
||||
'solutions':{name:{'success':value['success'],'l_I_m':value['final_l_I_m'],
|
||||
'std_m':value['lever_std_m'],'cost':value['final_cost'],
|
||||
'chi_square_per_dof':value['chi_square_per_dof'],
|
||||
'singular_values':value['lever_information_singular_values']}
|
||||
for name,value in results.items()},'maximum_solution_spread_m':spread,
|
||||
'data_only_translation_observable':observable}),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,106 @@
|
||||
#!/usr/bin/env python3
|
||||
'''P0.5 fixed-lever node-graph covariance and motion-window audit.'''
|
||||
from __future__ import annotations
|
||||
import argparse, json, sys
|
||||
from dataclasses import asdict
|
||||
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.rtk_imu_engineering import _height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import (
|
||||
build_problem,solve_fixed_lever,solve_fixed_lever_many)
|
||||
from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_jsonable
|
||||
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
|
||||
|
||||
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('--target-duration-s',type=float,default=15.)
|
||||
parser.add_argument('--max-nfev',type=int,default=50)
|
||||
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('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
args = parser.parse_args()
|
||||
categories = {'circle':args.circle_session,'left_right':args.left_right_session,
|
||||
'slope':args.slope_session}
|
||||
sessions = load_unified_sessions(args.manifest,selected_session_ids=set(categories.values()))
|
||||
reference = _height_reference(sessions)
|
||||
rotation = Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
lever = np.asarray(args.mechanical_l_I_m)
|
||||
problems, selections, separate = [], {}, {}
|
||||
for category,session_id in categories.items():
|
||||
local = [session for session in sessions if session.session_id == session_id]
|
||||
segment,selection = _select_window(
|
||||
local,reference,args.sample_period_s,args.target_duration_s)
|
||||
problem = build_problem(segment,rotation,lever,args.hpr_direct_sigma_rad)
|
||||
problems.append(problem); selections[category] = selection
|
||||
separate[category] = asdict(solve_fixed_lever(problem,args.max_nfev))
|
||||
joint = asdict(solve_fixed_lever_many(problems,args.max_nfev))
|
||||
fixed_pass = bool(
|
||||
all(value['success'] for value in separate.values()) and joint['success']
|
||||
and joint['chi_square_per_dof'] >= .25
|
||||
and joint['chi_square_per_dof'] <= 4.
|
||||
and joint['final_position_residual_m']['vector_p95'] <= .20
|
||||
and joint['final_velocity_residual_m_s']['vector_p95'] <= .50)
|
||||
payload = {'scope':'P0.5 three-motion fixed-lever only',
|
||||
'translation_variable_enabled':False,
|
||||
'manual_prior_loo_bootstrap_sensitivity_called':False,
|
||||
'covariance_model':{
|
||||
'source':'independent_innovation_audit_three_motion',
|
||||
'best_position_xyz_m':[.06,.06,.12],
|
||||
'doppler_xyz_m_s':[.15,.15,.30],
|
||||
'hpr_direct_angular_rad':args.hpr_direct_sigma_rad,
|
||||
'hpr_bridge_rule':'sqrt(direct_sigma^2 + dropout_extra_variance)',
|
||||
'imu_preintegration':'unchanged physical covariance',
|
||||
'bias_random_walk':'unchanged static/Allan/device model',
|
||||
'postfit_global_scale_applied':False},
|
||||
'fixed_l_I_m':lever,'selections':selections,
|
||||
'separate_fixed_lever':separate,'joint_fixed_lever':joint,
|
||||
'fixed_lever_covariance_gate':{
|
||||
'chi_square_per_dof_range':[.25,4.],
|
||||
'position_vector_p95_max_m':.20,
|
||||
'velocity_vector_p95_max_m_s':.50,
|
||||
'passed':fixed_pass},
|
||||
'whitening_diagnosis':{
|
||||
'normalized_scale_consistent':joint['chi_square_per_dof'] >= .25,
|
||||
'joint_preintegration_nis_per_dof':
|
||||
joint['final_residual_by_factor']['imu_preintegration']['chi_square_per_dof'],
|
||||
'joint_preintegration_normalized_p95':
|
||||
joint['final_residual_by_factor']['imu_preintegration']['p95_abs'],
|
||||
'preintegration_sigma_distribution':joint['preintegration_covariance_sigma'],
|
||||
'interpretation':(
|
||||
'absolute preintegration sigma is small, but per-node states satisfy process '
|
||||
'factors almost exactly while BEST/Doppler/HPR normalized residuals are also '
|
||||
'well below one; current factor covariance set is collectively overconservative'
|
||||
)},
|
||||
'free_lever_unlocked':fixed_pass}
|
||||
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')
|
||||
compact = {'selections':selections,
|
||||
'separate':{key:{'success':value['success'],
|
||||
'chi_square_per_dof':value['chi_square_per_dof'],
|
||||
'position_p95_m':value['final_position_residual_m']['vector_p95'],
|
||||
'velocity_p95_m_s':value['final_velocity_residual_m_s']['vector_p95'],
|
||||
'factor_stats':value['final_residual_by_factor']}
|
||||
for key,value in separate.items()},
|
||||
'joint':{'success':joint['success'],'chi_square_per_dof':joint['chi_square_per_dof'],
|
||||
'position_p95_m':joint['final_position_residual_m']['vector_p95'],
|
||||
'velocity_p95_m_s':joint['final_velocity_residual_m_s']['vector_p95'],
|
||||
'factor_stats':joint['final_residual_by_factor']},
|
||||
'free_lever_unlocked':fixed_pass}
|
||||
print(json.dumps(_jsonable(compact),ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
if __name__ == '__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,174 @@
|
||||
#!/usr/bin/env python3
|
||||
'''Select non-overlapping qualified windows by incremental lever information.'''
|
||||
from __future__ import annotations
|
||||
import argparse,json,sys
|
||||
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.rtk_imu_engineering import _all_hpr,_height_reference
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
from imu_lidar.rtk_imu_node_graph import build_problem,linearized_lever_information
|
||||
from tools.audit_rtk_imu_factor_consistency import _build_segment,_jsonable,_qualified_runs
|
||||
from tools.run_rtk_imu_node_graph_fixed_lever import _select_window
|
||||
|
||||
|
||||
STD_GATE=np.array([.15,.15,.20])
|
||||
|
||||
|
||||
def _overlap(left,right):
|
||||
return left['session_id']==right['session_id'] and not (
|
||||
left['end_s']<right['start_s'] or right['end_s']<left['start_s'])
|
||||
|
||||
|
||||
def _summary(information):
|
||||
covariance=np.linalg.pinv(information,rcond=1e-9)
|
||||
singular=np.linalg.svd(information,compute_uv=False)
|
||||
sign,logdet=np.linalg.slogdet(information)
|
||||
return {'std_m':np.sqrt(np.maximum(np.diag(covariance),0.)),
|
||||
'singular_values':singular,'lambda_min':singular[-1],
|
||||
'logdet':float(logdet) if sign>0 else -np.inf}
|
||||
|
||||
|
||||
def _window_candidates(sessions,reference,period_s,duration_s):
|
||||
candidates=[]
|
||||
for session,run,segment_id in _qualified_runs(sessions,reference,period_s):
|
||||
times=np.asarray([node.t_s for node in run])
|
||||
start=0
|
||||
while start<len(run):
|
||||
end=int(np.searchsorted(times,times[start]+duration_s))
|
||||
if end>=len(run): break
|
||||
window=tuple(run[start:end+1])
|
||||
if 10.<=window[-1].t_s-window[0].t_s<=20.:
|
||||
candidate_id=f'{segment_id}:info_window_{start:04d}'
|
||||
segment=_build_segment(session,window,candidate_id)
|
||||
if segment is not None:
|
||||
candidates.append({'candidate_id':candidate_id,
|
||||
'session_id':session.session_id,'start_s':window[0].t_s,
|
||||
'end_s':window[-1].t_s,'duration_s':window[-1].t_s-window[0].t_s,
|
||||
'node_count':len(window),'segment':segment,'session':session})
|
||||
start=end+1
|
||||
return candidates
|
||||
|
||||
|
||||
def _sample_keys(entry):
|
||||
session=entry['session']; start,end=entry['start_s'],entry['end_s']
|
||||
imu=set((session.session_id,int(round(t*1e6))) for t in session.imu.t_s
|
||||
if start<=t<=end)
|
||||
gnss=set((session.session_id,int(round(node.t_s*1e6)),node.source)
|
||||
for node in entry['segment'].nodes)
|
||||
hpr=_all_hpr(session)
|
||||
hpr_keys=set((session.session_id,int(round(hpr.t_s[i]*1e6)))
|
||||
for i in hpr.valid_indices if start<=hpr.t_s[i]<=end)
|
||||
return {'imu':imu,'gnss':gnss,'hpr':hpr_keys}
|
||||
|
||||
|
||||
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('--window-duration-s',type=float,default=15.)
|
||||
parser.add_argument('--max-additional-windows',type=int,default=100)
|
||||
parser.add_argument('--hpr-direct-sigma-rad',type=float,default=.006)
|
||||
parser.add_argument('--linearization-l-I-m',nargs=3,type=float,
|
||||
default=[-.53243,-.42662,.74660])
|
||||
parser.add_argument('--rotation-rpy-deg',nargs=3,type=float,
|
||||
default=[.4543066225,-.0026392019,.0122384129])
|
||||
args=parser.parse_args()
|
||||
sessions=load_unified_sessions(args.manifest)
|
||||
reference=_height_reference(sessions)
|
||||
rotation=Rotation.from_euler('xyz',args.rotation_rpy_deg,degrees=True).as_matrix()
|
||||
lever=np.asarray(args.linearization_l_I_m,dtype=float)
|
||||
categories={'circle':args.circle_session,'left_right':args.left_right_session,
|
||||
'slope':args.slope_session}
|
||||
selected=[]
|
||||
for motion,session_id in categories.items():
|
||||
session=[s for s in sessions if s.session_id==session_id]
|
||||
segment,meta=_select_window(
|
||||
session,reference,args.sample_period_s,args.window_duration_s)
|
||||
selected.append({**meta,'candidate_id':meta['segment_id'],'motion':motion,
|
||||
'segment':segment,'session':session[0],'seed_window':True})
|
||||
candidates=[entry for entry in _window_candidates(
|
||||
sessions,reference,args.sample_period_s,args.window_duration_s)
|
||||
if not any(_overlap(entry,seed) for seed in selected)]
|
||||
for entry in [*selected,*candidates]:
|
||||
problem=build_problem(entry['segment'],rotation,lever,args.hpr_direct_sigma_rad)
|
||||
information,_,singular,weak=linearized_lever_information(problem,lever)
|
||||
entry['problem']=problem; entry['information']=information
|
||||
entry['single_window_singular_values']=singular
|
||||
entry['single_window_weakest_direction_I']=weak
|
||||
total=sum((entry['information'] for entry in selected),np.zeros((3,3)))
|
||||
curve=[{'window_count':len(selected),'added_candidate_id':'seed_three_motion',
|
||||
**_summary(total)}]
|
||||
remaining=list(candidates)
|
||||
saturation_count=0
|
||||
stop_reason='candidate_exhausted'
|
||||
while remaining and len(selected)<3+args.max_additional_windows:
|
||||
feasible=[entry for entry in remaining
|
||||
if not any(_overlap(entry,item) for item in selected)]
|
||||
if not feasible: break
|
||||
ranked=[]
|
||||
for entry in feasible:
|
||||
summary=_summary(total+entry['information'])
|
||||
ranked.append((summary['lambda_min'],summary['logdet'],entry,summary))
|
||||
_,_,choice,summary=max(ranked,key=lambda item:(item[0],item[1]))
|
||||
gain=summary['lambda_min']-curve[-1]['lambda_min']
|
||||
selected.append(choice); remaining.remove(choice); total+=choice['information']
|
||||
curve.append({'window_count':len(selected),'added_candidate_id':choice['candidate_id'],
|
||||
'delta_lambda_min':gain,**summary})
|
||||
saturation_count=saturation_count+1 if gain<.01 else 0
|
||||
if np.all(np.asarray(summary['std_m'])<=STD_GATE):
|
||||
stop_reason='linearized_observability_gate_reached'; break
|
||||
if saturation_count>=3:
|
||||
stop_reason='incremental_lambda_min_gain_saturated'; break
|
||||
else:
|
||||
if len(selected)>=3+args.max_additional_windows:
|
||||
stop_reason='max_additional_windows_reached'
|
||||
seen={'imu':set(),'gnss':set(),'hpr':set()}
|
||||
duplicate={'imu':0,'gnss':0,'hpr':0}
|
||||
selected_output=[]
|
||||
for order,entry in enumerate(selected):
|
||||
keys=_sample_keys(entry)
|
||||
shared={name:len(value&seen[name]) for name,value in keys.items()}
|
||||
for name,value in keys.items():
|
||||
duplicate[name]+=shared[name]; seen[name].update(value)
|
||||
selected_output.append({'selection_order':order,
|
||||
'candidate_id':entry['candidate_id'],'session_id':entry['session_id'],
|
||||
'start_s':entry['start_s'],'end_s':entry['end_s'],
|
||||
'duration_s':entry['duration_s'],'node_count':entry['node_count'],
|
||||
'seed_window':entry.get('seed_window',False),
|
||||
'single_window_information':entry['information'],
|
||||
'single_window_singular_values':entry['single_window_singular_values'],
|
||||
'single_window_weakest_direction_I':entry['single_window_weakest_direction_I'],
|
||||
'sample_count':{name:len(value) for name,value in keys.items()},
|
||||
'shared_sample_count_with_previous':shared})
|
||||
payload={'scope':'all recovered qualified dynamics; information-based window selection',
|
||||
'manual_prior_used':False,'linearization_l_I_m':lever,
|
||||
'linearization_point_is_not_prior_factor':True,
|
||||
'candidate_count':len(candidates),'selected_window_count':len(selected),
|
||||
'selection_objective':'maximize lambda_min(H_l); break ties by logdet(H_l)',
|
||||
'std_gate_m':STD_GATE,'stop_reason':stop_reason,
|
||||
'selected_windows':selected_output,'lever_std_vs_information_curve':curve,
|
||||
'sample_overlap_audit':{'duplicate_sample_count':duplicate,
|
||||
'all_selected_windows_time_nonoverlapping_within_session':not any(
|
||||
_overlap(a,b) for i,a in enumerate(selected) for b in selected[i+1:]),
|
||||
'unique_sample_count':{name:len(value) for name,value in seen.items()}},
|
||||
'final_linearized_observable':bool(np.all(
|
||||
np.asarray(curve[-1]['std_m'])<=STD_GATE))}
|
||||
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({'candidate_count':len(candidates),
|
||||
'selected_window_count':len(selected),'stop_reason':stop_reason,
|
||||
'final_curve':curve[-1],'sample_overlap_audit':payload['sample_overlap_audit']}),
|
||||
ensure_ascii=False,indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,182 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Read-only artificial GNHPR dropout validation for the engineering R0 bridge.
|
||||
|
||||
For each requested gap, Q4 HPR samples inside an otherwise 0.1 s contiguous
|
||||
run are withheld. Their true baseline directions are compared with normalized
|
||||
linear interpolation and short-horizon HI13 gyro propagation. No lever-arm
|
||||
solve, prior, bootstrap, sensitivity run, or threshold update is performed.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
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.rtk_imu_engineering import _all_hpr, _baseline_angle_deg, _interpolate_baseline, _world_rtk
|
||||
from imu_lidar.rtk_imu_multisource import load_unified_sessions
|
||||
|
||||
GAPS_S = (0.2, 0.3, 0.4, 0.5, 0.6, 0.8)
|
||||
R2G_RPY_DEG = (0.4543066225, -0.0026392019, 0.0122384129)
|
||||
BASELINE_FACTOR_SIGMA_DEG = float(np.degrees(0.035))
|
||||
|
||||
|
||||
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 isinstance(value, dict):
|
||||
return {str(k): _jsonable(v) for k, v in value.items()}
|
||||
if isinstance(value, (tuple, list)):
|
||||
return [_jsonable(v) for v in value]
|
||||
return value
|
||||
|
||||
|
||||
def _contiguous_runs(times: np.ndarray, valid: np.ndarray) -> list[np.ndarray]:
|
||||
indices = np.flatnonzero(valid)
|
||||
if indices.size == 0:
|
||||
return []
|
||||
runs: list[list[int]] = [[int(indices[0])]]
|
||||
for previous, index in zip(indices[:-1], indices[1:]):
|
||||
dt = float(times[index] - times[previous])
|
||||
if 0.075 <= dt <= 0.125:
|
||||
runs[-1].append(int(index))
|
||||
else:
|
||||
runs.append([int(index)])
|
||||
return [np.asarray(run, dtype=int) for run in runs if len(run) >= 3]
|
||||
|
||||
|
||||
def _propagate_baseline(session, R_WI_start: np.ndarray, baseline_I: np.ndarray,
|
||||
start_s: float, targets_s: np.ndarray) -> list[np.ndarray]:
|
||||
"""Propagate with vectorized gyro interpolation over the short withheld gap."""
|
||||
targets = np.asarray(targets_s, dtype=float)
|
||||
if targets.size == 0:
|
||||
return []
|
||||
grid = np.unique(np.concatenate(([start_s], session.imu.t_s[
|
||||
(session.imu.t_s > start_s) & (session.imu.t_s < targets[-1])], targets)))
|
||||
gyro = np.column_stack([
|
||||
np.interp(grid, session.imu.t_s, session.imu.gyro_rad_s[:, axis]) for axis in range(3)
|
||||
])
|
||||
R_WI = np.asarray(R_WI_start, dtype=float).copy()
|
||||
predicted: list[np.ndarray] = []
|
||||
target_index = 0
|
||||
for index, (left, right) in enumerate(zip(grid[:-1], grid[1:])):
|
||||
omega = 0.5 * (gyro[index] + gyro[index + 1])
|
||||
R_WI = R_WI @ Rotation.from_rotvec(omega * float(right - left)).as_matrix()
|
||||
while target_index < targets.size and abs(targets[target_index] - right) < 1e-9:
|
||||
predicted.append(R_WI @ baseline_I)
|
||||
target_index += 1
|
||||
return predicted
|
||||
|
||||
def _session_validation(session, rotation: np.ndarray, max_windows: int) -> dict[str, object]:
|
||||
hpr = _all_hpr(session)
|
||||
runs = _contiguous_runs(hpr.t_s, hpr.valid)
|
||||
baseline_I = rotation.T[:, 0]
|
||||
result: dict[str, object] = {"session_id": session.session_id, "q4_runs": len(runs), "gaps": {}}
|
||||
for requested_gap in GAPS_S:
|
||||
interpolation_errors: list[float] = []
|
||||
propagation_errors: list[float] = []
|
||||
actual_gaps: list[float] = []
|
||||
windows = 0
|
||||
for run in runs:
|
||||
nominal_dt = float(np.median(np.diff(hpr.t_s[run])))
|
||||
step_count = max(2, int(round(requested_gap / nominal_dt)))
|
||||
# The endpoints remain observed, every internal Q4 point is withheld.
|
||||
for start in range(0, run.size - step_count, max(1, step_count)):
|
||||
subset = run[start:start + step_count + 1]
|
||||
if subset.size != step_count + 1:
|
||||
continue
|
||||
left, right = int(subset[0]), int(subset[-1])
|
||||
actual_gap = float(hpr.t_s[right] - hpr.t_s[left])
|
||||
if abs(actual_gap - requested_gap) > 0.08:
|
||||
continue
|
||||
withheld = subset[1:-1]
|
||||
if withheld.size == 0:
|
||||
continue
|
||||
left_b, right_b = hpr.baseline_enu[left], hpr.baseline_enu[right]
|
||||
targets = hpr.t_s[withheld]
|
||||
predicted_prop = _propagate_baseline(
|
||||
session, _world_rtk(left_b) @ rotation, baseline_I, float(hpr.t_s[left]), targets
|
||||
)
|
||||
for index, predicted in zip(withheld, predicted_prop):
|
||||
fraction = float((hpr.t_s[index] - hpr.t_s[left]) / actual_gap)
|
||||
interpolation_errors.append(_baseline_angle_deg(
|
||||
_interpolate_baseline(left_b, right_b, fraction), hpr.baseline_enu[index]
|
||||
))
|
||||
propagation_errors.append(_baseline_angle_deg(predicted, hpr.baseline_enu[index]))
|
||||
actual_gaps.append(actual_gap)
|
||||
windows += 1
|
||||
if windows >= max_windows:
|
||||
break
|
||||
if windows >= max_windows:
|
||||
break
|
||||
def summary(errors: list[float]) -> dict[str, float | int | None]:
|
||||
finite = np.asarray([e for e in errors if np.isfinite(e)], dtype=float)
|
||||
if finite.size == 0:
|
||||
return {"count": 0, "p50_deg": None, "p95_deg": None, "max_deg": None}
|
||||
return {"count": int(finite.size), "p50_deg": float(np.percentile(finite, 50)),
|
||||
"p95_deg": float(np.percentile(finite, 95)), "max_deg": float(np.max(finite))}
|
||||
interp, prop = summary(interpolation_errors), summary(propagation_errors)
|
||||
choice = "linear_baseline_interpolation" if (interp["p95_deg"] or np.inf) <= (prop["p95_deg"] or np.inf) else "imu_gyro_propagation"
|
||||
selected_p95 = interp["p95_deg"] if choice.startswith("linear") else prop["p95_deg"]
|
||||
result["gaps"][f"{requested_gap:.1f}"] = {
|
||||
"requested_gap_s": requested_gap,
|
||||
"actual_gap_p50_s": float(np.median(actual_gaps)) if actual_gaps else None,
|
||||
"window_count": windows,
|
||||
"linear_baseline_interpolation": interp,
|
||||
"imu_gyro_propagation": prop,
|
||||
"preferred_method_by_p95": choice,
|
||||
"selected_p95_deg": selected_p95,
|
||||
"within_baseline_factor_sigma": bool(selected_p95 is not None and selected_p95 <= BASELINE_FACTOR_SIGMA_DEG),
|
||||
}
|
||||
return result
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--manifest", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument("--session", action="append")
|
||||
parser.add_argument("--max-windows", type=int, default=500)
|
||||
args = parser.parse_args()
|
||||
sessions = load_unified_sessions(args.manifest, selected_session_ids=None if args.session is None else set(args.session))
|
||||
rotation = Rotation.from_euler("xyz", R2G_RPY_DEG, degrees=True).as_matrix()
|
||||
session_results = [_session_validation(session, rotation, args.max_windows) for session in sessions]
|
||||
recommendation: dict[str, object] = {}
|
||||
for gap in GAPS_S:
|
||||
entries = [item["gaps"][f"{gap:.1f}"] for item in session_results if item["gaps"].get(f"{gap:.1f}")]
|
||||
p95 = [entry["selected_p95_deg"] for entry in entries if entry["selected_p95_deg"] is not None]
|
||||
recommendation[f"{gap:.1f}"] = {
|
||||
"sessions_with_samples": len(p95),
|
||||
"worst_session_selected_p95_deg": float(max(p95)) if p95 else None,
|
||||
"passes_all_sessions_factor_sigma": bool(p95 and max(p95) <= BASELINE_FACTOR_SIGMA_DEG),
|
||||
}
|
||||
passing = [float(key) for key, value in recommendation.items() if value["passes_all_sessions_factor_sigma"]]
|
||||
payload = {
|
||||
"scope": "artificial HPR dropout validation only; no solve/prior/bootstrap/sensitivity/threshold change",
|
||||
"r2g_rotation_rpy_deg": R2G_RPY_DEG,
|
||||
"baseline_factor_sigma_deg": BASELINE_FACTOR_SIGMA_DEG,
|
||||
"sessions": session_results,
|
||||
"bridge_recommendation": recommendation,
|
||||
"largest_gap_passing_all_sessions_s": max(passing) if passing else None,
|
||||
}
|
||||
args.output.parent.mkdir(parents=True, exist_ok=True)
|
||||
args.output.write_text(json.dumps(_jsonable(payload), ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
|
||||
print(json.dumps({"largest_gap_passing_all_sessions_s": payload["largest_gap_passing_all_sessions_s"],
|
||||
"bridge_recommendation": recommendation}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
Reference in New Issue
Block a user