def get_pt_from_img(img, depth, K, Twc): H, W = depth.shape x_2d = x_2d_coords(H, W).reshape((H * W, 2)) X_3d = pi_inv(K, x_2d, depth.reshape((H * W, 1))) X_3d = transpose(Twc[:3, :3], Twc[:3, 3], X_3d) colors = img.reshape((H * W, 3)) / 255.0 return X_3d, colors
def keyPressEvent(obj, event): global frame_idx global refer_T key = obj.GetKeySym() if key == 'Right': # vis.clear_frame_poses() if frame_idx > 305: return cur_frame = frames.frames[frame_idx] cur_Tcw = cur_frame['extrinsic_Tcw'] cur_name = cur_frame['file_name'] cur_depth_name = cur_frame['depth_file_name'] print(cur_name) K = K_from_frame(cur_frame) # Read image cur_img = cv2.imread(os.path.join(base_dir, cur_name)).astype( np.float32) / 255.0 cur_depth = load_depth_from_png(os.path.join(base_dir, cur_depth_name), div_factor=5000) # Crop with new intrinsic cur_img = crop_by_intrinsic(cur_img, K, in_K, interp_method='nearest') # next_img = crop_by_intrinsic(next_img, K, in_K) cur_depth = crop_by_intrinsic(cur_depth, K, in_K, interp_method='nearest') h, w, c = cur_img.shape # rel_T = cam_opt.relateive_pose(cur_Tcw[:3, :3], cur_Tcw[:3, 3], next_Tcw[:3, :3], next_Tcw[:3, 3]) X_3d = cam_opt.pi_inv(K, x_2d.reshape((h * w, 2)), cur_depth.reshape((h * w, 1))) cur_Twc = cam_opt.camera_pose_inv(cur_Tcw[:3, :3], cur_Tcw[:3, 3]) X_3d = cam_opt.transpose(cur_Twc[:3, :3], cur_Twc[:3, 3], X_3d) vis.set_point_cloud(X_3d, cur_img.reshape((h * w, 3))) vis.add_frame_pose(cur_Tcw[:3, :3], cur_Tcw[:3, 3], camera_obj_scale=0.01) frame_idx += 1 return
def keyPressEvent(obj, event): global frame_idx key = obj.GetKeySym() if key == 'Right': cur_frame = frames.frames[frame_idx] cur_Tcw = cur_frame['extrinsic_Tcw'] cur_name = cur_frame['file_name'] cur_depth_name = cur_frame['depth_file_name'] next_frame = frames.frames[frame_idx + 1] next_Tcw = next_frame['extrinsic_Tcw'] next_name = next_frame['file_name'] K = K_from_frame(cur_frame) # Read image cur_img = cv2.imread(os.path.join(base_dir, cur_name)).astype( np.float32) / 255.0 next_img = cv2.imread(os.path.join(base_dir, next_name)).astype( np.float32) / 255.0 cur_depth = load_depth_from_png(os.path.join(base_dir, cur_depth_name)) h, w, c = cur_img.shape rel_T = cam_opt.relateive_pose(cur_Tcw[:3, :3], cur_Tcw[:3, 3], next_Tcw[:3, :3], next_Tcw[:3, 3]) X_3d = cam_opt.pi_inv(K, x_2d.reshape((h * w, 2)), cur_depth.reshape((h * w, 1))) cur_Twc = cam_opt.camera_pose_inv(cur_Tcw[:3, :3], cur_Tcw[:3, 3]) X_3d = cam_opt.transpose(cur_Twc[:3, :3], cur_Twc[:3, 3], X_3d) vis.set_point_cloud(X_3d, cur_img.reshape((h * w, 3))) vis.add_frame_pose(cur_Tcw[:3, :3], cur_Tcw[:3, 3]) frame_idx += 20 return
def get_pt_from_img(img, depth, K): H, W = depth.shape x_2d = x_2d_coords(H, W).reshape((H * W, 2)) X_3d = pi_inv(K, x_2d, depth.reshape((H * W, 1))) colors = img.reshape((H * W, 3)) / 255.0 return X_3d, colors