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