145 lines
7.6 KiB
Python
145 lines
7.6 KiB
Python
#!/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 rtk_imu.rtk_imu_engineering import _height_reference
|
|
from rtk_imu.rtk_imu_multisource import load_unified_sessions
|
|
from rtk_imu.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())
|