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(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 correct_pts(pts, rot_speeds, dt): pts = np.hstack((pts, np.ones((pts.shape[0],1)))) cam_rot = dt * view_frame_from_device_frame.dot(rot_speeds) rot = orient.rot_matrix(*cam_rot.T).T pts_corrected = rot.dot(pts.T).T pts_corrected[:,0] /= pts_corrected[:,2] pts_corrected[:,1] /= pts_corrected[:,2] return pts_corrected[:,:2]
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.MAX_DEPTH = MAX_DEPTH
self.MIN_DEPTH = MIN_DEPTH
name = f"{LstSqComputer.name}_{K}"
ffi, lib = load_code(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 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
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
