def get_point_cloud(pairdb, config, scale_ind_list, X_list=None, phase='train'): assert config.TRAIN.MASK_SYN == False, "NOT IMPLEMENTED" from lib.utils.projection import backproject_camera, se3_inverse, se3_mul num_pairs = len(pairdb) X_obj_tensor = [] X_obj_weights_tensor = [] for batch_idx in range(num_pairs): pair_rec = pairdb[batch_idx] if 'depth_render_real' in pair_rec: depth_real_raw = cv2.imread(pair_rec['depth_render_real'], cv2.IMREAD_UNCHANGED).astype( np.float32) else: depth_real_raw = cv2.imread(pair_rec['depth_real'], cv2.IMREAD_UNCHANGED).astype( np.float32) depth_real_raw /= config.dataset.DEPTH_FACTOR # needs to be checked !!! if 'mask_real_gt' in pair_rec and config.network.MASK_INPUTS: mask_real_path = pair_rec['mask_real_gt'] assert os.path.exists(mask_real_path), '{} does not exist'.format( pair_rec['mask_real']) mask_real = cv2.imread(mask_real_path, cv2.IMREAD_UNCHANGED) depth_real = np.zeros(depth_real_raw.shape) depth_real[mask_real == pair_rec['mask_idx']] = depth_real_raw[ mask_real == pair_rec['mask_idx']] else: depth_real = depth_real_raw if X_list: X = X_list[batch_idx] else: X = backproject_camera( depth_real, intrinsic_matrix=config.dataset.INTRINSIC_MATRIX) transform_r2i = se3_mul(pair_rec['pose_est'], se3_inverse(pair_rec['pose_real'])) X_obj = np.matmul(transform_r2i, np.append(X, np.ones([1, X.shape[1]], dtype=np.float32), axis=0))\ .reshape((1, 3, depth_real.shape[0], depth_real.shape[1])) X_obj_weights = (depth_real != 0).astype(np.float32) X_obj_weights = np.tile(X_obj_weights[np.newaxis, np.newaxis, :, :], (1, 3, 1, 1)) # X_obj_weights = X_obj_weights[np.newaxis, np.newaxis, :, :] X_obj_tensor.append(X_obj) X_obj_weights_tensor.append(X_obj_weights) return X_obj_tensor, X_obj_weights_tensor
def flow2se3(depth_object, flow, mask_image, K): """ give flow from object to image, calculate the pose :param depth_object: height x width, ndarray the depth map of object image. :param flow: height x width x (w, h) flow from object image to real image :param mask_image: height x width, the mask of real image :param K: 3x3 intrinsic matrix :return: se3: 3x4 matrix. """ height = depth_object.shape[0] width = depth_object.shape[1] assert mask_image.shape == (height, width) valid_in_object = (depth_object != 0).flatten() all_op = backproject_camera(depth_object, intrinsic_matrix=K) # all_op = all_op.reshape((3, width, height)) x, y = np.meshgrid(np.arange(width), np.arange(height)) x = x.astype(np.float64) y = y.astype(np.float64) x += flow[:, :, 0] y += flow[:, :, 1] x = x.flatten() y = y.flatten() all_ip = np.vstack((x, y)) valid_in_image = (mask_image != 0).flatten() valid = np.where(np.logical_and(valid_in_object, valid_in_image))[0] objectPoints = all_op[:, valid].astype(np.float64).transpose() imagePoints = all_ip[:, valid].astype(np.float64).transpose() convex, rvec, tvec, inliers = cv2.solvePnPRansac(objectPoints, imagePoints, K, np.zeros(4)) se3_q = np.zeros(7) if convex: R, _ = cv2.Rodrigues(rvec) se3_q[:4] = RT_transform.mat2quat(R) se3_q[4:] = tvec.flatten() return convex, se3_q else: se3_q[0] = 1 return convex, se3_q
def calc_flow(depth_src, pose_src, pose_tgt, K, depth_tgt, thresh=3E-3, standard_rep=False): """ project the points in source corrd to target corrd :param standard_rep: :param depth_src: depth image of source(m) :param pose_src: pose matrix of soucre, [R|T], 3x4 :param depth_tgt: depth image of target :param pose_tgt: pose matrix of target, [R|T], 3x4 :param K: intrinsic_matrix :param depth_tgt: depth image of target(m) :return: visible: whether points in source can be viewed in target :return: flow: flow from source to target """ height = depth_src.shape[0] width = depth_src.shape[1] visible = np.zeros(depth_src.shape[:2]).flatten() X = backproject_camera(depth_src, intrinsic_matrix=K) transform = np.matmul(K, se3_mul(pose_tgt, se3_inverse(pose_src))) Xp = np.matmul( transform, np.append(X, np.ones([1, X.shape[1]], dtype=np.float32), axis=0)) pz = Xp[2] + 1E-15 pw = Xp[0] / pz ph = Xp[1] / pz valid_points = np.where(depth_src.flatten() != 0)[0] depth_proj_valid = pz[valid_points] pw_valid_raw = np.round(pw[valid_points]).astype(int) pw_valid = np.minimum(np.maximum(pw_valid_raw, 0), width - 1) ph_valid_raw = np.round(ph[valid_points]).astype(int) ph_valid = np.minimum(np.maximum(ph_valid_raw, 0), height - 1) p_within = np.logical_and( np.logical_and(pw_valid_raw >= 0, pw_valid_raw < width), np.logical_and(ph_valid_raw >= 0, ph_valid_raw < height)) depth_tgt_valid = depth_tgt[ph_valid, pw_valid] p_within = np.logical_and( p_within, np.abs(depth_tgt_valid - depth_proj_valid) < thresh) p_valid = np.abs(depth_tgt_valid) > 1E-10 fg_points = valid_points[np.logical_and(p_within, p_valid)] visible[fg_points] = 1 visible = visible.reshape(depth_src.shape[:2]) w_ori, h_ori = np.meshgrid(np.linspace(0, width - 1, width), np.linspace(0, height - 1, height)) if standard_rep: flow = np.dstack([ pw.reshape(depth_src.shape[:2]) - w_ori, ph.reshape(depth_src.shape[:2]) - h_ori ]) else: # depleted version, only used in old code flow = np.dstack([ ph.reshape(depth_src.shape[:2]) - h_ori, pw.reshape(depth_src.shape[:2]) - w_ori ]) flow[np.dstack([visible, visible]) != 1] = 0 assert np.isnan(flow).sum() == 0 X_valid = np.array([c[np.where(visible.flatten())] for c in X]) return flow, visible, X_valid