重构RTK-IMU标定链路并完成机械先验工程验证
This commit is contained in:
lichun.qu
@@ -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())
|
||||
Reference in New Issue
Block a user