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