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
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
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