def calc_RT_delta( pose_src, pose_tgt, T_means, T_stds, rot_coord="MODEL", rot_type="MATRIX" ): """ project the points in source corrd to target corrd :param pose_src: pose matrix of soucre, [R|T], 3x4 :param pose_tgt: pose matrix of target, [R|T], 3x4 :param rot_coord: model/camera :param rot_type: quat/euler/matrix :return: Rm_delta :return: T_delta """ if rot_coord.lower() == "naive": se3_src2tgt = se3_mul(pose_tgt, se3_inverse(pose_src)) Rm_delta = se3_src2tgt[:, :3] T_delta = se3_src2tgt[:, 3].reshape((3)) else: Rm_delta = R_inv_transform(pose_src[:3, :3], pose_tgt[:3, :3], rot_coord) T_delta = T_inv_transform( pose_src[:, 3], pose_tgt[:, 3], T_means, T_stds, rot_coord ) if rot_type.lower() == "quat": r = mat2quat(Rm_delta) elif rot_type.lower() == "euler": r = mat2euler(Rm_delta) elif rot_type.lower() == "matrix": r = Rm_delta else: raise Exception("Unknown rot_type: {}".format(rot_type)) t = np.squeeze(T_delta) return r, t
def get_point_cloud(pairdb, config, scale_ind_list, X_list=None, phase="train"): assert config.TRAIN.MASK_SYN is 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_gt_observed" in pair_rec: depth_observed_raw = cv2.imread(pair_rec["depth_gt_observed"], cv2.IMREAD_UNCHANGED).astype( np.float32) else: depth_observed_raw = cv2.imread(pair_rec["depth_observed"], cv2.IMREAD_UNCHANGED).astype( np.float32) depth_observed_raw /= config.dataset.DEPTH_FACTOR # needs to be checked !!! if "mask_gt_observed" in pair_rec and config.network.MASK_INPUTS: mask_observed_path = pair_rec["mask_gt_observed"] assert os.path.exists( mask_observed_path), "{} does not exist".format( pair_rec["mask_observed"]) mask_observed = cv2.imread(mask_observed_path, cv2.IMREAD_UNCHANGED) depth_observed = np.zeros(depth_observed_raw.shape) depth_observed[mask_observed == pair_rec["mask_idx"]] = depth_observed_raw[ mask_observed == pair_rec["mask_idx"]] else: depth_observed = depth_observed_raw if X_list: X = X_list[batch_idx] else: X = backproject_camera( depth_observed, intrinsic_matrix=config.dataset.INTRINSIC_MATRIX) transform_r2i = se3_mul(pair_rec["pose_rendered"], se3_inverse(pair_rec["pose_observed"])) X_obj = np.matmul( transform_r2i, np.append(X, np.ones([1, X.shape[1]], dtype=np.float32), axis=0)).reshape((1, 3, depth_observed.shape[0], depth_observed.shape[1])) X_obj_weights = (depth_observed != 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 calc_se3(pose_src, pose_tgt): """ project the points in source corrd to target corrd :param pose_src: pose matrix of soucre, [R|T], 3x4 :param pose_tgt: pose matrix of target, [R|T], 3x4 :return: visible: whether points in source can be viewed in target """ se3_src2tgt = se3_mul(pose_tgt, se3_inverse(pose_src)) rotm = se3_src2tgt[:, :3] t = se3_src2tgt[:, 3].reshape((3)) return rotm, t
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