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