mirror of https://github.com/commaai/rednose.git
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
This commit is contained in:
Kacper Rączy
GitHub
parent
8658bed296
commit
44e8a891a2
|
|
@ -15,6 +15,7 @@ __pycache__/
|
|||
*$py.class
|
||||
|
||||
# C extensions
|
||||
*.a
|
||||
*.o
|
||||
*.so
|
||||
|
||||
|
|
|
|||
17
SConstruct
17
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'])
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
)
|
||||
|
|
@ -38,11 +38,11 @@ class TestKinematic(unittest.TestCase):
|
|||
|
||||
# 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
|
||||
|
|
|
|||
|
|
@ -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')
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -1,19 +0,0 @@
|
|||
#include "common_ekf.h"
|
||||
|
||||
std::vector<const EKF*>& get_ekfs() {
|
||||
static std::vector<const EKF*> 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;
|
||||
}
|
||||
|
|
@ -32,12 +32,11 @@ struct EKF {
|
|||
std::unordered_map<std::string, extra_routine_t> extra_routines = {};
|
||||
};
|
||||
|
||||
std::vector<const EKF*>& 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); \
|
||||
}
|
||||
|
|
@ -0,0 +1,39 @@
|
|||
#include "ekf_load.h"
|
||||
#include <dlfcn.h>
|
||||
|
||||
std::vector<const EKF*>& ekf_get_all() {
|
||||
static std::vector<const EKF*> 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);
|
||||
}
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
#include "ekf.h"
|
||||
|
||||
std::vector<const EKF*>& 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);
|
||||
|
|
@ -9,7 +9,6 @@ EKFSym::EKFSym(std::string name, Map<MatrixXdr> Q, Map<VectorXd> x_initial, Map<
|
|||
std::vector<int> quaternion_idxs, std::vector<std::string> global_vars, double max_rewind_age)
|
||||
{
|
||||
// TODO: add logger
|
||||
|
||||
this->ekf = ekf_lookup(name);
|
||||
assert(this->ekf);
|
||||
|
||||
|
|
|
|||
|
|
@ -12,7 +12,8 @@
|
|||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
#include "common_ekf.h"
|
||||
#include "ekf.h"
|
||||
#include "ekf_load.h"
|
||||
|
||||
#define REWIND_TO_KEEP 512
|
||||
|
||||
|
|
|
|||
|
|
@ -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 @@ class EKF_sym():
|
|||
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_':
|
||||
|
|
|
|||
|
|
@ -11,12 +11,16 @@ cimport numpy as np
|
|||
|
||||
import numpy as np
|
||||
|
||||
|
||||
cdef extern from "<optional>" 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<Eigen::VectorXd>":
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -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 <math.h>\n"
|
||||
c_code += "#include <string.h>\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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
Loading…
Reference in New Issue