140 lines
7.7 KiB
Python
140 lines
7.7 KiB
Python
#!/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())
|