def get_poses(self, path, seq_index, dt, keep_in_ram=False):
if self.is_on_disk():
self.load(path)
poses = self.poses
poseA = Transform.from_matrix(poses[0])
poseB = Transform.from_matrix(poses[1])
delta = delta_transform(poseA, poseB).to_parameters(isDegree=True)
delta = torch.tensor(delta).type(torch.FloatTensor)
if not keep_in_ram:
self.clear()
return [delta, poseA, poseB]
def get_frame(self, i):
pose = Transform.from_matrix(self.poses[i].reshape(4, 4))
rgb = np.array(Image.open(os.path.join(self.root, "{}.png".format(i))))
depth = np.array(
Image.open(os.path.join(self.root,
"{}d.png".format(i)))).astype(np.uint16)
return pose, rgb, depth
def matrices_to_param(matrices):
parameters = []
for matrix in matrices:
# convert a 4x4 matrix to euler
parameters.append(
Transform.from_matrix(matrix.reshape(4, 4)).to_parameters())
return np.array(parameters)
def eval_pose_error(ground_truth, prediction):
diffs = []
for pred, gt in zip(prediction, ground_truth):
# Comput RT*R
rtr = Transform()
rtr[0:3, 0:3] = pred.reshape(4, 4)[0:3, 0:3].dot(
gt.reshape(4, 4)[0:3, 0:3].transpose())
# Keep rotation and translation differences
pred_param = Transform.from_matrix(pred.reshape(4, 4)).to_parameters()
gt_param = Transform.from_matrix(gt.reshape(4, 4)).to_parameters()
diff = np.zeros(6)
diff[3:] = np.abs(Transform.from_matrix(rtr).to_parameters()[3:])
diff[:3] = np.abs(pred_param[:3] - gt_param[:3])
diffs.append(diff)
pose_diff = np.array(diffs)
pose_diff_t = translation_distance(pose_diff[:, 0], pose_diff[:, 1],
pose_diff[:, 2])
pose_diff_r = rotation_distance(pose_diff[:, 3], pose_diff[:, 4],
pose_diff[:, 5])
return pose_diff_t, pose_diff_r