sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Whitespace

Browse Source 
old-commit-hash: 1716f0b11b
commatwo_master
Willem Melching 5 years ago
parent f97b7b381f
commit 3fed3b9ab4
5 changed files with 184 additions and 179 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 9
      selfdrive/locationd/kalman/helpers/chi2_lookup.py
  2. 111
      selfdrive/locationd/kalman/helpers/ekf_sym.py
  3. 85
      selfdrive/locationd/kalman/helpers/feature_handler.py
  4. 107
      selfdrive/locationd/kalman/helpers/lst_sq_computer.py
  5. 51
      selfdrive/locationd/kalman/helpers/sympy_helpers.py

9
selfdrive/locationd/kalman/helpers/chi2_lookup.py
Unescape View File

@ -1,13 +1,14 @@
import numpy as np
import os import os
import numpy as np
def gen_chi2_ppf_lookup(max_dim=200): def gen_chi2_ppf_lookup(max_dim=200):
from scipy.stats import chi2 from scipy.stats import chi2
table = np.zeros((max_dim, 98)) table = np.zeros((max_dim, 98))
for dim in range(1,max_dim): for dim in range(1, max_dim):
table[dim] = chi2.ppf(np.arange(.01, .99, .01), dim) table[dim] = chi2.ppf(np.arange(.01, .99, .01), dim)
#outfile = open('chi2_lookup_table', 'w')
np.save('chi2_lookup_table', table) np.save('chi2_lookup_table', table)
@ -17,5 +18,5 @@ def chi2_ppf(p, dim):
return result return result
if __name__== "__main__": if __name__ == "__main__":
gen_chi2_ppf_lookup() gen_chi2_ppf_lookup()

111
selfdrive/locationd/kalman/helpers/ekf_sym.py
Unescape View File

@ -14,16 +14,15 @@ from selfdrive.locationd.kalman.helpers.chi2_lookup import chi2_ppf
def solve(a, b): def solve(a, b):
if a.shape[0] == 1 and a.shape[1] == 1: if a.shape[0] == 1 and a.shape[1] == 1:
#assert np.allclose(b/a[0][0], np.linalg.solve(a, b)) return b / a[0][0]
return b/a[0][0]
else: else:
return np.linalg.solve(a, b) return np.linalg.solve(a, b)
def null(H, eps=1e-12): def null(H, eps=1e-12):
u, s, vh = np.linalg.svd(H) u, s, vh = np.linalg.svd(H)
padding = max(0,np.shape(H)[1]-np.shape(s)[0]) padding = max(0, np.shape(H)[1] - np.shape(s)[0])
null_mask = np.concatenate(((s <= eps), np.ones((padding,),dtype=bool)),axis=0) null_mask = np.concatenate(((s <= eps), np.ones((padding,), dtype=bool)), axis=0)
null_space = np.compress(null_mask, vh, axis=0) null_space = np.compress(null_mask, vh, axis=0)
return np.transpose(null_space) return np.transpose(null_space)
@ -41,9 +40,9 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No
f_err_sym = eskf_params[3] f_err_sym = eskf_params[3]
x_err_sym = eskf_params[4] x_err_sym = eskf_params[4]
else: else:
nom_x = sp.MatrixSymbol('nom_x',dim_x,1) nom_x = sp.MatrixSymbol('nom_x', dim_x, 1)
true_x = sp.MatrixSymbol('true_x',dim_x,1) true_x = sp.MatrixSymbol('true_x', dim_x, 1)
delta_x = sp.MatrixSymbol('delta_x',dim_x,1) delta_x = sp.MatrixSymbol('delta_x', dim_x, 1)
err_function_sym = sp.Matrix(nom_x + delta_x) err_function_sym = sp.Matrix(nom_x + delta_x)
inv_err_function_sym = sp.Matrix(true_x - nom_x) inv_err_function_sym = sp.Matrix(true_x - nom_x)
err_eqs = [err_function_sym, nom_x, delta_x] err_eqs = [err_function_sym, nom_x, delta_x]
@ -63,8 +62,8 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No
dim_augment_err = msckf_params[3] dim_augment_err = msckf_params[3]
N = msckf_params[4] N = msckf_params[4]
feature_track_kinds = msckf_params[5] feature_track_kinds = msckf_params[5]
assert dim_main + dim_augment*N == dim_x assert dim_main + dim_augment * N == dim_x
assert dim_main_err + dim_augment_err*N == dim_err assert dim_main_err + dim_augment_err * N == dim_err
else: else:
msckf = False msckf = False
dim_main = dim_x dim_main = dim_x
@ -123,7 +122,7 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No
else: else:
He_str = 'NULL' He_str = 'NULL'
# ea_dim = 1 # not really dim of ea but makes c function work # 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_thresh = chi2_ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection
maha_test = kind in maha_test_kinds maha_test = kind in maha_test_kinds
extra_post += """ extra_post += """
void update_%d(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { void update_%d(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
@ -143,16 +142,9 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No
class EKF_sym(): class EKF_sym():
def __init__(self, name, Q, x_initial, P_initial, dim_main, dim_main_err, def __init__(self, name, Q, x_initial, P_initial, dim_main, dim_main_err,
N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[]): N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[]):
''' """Generates process function and all observation functions for the kalman filter."""
Generates process function and all self.msckf = N > 0
observation functions for the kalman
filter.
'''
if N > 0:
self.msckf = True
else:
self.msckf = False
self.N = N self.N = N
self.dim_augment = dim_augment self.dim_augment = dim_augment
self.dim_augment_err = dim_augment_err self.dim_augment_err = dim_augment_err
@ -163,8 +155,8 @@ class EKF_sym():
x_initial = x_initial.reshape((-1, 1)) x_initial = x_initial.reshape((-1, 1))
self.dim_x = x_initial.shape[0] self.dim_x = x_initial.shape[0]
self.dim_err = P_initial.shape[0] self.dim_err = P_initial.shape[0]
assert dim_main + dim_augment*N == self.dim_x assert dim_main + dim_augment * N == self.dim_x
assert dim_main_err + dim_augment_err*N == self.dim_err assert dim_main_err + dim_augment_err * N == self.dim_err
assert Q.shape == P_initial.shape assert Q.shape == P_initial.shape
# kinds that should get mahalanobis distance # kinds that should get mahalanobis distance
@ -190,24 +182,29 @@ class EKF_sym():
# wrap all the sympy functions # wrap all the sympy functions
def wrap_1lists(name): def wrap_1lists(name):
func = eval("lib.%s" % name, {"lib":lib}) func = eval("lib.%s" % name, {"lib": lib})
def ret(lst1, out): def ret(lst1, out):
func(ffi.cast("double *", lst1.ctypes.data), func(ffi.cast("double *", lst1.ctypes.data),
ffi.cast("double *", out.ctypes.data)) ffi.cast("double *", out.ctypes.data))
return ret return ret
def wrap_2lists(name): def wrap_2lists(name):
func = eval("lib.%s" % name, {"lib":lib}) func = eval("lib.%s" % name, {"lib": lib})
def ret(lst1, lst2, out): def ret(lst1, lst2, out):
func(ffi.cast("double *", lst1.ctypes.data), func(ffi.cast("double *", lst1.ctypes.data),
ffi.cast("double *", lst2.ctypes.data), ffi.cast("double *", lst2.ctypes.data),
ffi.cast("double *", out.ctypes.data)) ffi.cast("double *", out.ctypes.data))
return ret return ret
def wrap_1list_1float(name): def wrap_1list_1float(name):
func = eval("lib.%s" % name, {"lib":lib}) func = eval("lib.%s" % name, {"lib": lib})
def ret(lst1, fl, out): def ret(lst1, fl, out):
func(ffi.cast("double *", lst1.ctypes.data), func(ffi.cast("double *", lst1.ctypes.data),
ffi.cast("double", fl), ffi.cast("double", fl),
ffi.cast("double *", out.ctypes.data)) ffi.cast("double *", out.ctypes.data))
return ret return ret
self.f = wrap_1list_1float("f_fun") self.f = wrap_1list_1float("f_fun")
@ -221,7 +218,7 @@ class EKF_sym():
for kind in kinds: for kind in kinds:
self.hs[kind] = wrap_2lists("h_%d" % kind) self.hs[kind] = wrap_2lists("h_%d" % kind)
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: if self.msckf and kind in self.feature_track_kinds:
self.Hes[kind] = wrap_2lists("He_%d" % kind) self.Hes[kind] = wrap_2lists("He_%d" % kind)
# wrap the C++ predict function # wrap the C++ predict function
@ -235,6 +232,7 @@ class EKF_sym():
# wrap the C++ update function # wrap the C++ update function
def fun_wrapper(f, kind): def fun_wrapper(f, kind):
f = eval("lib.%s" % f, {"lib": lib}) f = eval("lib.%s" % f, {"lib": lib})
def _update_inner_blas(x, P, z, R, extra_args): def _update_inner_blas(x, P, z, R, extra_args):
f(ffi.cast("double *", x.ctypes.data), f(ffi.cast("double *", x.ctypes.data),
ffi.cast("double *", P.ctypes.data), ffi.cast("double *", P.ctypes.data),
@ -257,43 +255,42 @@ class EKF_sym():
# assign the functions # assign the functions
self._predict = _predict_blas self._predict = _predict_blas
#self._predict = self._predict_python # self._predict = self._predict_python
self._update = _update_blas self._update = _update_blas
#self._update = self._update_python # self._update = self._update_python
def init_state(self, state, covs, filter_time): def init_state(self, state, covs, filter_time):
self.x = np.array(state.reshape((-1, 1))).astype(np.float64) self.x = np.array(state.reshape((-1, 1))).astype(np.float64)
self.P = np.array(covs).astype(np.float64) self.P = np.array(covs).astype(np.float64)
self.filter_time = filter_time self.filter_time = filter_time
self.augment_times = [0]*self.N self.augment_times = [0] * self.N
self.rewind_obscache = [] self.rewind_obscache = []
self.rewind_t = [] self.rewind_t = []
self.rewind_states = [] self.rewind_states = []
def augment(self): def augment(self):
# TODO this is not a generalized way of doing # TODO this is not a generalized way of doing this and implies that the augmented states
# this and implies that the augmented states # are simply the first (dim_augment_state) elements of the main state.
# are simply the first (dim_augment_state)
# elements of the main state.
assert self.msckf assert self.msckf
d1 = self.dim_main d1 = self.dim_main
d2 = self.dim_main_err d2 = self.dim_main_err
d3 = self.dim_augment d3 = self.dim_augment
d4 = self.dim_augment_err d4 = self.dim_augment_err
# push through augmented states # push through augmented states
self.x[d1:-d3] = self.x[d1+d3:] self.x[d1:-d3] = self.x[d1 + d3:]
self.x[-d3:] = self.x[:d3] self.x[-d3:] = self.x[:d3]
assert self.x.shape == (self.dim_x, 1) assert self.x.shape == (self.dim_x, 1)
# push through augmented covs # push through augmented covs
assert self.P.shape == (self.dim_err, self.dim_err) assert self.P.shape == (self.dim_err, self.dim_err)
P_reduced = self.P P_reduced = self.P
P_reduced = np.delete(P_reduced, np.s_[d2:d2+d4], axis=1) P_reduced = np.delete(P_reduced, np.s_[d2:d2 + d4], axis=1)
P_reduced = np.delete(P_reduced, np.s_[d2:d2+d4], axis=0) P_reduced = np.delete(P_reduced, np.s_[d2:d2 + d4], axis=0)
assert P_reduced.shape == (self.dim_err -d4, self.dim_err -d4) assert P_reduced.shape == (self.dim_err - d4, self.dim_err - d4)
to_mult = np.zeros((self.dim_err, self.dim_err - d4)) to_mult = np.zeros((self.dim_err, self.dim_err - d4))
to_mult[:-d4,:] = np.eye(self.dim_err - d4) to_mult[:-d4, :] = np.eye(self.dim_err - d4)
to_mult[-d4:,:d4] = np.eye(d4) to_mult[-d4:, :d4] = np.eye(d4)
self.P = to_mult.dot(P_reduced.dot(to_mult.T)) self.P = to_mult.dot(P_reduced.dot(to_mult.T))
self.augment_times = self.augment_times[1:] self.augment_times = self.augment_times[1:]
self.augment_times.append(self.filter_time) self.augment_times.append(self.filter_time)
@ -308,13 +305,13 @@ class EKF_sym():
def rewind(self, t): def rewind(self, t):
# find where we are rewinding to # find where we are rewinding to
idx = bisect_right(self.rewind_t, t) idx = bisect_right(self.rewind_t, t)
assert self.rewind_t[idx-1] <= t assert self.rewind_t[idx - 1] <= t
assert self.rewind_t[idx] > t # must be true, or rewind wouldn't be called assert self.rewind_t[idx] > t # must be true, or rewind wouldn't be called
# set the state to the time right before that # set the state to the time right before that
self.filter_time = self.rewind_t[idx-1] self.filter_time = self.rewind_t[idx - 1]
self.x[:] = self.rewind_states[idx-1][0] self.x[:] = self.rewind_states[idx - 1][0]
self.P[:] = self.rewind_states[idx-1][1] self.P[:] = self.rewind_states[idx - 1][1]
# return the observations we rewound over for fast forwarding # return the observations we rewound over for fast forwarding
ret = self.rewind_obscache[idx:] ret = self.rewind_obscache[idx:]
@ -344,7 +341,7 @@ class EKF_sym():
# rewind # rewind
if self.filter_time is not None and t < self.filter_time: if self.filter_time is not None and t < self.filter_time:
if len(self.rewind_t) == 0 or t < self.rewind_t[0] or t < self.rewind_t[-1] -1.0: if len(self.rewind_t) == 0 or t < self.rewind_t[0] or t < self.rewind_t[-1] - 1.0:
print("observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time)) print("observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time))
return None return None
rewound = self.rewind(t) rewound = self.rewind(t)
@ -430,7 +427,7 @@ class EKF_sym():
P[:d2, d2:] = F_curr.dot(P[:d2, d2:]) P[:d2, d2:] = F_curr.dot(P[:d2, d2:])
P[d2:, :d2] = P[d2:, :d2].dot(F_curr.T) P[d2:, :d2] = P[d2:, :d2].dot(F_curr.T)
P += dt*self.Q P += dt * self.Q
return x_new, P return x_new, P
def _update_python(self, x, P, kind, z, R, extra_args=[]): def _update_python(self, x, P, kind, z, R, extra_args=[]):
@ -477,15 +474,15 @@ class EKF_sym():
a = np.linalg.inv(H.dot(P).dot(H.T) + R) a = np.linalg.inv(H.dot(P).dot(H.T) + R)
maha_dist = y.T.dot(a.dot(y)) maha_dist = y.T.dot(a.dot(y))
if maha_dist > chi2_ppf(0.95, y.shape[0]): if maha_dist > chi2_ppf(0.95, y.shape[0]):
R = 10e16*R R = 10e16 * R
# *** same below this line *** # *** same below this line ***
# Outlier resilient weighting as described in: # Outlier resilient weighting as described in:
# "A Kalman Filter for Robust Outlier Detection - Jo-Anne Ting, ..." # "A Kalman Filter for Robust Outlier Detection - Jo-Anne Ting, ..."
weight = 1 #(1.5)/(1 + np.sum(y**2)/np.sum(R)) weight = 1 # (1.5)/(1 + np.sum(y**2)/np.sum(R))
S = dot(dot(H, P), H.T) + R/weight S = dot(dot(H, P), H.T) + R / weight
K = solve(S, dot(H, P.T)).T K = solve(S, dot(H, P.T)).T
I_KH = np.eye(P.shape[0]) - dot(K, H) I_KH = np.eye(P.shape[0]) - dot(K, H)
@ -550,16 +547,16 @@ class EKF_sym():
d1 = self.dim_main d1 = self.dim_main
d2 = self.dim_main_err d2 = self.dim_main_err
Ck = np.linalg.solve(Pk1_k[:d2,:d2], Fk_1[:d2,:d2].dot(Pk_k[:d2,:d2].T)).T Ck = np.linalg.solve(Pk1_k[:d2, :d2], Fk_1[:d2, :d2].dot(Pk_k[:d2, :d2].T)).T
xk_n = xk_k xk_n = xk_k
delta_x = np.zeros((Pk_n.shape[0], 1), dtype=np.float64) delta_x = np.zeros((Pk_n.shape[0], 1), dtype=np.float64)
self.inv_err_function(xk1_k, xk1_n, delta_x) self.inv_err_function(xk1_k, xk1_n, delta_x)
delta_x[:d2] = Ck.dot(delta_x[:d2]) delta_x[:d2] = Ck.dot(delta_x[:d2])
x_new = np.zeros((xk_n.shape[0], 1), dtype=np.float64) x_new = np.zeros((xk_n.shape[0], 1), dtype=np.float64)
self.err_function(xk_k, delta_x, x_new) self.err_function(xk_k, delta_x, x_new)
xk_n[:d1] = x_new[:d1,0] xk_n[:d1] = x_new[:d1, 0]
Pk_n = Pk_k Pk_n = Pk_k
Pk_n[:d2,:d2] = Pk_k[:d2,:d2] + Ck.dot(Pk1_n[:d2,:d2] - Pk1_k[:d2,:d2]).dot(Ck.T) Pk_n[:d2, :d2] = Pk_k[:d2, :d2] + Ck.dot(Pk1_n[:d2, :d2] - Pk1_k[:d2, :d2]).dot(Ck.T)
states_smoothed.append(xk_n) states_smoothed.append(xk_n)
covs_smoothed.append(Pk_n) covs_smoothed.append(Pk_n)

85
selfdrive/locationd/kalman/helpers/feature_handler.py
Unescape View File

@ -1,26 +1,26 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
import os import os
import time
import numpy as np import numpy as np
import common.transformations.orientation as orient import common.transformations.orientation as orient
from selfdrive.locationd.kalman.helpers import (TEMPLATE_DIR, load_code,
write_code)
from selfdrive.locationd.kalman.helpers.sympy_helpers import quat_matrix_l from selfdrive.locationd.kalman.helpers.sympy_helpers import quat_matrix_l
from selfdrive.locationd.kalman.helpers import TEMPLATE_DIR, write_code, load_code
def sane(track): def sane(track):
img_pos = track[1:,2:4] img_pos = track[1:, 2:4]
diffs_x = abs(img_pos[1:,0] - img_pos[:-1,0]) diffs_x = abs(img_pos[1:, 0] - img_pos[:-1, 0])
diffs_y = abs(img_pos[1:,1] - img_pos[:-1,1]) diffs_y = abs(img_pos[1:, 1] - img_pos[:-1, 1])
for i in range(1, len(diffs_x)): for i in range(1, len(diffs_x)):
if ((diffs_x[i] > 0.05 or diffs_x[i-1] > 0.05) and \ 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] > 2 * diffs_x[i - 1] or
diffs_x[i] < .5*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] > 0.05 or diffs_y[i - 1] > 0.05) and
(diffs_y[i] > 2*diffs_y[i-1] or \ (diffs_y[i] > 2 * diffs_y[i - 1] or
diffs_y[i] < .5*diffs_y[i-1])): diffs_y[i] < .5 * diffs_y[i - 1])):
return False return False
return True return True
@ -45,16 +45,14 @@ class FeatureHandler():
self.MAX_TRACKS = 6000 self.MAX_TRACKS = 6000
self.K = K self.K = K
#Array of tracks, each track # Array of tracks, each track has K 5D features preceded
#has K 5D features preceded # by 5 params that inidicate [f_idx, last_idx, updated, complete, valid]
#by 5 params that inidicate
#[f_idx, last_idx, updated, complete, valid]
# f_idx: idx of current last feature in track # f_idx: idx of current last feature in track
# idx of of last feature in frame # idx of of last feature in frame
# bool for whether this track has been update # bool for whether this track has been update
# bool for whether this track is complete # bool for whether this track is complete
# bool for whether this track is valid # bool for whether this track is valid
self.tracks = np.zeros((self.MAX_TRACKS, K+1, 5)) self.tracks = np.zeros((self.MAX_TRACKS, K + 1, 5))
self.tracks[:] = np.nan self.tracks[:] = np.nan
name = f"{FeatureHandler.name}_{K}" name = f"{FeatureHandler.name}_{K}"
@ -65,7 +63,7 @@ class FeatureHandler():
ffi.cast("double *", features.ctypes.data), ffi.cast("double *", features.ctypes.data),
ffi.cast("long long *", empty_idxs.ctypes.data)) ffi.cast("long long *", empty_idxs.ctypes.data))
#self.merge_features = self.merge_features_python # self.merge_features = self.merge_features_python
self.merge_features = merge_features_c self.merge_features = merge_features_c
def reset(self): def reset(self):
@ -75,7 +73,7 @@ class FeatureHandler():
empty_idx = 0 empty_idx = 0
for f in features: for f in features:
match_idx = int(f[4]) match_idx = int(f[4])
if tracks[match_idx, 0, 1] == match_idx and tracks[match_idx, 0 ,2] == 0: 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, 0] += 1
tracks[match_idx, 0, 1] = f[1] tracks[match_idx, 0, 1] = f[1]
tracks[match_idx, 0, 2] = 1 tracks[match_idx, 0, 2] = 1
@ -95,56 +93,57 @@ class FeatureHandler():
empty_idx += 1 empty_idx += 1
def update_tracks(self, features): def update_tracks(self, features):
t0 = time.time() last_idxs = np.copy(self.tracks[:, 0, 1])
last_idxs = np.copy(self.tracks[:,0,1])
real = np.isfinite(last_idxs) real = np.isfinite(last_idxs)
self.tracks[last_idxs[real].astype(int)] = self.tracks[real] self.tracks[last_idxs[real].astype(int)] = self.tracks[real]
mask = np.ones(self.MAX_TRACKS, np.bool) mask = np.ones(self.MAX_TRACKS, np.bool)
mask[last_idxs[real].astype(int)] = 0 mask[last_idxs[real].astype(int)] = 0
empty_idxs = np.arange(self.MAX_TRACKS)[mask] empty_idxs = np.arange(self.MAX_TRACKS)[mask]
self.tracks[empty_idxs] = np.nan self.tracks[empty_idxs] = np.nan
self.tracks[:,0,2] = 0 self.tracks[:, 0, 2] = 0
self.merge_features(self.tracks, features, empty_idxs) self.merge_features(self.tracks, features, empty_idxs)
def handle_features(self, features): def handle_features(self, features):
self.update_tracks(features) self.update_tracks(features)
valid_idxs = self.tracks[:,0,4] == 1 valid_idxs = self.tracks[:, 0, 4] == 1
complete_idxs = self.tracks[:,0,3] == 1 complete_idxs = self.tracks[:, 0, 3] == 1
stale_idxs = self.tracks[:,0,2] == 0 stale_idxs = self.tracks[:, 0, 2] == 0
valid_tracks = self.tracks[valid_idxs] valid_tracks = self.tracks[valid_idxs]
self.tracks[complete_idxs] = np.nan self.tracks[complete_idxs] = np.nan
self.tracks[stale_idxs] = np.nan self.tracks[stale_idxs] = np.nan
return valid_tracks[:,1:,:4].reshape((len(valid_tracks), self.K*4)) return valid_tracks[:, 1:, :4].reshape((len(valid_tracks), self.K * 4))
def generate_orient_error_jac(K): def generate_orient_error_jac(K):
import sympy as sp import sympy as sp
from common.sympy_helpers import quat_rotate from common.sympy_helpers import quat_rotate
x_sym = sp.MatrixSymbol('abr', 3,1)
dtheta = sp.MatrixSymbol('dtheta', 3,1) x_sym = sp.MatrixSymbol('abr', 3, 1)
dtheta = sp.MatrixSymbol('dtheta', 3, 1)
delta_quat = sp.Matrix(np.ones(4)) delta_quat = sp.Matrix(np.ones(4))
delta_quat[1:,:] = sp.Matrix(0.5*dtheta[0:3,:]) delta_quat[1:, :] = sp.Matrix(0.5 * dtheta[0:3, :])
poses_sym = sp.MatrixSymbol('poses', 7*K,1) poses_sym = sp.MatrixSymbol('poses', 7 * K, 1)
img_pos_sym = sp.MatrixSymbol('img_positions', 2*K,1) img_pos_sym = sp.MatrixSymbol('img_positions', 2 * K, 1)
alpha, beta, rho = x_sym alpha, beta, rho = x_sym
to_c = sp.Matrix(orient.rot_matrix(-np.pi/2, -np.pi/2, 0)) to_c = sp.Matrix(orient.rot_matrix(-np.pi / 2, -np.pi / 2, 0))
pos_0 = sp.Matrix(np.array(poses_sym[K*7-7:K*7-4])[:,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 q = quat_matrix_l(poses_sym[K * 7 - 4:K * 7]) * delta_quat
quat_rot = quat_rotate(*q) quat_rot = quat_rotate(*q)
rot_g_to_0 = to_c*quat_rot.T rot_g_to_0 = to_c * quat_rot.T
rows = [] rows = []
for i in range(K): for i in range(K):
pos_i = sp.Matrix(np.array(poses_sym[i*7:i*7+3])[:,0]) 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 q = quat_matrix_l(poses_sym[7 * i + 3:7 * i + 7]) * delta_quat
quat_rot = quat_rotate(*q) quat_rot = quat_rotate(*q)
rot_g_to_i = to_c*quat_rot.T rot_g_to_i = to_c * quat_rot.T
rot_0_to_i = rot_g_to_i*(rot_g_to_0.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) 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 funct_vec = rot_0_to_i * sp.Matrix([alpha, beta, 1]) + rho * trans_0_to_i
h1, h2, h3 = funct_vec h1, h2, h3 = funct_vec
rows.append(h1/h3 - img_pos_sym[i*2 +0]) rows.append(h1 / h3 - img_pos_sym[i * 2 + 0])
rows.append(h2/h3 - img_pos_sym[i*2 + 1]) rows.append(h2 / h3 - img_pos_sym[i * 2 + 1])
img_pos_residual_sym = sp.Matrix(rows) img_pos_residual_sym = sp.Matrix(rows)
# sympy into c # sympy into c

107
selfdrive/locationd/kalman/helpers/lst_sq_computer.py
Unescape View File

@ -4,7 +4,6 @@ import os
import numpy as np import numpy as np
import sympy as sp import sympy as sp
#import scipy.optimize as opt
import common.transformations.orientation as orient import common.transformations.orientation as orient
from selfdrive.locationd.kalman.helpers import (TEMPLATE_DIR, load_code, from selfdrive.locationd.kalman.helpers import (TEMPLATE_DIR, load_code,
@ -14,27 +13,28 @@ from selfdrive.locationd.kalman.helpers.sympy_helpers import (quat_rotate,
def generate_residual(K): def generate_residual(K):
x_sym = sp.MatrixSymbol('abr', 3,1) x_sym = sp.MatrixSymbol('abr', 3, 1)
poses_sym = sp.MatrixSymbol('poses', 7*K,1) poses_sym = sp.MatrixSymbol('poses', 7 * K, 1)
img_pos_sym = sp.MatrixSymbol('img_positions', 2*K,1) img_pos_sym = sp.MatrixSymbol('img_positions', 2 * K, 1)
alpha, beta, rho = x_sym alpha, beta, rho = x_sym
to_c = sp.Matrix(orient.rot_matrix(-np.pi/2, -np.pi/2, 0)) to_c = sp.Matrix(orient.rot_matrix(-np.pi / 2, -np.pi / 2, 0))
pos_0 = sp.Matrix(np.array(poses_sym[K*7-7:K*7-4])[:,0]) pos_0 = sp.Matrix(np.array(poses_sym[K * 7 - 7:K * 7 - 4])[:, 0])
q = poses_sym[K*7-4:K*7] q = poses_sym[K * 7 - 4:K * 7]
quat_rot = quat_rotate(*q) quat_rot = quat_rotate(*q)
rot_g_to_0 = to_c*quat_rot.T rot_g_to_0 = to_c * quat_rot.T
rows = [] rows = []
for i in range(K): for i in range(K):
pos_i = sp.Matrix(np.array(poses_sym[i*7:i*7+3])[:,0]) pos_i = sp.Matrix(np.array(poses_sym[i * 7:i * 7 + 3])[:, 0])
q = poses_sym[7*i+3:7*i+7] q = poses_sym[7 * i + 3:7 * i + 7]
quat_rot = quat_rotate(*q) quat_rot = quat_rotate(*q)
rot_g_to_i = to_c*quat_rot.T rot_g_to_i = to_c * quat_rot.T
rot_0_to_i = rot_g_to_i*(rot_g_to_0.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) 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 funct_vec = rot_0_to_i * sp.Matrix([alpha, beta, 1]) + rho * trans_0_to_i
h1, h2, h3 = funct_vec h1, h2, h3 = funct_vec
rows.append(h1/h3 - img_pos_sym[i*2 +0]) rows.append(h1 / h3 - img_pos_sym[i * 2 + 0])
rows.append(h2/h3 - img_pos_sym[i*2 + 1]) rows.append(h2 / h3 - img_pos_sym[i * 2 + 1])
img_pos_residual_sym = sp.Matrix(rows) img_pos_residual_sym = sp.Matrix(rows)
# sympy into c # sympy into c
@ -64,7 +64,7 @@ class LstSqComputer():
write_code(filename, code, header) write_code(filename, code, header)
def __init__(self, K=4, MIN_DEPTH=2, MAX_DEPTH=500): def __init__(self, K=4, MIN_DEPTH=2, MAX_DEPTH=500):
self.to_c = orient.rot_matrix(-np.pi/2, -np.pi/2, 0) self.to_c = orient.rot_matrix(-np.pi / 2, -np.pi / 2, 0)
self.MAX_DEPTH = MAX_DEPTH self.MAX_DEPTH = MAX_DEPTH
self.MIN_DEPTH = MIN_DEPTH self.MIN_DEPTH = MIN_DEPTH
@ -73,20 +73,20 @@ class LstSqComputer():
# wrap c functions # wrap c functions
def residual_jac(x, poses, img_positions): def residual_jac(x, poses, img_positions):
out = np.zeros(((K*2, 3)), dtype=np.float64) out = np.zeros(((K * 2, 3)), dtype=np.float64)
lib.jac_fun(ffi.cast("double *", x.ctypes.data), lib.jac_fun(ffi.cast("double *", x.ctypes.data),
ffi.cast("double *", poses.ctypes.data), ffi.cast("double *", poses.ctypes.data),
ffi.cast("double *", img_positions.ctypes.data), ffi.cast("double *", img_positions.ctypes.data),
ffi.cast("double *", out.ctypes.data)) ffi.cast("double *", out.ctypes.data))
return out return out
self.residual_jac = residual_jac self.residual_jac = residual_jac
def residual(x, poses, img_positions): def residual(x, poses, img_positions):
out = np.zeros((K*2), dtype=np.float64) out = np.zeros((K * 2), dtype=np.float64)
lib.res_fun(ffi.cast("double *", x.ctypes.data), lib.res_fun(ffi.cast("double *", x.ctypes.data),
ffi.cast("double *", poses.ctypes.data), ffi.cast("double *", poses.ctypes.data),
ffi.cast("double *", img_positions.ctypes.data), ffi.cast("double *", img_positions.ctypes.data),
ffi.cast("double *", out.ctypes.data)) ffi.cast("double *", out.ctypes.data))
return out return out
self.residual = residual self.residual = residual
@ -96,24 +96,26 @@ class LstSqComputer():
# Can't be a view for the ctype # Can't be a view for the ctype
img_positions = np.copy(img_positions) img_positions = np.copy(img_positions)
lib.compute_pos(ffi.cast("double *", self.to_c.ctypes.data), lib.compute_pos(ffi.cast("double *", self.to_c.ctypes.data),
ffi.cast("double *", poses.ctypes.data), ffi.cast("double *", poses.ctypes.data),
ffi.cast("double *", img_positions.ctypes.data), ffi.cast("double *", img_positions.ctypes.data),
ffi.cast("double *", param.ctypes.data), ffi.cast("double *", param.ctypes.data),
ffi.cast("double *", pos.ctypes.data)) ffi.cast("double *", pos.ctypes.data))
return pos, param return pos, param
self.compute_pos_c = compute_pos_c self.compute_pos_c = compute_pos_c
def compute_pos(self, poses, img_positions, debug=False): def compute_pos(self, poses, img_positions, debug=False):
pos, param = self.compute_pos_c(poses, img_positions) pos, param = self.compute_pos_c(poses, img_positions)
#pos, param = self.compute_pos_python(poses, img_positions) # pos, param = self.compute_pos_python(poses, img_positions)
depth = 1/param[2]
depth = 1 / param[2]
if debug: if debug:
if not self.debug: if not self.debug:
raise NotImplementedError("This is not a debug computer") raise NotImplementedError("This is not a debug computer")
#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)) # orient_err_jac = self.orient_error_jac(param, poses, img_positions, np.zeros(3)).reshape((-1,2,3))
res = self.residual(param, poses, img_positions).reshape((-1,2)) jac = self.residual_jac(param, poses, img_positions).reshape((-1, 2, 3))
return pos, param, res, jac #, orient_err_jac 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): elif (self.MIN_DEPTH < depth < self.MAX_DEPTH):
return pos return pos
else: else:
@ -130,45 +132,44 @@ class LstSqComputer():
return [x] return [x]
def compute_pos_python(self, poses, img_positions, check_quality=False): def compute_pos_python(self, poses, img_positions, check_quality=False):
import scipy.optimize as opt
# This procedure is also described # This procedure is also described
# in the MSCKF paper (Mourikis et al. 2007) # in the MSCKF paper (Mourikis et al. 2007)
x = np.array([img_positions[-1][0], x = np.array([img_positions[-1][0],
img_positions[-1][1], 0.1]) img_positions[-1][1], 0.1])
res = opt.leastsq(self.residual, x, Dfun=self.residual_jac, args=(poses, img_positions)) # scipy opt 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 # res = self.gauss_newton(self.residual, self.residual_jac, x, (poses, img_positions)) # diy gauss_newton
alpha, beta, rho = res[0] alpha, beta, rho = res[0]
rot_0_to_g = (orient.rotations_from_quats(poses[-1,3:])).dot(self.to_c.T) rot_0_to_g = (orient.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] return (rot_0_to_g.dot(np.array([alpha, beta, 1]))) / rho + poses[-1, :3]
# EXPERIMENTAL CODE
'''
EXPERIMENTAL CODE
'''
def unroll_shutter(img_positions, poses, v, rot_rates, ecef_pos): def unroll_shutter(img_positions, poses, v, rot_rates, ecef_pos):
# only speed correction for now # only speed correction for now
t_roll = 0.016 # 16ms rolling shutter? t_roll = 0.016 # 16ms rolling shutter?
vroll, vpitch, vyaw = rot_rates vroll, vpitch, vyaw = rot_rates
A = 0.5*np.array([[-1, -vroll, -vpitch, -vyaw], A = 0.5 * np.array([[-1, -vroll, -vpitch, -vyaw],
[vroll, 0, vyaw, -vpitch], [vroll, 0, vyaw, -vpitch],
[vpitch, -vyaw, 0, vroll], [vpitch, -vyaw, 0, vroll],
[vyaw, vpitch, -vroll, 0]]) [vyaw, vpitch, -vroll, 0]])
q_dot = A.dot(poses[-1][3:7]) q_dot = A.dot(poses[-1][3:7])
v = np.append(v, q_dot) v = np.append(v, q_dot)
v = np.array([v[0], v[1], v[2],0,0,0,0]) v = np.array([v[0], v[1], v[2], 0, 0, 0, 0])
current_pose = poses[-1] + v*0.05 current_pose = poses[-1] + v * 0.05
poses = np.vstack((current_pose, poses)) poses = np.vstack((current_pose, poses))
dt = -img_positions[:,1]*t_roll/0.48 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) errs = project(poses, ecef_pos) - project(poses + np.atleast_2d(dt).T.dot(np.atleast_2d(v)), ecef_pos)
return img_positions - errs return img_positions - errs
def project(poses, ecef_pos): def project(poses, ecef_pos):
img_positions = np.zeros((len(poses), 2)) img_positions = np.zeros((len(poses), 2))
for i, p in enumerate(poses): for i, p in enumerate(poses):
cam_frame = orient.rotations_from_quats(p[3:]).T.dot(ecef_pos - p[:3]) cam_frame = orient.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]]) img_positions[i] = np.array([cam_frame[1] / cam_frame[0], cam_frame[2] / cam_frame[0]])
return img_positions return img_positions

51
selfdrive/locationd/kalman/helpers/sympy_helpers.py
Unescape View File

@ -2,31 +2,35 @@
import sympy as sp import sympy as sp
import numpy as np import numpy as np
def cross(x): def cross(x):
ret = sp.Matrix(np.zeros((3,3))) ret = sp.Matrix(np.zeros((3, 3)))
ret[0,1], ret[0,2] = -x[2], x[1] ret[0, 1], ret[0, 2] = -x[2], x[1]
ret[1,0], ret[1,2] = x[2], -x[0] ret[1, 0], ret[1, 2] = x[2], -x[0]
ret[2,0], ret[2,1] = -x[1], x[0] ret[2, 0], ret[2, 1] = -x[1], x[0]
return ret return ret
def euler_rotate(roll, pitch, yaw): def euler_rotate(roll, pitch, yaw):
# make symbolic rotation matrix from eulers # make symbolic rotation matrix from eulers
matrix_roll = sp.Matrix([[1, 0, 0], matrix_roll = sp.Matrix([[1, 0, 0],
[0, sp.cos(roll), -sp.sin(roll)], [0, sp.cos(roll), -sp.sin(roll)],
[0, sp.sin(roll), sp.cos(roll)]]) [0, sp.sin(roll), sp.cos(roll)]])
matrix_pitch = sp.Matrix([[sp.cos(pitch), 0, sp.sin(pitch)], matrix_pitch = sp.Matrix([[sp.cos(pitch), 0, sp.sin(pitch)],
[0, 1, 0], [0, 1, 0],
[-sp.sin(pitch), 0, sp.cos(pitch)]]) [-sp.sin(pitch), 0, sp.cos(pitch)]])
matrix_yaw = sp.Matrix([[sp.cos(yaw), -sp.sin(yaw), 0], matrix_yaw = sp.Matrix([[sp.cos(yaw), -sp.sin(yaw), 0],
[sp.sin(yaw), sp.cos(yaw), 0], [sp.sin(yaw), sp.cos(yaw), 0],
[0, 0, 1]]) [0, 0, 1]])
return matrix_yaw*matrix_pitch*matrix_roll return matrix_yaw * matrix_pitch * matrix_roll
def quat_rotate(q0, q1, q2, q3): def quat_rotate(q0, q1, q2, q3):
# make symbolic rotation matrix from quat # make symbolic rotation matrix from quat
return sp.Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 + q0*q3), 2*(q1*q3 - q0*q2)], return sp.Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2 * (q1 * q2 + q0 * q3), 2 * (q1 * q3 - q0 * q2)],
[2*(q1*q2 - q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 + q0*q1)], [2 * (q1 * q2 - q0 * q3), q0**2 - q1**2 + q2**2 - q3**2, 2 * (q2 * q3 + q0 * q1)],
[2*(q1*q3 + q0*q2), 2*(q2*q3 - q0*q1), q0**2 - q1**2 - q2**2 + q3**2]]).T [2 * (q1 * q3 + q0 * q2), 2 * (q2 * q3 - q0 * q1), q0**2 - q1**2 - q2**2 + q3**2]]).T
def quat_matrix_l(p): def quat_matrix_l(p):
return sp.Matrix([[p[0], -p[1], -p[2], -p[3]], return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
@ -34,6 +38,7 @@ def quat_matrix_l(p):
[p[2], p[3], p[0], -p[1]], [p[2], p[3], p[0], -p[1]],
[p[3], -p[2], p[1], p[0]]]) [p[3], -p[2], p[1], p[0]]])
def quat_matrix_r(p): def quat_matrix_r(p):
return sp.Matrix([[p[0], -p[1], -p[2], -p[3]], return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
[p[1], p[0], p[3], -p[2]], [p[1], p[0], p[3], -p[2]],
@ -49,10 +54,11 @@ def sympy_into_c(sympy_functions):
# argument ordering input to sympy is broken with function with output arguments # argument ordering input to sympy is broken with function with output arguments
nargs = [] nargs = []
# reorder the input arguments # reorder the input arguments
for aa in args: for aa in args:
if aa is None: if aa is None:
nargs.append(codegen.InputArgument(sp.Symbol('unused'), dimensions=[1,1])) nargs.append(codegen.InputArgument(sp.Symbol('unused'), dimensions=[1, 1]))
continue continue
found = False found = False
for a in r.arguments: for a in r.arguments:
@ -62,22 +68,23 @@ def sympy_into_c(sympy_functions):
break break
if not found: if not found:
# [1,1] is a hack for Matrices # [1,1] is a hack for Matrices
nargs.append(codegen.InputArgument(aa, dimensions=[1,1])) nargs.append(codegen.InputArgument(aa, dimensions=[1, 1]))
# add the output arguments # add the output arguments
for a in r.arguments: for a in r.arguments:
if type(a) == codegen.OutputArgument: if type(a) == codegen.OutputArgument:
nargs.append(a) nargs.append(a)
#assert len(r.arguments) == len(args)+1 # assert len(r.arguments) == len(args)+1
r.arguments = nargs r.arguments = nargs
# add routine to list # add routine to list
routines.append(r) routines.append(r)
[(c_name, c_code), (h_name, c_header)] = codegen.get_code_generator('C', 'ekf', 'C99').write(routines, "ekf") [(c_name, c_code), (h_name, c_header)] = codegen.get_code_generator('C', 'ekf', 'C99').write(routines, "ekf")
c_code = '\n'.join(x for x in c_code.split("\n") if len(x) > 0 and x[0] != '#') c_header = '\n'.join(x for x in c_header.split("\n") if len(x) > 0 and x[0] != '#')
c_header = '\n'.join(x for x in c_header.split("\n") if len(x) > 0 and x[0] != '#')
c_code = '\n'.join(x for x in c_code.split("\n") if len(x) > 0 and x[0] != '#')
c_code = 'extern "C" {\n#include <math.h>\n' + c_code + "\n}\n" c_code = 'extern "C" {\n#include <math.h>\n' + c_code + "\n}\n"
return c_header, c_code return c_header, c_code

Write Preview
Loading…
Cancel
Save