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