#!/usr/bin/env python3 '''Three-start joint free solve on information-selected windows.''' from __future__ import annotations import argparse,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 ( build_problem,fit_states_at_fixed_lever,solve_free_lever_many) from tools.audit_rtk_imu_factor_consistency import MECHANICAL_L_I_M,_build_segment,_jsonable,_qualified_runs def _restore_segments(sessions,reference,selections,period_s): runs=list(_qualified_runs(sessions,reference,period_s)); restored=[] for item in selections: match=None for session,run,_ in runs: if session.session_id!=item['session_id']: continue contained=(run[0].t_s<=item['start_s']+1e-6 and run[-1].t_s>=item['end_s']-1e-6) if not contained: continue nodes=tuple(node for node in run if item['start_s']-1e-6<=node.t_s<=item['end_s']+1e-6) if len(nodes)==item['node_count']: match=_build_segment(session,nodes,item['candidate_id']); break if match is None: raise RuntimeError('cannot restore '+item['candidate_id']) restored.append(match) return restored 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('--output',type=Path,required=True) parser.add_argument('--sample-period-s',type=float,default=1.) parser.add_argument('--max-nfev',type=int,default=120) parser.add_argument('--prefit-max-nfev',type=int,default=50) parser.add_argument('--prefit-workers',type=int,default=4) parser.add_argument('--start-name',choices=['all','zero','mechanical', 'mechanical_large_perturbation'],default='all') 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() selection=json.loads(args.selection.read_text(encoding='utf-8')) 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=np.asarray(args.mechanical_l_I_m,dtype=float) problems=[build_problem(segment,rotation,mechanical,args.hpr_direct_sigma_rad) for segment in segments] starts={'zero':np.zeros(3),'mechanical':mechanical, 'mechanical_large_perturbation':mechanical+args.large_perturbation_m} if args.start_name!='all': starts={args.start_name:starts[args.start_name]} results={}; prefit={} for name,value in starts.items(): with ThreadPoolExecutor(max_workers=args.prefit_workers) as executor: fitted=list(executor.map( lambda problem:fit_states_at_fixed_lever( problem,value,args.prefit_max_nfev),problems)) state_values=[item[0] for item in fitted] prefit[name]=[item[1] for item in fitted] results[name]=asdict(solve_free_lever_many( problems,value,args.max_nfev,state_values)) 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':'information-selected multi-window no-prior three-start free lever', 'translation_variable_enabled':True,'manual_prior_used':False, 'loo_bootstrap_sensitivity_called':False, 'selection_artifact':str(args.selection),'selected_window_count':len(segments), 'start_name':args.start_name,'nuisance_prefit':prefit, 'nuisance_prefit_is_not_lever_prior':True, 'sample_overlap_audit':selection['sample_overlap_audit'], 'information_curve':selection['lever_std_vs_information_curve'], '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, '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({'selected_window_count':len(segments), 'solutions':{name:{'success':value['success'],'l_I_m':value['final_l_I_m'], 'std_m':value['lever_std_m'],'cost':value['final_cost'], 'chi_square_per_dof':value['chi_square_per_dof'], 'singular_values':value['lever_information_singular_values']} for name,value in results.items()},'maximum_solution_spread_m':spread, 'data_only_translation_observable':observable}),ensure_ascii=False,indent=2)) return 0 if __name__=='__main__': raise SystemExit(main())