From 44e8a891a2810f274a1fa980775155d9463e87b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Tue, 21 Nov 2023 21:00:57 -0800 Subject: [PATCH] Load model libraries at runtime in cython ekf (#37) * RednoseCompileFilter tool * Refactor sconscripts * Move lst_sq and feature_handler to examples * add ekf_load_code_if_needed * Fat binary for cython ext * Restructure. Remove common_ekf. Load libs at runtime * Build compatibility with openpilot * ekf_lib_init * Fix styling issues * Fix linker flags for mac * Remove useless pyx imports * Build static lib instead of shared * newlines * Remove lst_sq_computer and feature_handler --- .gitignore | 1 + SConstruct | 17 +-- examples/SConscript | 19 +++ examples/test_kinematic_kf.py | 8 +- rednose/SConscript | 54 ++------ rednose/helpers/__init__.py | 6 +- rednose/helpers/common_ekf.cc | 19 --- rednose/helpers/{common_ekf.h => ekf.h} | 13 +- rednose/helpers/ekf_load.cc | 39 ++++++ rednose/helpers/ekf_load.h | 9 ++ rednose/helpers/ekf_sym.cc | 1 - rednose/helpers/ekf_sym.h | 3 +- rednose/helpers/ekf_sym.py | 6 +- rednose/helpers/ekf_sym_pyx.pyx | 5 + rednose/helpers/feature_handler.py | 160 ---------------------- rednose/helpers/lst_sq_computer.py | 175 ------------------------ site_scons/site_tools/rednose_filter.py | 48 +++++++ 17 files changed, 155 insertions(+), 428 deletions(-) create mode 100644 examples/SConscript delete mode 100644 rednose/helpers/common_ekf.cc rename rednose/helpers/{common_ekf.h => ekf.h} (85%) create mode 100644 rednose/helpers/ekf_load.cc create mode 100644 rednose/helpers/ekf_load.h delete mode 100755 rednose/helpers/feature_handler.py delete mode 100755 rednose/helpers/lst_sq_computer.py create mode 100644 site_scons/site_tools/rednose_filter.py diff --git a/.gitignore b/.gitignore index b411c92..bf2b50d 100644 --- a/.gitignore +++ b/.gitignore @@ -15,6 +15,7 @@ __pycache__/ *$py.class # C extensions +*.a *.o *.so diff --git a/SConstruct b/SConstruct index 012ae7e..c668d1b 100644 --- a/SConstruct +++ b/SConstruct @@ -36,7 +36,8 @@ env = Environment( CFLAGS="-std=gnu11", CXXFLAGS="-std=c++1z", CPPPATH=cpppath, - tools=["default", "cython"], + REDNOSE_ROOT=Dir("#").abspath, + tools=["default", "cython", "rednose_filter"], ) # Cython build enviroment @@ -52,17 +53,7 @@ elif arch == "aarch64": else: envCython["LINKFLAGS"] = ["-pthread", "-shared"] -rednose_config = { - 'generated_folder': '#examples/generated', - 'to_build': { - 'live': ('#examples/live_kf.py', True, [], []), - 'kinematic': ('#examples/kinematic_kf.py', True, [], []), - 'compare': ('#examples/test_compare.py', True, [], []), - 'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, [], []), - 'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, [], []), - 'feature_handler_5': ('#rednose/helpers/feature_handler.py', False, [], []), - }, -} +Export('env', 'envCython', 'common') -Export('env', 'envCython', 'arch', 'rednose_config', 'common') SConscript(['#rednose/SConscript']) +SConscript(['#examples/SConscript']) diff --git a/examples/SConscript b/examples/SConscript new file mode 100644 index 0000000..3d178ad --- /dev/null +++ b/examples/SConscript @@ -0,0 +1,19 @@ +Import('env') + +gen_dir = Dir('generated/').abspath + +env.RednoseCompileFilter( + target="live", + filter_gen_script="live_kf.py", + output_dir=gen_dir, +) +env.RednoseCompileFilter( + target="kinematic", + filter_gen_script="kinematic_kf.py", + output_dir=gen_dir, +) +env.RednoseCompileFilter( + target="compare", + filter_gen_script="test_compare.py", + output_dir=gen_dir, +) diff --git a/examples/test_kinematic_kf.py b/examples/test_kinematic_kf.py index 2d6c5e5..9f7c908 100644 --- a/examples/test_kinematic_kf.py +++ b/examples/test_kinematic_kf.py @@ -38,11 +38,11 @@ def test_kinematic_kf(self): # Retrieve kf values state = kf.x - xs_kf.append(float(state[States.POSITION])) - vs_kf.append(float(state[States.VELOCITY])) + xs_kf.append(float(state[States.POSITION].item())) + vs_kf.append(float(state[States.VELOCITY].item())) std = np.sqrt(kf.P) - xs_kf_std.append(float(std[States.POSITION, States.POSITION])) - vs_kf_std.append(float(std[States.VELOCITY, States.VELOCITY])) + xs_kf_std.append(float(std[States.POSITION, States.POSITION].item())) + vs_kf_std.append(float(std[States.VELOCITY, States.VELOCITY].item())) # Update simulation x += v * dt diff --git a/rednose/SConscript b/rednose/SConscript index 398b4b4..52e36e0 100644 --- a/rednose/SConscript +++ b/rednose/SConscript @@ -1,45 +1,17 @@ -Import('env', 'envCython', 'arch', 'rednose_config', 'common') +Import('env', 'envCython', 'common') -generated_folder = rednose_config['generated_folder'] - -templates = Glob('#rednose/templates/*') - -sympy_helpers = "#rednose/helpers/sympy_helpers.py" -ekf_sym = "#rednose/helpers/ekf_sym.py" -ekf_sym_pyx = "#rednose/helpers/ekf_sym_pyx.pyx" -ekf_sym_cc = env.Object("#rednose/helpers/ekf_sym.cc") -common_ekf = "#rednose/helpers/common_ekf.cc" - -found = {} -for target, (command, *args) in rednose_config['to_build'].items(): - if File(command).exists(): - found[target] = (command, *args) - -lib_target = [common_ekf] -for target, (command, combined_lib, extra_generated, deps) in found.items(): - target_files = File([f'{generated_folder}/{target}.cpp', f'{generated_folder}/{target}.h']) - extra_generated = [File(f'{generated_folder}/{x}') for x in extra_generated] - command_file = File(command) - - env.Command(target_files + extra_generated, - [templates, command_file, sympy_helpers, ekf_sym] + deps, - command_file.get_abspath() + " " + target + " " + Dir(generated_folder).get_abspath()) - - if combined_lib: - lib_target.append(target_files[0]) - else: - env.SharedLibrary(f'{generated_folder}/' + target, [target_files[0], common_ekf]) - -libkf = env.SharedLibrary(f'{generated_folder}/libkf', lib_target) - -lenv = envCython.Clone() -lenv["LINKFLAGS"] += [libkf[0].get_labspath()] - -# for SWAGLOG support +cc_sources = [ + "helpers/ekf_load.cc", + "helpers/ekf_sym.cc", +] +libs = ["dl"] if common != "": - lenv["LIBS"] = ['zmq', common] + lenv["LIBS"] + # for SWAGLOG support + libs += [common, 'zmq'] -ekf_sym_so = lenv.Program('#rednose/helpers/ekf_sym_pyx.so', [ekf_sym_pyx, ekf_sym_cc, common_ekf]) -lenv.Depends(ekf_sym_so, libkf) +ekf_objects = env.SharedObject(cc_sources) +rednose = env.Library("helpers/ekf_sym", ekf_objects, LIBS=libs) +rednose_python = envCython.Program("helpers/ekf_sym_pyx.so", ["helpers/ekf_sym_pyx.pyx", ekf_objects], + LIBS=libs + envCython["LIBS"]) -Export('libkf') +Export('rednose', 'rednose_python') diff --git a/rednose/helpers/__init__.py b/rednose/helpers/__init__.py index 12cce34..b2beede 100644 --- a/rednose/helpers/__init__.py +++ b/rednose/helpers/__init__.py @@ -13,11 +13,9 @@ def write_code(folder, name, code, header): open(os.path.join(folder, f"{name}.h"), 'w', encoding='utf-8').write(header) -def load_code(folder, name, lib_name=None): - if lib_name is None: - lib_name = name +def load_code(folder, name): shared_ext = "dylib" if platform.system() == "Darwin" else "so" - shared_fn = os.path.join(folder, f"lib{lib_name}.{shared_ext}") + shared_fn = os.path.join(folder, f"lib{name}.{shared_ext}") header_fn = os.path.join(folder, f"{name}.h") with open(header_fn, encoding='utf-8') as f: diff --git a/rednose/helpers/common_ekf.cc b/rednose/helpers/common_ekf.cc deleted file mode 100644 index 1e63902..0000000 --- a/rednose/helpers/common_ekf.cc +++ /dev/null @@ -1,19 +0,0 @@ -#include "common_ekf.h" - -std::vector& get_ekfs() { - static std::vector vec; - return vec; -} - -void ekf_register(const EKF* ekf) { - get_ekfs().push_back(ekf); -} - -const EKF* ekf_lookup(const std::string& ekf_name) { - for (const auto& ekfi : get_ekfs()) { - if (ekf_name == ekfi->name) { - return ekfi; - } - } - return NULL; -} diff --git a/rednose/helpers/common_ekf.h b/rednose/helpers/ekf.h similarity index 85% rename from rednose/helpers/common_ekf.h rename to rednose/helpers/ekf.h index 5dfdd44..2afe6dd 100644 --- a/rednose/helpers/common_ekf.h +++ b/rednose/helpers/ekf.h @@ -32,12 +32,11 @@ struct EKF { std::unordered_map extra_routines = {}; }; -std::vector& get_ekfs(); -const EKF* ekf_lookup(const std::string& ekf_name); - -void ekf_register(const EKF* ekf); - -#define ekf_init(ekf) \ +#define ekf_lib_init(ekf) \ +extern "C" void* ekf_get() { \ + return (void*) &ekf; \ +} \ +extern void __attribute__((weak)) ekf_register(const EKF* ptr); \ static void __attribute__((constructor)) do_ekf_init_ ## ekf(void) { \ - ekf_register(&ekf); \ + if (ekf_register) ekf_register(&ekf); \ } diff --git a/rednose/helpers/ekf_load.cc b/rednose/helpers/ekf_load.cc new file mode 100644 index 0000000..882b0b4 --- /dev/null +++ b/rednose/helpers/ekf_load.cc @@ -0,0 +1,39 @@ +#include "ekf_load.h" +#include + +std::vector& ekf_get_all() { + static std::vector vec; + return vec; +} + +void ekf_register(const EKF* ekf) { + ekf_get_all().push_back(ekf); +} + +const EKF* ekf_lookup(const std::string& ekf_name) { + for (const auto& ekfi : ekf_get_all()) { + if (ekf_name == ekfi->name) { + return ekfi; + } + } + return NULL; +} + +void ekf_load_and_register(const std::string& ekf_directory, const std::string& ekf_name) { + if (ekf_lookup(ekf_name)) { + return; + } + +#ifdef __APPLE__ + std::string dylib_ext = ".dylib"; +#else + std::string dylib_ext = ".so"; +#endif + std::string ekf_path = ekf_directory + "/lib" + ekf_name + dylib_ext; + void* handle = dlopen(ekf_path.c_str(), RTLD_NOW); + assert(handle); + void* (*ekf_get)() = (void*(*)())dlsym(handle, "ekf_get"); + assert(ekf_get != NULL); + const EKF* ekf = (const EKF*)ekf_get(); + ekf_register(ekf); +} diff --git a/rednose/helpers/ekf_load.h b/rednose/helpers/ekf_load.h new file mode 100644 index 0000000..89180b8 --- /dev/null +++ b/rednose/helpers/ekf_load.h @@ -0,0 +1,9 @@ +#include +#include + +#include "ekf.h" + +std::vector& ekf_get_all(); +const EKF* ekf_lookup(const std::string& ekf_name); +void ekf_register(const EKF* ekf); +void ekf_load_and_register(const std::string& ekf_directory, const std::string& ekf_name); diff --git a/rednose/helpers/ekf_sym.cc b/rednose/helpers/ekf_sym.cc index cf3c88f..38d419d 100644 --- a/rednose/helpers/ekf_sym.cc +++ b/rednose/helpers/ekf_sym.cc @@ -9,7 +9,6 @@ EKFSym::EKFSym(std::string name, Map Q, Map x_initial, Map< std::vector quaternion_idxs, std::vector global_vars, double max_rewind_age) { // TODO: add logger - this->ekf = ekf_lookup(name); assert(this->ekf); diff --git a/rednose/helpers/ekf_sym.h b/rednose/helpers/ekf_sym.h index 86d6ca1..b83d7f3 100644 --- a/rednose/helpers/ekf_sym.h +++ b/rednose/helpers/ekf_sym.h @@ -12,7 +12,8 @@ #include -#include "common_ekf.h" +#include "ekf.h" +#include "ekf_load.h" #define REWIND_TO_KEEP 512 diff --git a/rednose/helpers/ekf_sym.py b/rednose/helpers/ekf_sym.py index de68991..8f0425c 100644 --- a/rednose/helpers/ekf_sym.py +++ b/rednose/helpers/ekf_sym.py @@ -116,7 +116,7 @@ def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_p sympy_header, code = sympy_into_c(sympy_functions, global_vars) header = "#pragma once\n" - header += "#include \"rednose/helpers/common_ekf.h\"\n" + header += "#include \"rednose/helpers/ekf.h\"\n" header += "extern \"C\" {\n" pre_code = f"#include \"{name}.h\"\n" @@ -200,7 +200,7 @@ def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_p post_code += f" {{ \"{f}\", {name}_{f} }},\n" post_code += " },\n" post_code += "};\n\n" - post_code += f"ekf_init({name});\n" + post_code += f"ekf_lib_init({name})\n" # merge code blocks header += "}" @@ -252,7 +252,7 @@ def __init__(self, folder, name, Q, x_initial, P_initial, dim_main, dim_main_err self.rewind_obscache = [] self.init_state(x_initial, P_initial, None) - ffi, lib = load_code(folder, name, "kf") + ffi, lib = load_code(folder, name) kinds, self.feature_track_kinds = [], [] for func in dir(lib): if func[:len(name) + 3] == f'{name}_h_': diff --git a/rednose/helpers/ekf_sym_pyx.pyx b/rednose/helpers/ekf_sym_pyx.pyx index e746128..f552ea7 100644 --- a/rednose/helpers/ekf_sym_pyx.pyx +++ b/rednose/helpers/ekf_sym_pyx.pyx @@ -11,12 +11,16 @@ cimport numpy as np import numpy as np + cdef extern from "" namespace "std" nogil: cdef cppclass optional[T]: ctypedef T value_type bool has_value() T& value() +cdef extern from "rednose/helpers/ekf_load.h": + cdef void ekf_load_and_register(string directory, string name) + cdef extern from "rednose/helpers/ekf_sym.h" namespace "EKFS": cdef cppclass MapVectorXd "Eigen::Map": MapVectorXd(double*, int) @@ -85,6 +89,7 @@ cdef class EKF_sym_pyx: int dim_main_err, int N=0, int dim_augment=0, int dim_augment_err=0, list maha_test_kinds=[], list quaternion_idxs=[], list global_vars=[], double max_rewind_age=1.0, logger=None): # TODO logger + ekf_load_and_register(gen_dir.encode('utf8'), name.encode('utf8')) cdef np.ndarray[np.float64_t, ndim=2, mode='c'] Q_b = np.ascontiguousarray(Q, dtype=np.double) cdef np.ndarray[np.float64_t, ndim=1, mode='c'] x_initial_b = np.ascontiguousarray(x_initial, dtype=np.double) diff --git a/rednose/helpers/feature_handler.py b/rednose/helpers/feature_handler.py deleted file mode 100755 index 165c9a4..0000000 --- a/rednose/helpers/feature_handler.py +++ /dev/null @@ -1,160 +0,0 @@ -#!/usr/bin/env python3 - -import os -import sys - -import numpy as np - -from rednose.helpers import TEMPLATE_DIR, load_code, write_code -from rednose.helpers.sympy_helpers import quat_matrix_l, rot_matrix - - -def sane(track): - img_pos = track[1:, 2:4] - diffs_x = abs(img_pos[1:, 0] - img_pos[:-1, 0]) - diffs_y = abs(img_pos[1:, 1] - img_pos[:-1, 1]) - for i in range(1, len(diffs_x)): - if ((diffs_x[i] > 0.05 or diffs_x[i - 1] > 0.05) and - (diffs_x[i] > 2 * diffs_x[i - 1] or - diffs_x[i] < .5 * diffs_x[i - 1])) or \ - ((diffs_y[i] > 0.05 or diffs_y[i - 1] > 0.05) and - (diffs_y[i] > 2 * diffs_y[i - 1] or - diffs_y[i] < .5 * diffs_y[i - 1])): - return False - return True - - -class FeatureHandler(): - name = 'feature_handler' - - @staticmethod - def generate_code(generated_dir, K=5): - # Wrap c code for slow matching - c_header = "\nvoid merge_features(double *tracks, double *features, long long *empty_idxs);" - - c_code = "#include \n" - c_code += "#include \n" - c_code += "#define K %d\n" % K - c_code += "extern \"C\" {\n" - c_code += "\n" + open(os.path.join(TEMPLATE_DIR, "feature_handler.c"), encoding='utf-8').read() - c_code += "\n}\n" - - filename = f"{FeatureHandler.name}_{K}" - write_code(generated_dir, filename, c_code, c_header) - - 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 reset(self): - self.tracks[:] = np.nan - - def merge_features_python(self, tracks, features, empty_idxs): - empty_idx = 0 - for f in features: - match_idx = int(f[4]) - if tracks[match_idx, 0, 1] == match_idx and tracks[match_idx, 0, 2] == 0: - tracks[match_idx, 0, 0] += 1 - tracks[match_idx, 0, 1] = f[1] - tracks[match_idx, 0, 2] = 1 - tracks[match_idx, int(tracks[match_idx, 0, 0])] = f - if tracks[match_idx, 0, 0] == self.K: - tracks[match_idx, 0, 3] = 1 - if sane(tracks[match_idx]): - tracks[match_idx, 0, 4] = 1 - else: - if empty_idx == len(empty_idxs): - print('need more empty space') - continue - tracks[empty_idxs[empty_idx], 0, 0] = 1 - tracks[empty_idxs[empty_idx], 0, 1] = f[1] - tracks[empty_idxs[empty_idx], 0, 2] = 1 - tracks[empty_idxs[empty_idx], 1] = f - empty_idx += 1 - - def update_tracks(self, features): - last_idxs = np.copy(self.tracks[:, 0, 1]) - real = np.isfinite(last_idxs) - self.tracks[last_idxs[real].astype(int)] = self.tracks[real] - - mask = np.ones(self.MAX_TRACKS, bool) - mask[last_idxs[real].astype(int)] = 0 - empty_idxs = np.arange(self.MAX_TRACKS)[mask] - - self.tracks[empty_idxs] = np.nan - self.tracks[:, 0, 2] = 0 - self.merge_features(self.tracks, features, empty_idxs) - - def handle_features(self, features): - self.update_tracks(features) - valid_idxs = self.tracks[:, 0, 4] == 1 - complete_idxs = self.tracks[:, 0, 3] == 1 - stale_idxs = self.tracks[:, 0, 2] == 0 - valid_tracks = self.tracks[valid_idxs] - self.tracks[complete_idxs] = np.nan - self.tracks[stale_idxs] = np.nan - return valid_tracks[:, 1:, :4].reshape((len(valid_tracks), self.K * 4)) - - -def generate_orient_error_jac(K): - import sympy as sp - from rednose.helpers.sympy_helpers import quat_rotate - - x_sym = sp.MatrixSymbol('abr', 3, 1) - dtheta = sp.MatrixSymbol('dtheta', 3, 1) - delta_quat = sp.Matrix(np.ones(4)) - delta_quat[1:, :] = sp.Matrix(0.5 * dtheta[0:3, :]) - poses_sym = sp.MatrixSymbol('poses', 7 * K, 1) - img_pos_sym = sp.MatrixSymbol('img_positions', 2 * K, 1) - alpha, beta, rho = x_sym - to_c = sp.Matrix(rot_matrix(-np.pi / 2, -np.pi / 2, 0)) - pos_0 = sp.Matrix(np.array(poses_sym[K * 7 - 7:K * 7 - 4])[:, 0]) - q = quat_matrix_l(poses_sym[K * 7 - 4:K * 7]) * delta_quat - quat_rot = quat_rotate(*q) - rot_g_to_0 = to_c * quat_rot.T - rows = [] - for i in range(K): - pos_i = sp.Matrix(np.array(poses_sym[i * 7:i * 7 + 3])[:, 0]) - q = quat_matrix_l(poses_sym[7 * i + 3:7 * i + 7]) * delta_quat - quat_rot = quat_rotate(*q) - rot_g_to_i = to_c * quat_rot.T - rot_0_to_i = rot_g_to_i * (rot_g_to_0.T) - trans_0_to_i = rot_g_to_i * (pos_0 - pos_i) - funct_vec = rot_0_to_i * sp.Matrix([alpha, beta, 1]) + rho * trans_0_to_i - h1, h2, h3 = funct_vec - rows.append(h1 / h3 - img_pos_sym[i * 2 + 0]) - rows.append(h2 / h3 - img_pos_sym[i * 2 + 1]) - img_pos_residual_sym = sp.Matrix(rows) - - # sympy into c - sympy_functions = [] - sympy_functions.append(('orient_error_jac', img_pos_residual_sym.jacobian(dtheta), [x_sym, poses_sym, img_pos_sym, dtheta])) - - return sympy_functions - - -if __name__ == "__main__": - K = int(sys.argv[1].split("_")[-1]) - generated_dir = sys.argv[2] - FeatureHandler.generate_code(generated_dir, K=K) diff --git a/rednose/helpers/lst_sq_computer.py b/rednose/helpers/lst_sq_computer.py deleted file mode 100755 index 3c74eaa..0000000 --- a/rednose/helpers/lst_sq_computer.py +++ /dev/null @@ -1,175 +0,0 @@ -#!/usr/bin/env python3 -import os -import sys - -import numpy as np -import sympy as sp - -from rednose.helpers import TEMPLATE_DIR, load_code, write_code -from rednose.helpers.sympy_helpers import quat_rotate, sympy_into_c, rot_matrix, rotations_from_quats - - -def generate_residual(K): - x_sym = sp.MatrixSymbol('abr', 3, 1) - poses_sym = sp.MatrixSymbol('poses', 7 * K, 1) - img_pos_sym = sp.MatrixSymbol('img_positions', 2 * K, 1) - alpha, beta, rho = x_sym - to_c = sp.Matrix(rot_matrix(-np.pi / 2, -np.pi / 2, 0)) - pos_0 = sp.Matrix(np.array(poses_sym[K * 7 - 7:K * 7 - 4])[:, 0]) - q = poses_sym[K * 7 - 4:K * 7] - quat_rot = quat_rotate(*q) - rot_g_to_0 = to_c * quat_rot.T - rows = [] - - for i in range(K): - pos_i = sp.Matrix(np.array(poses_sym[i * 7:i * 7 + 3])[:, 0]) - q = poses_sym[7 * i + 3:7 * i + 7] - quat_rot = quat_rotate(*q) - rot_g_to_i = to_c * quat_rot.T - rot_0_to_i = rot_g_to_i * rot_g_to_0.T - trans_0_to_i = rot_g_to_i * (pos_0 - pos_i) - funct_vec = rot_0_to_i * sp.Matrix([alpha, beta, 1]) + rho * trans_0_to_i - h1, h2, h3 = funct_vec - rows.append(h1 / h3 - img_pos_sym[i * 2 + 0]) - rows.append(h2 / h3 - img_pos_sym[i * 2 + 1]) - img_pos_residual_sym = sp.Matrix(rows) - - # sympy into c - sympy_functions = [] - sympy_functions.append(('res_fun', img_pos_residual_sym, [x_sym, poses_sym, img_pos_sym])) - sympy_functions.append(('jac_fun', img_pos_residual_sym.jacobian(x_sym), [x_sym, poses_sym, img_pos_sym])) - - return sympy_functions - - -class LstSqComputer(): - name = 'pos_computer' - - @staticmethod - def generate_code(generated_dir, K=4): - sympy_functions = generate_residual(K) - header, sympy_code = sympy_into_c(sympy_functions) - - code = "\n#include \"rednose/helpers/common_ekf.h\"\n" - code += "\n#define KDIM %d\n" % K - code += "extern \"C\" {\n" - code += sympy_code - code += "\n" + open(os.path.join(TEMPLATE_DIR, "compute_pos.c"), encoding='utf-8').read() + "\n" - code += "}\n" - - header += "\nvoid compute_pos(double *to_c, double *in_poses, double *in_img_positions, double *param, double *pos);\n" - - filename = f"{LstSqComputer.name}_{K}" - write_code(generated_dir, filename, code, header) - - 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 compute_pos(self, poses, img_positions, debug=False): - pos, param = self.compute_pos_c(poses, img_positions) - # pos, param = self.compute_pos_python(poses, img_positions) - - depth = 1 / param[2] - if debug: - # orient_err_jac = self.orient_error_jac(param, poses, img_positions, np.zeros(3)).reshape((-1,2,3)) - jac = self.residual_jac(param, poses, img_positions).reshape((-1, 2, 3)) - res = self.residual(param, poses, img_positions).reshape((-1, 2)) - return pos, param, res, jac # , orient_err_jac - elif (self.MIN_DEPTH < depth < self.MAX_DEPTH): - return pos - else: - return None - - def gauss_newton(self, fun, jac, x, args): - poses, img_positions = args - delta = 1 - counter = 0 - while abs(np.linalg.norm(delta)) > 1e-4 and counter < 30: - delta = np.linalg.pinv(jac(x, poses, img_positions)).dot(fun(x, poses, img_positions)) - x = x - delta - counter += 1 - return [x] - - def compute_pos_python(self, poses, img_positions, check_quality=False): - import scipy.optimize as opt - - # This procedure is also described - # in the MSCKF paper (Mourikis et al. 2007) - x = np.array([img_positions[-1][0], - img_positions[-1][1], 0.1]) - res = opt.leastsq(self.residual, x, Dfun=self.residual_jac, args=(poses, img_positions)) # scipy opt - # res = self.gauss_newton(self.residual, self.residual_jac, x, (poses, img_positions)) # diy gauss_newton - - alpha, beta, rho = res[0] - rot_0_to_g = (rotations_from_quats(poses[-1, 3:])).dot(self.to_c.T) - return (rot_0_to_g.dot(np.array([alpha, beta, 1]))) / rho + poses[-1, :3] - - -# EXPERIMENTAL CODE -def unroll_shutter(img_positions, poses, v, rot_rates, ecef_pos): - # only speed correction for now - t_roll = 0.016 # 16ms rolling shutter? - vroll, vpitch, vyaw = rot_rates - A = 0.5 * np.array([[-1, -vroll, -vpitch, -vyaw], - [vroll, 0, vyaw, -vpitch], - [vpitch, -vyaw, 0, vroll], - [vyaw, vpitch, -vroll, 0]]) - q_dot = A.dot(poses[-1][3:7]) - v = np.append(v, q_dot) - v = np.array([v[0], v[1], v[2], 0, 0, 0, 0]) - current_pose = poses[-1] + v * 0.05 - poses = np.vstack((current_pose, poses)) - dt = -img_positions[:, 1] * t_roll / 0.48 - errs = project(poses, ecef_pos) - project(poses + np.atleast_2d(dt).T.dot(np.atleast_2d(v)), ecef_pos) - return img_positions - errs - - -def project(poses, ecef_pos): - img_positions = np.zeros((len(poses), 2)) - for i, p in enumerate(poses): - cam_frame = rotations_from_quats(p[3:]).T.dot(ecef_pos - p[:3]) - img_positions[i] = np.array([cam_frame[1] / cam_frame[0], cam_frame[2] / cam_frame[0]]) - return img_positions - - -if __name__ == "__main__": - K = int(sys.argv[1].split("_")[-1]) - generated_dir = sys.argv[2] - LstSqComputer.generate_code(generated_dir, K=K) diff --git a/site_scons/site_tools/rednose_filter.py b/site_scons/site_tools/rednose_filter.py new file mode 100644 index 0000000..b3fee62 --- /dev/null +++ b/site_scons/site_tools/rednose_filter.py @@ -0,0 +1,48 @@ +import platform + +from SCons.Script import Dir, File + + +def compile_single_filter(env, target, filter_gen_script, output_dir, extra_gen_artifacts, script_deps): + generated_src_files = [File(f) for f in [f'{output_dir}/{target}.cpp', f'{output_dir}/{target}.h']] + extra_generated_files = [File(f'{output_dir}/{x}') for x in extra_gen_artifacts] + generator_file = File(filter_gen_script) + + env.Command(generated_src_files + extra_generated_files, + [generator_file] + script_deps, f"{generator_file.get_abspath()} {target} {Dir(output_dir).get_abspath()}") + + generated_cc_file = File(generated_src_files[:1]) + + return generated_cc_file + + +class BaseRednoseCompileMethod: + def __init__(self, base_py_deps, base_cc_deps): + self.base_py_deps = base_py_deps + self.base_cc_deps = base_cc_deps + + +class CompileFilterMethod(BaseRednoseCompileMethod): + def __call__(self, env, target, filter_gen_script, output_dir, extra_gen_artifacts=[], gen_script_deps=[]): + objects = compile_single_filter(env, target, filter_gen_script, output_dir, extra_gen_artifacts, self.base_py_deps + gen_script_deps) + linker_flags = env.get("LINKFLAGS", []) + if platform.system() == "Darwin": + linker_flags = ["-undefined", "dynamic_lookup"] + lib_target = env.SharedLibrary(f'{output_dir}/{target}', [self.base_cc_deps, objects], LINKFLAGS=linker_flags) + + return lib_target + + +def generate(env): + templates = env.Glob("$REDNOSE_ROOT/rednose/templates/*") + sympy_helpers = env.File("$REDNOSE_ROOT/rednose/helpers/sympy_helpers.py") + ekf_sym = env.File("$REDNOSE_ROOT/rednose/helpers/ekf_sym.py") + + gen_script_deps = templates + [sympy_helpers, ekf_sym] + filter_lib_deps = [] + + env.AddMethod(CompileFilterMethod(gen_script_deps, filter_lib_deps), "RednoseCompileFilter") + + +def exists(env): + return True