def generate_code(K=4): sympy_functions = generate_residual(K) header, code = sympy_into_c(sympy_functions) code += "\n#define KDIM %d\n" % K code += "\n" + open(os.path.join(TEMPLATE_DIR, "compute_pos.c")).read() header += """ void compute_pos(double *to_c, double *in_poses, double *in_img_positions, double *param, double *pos); """ filename = f"{LstSqComputer.name}_{K}" write_code(filename, code, header)
def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[]): # optional state transition matrix, H modifier # and err_function if an error-state kalman filter (ESKF) # is desired. Best described in "Quaternion kinematics # for the error-state Kalman filter" by Joan Sola if eskf_params: err_eqs = eskf_params[0] inv_err_eqs = eskf_params[1] H_mod_sym = eskf_params[2] f_err_sym = eskf_params[3] x_err_sym = eskf_params[4] else: nom_x = sp.MatrixSymbol('nom_x', dim_x, 1) true_x = sp.MatrixSymbol('true_x', dim_x, 1) delta_x = sp.MatrixSymbol('delta_x', dim_x, 1) err_function_sym = sp.Matrix(nom_x + delta_x) inv_err_function_sym = sp.Matrix(true_x - nom_x) err_eqs = [err_function_sym, nom_x, delta_x] inv_err_eqs = [inv_err_function_sym, nom_x, true_x] H_mod_sym = sp.Matrix(np.eye(dim_x)) f_err_sym = f_sym x_err_sym = x_sym # This configures the multi-state augmentation # needed for EKF-SLAM with MSCKF (Mourikis et al 2007) if msckf_params: msckf = True dim_main = msckf_params[0] # size of the main state dim_augment = msckf_params[1] # size of one augment state chunk dim_main_err = msckf_params[2] dim_augment_err = msckf_params[3] N = msckf_params[4] feature_track_kinds = msckf_params[5] assert dim_main + dim_augment * N == dim_x assert dim_main_err + dim_augment_err * N == dim_err else: msckf = False dim_main = dim_x dim_augment = 0 dim_main_err = dim_err dim_augment_err = 0 N = 0 # linearize with jacobians F_sym = f_err_sym.jacobian(x_err_sym) if eskf_params: for sym in x_err_sym: F_sym = F_sym.subs(sym, 0) assert dt_sym in F_sym.free_symbols for i in range(len(obs_eqs)): obs_eqs[i].append(obs_eqs[i][0].jacobian(x_sym)) if msckf and obs_eqs[i][1] in feature_track_kinds: obs_eqs[i].append(obs_eqs[i][0].jacobian(obs_eqs[i][2])) else: obs_eqs[i].append(None) # collect sympy functions sympy_functions = [] # error functions sympy_functions.append(('err_fun', err_eqs[0], [err_eqs[1], err_eqs[2]])) sympy_functions.append( ('inv_err_fun', inv_err_eqs[0], [inv_err_eqs[1], inv_err_eqs[2]])) # H modifier for ESKF updates sympy_functions.append(('H_mod_fun', H_mod_sym, [x_sym])) # state propagation function sympy_functions.append(('f_fun', f_sym, [x_sym, dt_sym])) sympy_functions.append(('F_fun', F_sym, [x_sym, dt_sym])) # observation functions for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs: sympy_functions.append(('h_%d' % kind, h_sym, [x_sym, ea_sym])) sympy_functions.append(('H_%d' % kind, H_sym, [x_sym, ea_sym])) if msckf and kind in feature_track_kinds: sympy_functions.append(('He_%d' % kind, He_sym, [x_sym, ea_sym])) # Generate and wrap all th c code header, code = sympy_into_c(sympy_functions) extra_header = "#define DIM %d\n" % dim_x extra_header += "#define EDIM %d\n" % dim_err extra_header += "#define MEDIM %d\n" % dim_main_err extra_header += "typedef void (*Hfun)(double *, double *, double *);\n" extra_header += "\nvoid predict(double *x, double *P, double *Q, double dt);" extra_post = "" for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs: if msckf and kind in feature_track_kinds: He_str = 'He_%d' % kind # ea_dim = ea_sym.shape[0] else: He_str = 'NULL' # ea_dim = 1 # not really dim of ea but makes c function work maha_thresh = chi2_ppf(0.95, int( h_sym.shape[0])) # mahalanobis distance for outlier detection maha_test = kind in maha_test_kinds extra_post += """ void update_%d(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<%d,%d,%d>(in_x, in_P, h_%d, H_%d, %s, in_z, in_R, in_ea, MAHA_THRESH_%d); } """ % (kind, h_sym.shape[0], 3, maha_test, kind, kind, He_str, kind) extra_header += "\nconst static double MAHA_THRESH_%d = %f;" % ( kind, maha_thresh) extra_header += "\nvoid update_%d(double *, double *, double *, double *, double *);" % kind code += '\nextern "C"{\n' + extra_header + "\n}\n" code += "\n" + open(os.path.join(TEMPLATE_DIR, "ekf_c.c")).read() code += '\nextern "C"{\n' + extra_post + "\n}\n" header += "\n" + extra_header write_code(name, code, header)