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