mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-03-01 15:33:52 +08:00
324 lines
12 KiB
Python
324 lines
12 KiB
Python
import common.transformations.orientation as orient
|
|
import numpy as np
|
|
import scipy.optimize as opt
|
|
import time
|
|
import os
|
|
from bisect import bisect_left
|
|
from common.sympy_helpers import sympy_into_c, quat_matrix_l
|
|
from common.ffi_wrapper import ffi_wrap, wrap_compiled, compile_code
|
|
|
|
EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__))
|
|
|
|
|
|
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():
|
|
def __init__(self, K):
|
|
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
|
|
|
|
# Wrap c code for slow matching
|
|
c_header = "\nvoid merge_features(double *tracks, double *features, long long *empty_idxs);"
|
|
c_code = "#define K %d\n" % K
|
|
c_code += "\n" + open(os.path.join(EXTERNAL_PATH, "feature_handler.c")).read()
|
|
ffi, lib = ffi_wrap('feature_handler', c_code, c_header)
|
|
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):
|
|
t0 = time.time()
|
|
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, np.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_residual(K):
|
|
import sympy as sp
|
|
from common.sympy_helpers import quat_rotate
|
|
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(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])
|
|
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
|
|
|
|
|
|
def generate_orient_error_jac(K):
|
|
import sympy as sp
|
|
from common.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(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])
|
|
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
|
|
|
|
|
|
class LstSqComputer():
|
|
def __init__(self, K, MIN_DEPTH=2, MAX_DEPTH=500, debug=False):
|
|
self.to_c = orient.rot_matrix(-np.pi/2, -np.pi/2, 0)
|
|
self.MAX_DEPTH = MAX_DEPTH
|
|
self.MIN_DEPTH = MIN_DEPTH
|
|
self.debug = debug
|
|
self.name = 'pos_computer_' + str(K)
|
|
if debug:
|
|
self.name += '_debug'
|
|
|
|
try:
|
|
dir_path = os.path.dirname(__file__)
|
|
deps = [dir_path + '/' + 'feature_handler.py',
|
|
dir_path + '/' + 'compute_pos.c']
|
|
|
|
outs = [dir_path + '/' + self.name + '.o',
|
|
dir_path + '/' + self.name + '.so',
|
|
dir_path + '/' + self.name + '.cpp']
|
|
out_times = list(map(os.path.getmtime, outs))
|
|
dep_times = list(map(os.path.getmtime, deps))
|
|
rebuild = os.getenv("REBUILD", False)
|
|
if min(out_times) < max(dep_times) or rebuild:
|
|
list(map(os.remove, outs))
|
|
# raise the OSError if removing didnt
|
|
# raise one to start the compilation
|
|
raise OSError()
|
|
except OSError as e:
|
|
# gen c code for sympy functions
|
|
sympy_functions = generate_residual(K)
|
|
#if debug:
|
|
# sympy_functions.extend(generate_orient_error_jac(K))
|
|
header, code = sympy_into_c(sympy_functions)
|
|
|
|
# ffi wrap c code
|
|
extra_header = "\nvoid compute_pos(double *to_c, double *in_poses, double *in_img_positions, double *param, double *pos);"
|
|
code += "\n#define KDIM %d\n" % K
|
|
header += "\n" + extra_header
|
|
code += "\n" + open(os.path.join(EXTERNAL_PATH, "compute_pos.c")).read()
|
|
compile_code(self.name, code, header, EXTERNAL_PATH)
|
|
ffi, lib = wrap_compiled(self.name, EXTERNAL_PATH)
|
|
|
|
# wrap c functions
|
|
#if debug:
|
|
#def orient_error_jac(x, poses, img_positions, dtheta):
|
|
# out = np.zeros(((K*2, 3)), dtype=np.float64)
|
|
# lib.orient_error_jac(ffi.cast("double *", x.ctypes.data),
|
|
# ffi.cast("double *", poses.ctypes.data),
|
|
# ffi.cast("double *", img_positions.ctypes.data),
|
|
# ffi.cast("double *", dtheta.ctypes.data),
|
|
# ffi.cast("double *", out.ctypes.data))
|
|
# return out
|
|
#self.orient_error_jac = orient_error_jac
|
|
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
|
|
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
|
|
self.residual_jac = residual_jac
|
|
|
|
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:
|
|
if not self.debug:
|
|
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))
|
|
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):
|
|
# 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 = (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]
|
|
|
|
|
|
|
|
|
|
'''
|
|
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 = 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]])
|
|
return img_positions
|
|
|