def __init__(self, generated_dir, K=4, MIN_DEPTH=2, MAX_DEPTH=500): self.to_c = rot_matrix(-np.pi / 2, -np.pi / 2, 0) self.MAX_DEPTH = MAX_DEPTH self.MIN_DEPTH = MIN_DEPTH name = f"{LstSqComputer.name}_{K}" ffi, lib = load_code(generated_dir, name) # wrap c functions def residual_jac(x, poses, img_positions): out = np.zeros(((K * 2, 3)), dtype=np.float64) lib.jac_fun(ffi.cast("double *", x.ctypes.data), ffi.cast("double *", poses.ctypes.data), ffi.cast("double *", img_positions.ctypes.data), ffi.cast("double *", out.ctypes.data)) return out self.residual_jac = residual_jac def residual(x, poses, img_positions): out = np.zeros((K * 2), dtype=np.float64) lib.res_fun(ffi.cast("double *", x.ctypes.data), ffi.cast("double *", poses.ctypes.data), ffi.cast("double *", img_positions.ctypes.data), ffi.cast("double *", out.ctypes.data)) return out self.residual = residual def compute_pos_c(poses, img_positions): pos = np.zeros(3, dtype=np.float64) param = np.zeros(3, dtype=np.float64) # Can't be a view for the ctype img_positions = np.copy(img_positions) lib.compute_pos(ffi.cast("double *", self.to_c.ctypes.data), ffi.cast("double *", poses.ctypes.data), ffi.cast("double *", img_positions.ctypes.data), ffi.cast("double *", param.ctypes.data), ffi.cast("double *", pos.ctypes.data)) return pos, param self.compute_pos_c = compute_pos_c
def __init__(self, generated_dir, K=5): self.MAX_TRACKS = 6000 self.K = K # Array of tracks, each track has K 5D features preceded # by 5 params that inidicate [f_idx, last_idx, updated, complete, valid] # f_idx: idx of current last feature in track # idx of of last feature in frame # bool for whether this track has been update # bool for whether this track is complete # bool for whether this track is valid self.tracks = np.zeros((self.MAX_TRACKS, K + 1, 5)) self.tracks[:] = np.nan name = f"{FeatureHandler.name}_{K}" ffi, lib = load_code(generated_dir, name) def merge_features_c(tracks, features, empty_idxs): lib.merge_features(ffi.cast("double *", tracks.ctypes.data), ffi.cast("double *", features.ctypes.data), ffi.cast("long long *", empty_idxs.ctypes.data)) # self.merge_features = self.merge_features_python self.merge_features = merge_features_c
def __init__(self, folder, name, Q, x_initial, P_initial, dim_main, dim_main_err, # pylint: disable=dangerous-default-value N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[], global_vars=None, max_rewind_age=1.0, logger=logging): """Generates process function and all observation functions for the kalman filter.""" self.msckf = N > 0 self.N = N self.dim_augment = dim_augment self.dim_augment_err = dim_augment_err self.dim_main = dim_main self.dim_main_err = dim_main_err self.logger = logger # state x_initial = x_initial.reshape((-1, 1)) self.dim_x = x_initial.shape[0] self.dim_err = P_initial.shape[0] assert dim_main + dim_augment * N == self.dim_x assert dim_main_err + dim_augment_err * N == self.dim_err assert Q.shape == P_initial.shape # kinds that should get mahalanobis distance # tested for outlier rejection self.maha_test_kinds = maha_test_kinds # process noise self.Q = Q # rewind stuff self.max_rewind_age = max_rewind_age self.rewind_t = [] self.rewind_states = [] self.rewind_obscache = [] self.init_state(x_initial, P_initial, None) ffi, lib = load_code(folder, name, "kf") kinds, self.feature_track_kinds = [], [] for func in dir(lib): if func[:len(name) + 3] == f'{name}_h_': kinds.append(int(func[len(name) + 3:])) if func[:len(name) + 4] == f'{name}_He_': self.feature_track_kinds.append(int(func[len(name) + 4:])) # wrap all the sympy functions def wrap_1lists(func_name): func = eval(f"lib.{name}_{func_name}", {"lib": lib}) # pylint: disable=eval-used def ret(lst1, out): func(ffi.cast("double *", lst1.ctypes.data), ffi.cast("double *", out.ctypes.data)) return ret def wrap_2lists(func_name): func = eval(f"lib.{name}_{func_name}", {"lib": lib}) # pylint: disable=eval-used def ret(lst1, lst2, out): func(ffi.cast("double *", lst1.ctypes.data), ffi.cast("double *", lst2.ctypes.data), ffi.cast("double *", out.ctypes.data)) return ret def wrap_1list_1float(func_name): func = eval(f"lib.{name}_{func_name}", {"lib": lib}) # pylint: disable=eval-used def ret(lst1, fl, out): func(ffi.cast("double *", lst1.ctypes.data), ffi.cast("double", fl), ffi.cast("double *", out.ctypes.data)) return ret self.f = wrap_1list_1float("f_fun") self.F = wrap_1list_1float("F_fun") self.err_function = wrap_2lists("err_fun") self.inv_err_function = wrap_2lists("inv_err_fun") self.H_mod = wrap_1lists("H_mod_fun") self.hs, self.Hs, self.Hes = {}, {}, {} for kind in kinds: self.hs[kind] = wrap_2lists(f"h_{kind}") self.Hs[kind] = wrap_2lists(f"H_{kind}") if self.msckf and kind in self.feature_track_kinds: self.Hes[kind] = wrap_2lists(f"He_{kind}") self.set_globals = {} if global_vars is not None: for global_var in global_vars: self.set_globals[global_var] = getattr(lib, f"{name}_set_{global_var}") # wrap the C++ predict function def _predict_blas(x, P, dt): func = eval(f"lib.{name}_predict", {"lib": lib}) # pylint: disable=eval-used func(ffi.cast("double *", x.ctypes.data), ffi.cast("double *", P.ctypes.data), ffi.cast("double *", self.Q.ctypes.data), ffi.cast("double", dt)) return x, P # wrap the C++ update function def fun_wrapper(f, kind): f = eval(f"lib.{name}_{f}", {"lib": lib}) # pylint: disable=eval-used def _update_inner_blas(x, P, z, R, extra_args): f(ffi.cast("double *", x.ctypes.data), ffi.cast("double *", P.ctypes.data), ffi.cast("double *", z.ctypes.data), ffi.cast("double *", R.ctypes.data), ffi.cast("double *", extra_args.ctypes.data)) if self.msckf and kind in self.feature_track_kinds: y = z[:-len(extra_args)] else: y = z return x, P, y return _update_inner_blas self._updates = {} for kind in kinds: self._updates[kind] = fun_wrapper("update_%d" % kind, kind) def _update_blas(x, P, kind, z, R, extra_args=[]): # pylint: disable=dangerous-default-value return self._updates[kind](x, P, z, R, extra_args) # assign the functions self._predict = _predict_blas # self._predict = self._predict_python self._update = _update_blas
def __init__(self, folder, name, Q, x_initial, P_initial, dim_main, dim_main_err, N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[], global_vars=None): """Generates process function and all observation functions for the kalman filter.""" self.msckf = N > 0 self.N = N self.dim_augment = dim_augment self.dim_augment_err = dim_augment_err self.dim_main = dim_main self.dim_main_err = dim_main_err # state x_initial = x_initial.reshape((-1, 1)) self.dim_x = x_initial.shape[0] self.dim_err = P_initial.shape[0] assert dim_main + dim_augment * N == self.dim_x assert dim_main_err + dim_augment_err * N == self.dim_err assert Q.shape == P_initial.shape # kinds that should get mahalanobis distance # tested for outlier rejection self.maha_test_kinds = maha_test_kinds self.global_vars = global_vars # process noise self.Q = Q # rewind stuff self.rewind_t = [] self.rewind_states = [] self.rewind_obscache = [] self.init_state(x_initial, P_initial, None) ffi, lib = load_code(folder, name) kinds, self.feature_track_kinds = [], [] for func in dir(lib): if func[:2] == 'h_': kinds.append(int(func[2:])) if func[:3] == 'He_': self.feature_track_kinds.append(int(func[3:])) # wrap all the sympy functions def wrap_1lists(name): func = eval("lib.%s" % name, {"lib": lib}) def ret(lst1, out): func(ffi.cast("double *", lst1.ctypes.data), ffi.cast("double *", out.ctypes.data)) return ret def wrap_2lists(name): func = eval("lib.%s" % name, {"lib": lib}) def ret(lst1, lst2, out): func(ffi.cast("double *", lst1.ctypes.data), ffi.cast("double *", lst2.ctypes.data), ffi.cast("double *", out.ctypes.data)) return ret def wrap_1list_1float(name): func = eval("lib.%s" % name, {"lib": lib}) def ret(lst1, fl, out): func(ffi.cast("double *", lst1.ctypes.data), ffi.cast("double", fl), ffi.cast("double *", out.ctypes.data)) return ret self.f = wrap_1list_1float("f_fun") self.F = wrap_1list_1float("F_fun") self.err_function = wrap_2lists("err_fun") self.inv_err_function = wrap_2lists("inv_err_fun") self.H_mod = wrap_1lists("H_mod_fun") self.hs, self.Hs, self.Hes = {}, {}, {} for kind in kinds: self.hs[kind] = wrap_2lists("h_%d" % kind) self.Hs[kind] = wrap_2lists("H_%d" % kind) if self.msckf and kind in self.feature_track_kinds: self.Hes[kind] = wrap_2lists("He_%d" % kind) if self.global_vars is not None: for var in self.global_vars: fun_name = f"set_{var.name}" setattr(self, fun_name, getattr(lib, fun_name)) # wrap the C++ predict function def _predict_blas(x, P, dt): lib.predict(ffi.cast("double *", x.ctypes.data), ffi.cast("double *", P.ctypes.data), ffi.cast("double *", self.Q.ctypes.data), ffi.cast("double", dt)) return x, P # wrap the C++ update function def fun_wrapper(f, kind): f = eval("lib.%s" % f, {"lib": lib}) def _update_inner_blas(x, P, z, R, extra_args): f(ffi.cast("double *", x.ctypes.data), ffi.cast("double *", P.ctypes.data), ffi.cast("double *", z.ctypes.data), ffi.cast("double *", R.ctypes.data), ffi.cast("double *", extra_args.ctypes.data)) if self.msckf and kind in self.feature_track_kinds: y = z[:-len(extra_args)] else: y = z return x, P, y return _update_inner_blas self._updates = {} for kind in kinds: self._updates[kind] = fun_wrapper("update_%d" % kind, kind) def _update_blas(x, P, kind, z, R, extra_args=[]): return self._updates[kind](x, P, z, R, extra_args) # assign the functions self._predict = _predict_blas # self._predict = self._predict_python self._update = _update_blas