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