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